Go to the documentation of this file.00001 #include "Mobility.h"
00002 #include "PathPlanner.h"
00003 #include "ShortcutOptimizer.h"
00004
00005 using namespace PathEngine;
00006
00007 std::vector<Configuration> ShortcutOptimizer::optimize(const std::vector<Configuration> &path)
00008 {
00009 std::vector<Configuration> optimized = path;
00010 if (path.size() < 3) return optimized;
00011
00012 Mobility *mobility = planner_->getMobility();
00013 unsigned int index = 1;
00014 while(index != optimized.size() -1 ){
00015 if (mobility->isReachable(optimized[index-1], optimized[index+1])){
00016 optimized.erase(optimized.begin()+index);
00017 }else{
00018 index++;
00019 }
00020 }
00021 return optimized;
00022 }