Path::Path(const list<VectorXd> &path, double maxDeviation) : length(0.0) { if(path.size() < 2) return; list<VectorXd>::const_iterator config1 = path.begin(); list<VectorXd>::const_iterator config2 = config1; ++config2; list<VectorXd>::const_iterator config3; VectorXd startConfig = *config1; while(config2 != path.end()) { config3 = config2; ++config3; if(maxDeviation > 0.0 && config3 != path.end()) { CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation); VectorXd endConfig = blendSegment->getConfig(0.0); if((endConfig - startConfig).norm() > 0.000001) { pathSegments.push_back(new LinearPathSegment(startConfig, endConfig)); } pathSegments.push_back(blendSegment); startConfig = blendSegment->getConfig(blendSegment->getLength()); //debug if(((endConfig - *config1).norm() > 0.000001 && (*config2 - endConfig).norm() > 0.000001 && std::abs((endConfig - *config1).normalized().dot((*config2 - endConfig).normalized()) - 1.0) > 0.000001) || ((startConfig - *config2).norm() > 0.000001 && (*config3 - startConfig).norm() > 0.000001 && std::abs((startConfig - *config2).normalized().dot((*config3 - startConfig).normalized()) - 1.0) > 0.000001)) { cout << "error" << endl; } } else { pathSegments.push_back(new LinearPathSegment(startConfig, *config2)); startConfig = *config2; } config1 = config2; ++config2; } // create list of switching point candidates, calculate total path length and absolute positions of path segments for(list<PathSegment*>::iterator segment = pathSegments.begin(); segment != pathSegments.end(); ++segment) { (*segment)->position = length; list<double> localSwitchingPoints = (*segment)->getSwitchingPoints(); for(list<double>::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); ++point) { switchingPoints.push_back(make_pair(length + *point, false)); } length += (*segment)->getLength(); switchingPoints.push_back(make_pair(length, true)); } switchingPoints.pop_back(); }
Path::Path(const list<VectorXd> &path, double maxDeviation) : length(0.0) { if(path.size() < 2) return; list<VectorXd>::const_iterator config1 = path.begin(); list<VectorXd>::const_iterator config2 = config1; config2++; list<VectorXd>::const_iterator config3; VectorXd startConfig = *config1; while(config2 != path.end()) { config3 = config2; config3++; if(maxDeviation > 0.0 && config3 != path.end()) { CircularPathSegment* blendSegment = new CircularPathSegment(0.5 * (*config1 + *config2), *config2, 0.5 * (*config2 + *config3), maxDeviation); VectorXd endConfig = blendSegment->getConfig(0.0); if((endConfig - startConfig).norm() > 0.000001) { pathSegments.push_back(new LinearPathSegment(startConfig, endConfig)); } pathSegments.push_back(blendSegment); startConfig = blendSegment->getConfig(blendSegment->getLength()); } else { pathSegments.push_back(new LinearPathSegment(startConfig, *config2)); startConfig = *config2; } config1 = config2; config2++; } // create list of switching point candidates, calculate total path length and absolute positions of path segments for(list<PathSegment*>::iterator segment = pathSegments.begin(); segment != pathSegments.end(); segment++) { (*segment)->position = length; list<double> localSwitchingPoints = (*segment)->getSwitchingPoints(); for(list<double>::const_iterator point = localSwitchingPoints.begin(); point != localSwitchingPoints.end(); point++) { switchingPoints.push_back(make_pair(length + *point, false)); } length += (*segment)->getLength(); while(!switchingPoints.empty() && switchingPoints.back().first >= length) switchingPoints.pop_back(); switchingPoints.push_back(make_pair(length, true)); } switchingPoints.pop_back(); }