Shogi/PathFinding/PathFinder2D.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;

namespace PathFinding
{
  public class PathFinder2D<T> where T : IPlanarElement
  {
    /// <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, respecting the Paths available to the element at origin.
    /// </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(Vector2 origin, Vector2 destination, Callback callback = null)
    {
      if (destination.X > width - 1 || destination.Y > height - 1 || destination.X < 0 || destination.Y < 0)
      {
        return false;
      }
      var element = collection[origin.X, origin.Y];
      var path = FindDirectionTowardsDestination(element.GetPaths(), origin, destination);

      if (!IsPathable(origin, destination, path.Direction))
      {
        // Assumption: if a single best-choice step towards the destination cannot happen, no pathing can happen.
        return false;
      }

      var shouldPath = true;
      var next = origin;
      while (shouldPath && next != destination)
      {
        next = Vector2.Add(next, path.Direction);
        var collider = collection[(int)next.X, (int)next.Y];
        if (collider != null)
        {
          callback?.Invoke(collider, next);
          shouldPath = false;
        }
        else if (path.Distance == Distance.OneStep)
        {
          shouldPath = false;
        }
      }
      return next == destination;
    }

    public void PathEvery(Vector2 from, Callback callback)
    {
      var element = collection[from.X, from.Y];
      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);
            shouldPath = false;
          }
          if (path.Distance == Distance.OneStep)
          {
            shouldPath = false;
          }
          next = Vector2.Add(next, path.Direction);
        }
      }
    }

    /// <summary>
    /// Path the line from origin to destination, ignoring any Paths defined by the element at origin.
    /// </summary>
    public void LinePathTo(Vector2 origin, Vector2 direction, Callback callback)
    {
      direction = Vector2.Normalize(direction);

      var next = Vector2.Add(origin, direction);
      while (next.X >= 0 && next.X < width && next.Y >= 0 && next.Y < height)
      {
        var element = collection[next.X, next.Y];
        if (element != null) callback(element, next);
        next = Vector2.Add(next, direction);
      }
    }

    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);
      return IsPathable(origin, destination, path.Direction);
    }
    public bool IsPathable(Vector2 origin, Vector2 destination, Vector2 direction)
    {
      direction = Vector2.Normalize(direction);
      var next = Vector2.Add(origin, direction);
      if (Vector2.Distance(next, destination) >= Vector2.Distance(origin, destination)) return false;

      var slope = (destination.Y - origin.Y) / (destination.X - origin.X);
      if (float.IsInfinity(slope))
      {
        return next.X == destination.X;
      }
      else
      {
        // b = -mx + y
        var yIntercept = -slope * origin.X + origin.Y;
        // y = mx + b
        return next.Y == slope * next.X + yIntercept;
      }
    }
  }
}