int RRT::extend(RoadmapPtr tree, Configuration& qRand, bool reverse) {
if (debug) std::cout << "RRT::extend("<< qRand << ", " << reverse << ")"
<< std::endl;
RoadmapNodePtr minNode;
double min;
tree->findNearestNode(qRand, minNode, min);
if (debug) std::cout << "nearest : pos = (" << minNode->position()
<< "), d = " << min << std::endl;
if (minNode != NULL) {
Mobility* mobility = planner_->getMobility();
if (min > eps_){
Configuration qRandOrg = qRand;
qRand = mobility->interpolate(minNode->position(), qRand, eps_/min);
if (debug) std::cout << "qRand = (" << qRand << ")" << std::endl;
if (mobility->distance(minNode->position(), qRand) > min){
std::cout << "distance didn't decrease" << std::endl;
std::cout << "qRandOrg : (" << qRandOrg << "), d = " << min
<< std::endl;
std::cout << "qRand : (" << qRand << "), d = "
<< mobility->distance(minNode->position(), qRand)
<< std::endl;
getchar();
}
}
if (reverse){
if (mobility->isReachable(qRand, minNode->position())){
RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
tree->addNode(newNode);
tree->addEdge(newNode, minNode);
if (min <= eps_) {
if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
return Reached;
}
else {
if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
return Advanced;
}
} else {
if (debug) std::cout << "trapped" << std::endl;
return Trapped;
}
}else{
if (mobility->isReachable(minNode->position(), qRand)){
RoadmapNodePtr newNode = RoadmapNodePtr(new RoadmapNode(qRand));
tree->addNode(newNode);
tree->addEdge(minNode, newNode);
if (min <= eps_) {
if (debug) std::cout << "reached(" << qRand << ")"<< std::endl;
return Reached;
}
else {
if (debug) std::cout << "advanced(" << qRand << ")" << std::endl;
return Advanced;
}
} else {
if (debug) std::cout << "trapped" << std::endl;
return Trapped;
}
}
}
return Trapped;
}
