public Graph_SearchDijkstra(NavGraph navGraph,
int sourceNodeID,
int targetNodeID = -1)
{
navGraph_ = navGraph;
shortestPathTree_ = new NavGraphEdge[navGraph.NumNodes()];
searchFrontier_ = new NavGraphEdge[navGraph.NumNodes()];
costToThisNode_ = new float[navGraph.NumNodes()];
sourceNodeID_ = sourceNodeID;
targetNodeID_ = targetNodeID;
Search();
}
public Graph_SearchDijkstra(NavGraph navGraph,
int sourceNodeID,
int targetNodeID = -1)
{
navGraph_ = navGraph;
shortestPathTree_ = new NavGraphEdge[navGraph.NumNodes()];
searchFrontier_ = new NavGraphEdge[navGraph.NumNodes()];
costToThisNode_ = new float[navGraph.NumNodes()];
sourceNodeID_ = sourceNodeID;
targetNodeID_ = targetNodeID;
Search();
}
public Graph_SearchAStar_TS(NavGraph graph, int source, int target) : base(SearchType.AStar)
{
graph_ = graph;
shortestPathTree_ = new NavGraphEdge[graph.NumNodes()];
searchFrontier_ = new NavGraphEdge[graph.NumNodes()];
gCosts_ = new float[graph.NumNodes()];
fCosts_ = new float[graph.NumNodes()];
sourceIdx_ = source;
targetIdx_ = target;
//create the PQ
pq_ = new IndexedPriorityQLow(fCosts_, graph_.NumNodes());
//put the source node on the queue
pq_.Insert(sourceIdx_);
}
public Graph_SearchAStar_TS(NavGraph graph, int source, int target)
: base(SearchType.AStar)
{
graph_ = graph;
shortestPathTree_ = new NavGraphEdge[graph.NumNodes()];
searchFrontier_ = new NavGraphEdge[graph.NumNodes()];
gCosts_ = new float[graph.NumNodes()];
fCosts_ = new float[graph.NumNodes()];
sourceIdx_ = source;
targetIdx_ = target;
//create the PQ
pq_ = new IndexedPriorityQLow(fCosts_, graph_.NumNodes());
//put the source node on the queue
pq_.Insert(sourceIdx_);
}
public Graph_SearchDijkstras_TS(NavGraph navGraph,
int sourceNodeIndex,
int targetNodeIndex) : base(Graph_SearchTimeSliced.SearchType.Dijkstra)
{
navGraph_ = navGraph;
shortestPathTree_ = new NavGraphEdge[navGraph_.NumNodes()];
searchFrontier_ = new NavGraphEdge[navGraph_.NumNodes()];
costToThisNode_ = new float[navGraph_.NumNodes()];
sourceNodeIndex_ = sourceNodeIndex;
targetNodeIndex_ = targetNodeIndex;
//create the PQ ,
pq_ = new IndexedPriorityQLow(costToThisNode_, navGraph_.NumNodes());
//put the source node on the queue
pq_.Insert(sourceNodeIndex_);
}
public Graph_SearchDijkstras_TS( NavGraph navGraph,
int sourceNodeIndex,
int targetNodeIndex)
: base(Graph_SearchTimeSliced.SearchType.Dijkstra)
{
navGraph_ = navGraph;
shortestPathTree_ = new NavGraphEdge[navGraph_.NumNodes()];
searchFrontier_ = new NavGraphEdge[navGraph_.NumNodes()];
costToThisNode_ = new float[navGraph_.NumNodes()];
sourceNodeIndex_ = sourceNodeIndex;
targetNodeIndex_ = targetNodeIndex;
//create the PQ ,
pq_ = new IndexedPriorityQLow( costToThisNode_, navGraph_.NumNodes() );
//put the source node on the queue
pq_.Insert( sourceNodeIndex_ );
}
protected void Search()
{
//create an indexed priority queue that sorts smallest to largest
//(front to back).Note that the maximum number of elements the iPQ
//may contain is N. This is because no node can be represented on the
//queue more than once.
IndexedPriorityQLow pq = new IndexedPriorityQLow(costToThisNode_, navGraph_.NumNodes());
//put the source node on the queue
pq.Insert(sourceNodeID_);
//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 == targetNodeID_)
{
return;
}
//now to relax the edges.
List <NavGraphEdge> edges = navGraph_.GetEdgeListList()[nextClosestNode];
NavGraphEdge currEdge = null;
for (int i = 0; i < edges.Count; ++i)
{
currEdge = edges[i];
//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] + currEdge.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_[currEdge.To()] == null)
{
costToThisNode_[currEdge.To()] = newCost;
pq.Insert(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
//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_[currEdge.To()]) &&
(shortestPathTree_[currEdge.To()] == null))
{
costToThisNode_[currEdge.To()] = newCost;
//because the cost is less than it was previously, the PQ must be
//re-sorted to account for this.
pq.ChangePriority(currEdge.To());
searchFrontier_[currEdge.To()] = currEdge;
}
}
}
}
