コード例 #1
0
void
ConfigurationSpaceScene::drawConfigurationPath(const rl::plan::VectorList& path)
{
	this->resetPath();
	
	rl::plan::VectorList::const_iterator i = path.begin();
	rl::plan::VectorList::const_iterator j = ++path.begin();
	
	while (i != path.end() && j != path.end())
	{
		QGraphicsLineItem* line = this->addLine(
			(*i)(this->x),
			-(*i)(this->y),
			(*j)(this->x),
			-(*j)(this->y),
			QPen(QBrush(QColor(0, 255, 0)), 0.0f)
		);
		
		line->setZValue(3);
		
		this->path.push_back(line);
		
		++i;
		++j;
	}
}
コード例 #2
0
ファイル: Viewer.cpp プロジェクト: roboticslibrary/rl
void
Viewer::drawWorkPath(const rl::plan::VectorList& path)
{
	this->path3->enableNotify(false);
	
	this->path3Coordinate->point.setNum(0);
	this->path3IndexedLineSet->coordIndex.setNum(0);
	
	for (rl::plan::VectorList::const_iterator i = path.begin(); i != path.end(); ++i)
	{
		this->path3Coordinate->point.set1Value(
			this->path3Coordinate->point.getNum(),
			(*i)(0),
			(*i)(1),
			(*i)(2)
		);
		
		this->path3IndexedLineSet->coordIndex.set1Value(
			this->path3IndexedLineSet->coordIndex.getNum(),
			this->path3Coordinate->point.getNum() - 1
		);
	}
	
	this->path3IndexedLineSet->coordIndex.set1Value(
		this->path3IndexedLineSet->coordIndex.getNum(),
		SO_END_FACE_INDEX
	);
	
	this->path3->enableNotify(true);
	
	this->path3->touch();
}
コード例 #3
0
ファイル: Viewer.cpp プロジェクト: roboticslibrary/rl
void
Viewer::drawSweptVolume(const rl::plan::VectorList& path)
{
	this->sweptGroup->enableNotify(false);
	
	this->sweptGroup->removeAllChildren();
	
	rl::math::Real length = 0;
	
	rl::plan::VectorList::const_iterator i = path.begin();
	rl::plan::VectorList::const_iterator j = ++path.begin();
	
	for (; i != path.end() && j != path.end(); ++i, ++j)
	{
		length += this->model->distance(*i, *j);
	}
	
	rl::math::Real delta = length / static_cast<std::size_t>(std::ceil(length / this->deltaSwept));
	
	rl::math::Vector inter(this->model->getDofPosition());
	
	rl::math::Real x0 = 0;
	rl::math::Real x1 = x0;
	rl::math::Real x = 0;
	
	i = path.begin();
	j = ++path.begin();
	
	for (; i != path.end() && j != path.end(); ++i, ++j)
	{
		x1 += this->model->distance(*i, *j);
		
		for (; x < x1; x += delta)
		{
			this->model->interpolate(*i, *j, (x - x0) / (x1 - x0), inter);
			
			this->model->setPosition(inter);
			this->model->updateFrames();
			
			SoVRMLGroup* model = new SoVRMLGroup();
			
			for (std::size_t k = 0; k < this->model->model->getNumBodies(); ++k)
			{
				SoVRMLTransform* frame = new SoVRMLTransform();
				frame->copyFieldValues(static_cast<rl::sg::so::Body*>(this->model->model->getBody(k))->root);
				
				for (std::size_t l = 0; l < this->model->model->getBody(k)->getNumShapes(); ++l)
				{
					SoVRMLTransform* transform = new SoVRMLTransform();
					transform->copyFieldValues(static_cast<rl::sg::so::Shape*>(this->model->model->getBody(k)->getShape(l))->root);
					transform->addChild(static_cast<rl::sg::so::Shape*>(this->model->model->getBody(k)->getShape(l))->shape);
					frame->addChild(transform);
				}
				
				model->addChild(frame);
			}
			
			this->sweptGroup->addChild(model);
		}
		
		x0 = x1;
		
		if (j == --path.end())
		{
			this->model->interpolate(*i, *j, 1, inter);
			
			this->model->setPosition(inter);
			this->model->updateFrames();
			
			SoVRMLGroup* model = new SoVRMLGroup();
			
			for (std::size_t k = 0; k < this->model->model->getNumBodies(); ++k)
			{
				SoVRMLTransform* frame = new SoVRMLTransform();
				frame->copyFieldValues(static_cast<rl::sg::so::Body*>(this->model->model->getBody(k))->root);
				
				for (std::size_t l = 0; l < this->model->model->getBody(k)->getNumShapes(); ++l)
				{
					SoVRMLTransform* transform = new SoVRMLTransform();
					transform->copyFieldValues(static_cast<rl::sg::so::Shape*>(this->model->model->getBody(k)->getShape(l))->root);
					transform->addChild(static_cast<rl::sg::so::Shape*>(this->model->model->getBody(k)->getShape(l))->shape);
					frame->addChild(transform);
				}
				
				model->addChild(frame);
			}
			
			this->sweptGroup->addChild(model);
		}
	}
	
	this->sweptGroup->enableNotify(true);
	
	this->sweptGroup->touch();
}
コード例 #4
0
ファイル: Viewer.cpp プロジェクト: roboticslibrary/rl
void
Viewer::drawConfigurationPath(const rl::plan::VectorList& path)
{
	this->path->enableNotify(false);
	
	this->pathCoordinate->point.setNum(0);
	this->pathIndexedLineSet->coordIndex.setNum(0);
	
	rl::math::Vector inter(this->model->getDofPosition());
	
	for (std::size_t l = 0; l < this->model->getOperationalDof(); ++l)
	{
		rl::plan::VectorList::const_iterator i = path.begin();
		rl::plan::VectorList::const_iterator j = ++path.begin();
		
		if (i != path.end() && j != path.end())
		{
			this->model->setPosition(*i);
			this->model->updateFrames();
			
			this->pathCoordinate->point.set1Value(
				this->pathCoordinate->point.getNum(),
				this->model->forwardPosition(l)(0, 3),
				this->model->forwardPosition(l)(1, 3),
				this->model->forwardPosition(l)(2, 3)
			);
			
			this->pathIndexedLineSet->coordIndex.set1Value(
				this->pathIndexedLineSet->coordIndex.getNum(),
				this->pathCoordinate->point.getNum() - 1
			);
		}
		
		for (; i != path.end() && j != path.end(); ++i, ++j)
		{
			rl::math::Real steps = std::ceil(this->model->distance(*i, *j) / this->delta);
			
			for (std::size_t k = 1; k < steps + 1; ++k)
			{
				this->model->interpolate(*i, *j, k / steps, inter);
				
				this->model->setPosition(inter);
				this->model->updateFrames(false);
				
				this->pathCoordinate->point.set1Value(
					this->pathCoordinate->point.getNum(),
					this->model->forwardPosition(l)(0, 3),
					this->model->forwardPosition(l)(1, 3),
					this->model->forwardPosition(l)(2, 3)
				);
				
				this->pathIndexedLineSet->coordIndex.set1Value(
					this->pathIndexedLineSet->coordIndex.getNum(),
					this->pathCoordinate->point.getNum() - 1
				);
			}
		}
		
		this->pathIndexedLineSet->coordIndex.set1Value(
			this->pathIndexedLineSet->coordIndex.getNum(),
			SO_END_FACE_INDEX
		);
	}
	
	this->path->enableNotify(true);
	
	this->path->touch();
}