public SysClG.List <LibPF.Position> FindPath(LibPF.Grid grid, LibPF.Position start, LibPF.Position end, LibPF.Offset[] movementPattern, int iterationLimit)
{
#if DEBUG
this.ClearStepList();
#endif
if (start == end)
{
return(new SysClG.List <LibPF.Position> {
start
});
}
LibPF.Finder.MinHeapNode head = new LibPF.Finder.MinHeapNode(start, this.ManhattanDistance(start, end));
LibPF.Finder.MinHeap open = new LibPF.Finder.MinHeap();
open.Push(head);
float[] costSoFar = new float[grid.DimX * grid.DimY];
LibPF.Position[] cameFrom = new LibPF.Position[grid.DimX * grid.DimY];
while (open.HasNext() && iterationLimit > 0)
{
LibPF.Position current = open.Pop().Position;
#if DEBUG
this.MessageCurrent(current, this.PartiallyReconstructPath(grid, start, current, cameFrom));
#endif
if (current == end)
{
return(this.ReconstructPath(grid, start, end, cameFrom));
}
this.StepOn(grid, open, cameFrom, costSoFar, movementPattern, current, end);
#if DEBUG
this.MessageClose(current);
#endif
--iterationLimit;
}
return(null);
}
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
}
}