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();
for (unsigned int i = 0; i < openNewSize; ++i)
{
Open_.insert(Open_new[i]);
Open_new[i]->setSetType(Motion::SET_OPEN);
}
Open_new.clear();
if (Open_.empty())
{
if(!extendedFMT_)
OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
return false;
}
// Take the top of Open as the new z
*z = Open_.top()->data;
return true;
}
