/// <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));
}
