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