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