void vtkPolyDataSingleSourceShortestPath::ShortestPath(int startv, int endv) { int i, u, v; InitSingleSource(startv); HeapInsert(startv); this->f->SetValue(startv, 1); int stop = 0; while ((u = HeapExtractMin()) >= 0 && !stop) { // u is now in s since the shortest path to u is determined this->s->SetValue(u, 1); // remove u from the front set this->f->SetValue(u, 0); if (u == endv && StopWhenEndReached) stop = 1; // Update all vertices v adjacent to u for (i = 0; i < Adj[u]->GetNumberOfIds(); i++) { v = Adj[u]->GetId(i); // s is the set of vertices with determined shortest path...do not use them again if (!this->s->GetValue(v)) { // Only relax edges where the end is not in s and edge is in the front set double w = EdgeCost(this->GetInput(), u, v); if (this->f->GetValue(v)) { Relax(u, v, w); } // add edge v to front set else { this->f->SetValue(v, 1); this->d->SetValue(v, this->d->GetValue(u) + w); // Set Predecessor of v to be u this->pre->SetValue(v, u); HeapInsert(v); } } } } }
/** * @function CalculateDistanceFromPathSet */ void LJM2::CalculateDistanceFromPathSet( std::vector<int> _path ) { printf("--> Calculating Distance From Path Set \n"); std::vector<int> queue = _path; //-- 1. Initialize all to infinity for( int i = 0; i < mNumNodes; ++i ) { if( GetState( i ) == OBSTACLE_STATE || GetState( i ) == INFLATED_STATE ) { mNodes[i].s.brushDist = 0; } else { mNodes[i].s.brushDist = LJM2_INF; } } // Path nodes with zero initial value for( int i = 0; i < _path.size(); ++i ) { mNodes[ _path[i] ].s.brushDist = 0; } //-- 2. Loop until no more new elements in Queue std::vector<int> newQueue(0); while( queue.size() > 0 ) { for( int i = 0; i < queue.size(); ++i ) { std::vector<int> n = mGeometricNeighbors[ queue[i] ]; double dist = mNodes[ queue[i] ].s.brushDist; for( int j = 0; j < n.size(); ++j ) { double new_dist = dist + EdgeCost( queue[i], n[j], sNominalValue ); if( new_dist < mNodes[ n[j] ].s.brushDist ) { mNodes[ n[j] ].s.brushDist = new_dist; newQueue.push_back( n[j] ); } } } queue.clear(); queue = newQueue; newQueue.clear(); } printf("--) Finished calculating DT Paths \n"); }
/** * @function FindPath * @brief This is in the whole free space */ std::vector<Eigen::Vector3i> LJM2::FindPath( int _n1, int _n2 ) { std::vector<Eigen::Vector3i> path; printf( "--> Start FindPath in whole free space \n" ); int nodeStart = _n1; int nodeTarget = _n2; //-- Fill start space Node3D *node; node = &mNodes[nodeStart]; node->s.costG = 0; node->s.costH = CostHeuristic( nodeStart, nodeTarget )*( 0 + mAlpha*1 ); node->s.costF = node->s.costG + node->s.costH; node->s.parent = -1; node->s.status = IN_NO_SET; //-- Push it into the open set PushOpenSet( nodeStart ); //-- Loop int x; int count = 0; while( count < sMaxIter ) { count++; //-- Remove top node in OpenSet try { x = PopOpenSet(); } catch( int i ) { printf( "-- (%d) No more nodes to pop out \n", i ); break; } //-- Check if it is goal if( x == nodeTarget ) { printf( "--> Found a path! : iters: %d Cost: %.3f \n", count, mNodes[x].s.costF ); TracePath( x, path ); break; } //-- Add node to closed set mNodes[x].s.status = IN_CLOSED_SET; std::vector<int> neighbors = mGeometricNeighbors[x]; //-- for( int i = 0; i < neighbors.size(); i++ ) { if( mNodes[ neighbors[i] ].s.status == IN_CLOSED_SET ) { continue; } int y = mNodes[ neighbors[i] ].index; // Same as neighbors[i] actually float tentative_G_score = mNodes[x].s.costG + EdgeCost( x, y, sNominalValue )*( mNodes[y].s.value + mAlpha*1 ); if( mNodes[y].s.status != IN_OPEN_SET ) { node = &mNodes[y]; node->s.parent = x; node->s.costG = tentative_G_score; node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 ); node->s.costF = node->s.costG + node->s.costH; PushOpenSet(y); } else { if( tentative_G_score < mNodes[y].s.costG ) { node = &mNodes[y]; node->s.parent = x; node->s.costG = tentative_G_score; node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 ); node->s.costF = node->s.costG + node->s.costH; //-- Reorder your OpenSet UpdateLowerOpenSet( y ); } } } //-- End for every neighbor } //-- End of while node = NULL; printf("Finished FindPath whole free space \n"); return path; }