111 lines
3.7 KiB
C#
111 lines
3.7 KiB
C#
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.
|
|
/// </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;
|
|
}
|
|
}
|
|
}
|