private void Search()
{
//create an indexed priority queue of nodes. The nodes with the
//lowest overall F cost (G+H) are positioned at the front.
IndexedPriorityQueueLow 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();
//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 (NavGraphEdge curEdge in Graph.Edges[nextClosestNode])
{
//calculate (H) the heuristic cost from this node to
//the target
float hCost =
HeuristicEuclid.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(int source)
{
//create a priority queue
IndexedPriorityQueueLow pq =
new IndexedPriorityQueueLow(CostToThisNode, Graph.NumNodes);
//put the source node on the queue
pq.Insert(source);
//while the queue is not empty
while (!pq.Empty())
{
//get lowest cost edge from the queue
int best = pq.Pop();
//move this edge from the fringe to the spanning tree
SpanningTree[best] = Fringe[best];
//now to test the edges attached to this node
foreach (NavGraphEdge curEdge in Graph.Edges[best])
{
float priority = curEdge.Cost;
if (Fringe[curEdge.To] == null)
{
CostToThisNode[curEdge.To] = priority;
pq.Insert(curEdge.To);
Fringe[curEdge.To] = curEdge;
}
else if ((priority < CostToThisNode[curEdge.To]) &&
(SpanningTree[curEdge.To] == null))
{
CostToThisNode[curEdge.To] = priority;
pq.ChangePriority(curEdge.To);
Fringe[curEdge.To] = curEdge;
}
}
}
}
private void Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back). Note that the maximum number of elements the
//priority queue may contain is N. This is because no node can be
//represented on the queue more than once.
IndexedPriorityQueueLow pq =
new IndexedPriorityQueueLow(CostToThisNode, 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. Don't forget, the return
//value /is a *node index*, not the node itself. This node is
//the node not already on the SPT that is the closest to the
//source node
int nextClosestNode = pq.Pop();
//move this edge from the frontier to the shortest path tree
ShortestPathTree[nextClosestNode] =
SearchFrontier[nextClosestNode];
//if the target has been found exit
if (nextClosestNode == Target) return;
//now to relax the edges.
foreach (NavGraphEdge curEdge in Graph.Edges[nextClosestNode])
{
//the total cost to the node this edge points to is the cost
//to the current node plus the cost of the edge connecting
//them.
float newCost =
CostToThisNode[nextClosestNode] + curEdge.Cost;
//if this edge has never been on the frontier make a note
//of the cost to get to the node it points to, then add the
//edge to the frontier and the destination node to the PQ.
if (SearchFrontier[curEdge.To] == null)
{
CostToThisNode[curEdge.To] = newCost;
pq.Insert(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
//else test to see if the cost to reach the destination node
//via the current node is cheaper than the cheapest cost
//found so far. If this path is cheaper, we assign the new
//cost to the destination node, update its entry in the PQ
//to reflect the change and add the edge to the frontier
else if ((newCost < CostToThisNode[curEdge.To]) &&
(ShortestPathTree[curEdge.To] == null))
{
CostToThisNode[curEdge.To] = newCost;
//because the cost is less than it was previously,
//the PQ must be re-sorted to account for this.
pq.ChangePriority(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
}
}
}