C++ (Cpp) PlannerDataPtr::getVertex示例

示例#1

文件： LightningDB.cpp 项目： HRZaheri/batch-informed-trees

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

示例#2

文件： Lightning.cpp 项目： dbolkensteyn/ompl

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