bool CalculatePath(NavNode source, NavNode target, out List <NavNode> outPath)
{
List <NavNode> openNodes = new List <NavNode>();
HashSet <NavNode> closedNodes = new HashSet <NavNode>();
NavNode[] cameFromTable = new NavNode[nodes.Count];
// Add the source node
source.heuristicCost = useDijkstra ? 0 : target.Distance(source);
source.travelCost = 0;
openNodes.Add(source);
// Search for all avaliable paths
while (openNodes.Count != 0)
{
// Find lowest cost open node
NavNode current = openNodes[0];
for (int i = 1; i < openNodes.Count; ++i)
{
float a = openNodes[i].totalCost;
float b = current.totalCost;
if (a < b || (a == b && openNodes[i].travelCost < current.travelCost))
{
current = openNodes[i];
}
}
// Lock current node's states
openNodes.Remove(current);
closedNodes.Add(current);
// Reached target
if (current == target)
{
// Reconstruct path in reverse then flip
outPath = new List <NavNode>();
while (current != null)
{
outPath.Add(current);
current = cameFromTable[current.id];
}
outPath.Reverse();
return(true);
}
// Add neighbours
foreach (int neighbourId in current.neighbours)
{
NavNode neighbour = nodes[neighbourId];
// Ignore locked nodes
if (closedNodes.Contains(neighbour))
{
continue;
}
// Check to see if this path is faster for open nodes
if (openNodes.Contains(neighbour))
{
float h = useDijkstra ? 0 : target.Distance(neighbour);
float g = current.Distance(neighbour) + current.totalCost;
// New route is better
if (neighbour.totalCost > h + g)
{
neighbour.heuristicCost = h;
neighbour.travelCost = g;
openNodes.Add(neighbour);
cameFromTable[neighbour.id] = current;
}
}
// Not considered yet
else
{
// Workout it's values
neighbour.heuristicCost = useDijkstra ? 0 : target.Distance(neighbour);
neighbour.travelCost = current.Distance(neighbour) + current.totalCost;
openNodes.Add(neighbour);
cameFromTable[neighbour.id] = current;
}
}
}
// Has failed to reach target if reached here
outPath = null;
return(false);
}