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); }
public void Push(LibPF.Finder.MinHeapNode node) { if (this.head == null) { this.head = node; } else if (node.ExpectedCost < this.head.ExpectedCost) { node.Next = this.head; this.head = node; } else { LibPF.Finder.MinHeapNode current = this.head; while (current.Next != null && current.Next.ExpectedCost <= node.ExpectedCost) { current = current.Next; } node.Next = current.Next; current.Next = node; } }
public LibPF.Finder.MinHeapNode Pop() { LibPF.Finder.MinHeapNode top = this.head; this.head = this.head.Next; return(top); }