Go to the documentation of this file.00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "RandomShortcutOptimizer.h"
00004
00005 #ifdef WIN32
00006 #define random() rand()
00007 #endif
00008
00009 using namespace PathEngine;
00010
00011 std::vector<Configuration> RandomShortcutOptimizer::optimize(const std::vector<Configuration> &path)
00012 {
00013 if (path.size() < 3) return path;
00014
00015 Mobility *mobility = planner_->getMobility();
00016 int nSegment = path.size()-1;
00017 int index1 = ((float)random())/RAND_MAX*nSegment;
00018 int index2;
00019 do {
00020 index2 = ((float)random())/RAND_MAX*nSegment;
00021 }while(index1 == index2);
00022 if (index2 < index1) std::swap(index1, index2);
00023
00024 double ratio1 = ((double)random())/RAND_MAX;
00025 double ratio2 = ((double)random())/RAND_MAX;
00026 Configuration cfg1 = mobility->interpolate(path[index1], path[index1+1],
00027 ratio1);
00028 Configuration cfg2 = mobility->interpolate(path[index2], path[index2+1],
00029 ratio2);
00030 if (mobility->isReachable(cfg1, cfg2)){
00031 std::vector<Configuration> optimized;
00032 for (int i=0; i<=index1; i++) optimized.push_back(path[i]);
00033 optimized.push_back(cfg1);
00034 optimized.push_back(cfg2);
00035 for (unsigned int i=index2+1; i<path.size(); i++) optimized.push_back(path[i]);
00036 return optimized;
00037 }else{
00038 return path;
00039 }
00040 }