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