void SBLPRT::CreatePath(int i,int j,MilestonePath& path) { Assert(i >= 0 && i < (int)roadmap.nodes.size()); Assert(j >= 0 && j < (int)roadmap.nodes.size()); ConnectedSeedCallback callback(this,j); callback.node = j; roadmap.NewTraversal(); roadmap._BFS(i,callback); if(!callback.found) { cerr<<"SBLPRT::CreatePath: Warning, a path doesn't exist between nodes "<<i<<" and "<<j<<endl; return; } list<PathInfo> subpaths; int n=j; while(n != i) { int p=callback.parents[n]; Assert(p >= 0); PathInfo temp; temp.tstart = p; temp.tgoal = n; temp.path = roadmap.FindEdge(n,p); Assert(temp.path != NULL); Assert(!temp.path->edges.empty()); subpaths.push_front(temp); n=p; } for(list<PathInfo>::iterator it=subpaths.begin();it!=subpaths.end();it++) { int s=it->tstart; int g=it->tgoal; Assert(s >= 0 && s <(int)roadmap.nodes.size()); Assert(g >= 0 && g <(int)roadmap.nodes.size()); MilestonePath& subpath=*it->path; Assert(!subpath.edges.empty()); //forward path or backward path? if(subpath.edges.front()->Start() == *roadmap.nodes[s]->root) { Assert(subpath.edges.back()->Goal() == *roadmap.nodes[g]->root); //forward path path.Concat(subpath); if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on %d %d\n",s,g); } else { Assert(subpath.edges.front()->Start() == *roadmap.nodes[g]->root); Assert(subpath.edges.back()->Goal() == *roadmap.nodes[s]->root); //backward path for(int k=(int)subpath.edges.size()-1;k>=0;k--) { path.edges.push_back(subpath.edges[k]->ReverseCopy()); } if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on backwards %d %d\n",s,g); } } Assert(path.IsValid()); }