/// <summary>
/// Find path
/// </summary>
/// <param name="grid">Grid</param>
/// <param name="start">Start point</param>
/// <param name="end">End point</param>
/// <param name="movementPattern">Movement pattern</param>
/// <param name="shape">Shape of an agent</param>
/// <param name="iterationLimit">Maximum count of iterations</param>
/// <returns>List of movement steps</returns>
public static List <Position> FindPath(Grid grid, Position start, Position end, Offset[] movementPattern, AgentShape shape, int iterationLimit = int.MaxValue)
{
ClearStepList();
// If is where should be, then path is simple
if (start == end)
{
return(new List <Position> {
start
});
}
// To avoid lot of grid boundaries checking calculations during path finding, do the math here.
// So get the possible boundaries considering the shape of the agent.
var boundary = new Boundary()
{
X1 = -shape.TopLeft.X,
Y1 = -shape.TopLeft.Y,
X2 = grid.DimX - shape.BottomRight.X - 1,
Y2 = grid.DimY - shape.BottomRight.Y - 1
};
// Make sure start and end are within boundary
if (!boundary.IsInside(start) || !boundary.IsInside(end))
{
// Can't find such path
return(null);
}
var head = new MinHeapNode(start, ManhattanDistance(start, end));
var open = new MinHeap();
open.Push(head);
var costSoFar = new float[grid.DimX * grid.DimY];
var cameFrom = new Position[grid.DimX * grid.DimY];
while (open.HasNext() && iterationLimit > 0)
{
// Get the best candidate
var current = open.Pop().Position;
MessageCurrent(current, PartiallyReconstructPath(grid, start, current, cameFrom));
if (current == end)
{
return(ReconstructPath(grid, start, end, cameFrom));
}
Step(grid, boundary, open, cameFrom, costSoFar, movementPattern, shape, current, end);
MessageClose(current);
--iterationLimit;
}
return(null);
}
/// <summary>
/// Get movement options within boundary
/// </summary>
/// <param name="position">Current positin</param>
/// <param name="boundary">Boundary</param>
/// <param name="movementPattern">Movement pattern</param>
/// <returns></returns>
private static IEnumerable <Offset> GetMovementOptions(
Position position,
Boundary boundary,
IEnumerable <Offset> movementPattern)
{
return(movementPattern.Where(m => boundary.IsInside(position + m)));
}