private static List <Position> PartiallyReconstructPath(AStarGrid grid, Position start, Position end, Position[] cameFrom)
{
var path = new List <Position> {
end
};
#if DEBUG
var current = end;
do
{
var previous = cameFrom[grid.GetIndexUnchecked(current.x, current.y)];
// If the path is invalid, probably becase we've not closed
// a node yet, return an empty list
if (current == previous)
{
return(new List <Position>());
}
current = previous;
path.Add(current);
} while (current != start);
#endif
return(path);
}
private static void Step(
AStarGrid grid,
MinHeap open,
Position[] cameFrom,
float[] costSoFar,
Offset[] movementPattern,
Position current,
Position end)
{
// Get the cost associated with getting to the current position
var initialCost = costSoFar[grid.GetIndexUnchecked(current.x, current.y)];
// Get all directions we can move to according to the movement pattern and the dimensions of the grid
foreach (var option in GetMovementOptions(current, grid.DimX, grid.DimY, movementPattern))
{
var position = current + option;
var cellCost = grid.GetCellCostUnchecked(position);
// Ignore this option if the cell is blocked
if (float.IsInfinity(cellCost))
{
continue;
}
var index = grid.GetIndexUnchecked(position.x, position.y);
// Compute how much it would cost to get to the new position via this path
var newCost = initialCost + cellCost * option.Cost;
// Compare it with the best cost we have so far, 0 means we don't have any path that gets here yet
var oldCost = costSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
// Update the best path and the cost if this path is cheaper
costSoFar[index] = newCost;
cameFrom[index] = current;
// Use the heuristic to compute how much it will probably cost
// to get from here to the end, and store the node in the open list
var expectedCost = newCost + ManhattanDistance(position, end);
open.Push(new MinHeapNode(position, expectedCost));
MessageOpen(position);
}
}
private static List <Position> ReconstructPath(AStarGrid grid, Position start, Position end, Position[] cameFrom)
{
var path = new List <Position> {
end
};
var current = end;
do
{
var previous = cameFrom[grid.GetIndexUnchecked(current.x, current.y)];
current = previous;
path.Add(current);
} while (current != start);
return(path);
}
public static List <Position> FindPath(AStarGrid 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);
}
