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();
}
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;
}
}
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();
}
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();
}