public static IList <Node> FindPath <Node>(IGraphAdapter <Node> graph, Node start, Node goal)
{
var comparer = EqualityComparer <Node> .Default;
var searchSpace = new PriorityQueue <Node>();
var pathMap = new Dictionary <Node, Node>();
var costSoFar = new Dictionary <Node, int>();
searchSpace.Enqueue(start, 0);
costSoFar[start] = 0;
while (searchSpace.Count != 0)
{
var current = searchSpace.Dequeue();
if (comparer.Equals(current, goal))
{
break;
}
var linked = graph.GetLinked(current);
foreach (var linkedNode in linked)
{
var newCost = costSoFar[current] + graph.GetMoveCost(current, linkedNode);
if (newCost < costSoFar.GetOrDefault(linkedNode, int.MaxValue))
{
costSoFar[linkedNode] = newCost;
searchSpace.Enqueue(linkedNode, newCost + graph.GetScore(linkedNode, goal));
pathMap[linkedNode] = current;
}
}
}
return(ResolvePath(pathMap, start, goal));
}
