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());
}
