/// <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);
}
public static List <Position> FindPath(Grid grid, Position start, Position end, Offset[] movementPattern, int iterationLimit)
{
ClearStepList();
if (start == end)
{
return(new List <Position> {
start
});
}
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, open, cameFrom, costSoFar, movementPattern, current, end);
MessageClose(current);
--iterationLimit;
}
return(null);
}
/// <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="path">Returns shortest path from start to the end if found. Can also return partial path if end is not reachable. In case of error returns empty array.</param>
/// <param name="iterationLimit">Maximum count of iterations</param>
/// <returns>Result of the path finding.</returns>
public static PathFindResult TryFindPath(Grid grid, Position start, Position end, Offset[] movementPattern, AgentShape shape, out List <Position> path, int iterationLimit = int.MaxValue)
{
ClearStepList();
// If is where should be, then there's no path to take
if (start == end)
{
path = new List <Position>();
return(PathFindResult.AlreadyAtTheEnd);
}
// 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 = Boundary.FromSizeAndShape(grid.DimX, grid.DimY, shape);
// Make sure that agent is within grid. This means agent shape has to be considered.
if (!boundary.IsInside(start))
{
path = new List <Position>();
return(PathFindResult.StartOutsideBoundaries);
}
// Make sure that end position is within grid
if (!grid.IsInside(end))
{
path = new List <Position>();
return(PathFindResult.EndOutsideBoundaries);
}
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];
// Keep record of closest point in case the end point is not reachable
Position closestPosition = start;
float closestDistance = float.PositiveInfinity;
// Try to find path until options run out or iterations limit is exceeded
while (open.HasNext() && iterationLimit > 0)
{
// Get the best candidate
var current = open.Pop().Position;
MessageCurrent(current, PartiallyReconstructPath(grid, start, current, cameFrom));
if (current == end)
{
path = ReconstructPath(grid, start, end, cameFrom);
return(PathFindResult.PathFound);
}
Step(grid, boundary, open, cameFrom, costSoFar, movementPattern, shape, current, end, ref closestPosition, ref closestDistance);
MessageClose(current);
--iterationLimit;
}
// Construct path to the closest position
if (start != closestPosition)
{
MessageCurrent(closestPosition, PartiallyReconstructPath(grid, start, closestPosition, cameFrom));
path = ReconstructPath(grid, start, closestPosition, cameFrom);
return(PathFindResult.PartialPathFound);
}
// Stuck
path = new List <Position>();
return(PathFindResult.Stuck);
}
