Real operator () (const SmartPointer<EdgePlanner>& e,int s,int t)
{
if(!e) return 1.0;
assert(e->Space() != NULL);
Real res = e->Space()->Distance(e->Start(),e->Goal());
if(res <= 0) {
printf("RoadmapPlanner: Warning, edge has nonpositive length %g\n",res);
return Epsilon;
}
return res;
}
EdgePlannerWithCSpaceContainer::EdgePlannerWithCSpaceContainer(SmartPointer<CSpace> space,const SmartPointer<EdgePlanner>& e)
:PiggybackEdgePlanner(space,e->Start(),e->Goal(),e),spacePtr(space)
{
}
double LengthWeightFunc(const SmartPointer<EdgePlanner>& e,int s,int t)
{
Assert(e);
return e->Space()->Distance(e->Start(),e->Goal());
}
