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