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; } } } }
public override bool 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(m_FCosts, m_Graph.NumNodes()); //put the source node on the queue pq.insert(m_iSource); //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 m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode]; //if the target has been found exit if (NextClosestNode == m_iTarget) { return(true); } //now to relax the edges. SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextClosestNode); while (EdgeItr.MoveNext()) { //calculate the heuristic cost from this node to the target (H) double HCost = funcPointer(m_Graph, m_iTarget, EdgeItr.Current.To); //calculate the 'real' cost to this node from the source (G) double GCost = m_GCosts[NextClosestNode] + EdgeItr.Current.Cost; //if the node has not been added to the frontier, add it and update //the G and F costs if (NavGraphEdge.IsNull(m_SearchFrontier[EdgeItr.Current.To])) { m_FCosts[EdgeItr.Current.To] = GCost + HCost; m_GCosts[EdgeItr.Current.To] = GCost; pq.insert(EdgeItr.Current.To); m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current; } //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 < m_GCosts[EdgeItr.Current.To]) && NavGraphEdge.IsNull(m_ShortestPathTree[EdgeItr.Current.To])) { m_FCosts[EdgeItr.Current.To] = GCost + HCost; m_GCosts[EdgeItr.Current.To] = GCost; pq.ChangePriority(EdgeItr.Current.To); m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current; } } } return(false); }
public override bool 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(m_CostToThisNode, m_Graph.NumNodes()); //put the source node on the queue pq.insert(m_iSource); //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 m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode]; //if the target has been found exit if (NextClosestNode == m_iTarget) { return(true); } //now to relax the edges. SparseGraph.EdgeIterator EdgeItr = new SparseGraph.EdgeIterator(m_Graph, NextClosestNode); while (EdgeItr.MoveNext()) { //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. double NewCost = m_CostToThisNode[NextClosestNode] + EdgeItr.Current.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 (NavGraphEdge.IsNull(m_SearchFrontier[EdgeItr.Current.To])) { m_CostToThisNode[EdgeItr.Current.To] = NewCost; pq.insert(EdgeItr.Current.To); m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current; } //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 < m_CostToThisNode[EdgeItr.Current.To]) && NavGraphEdge.IsNull(m_ShortestPathTree[EdgeItr.Current.To])) { m_CostToThisNode[EdgeItr.Current.To] = NewCost; //because the cost is less than it was previously, the PQ must be //re-sorted to account for this. pq.ChangePriority(EdgeItr.Current.To); m_SearchFrontier[EdgeItr.Current.To] = EdgeItr.Current; } } } return(false); }
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; } } } }