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 } }