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