public static List <IPathNode> FindPath(IPathNode start, IPathNode finish)
{
if (start == finish)
{
return(new List <IPathNode>());
}
var frontier = new GenericPriorityQueue <IPathNode, int>(1000);
frontier.Enqueue(start, 0);
var cameFrom = new Dictionary <IPathNode, IPathNode>();
var costSoFar = new Dictionary <IPathNode, int>();
IPathNode current;
IPathNode next;
List <IPathNode> neighbours;
int newCost;
int priority;
cameFrom[start] = null;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (current == finish)
{
break;
}
neighbours = current.GetNeighbours();
for (int i = 0; i < neighbours.Count; i++)
{
next = neighbours[i];
newCost = costSoFar[current] + (next == finish || next.IsWalkable ? 1 : 10000);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
if (costSoFar.ContainsKey(next))
{
costSoFar[next] = newCost;
}
else
{
costSoFar.Add(next, newCost);
}
priority = newCost + finish.GetHeuristic(next);
if (frontier.Contains(next))
{
frontier.UpdatePriority(next, priority);
}
else
{
frontier.Enqueue(next, priority);
}
cameFrom[next] = current;
}
}
}
var path = new List <IPathNode>();
if (cameFrom.ContainsKey(finish))
{
IPathNode last = finish;
path.Add(finish);
while (cameFrom[last] != start)
{
path.Add(cameFrom[last]);
last = cameFrom[last];
}
path.Reverse();
}
return(path);
}