private void StepOn(LibPF.Grid grid, LibPF.Finder.MinHeap open, LibPF.Position[] cameFrom, float[] costSoFar, LibPF.Offset[] movementPattern, LibPF.Position current, LibPF.Position end)
{
float initialCost = costSoFar[grid.GetIndexUnchecked(current.X, current.Y)];
foreach (LibPF.Offset option in this.GetMovementOptions(current, grid.DimX, grid.DimY, movementPattern))
{
LibPF.Position position = current + option;
float cellCost = grid.GetCellCostUnchecked(position);
if (float.IsInfinity(cellCost))
{
continue;
}
int index = grid.GetIndexUnchecked(position.X, position.Y);
float newCost = initialCost + cellCost * option.Cost;
float oldCost = costSoFar[index];
if (!(oldCost <= 0) && !(newCost < oldCost))
{
continue;
}
costSoFar[index] = newCost;
cameFrom[index] = current;
newCost = newCost + this.ManhattanDistance(position, end);
open.Push(new LibPF.Finder.MinHeapNode(position, newCost));
#if DEBUG
this.MessageOpen(position);
#endif
}
}
private SysClG.List <LibPF.Position> ReconstructPath(LibPF.Grid grid, LibPF.Position start, LibPF.Position end, LibPF.Position[] cameFrom)
{
SysClG.List <LibPF.Position> path = new SysClG.List <LibPF.Position> {
end
};
LibPF.Position current = end;
do
{
LibPF.Position previous = cameFrom[grid.GetIndexUnchecked(current.X, current.Y)];
current = previous;
path.Add(current);
} while (current != start);
return(path);
}