本文整理汇总了C++中Motion::alreadyCC方法的典型用法代码示例。如果您正苦于以下问题：C++ Motion::alreadyCC方法的具体用法？C++ Motion::alreadyCC怎么用？C++ Motion::alreadyCC使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Motion的用法示例。
在下文中一共展示了Motion::alreadyCC方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: cMin
bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
{
// Find all nodes that are near z, and also in set Unvisited
std::vector<Motion*> xNear;
const std::vector<Motion*> &zNeighborhood = neighborhoods_[*z];
const unsigned int zNeighborhoodSize = zNeighborhood.size();
xNear.reserve(zNeighborhoodSize);
for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
{
Motion *x = zNeighborhood[i];
if (x->getSetType() == Motion::SET_UNVISITED)
{
saveNeighborhood(x);
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
xNear.push_back(x);
}
else
xNear.push_back(x);
}
}
// For each node near z and in set Unvisited, attempt to connect it to set Open
std::vector<Motion*> yNear;
std::vector<Motion*> Open_new;
const unsigned int xNearSize = xNear.size();
for (unsigned int i = 0 ; i < xNearSize; ++i)
{
Motion *x = xNear[i];
// Find all nodes that are near x and in set Open
const std::vector<Motion*> &xNeighborhood = neighborhoods_[x];
const unsigned int xNeighborhoodSize = xNeighborhood.size();
yNear.reserve(xNeighborhoodSize);
for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
{
if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
yNear.push_back(xNeighborhood[j]);
}
// Find the lowest cost-to-come connection from Open to x
base::Cost cMin(std::numeric_limits<double>::infinity());
Motion *yMin = getBestParent(x, yNear, cMin);
yNear.clear();
// If an optimal connection from Open to x was found
if (yMin != nullptr)
{
bool collision_free = false;
if (cacheCC_)
{
if (!yMin->alreadyCC(x))
{
collision_free = si_->checkMotion(yMin->getState(), x->getState());
++collisionChecks_;
// Due to FMT* design, it is only necessary to save unsuccesful
// connection attemps because of collision
if (!collision_free)
yMin->addCC(x);
}
}
else
{
++collisionChecks_;
collision_free = si_->checkMotion(yMin->getState(), x->getState());
}
if (collision_free)
{
// Add edge from yMin to x
x->setParent(yMin);
x->setCost(cMin);
x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
yMin->getChildren().push_back(x);
// Add x to Open
Open_new.push_back(x);
// Remove x from Unvisited
x->setSetType(Motion::SET_CLOSED);
}
} // An optimal connection from Open to x was found
} // For each node near z and in set Unvisited, try to connect it to set Open
// Update Open
Open_.pop();
(*z)->setSetType(Motion::SET_CLOSED);
// Add the nodes in Open_new to Open
unsigned int openNewSize = Open_new.size();
//.........这里部分代码省略.........