void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr& plannerData, og::PathGeometric &path)
{
// Convert the planner data verticies into a vector of states
for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
path.append(plannerData->getVertex(i).getState());
}
double ompl::tools::LightningDB::distanceFunction(const ompl::base::PlannerDataPtr a, const ompl::base::PlannerDataPtr b) const
{
// Bi-directional implementation - check path b from [start, goal] and [goal, start]
return std::min(
// [ a.start, b.start] + [a.goal + b.goal]
si_->distance(a->getVertex(0).getState(), b->getVertex(0).getState()) +
si_->distance(a->getVertex(a->numVertices()-1).getState(), b->getVertex(b->numVertices()-1).getState()),
// [ a.start, b.goal] + [a.goal + b.start]
si_->distance(a->getVertex(0).getState(), b->getVertex(b->numVertices()-1).getState()) +
si_->distance(a->getVertex(a->numVertices()-1).getState(), b->getVertex(0).getState()));
}