void Search()
{
IndexedPriorityQueueLow queue = new IndexedPriorityQueueLow(costToThisNode, graph.NumNodes);
//put the source node on the queue
queue.Enqueue(source);
//while the queue is not empty
while (!queue.IsEmpty())
{
//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 = queue.Dequeue();
//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.
//for each edge connected to the next closest node
foreach (GraphEdge edge 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] + edge.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[edge.To] == null)
{
costToThisNode[edge.To] = newCost;
queue.Enqueue(edge.To);
searchFrontier[edge.To] = edge;
}
//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[edge.To] && shortestPathTree[edge.To] == null)
{
costToThisNode[edge.To] = newCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
queue.ChangePriority(edge.To);
searchFrontier[edge.To] = edge;
}
}
}
}
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 queue = new IndexedPriorityQueueLow(FCosts, graph.NumNodes);
//put the source node on the queue
queue.Enqueue(source);
//while the queue is not empty
while (!queue.IsEmpty())
{
//get lowest cost node from the queue
int nextClosestNode = queue.Dequeue();
//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 relax the edges.
//for each edge connected to the next closest node
foreach (GraphEdge edge in graph.edges[nextClosestNode])
{
//calculate the heuristic cost from this node to the target (H)
float HCost = Calculate(graph, target, edge.To);
//calculate the 'real' cost to this node from the source (G)
float GCost = GCosts[nextClosestNode] + edge.Cost;
//if the node has not been added to the frontier, add it and update
//the G and F costs
if (searchFrontier[edge.To] == null)
{
FCosts[edge.To] = GCost + HCost;
GCosts[edge.To] = GCost;
queue.Enqueue(edge.To);
searchFrontier[edge.To] = edge;
}
//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[edge.To] && shortestPathTree[edge.To] == null)
{
FCosts[edge.To] = GCost + HCost;
GCosts[edge.To] = GCost;
queue.ChangePriority(edge.To);
searchFrontier[edge.To] = edge;
}
}
}
}