public static Node[] FindPath(Node start, Node goal, Graphs.DirectedGraph <NodeData, EData> graph, Func <Node, Node, double> heuristic)
{
List <Node> visited = new List <Node>();
dataStructureHeap frontier = new dataStructureHeap(new List <HeapNode>(), dataStructureHeap.HeapProperty.MinHeap);
start.IntermediateCost = 0;
start.EstimatedCost = 0;
start.Parent = null;
frontier.HeapInsert(start);
//Continue as long as there are nodes to explore in the frontier
while (frontier.Count > 0)
{
Node current = (Node)frontier.HeapExtractRoot();
if (current == goal)
{
return(ReconstructPath(current));
}
visited.Add(current);
foreach (var edgeTo in graph.GetEdges(current)) //Expand frontier
{
Node to = (Node)edgeTo.Key;
if (visited.Contains(to))
{
continue;
}
double tentativeScore = current.IntermediateCost + ((EdgeData)edgeTo.Value).cost;
if (tentativeScore < to.IntermediateCost) //Best path to this node so far
{
to.IntermediateCost = tentativeScore;
to.EstimatedCost = tentativeScore + heuristic(to, goal);
to.Parent = current;
}
if (!frontier.Contains(to))
{
frontier.HeapInsert(to);
}
}
}
return(null);
}