std::vector<Configuration> RandomShortcutOptimizer::optimize(const std::vector<Configuration> &path)
{
if (path.size() < 3) return path;
Mobility *mobility = planner_->getMobility();
int nSegment = path.size()-1;
int index1 = ((float)random())/RAND_MAX*nSegment;
int index2;
do {
index2 = ((float)random())/RAND_MAX*nSegment;
}while(index1 == index2);
int tmp;
if (index2 < index1) std::swap(index1, index2);
double ratio1 = ((double)random())/RAND_MAX;
double ratio2 = ((double)random())/RAND_MAX;
Configuration cfg1 = mobility->interpolate(path[index1], path[index1+1],
ratio1);
Configuration cfg2 = mobility->interpolate(path[index2], path[index2+1],
ratio2);
if (mobility->isReachable(cfg1, cfg2)){
std::vector<Configuration> optimized;
for (int i=0; i<=index1; i++) optimized.push_back(path[i]);
optimized.push_back(cfg1);
optimized.push_back(cfg2);
for (int i=index2+1; i<path.size(); i++) optimized.push_back(path[i]);
return optimized;
}else{
return path;
}
}
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;
}