UTests pass with Pathfinder2D

PathFinding/PathFinder2D.cs

@@ -1,10 +1,110 @@
-
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+
 namespace PathFinding
 {
-  public class PathFinder2D
+  public class PathFinder2D<T> where T : IPlanarElement
   {
-    public PathFinder2D()
+    /// <summary>
+    /// </summary>
+    /// <param name="element">Guaranteed to be non-null.</param>
+    /// <param name="position"></param>
+    public delegate void Callback(T collider, Vector2 position);
+
+
+    private readonly IPlanarCollection<T> collection;
+    private readonly int width;
+    private readonly int height;
+    public PathFinder2D(IPlanarCollection<T> collection)
     {
+      this.collection = collection;
+      width = collection.GetLength(0);
+      height = collection.GetLength(1);
+    }
+
+    /// <summary>
+    /// Navigate the collection such that each "step" is always towards the destination.
+    /// </summary>
+    /// <param name="element">The pathing element.</param>
+    /// <param name="origin">The starting location.</param>
+    /// <param name="destination">The destination.</param>
+    /// <param name="callback">Do cool stuff here.</param>
+    /// <returns>True if the element reached the destination.</returns>
+    public bool PathTo(T element, Vector2 origin, Vector2 destination, Callback callback)
+    {
+      if (destination.X > width - 1 || destination.Y > height - 1 || destination.X < 0 || destination.Y < 0)
+      {
+        return false;
+      }
+
+      var path = FindDirectionTowardsDestination(element.GetPaths(), origin, destination);
+      var next = Vector2.Add(origin, path.Direction);
+
+      if (!IsPathable(origin, destination, next))
+      {
+        // Assumption: if a single best-choice step towards the destination cannot happen, no pathing can happen.
+        return false;
+      }
+
+      var shouldPath = true;
+      while (shouldPath)
+      {
+        var collider = collection[(int)next.X, (int)next.Y];
+        if (collider != null) callback(collider, next);
+        if (next == destination) return true;
+        if (path.Distance == Distance.OneStep)
+        {
+          shouldPath = false;
+        }
+        next = Vector2.Add(next, path.Direction);
+      }
+      return true;
+    }
+
+    public void PathEvery(IPlanarElement element, Vector2 from, Callback callback)
+    {
+      foreach (var path in element.GetPaths())
+      {
+        var shouldPath = true;
+        var next = Vector2.Add(from, path.Direction);
+        while (shouldPath && next.X < width && next.Y < height && next.X >= 0 && next.Y >= 0)
+        {
+          var collider = collection[(int)next.X, (int)next.Y];
+          if (collider != null)
+          {
+            callback(collider, next);
+          }
+          next = Vector2.Add(from, path.Direction);
+          if (path.Distance == Distance.OneStep)
+          {
+            shouldPath = false;
+          }
+        }
+      }
+    }
+
+    public Path FindDirectionTowardsDestination(ICollection<Path> paths, Vector2 origin, Vector2 destination) =>
+      paths.Aggregate((a, b) => Vector2.Distance(destination, Vector2.Add(origin, a.Direction)) < Vector2.Distance(destination, Vector2.Add(origin, b.Direction)) ? a : b);
+
+
+    public bool IsPathable(Vector2 origin, Vector2 destination, T element)
+    {
+      var path = FindDirectionTowardsDestination(element.GetPaths(), origin, destination);
+      var next = Vector2.Add(origin, path.Direction);
+      return IsPathable(origin, destination, next);
+    }
+    public bool IsPathable(Vector2 origin, Vector2 destination, Vector2 next)
+    {
+      if (Vector2.Distance(next, destination) < Vector2.Distance(origin, destination))
+      {
+        // y = mx + b
+        // b = -mx + y
+        var slope = (destination.Y - origin.Y) / (destination.X - origin.X);
+        var yIntercept = -(slope * origin.X) + origin.Y;
+        return float.IsInfinity(slope) || next.Y == slope * next.X + yIntercept;
+      }
+      return false;
     }
   }
 }