/// <summary> /// Find the shortest path using the A* algorithm and a Euclidean distance heuristic /// </summary> public TilePath Plan(TilePosition start, TileRect end) { heap.Clear(); searchedTilesOverlay.Clear(); var startNode = heap.NodeAt(start); startNode.Predecessor = null; heap.DecreaseKey(startNode, 0, 0); var currentNode = startNode; while (!heap.IsEmpty && !end.Contains((currentNode = heap.ExtractMin()).Position)) { searchedTilesOverlay.Add(currentNode.Position); foreach (var n in currentNode.Neighbors) { float newDistanceFromStart = currentNode.DistanceFromStart // Cost so far + TilePosition.EuclideanDistance(currentNode.Position, n.Position) // Edge cost + TilePosition.EuclideanDistance(n.Position, end); // Estimated cost to end if (n.DistanceFromStart > newDistanceFromStart) { n.Predecessor = currentNode; heap.DecreaseKey(n, newDistanceFromStart, newDistanceFromStart); } } } heap.SetOverlayToComputedPath(this.computedPathOverlay, currentNode); searchedTilesOverlay.Clear(); searchedTilesOverlay.SetRect(end); if (!end.Contains(currentNode.Position)) { return(null); } return(this.MakePath(currentNode)); }