private void Search(HeuristicDelegate calculate) { NodesSearched = 0; // used for performance measuring // create an indexed priority queue of nodes. The nodes with the // lowest overall F cost (G+H) are positioned at the front. var pq = new IndexedPriorityQueueLow(FCosts, Graph.NumNodes); // put the source node on the queue pq.Insert(Source); // while the queue is not empty while (!pq.Empty()) { // get lowest cost node from the queue int nextClosestNode = pq.Pop(); NodesSearched++; // move this node from the frontier to the spanning tree ShortestPathTree[nextClosestNode] = SearchFrontier[nextClosestNode]; // if the target has been found exit if (nextClosestNode == Target) { return; } // now to test all the edges attached to this node foreach (Edge curEdge in Graph.Edges[nextClosestNode]) { // calculate (H) the heuristic cost from this node to // the target float hCost = calculate(Graph, Target, curEdge.To); // calculate (G) the 'real' cost to this node from the source float gCost = GCosts[nextClosestNode] + curEdge.Cost; // if the node has not been added to the frontier, add it and // update the G and F costs if (SearchFrontier[curEdge.To] == null) { FCosts[curEdge.To] = gCost + hCost; GCosts[curEdge.To] = gCost; pq.Insert(curEdge.To); SearchFrontier[curEdge.To] = curEdge; } // if this node is already on the frontier but the cost to // get here is cheaper than has been found previously, update // the node costs and frontier accordingly. else if ((gCost < GCosts[curEdge.To]) && (ShortestPathTree[curEdge.To] == null)) { FCosts[curEdge.To] = gCost + hCost; GCosts[curEdge.To] = gCost; pq.ChangePriority(curEdge.To); SearchFrontier[curEdge.To] = curEdge; } } } }
private void Search(HeuristicDelegate calculate) { NodesSearched = 0; // used for performance measuring // create an indexed priority queue of nodes. The nodes with the // lowest overall F cost (G+H) are positioned at the front. var pq = new IndexedPriorityQueueLow(FCosts, Graph.NumNodes); // put the source node on the queue pq.Insert(Source); // while the queue is not empty while (!pq.Empty()) { // get lowest cost node from the queue int nextClosestNode = pq.Pop(); NodesSearched++; // move this node from the frontier to the spanning tree ShortestPathTree[nextClosestNode] = SearchFrontier[nextClosestNode]; // if the target has been found exit if (nextClosestNode == Target) { return; } // now to test all the edges attached to this node foreach (Edge curEdge in Graph.Edges[nextClosestNode]) { // calculate (H) the heuristic cost from this node to // the target float hCost = calculate(Graph, Target, curEdge.To); // calculate (G) the 'real' cost to this node from the source float gCost = GCosts[nextClosestNode] + curEdge.Cost; // if the node has not been added to the frontier, add it and // update the G and F costs if (SearchFrontier[curEdge.To] == null) { FCosts[curEdge.To] = gCost + hCost; GCosts[curEdge.To] = gCost; pq.Insert(curEdge.To); SearchFrontier[curEdge.To] = curEdge; } // if this node is already on the frontier but the cost to // get here is cheaper than has been found previously, update // the node costs and frontier accordingly. else if ((gCost < GCosts[curEdge.To]) && (ShortestPathTree[curEdge.To] == null)) { FCosts[curEdge.To] = gCost + hCost; GCosts[curEdge.To] = gCost; pq.ChangePriority(curEdge.To); SearchFrontier[curEdge.To] = curEdge; } } } }