Пример #1
0
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();
}
Пример #2
0
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();
}