private static void Step(
Grid grid,
Boundary boundary,
MinHeap open,
Position[] cameFrom,
float[] costSoFar,
Offset[] movementPattern,
AgentShape shape,
Position current,
Position end)
{
// Get the cost associated with getting to the current position
var initialCost = costSoFar[grid.GetIndexUnchecked(current)];
// Get all directions we can move to according to the movement pattern and the dimensions of the grid
foreach (var option in GetMovementOptions(current, boundary, movementPattern))
{
var position = current + option;
var cellCost = grid.GetCellCostUnchecked(position, shape);
// Ignore this option if the cell is blocked
if (float.IsInfinity(cellCost))
{
continue;
}
var index = grid.GetIndexUnchecked(position);
// 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> PartiallyReconstructPath(Grid 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)];
// 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 List <Position> ReconstructPath(Grid 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 SearchNode FindPath(Grid grid, Position start, Position end, Offset[] movementPattern)
{
ClearStepList();
var head = new SearchNode(start);
var open = new MinHeap();
open.Push(head);
var marked = new bool[grid.DimX * grid.DimY];
while (open.HasNext())
{
var current = open.Pop();
MessageCurrent(current);
if (current.Position == end)
{
return(current);
}
foreach (var p in GetNeighbours(current.Position, grid.DimX, grid.DimY, movementPattern))
{
var index = grid.GetIndexUnchecked(p.X, p.Y);
// Use the unchecked variant here since GetNeighbours already filters out positions that are out of bounds
var cellCost = grid.GetCellCostUnchecked(p);
if (!marked[index] && !float.IsInfinity(cellCost))
{
marked[index] = true;
MessageOpen(p);
var costSoFar = current.CostSoFar + cellCost;
var expectedCost = costSoFar + ChebyshevDistance(p, end);
open.Push(new SearchNode(p, expectedCost, costSoFar)
{
Next = current
});
}
}
MessageClose(current.Position);
}
return(null);
}