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());
}
void ReversePath(MilestonePath& path)
{
for(size_t k=0;k<path.edges.size()/2;k++) {
SmartPointer<EdgePlanner> e1 = path.edges[k];
SmartPointer<EdgePlanner> e2 = path.edges[path.edges.size()-k];
path.edges[k] = e2->ReverseCopy();
path.edges[path.edges.size()-k] = e1->ReverseCopy();
}
if(path.edges.size()%2 == 1)
path.edges[path.edges.size()/2] = path.edges[path.edges.size()/2]->ReverseCopy();
if(!path.IsValid()) fprintf(stderr,"ReversePath : Path invalidated ?!?!\n");
}
void RoadmapPlanner::CreatePath(int i,int j,MilestonePath& path)
{
Assert(ccs.SameComponent(i,j));
//Graph::PathIntCallback callback(roadmap.nodes.size(),j);
//roadmap._BFS(i,callback);
EdgeDistance distanceWeightFunc;
Graph::ShortestPathProblem<Config,SmartPointer<EdgePlanner> > spp(roadmap);
spp.InitializeSource(i);
spp.FindPath_Undirected(j,distanceWeightFunc);
if(IsInf(spp.d[j])) {
FatalError("RoadmapPlanner::CreatePath: SameComponent is true, but no shortest path?");
return;
}
list<int> nodes;
//Graph::GetAncestorPath(callback.parents,j,i,nodes);
bool res=Graph::GetAncestorPath(spp.p,j,i,nodes);
if(!res) {
FatalError("RoadmapPlanner::CreatePath: GetAncestorPath returned false");
return;
}
if(nodes.front() != i || nodes.back() != j) {
FatalError("RoadmapPlanner::CreatePath: GetAncestorPath didn't return correct path? %d to %d vs %d to %d",nodes.front(),nodes.back(),i,j);
}
Assert(nodes.front()==i);
Assert(nodes.back()==j);
path.edges.clear();
path.edges.reserve(nodes.size());
for(list<int>::const_iterator p=nodes.begin();p!=--nodes.end();++p) {
list<int>::const_iterator n=p; ++n;
SmartPointer<EdgePlanner>* e=roadmap.FindEdge(*p,*n);
Assert(e);
if(*e == NULL) {
//edge data not stored
path.edges.push_back(space->LocalPlanner(roadmap.nodes[*p],roadmap.nodes[*n]));
}
else {
//edge data stored
if((*e)->Start() == roadmap.nodes[*p]) {
//path.edges.push_back((*e)->Copy());
path.edges.push_back(*e);
}
else {
Assert((*e)->Goal() == roadmap.nodes[*p]);
path.edges.push_back((*e)->ReverseCopy());
}
}
}
Assert(path.IsValid());
}