/// <summary> /// Calculate the shortest path between two points /// </summary> /// <param name="start"> Starting point </param> /// <param name="end"> Ending point </param> /// <param name="diagonal"> Allow diagonal movements </param> /// <returns> List of points included in the path (in order) </returns> public List <Point> FindPath(Point start, Point end, bool diagonal) { reset(); var openHeap = new BinaryHeap <PathNode>(); // find the node at the starting point, close it, and add it // to the heap var startNode = grid[start.X][start.Y]; startNode.Closed = true; openHeap.Add(startNode); // iterate until there are no more items in the heap while (openHeap.Count > 0) { var currentNode = openHeap.Pop(); // reached our destination if (currentNode.Index == end) { var current = currentNode; var path = new List <Point>(); path.Add(start); while (current.Parent != null) { path.Add(current.Index); current = current.Parent; } path.Reverse(); return(path); } currentNode.Closed = true; var neighbors = getNeighbors(currentNode, diagonal); foreach (var neighbor in neighbors) { // don't need to check it if (neighbor.Closed || !neighbor.IsPassible) { continue; } // the g score is the shortest distance from start // to the current node. We need to check if the current // neighbor to this node is the lowest cost so far int g = currentNode.G + neighbor.Cost; bool hasVisited = neighbor.Visited; if (!hasVisited || g < neighbor.G) { // found the best node so far, so estimate it's cost // and deal with it in the heap accordingly neighbor.Visited = true; neighbor.Parent = currentNode; neighbor.H = manhattanDistance(neighbor.Index, end); neighbor.G = g; neighbor.F = neighbor.G + neighbor.H; // add the neighbor to the heap if we haven't visited // it yet, otherwise re score it in the heap if (!hasVisited) { openHeap.Add(neighbor); } else { openHeap.RescoreItem(neighbor); } } } } return(new List <Point>()); }
public void AddTailUnchecked(PathTriangle p, BinaryHeap <float, PathTriangle> addHere) { float key = p.prevDistance * pathAccuracy + p.DistanceToTarget(target); addHere.Enqueue(key, p); }