void OpenGLRenderInterface::draw(dynamics::BodyNode* _node, bool _vizCol, bool _colMesh) {
if(_node == 0)
return;
// Get world transform
Eigen::Isometry3d pose;
pose = _node->getTransform();
// GL calls
if(_vizCol && _node->isColliding()) {
glDisable(GL_TEXTURE_2D);
glEnable(GL_COLOR_MATERIAL);
glColor3f(1.0f, .1f, .1f);
}
glPushMatrix();
glMultMatrixd(pose.data());
if(_colMesh) {
for (size_t i = 0; i < _node->getNumCollisionShapes(); i++)
draw(_node->getCollisionShape(i));
}
else {
for (size_t i = 0; i < _node->getNumVisualizationShapes(); i++)
draw(_node->getVisualizationShape(i));
}
glColor3f(1.0f,1.0f,1.0f);
glEnable( GL_TEXTURE_2D );
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}
void teleop_gui_t::render_xform_arrows(Eigen::Isometry3d xform, double alpha)
{
glPushMatrix();
glMultMatrixd(xform.data());
Eigen::Vector3d base = Eigen::Vector3d::Zero();
glColor4d(1,0,0,alpha);
yui::drawArrow3D(base, Eigen::Vector3d::UnitX(), 0.2, 0.01, 0.02);
glColor4d(0,1,0,alpha);
yui::drawArrow3D(base, Eigen::Vector3d::UnitY(), 0.2, 0.01, 0.02);
glColor4d(0,0,1,alpha);
yui::drawArrow3D(base, Eigen::Vector3d::UnitZ(), 0.2, 0.01, 0.02);
glPopMatrix();
}
void OpenGLRenderInterface::transform(const Eigen::Isometry3d& _transform) {
glMultMatrixd(_transform.data());
}
//FIXME: Refactor this to use polymorphism.
void OpenGLRenderInterface::draw(dynamics::Shape* _shape) {
if(_shape == 0)
return;
Eigen::Isometry3d pose = _shape->getLocalTransform();
Eigen::Vector3d color = _shape->getColor();
glPushMatrix();
glColorMaterial ( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE );
glEnable ( GL_COLOR_MATERIAL );
glColor3d(color[0], color[1], color[2]);
glMultMatrixd(pose.data());
switch(_shape->getShapeType()) {
case dynamics::Shape::BOX: {
//FIXME: We are not in a glut instance
dynamics::BoxShape* box = static_cast<dynamics::BoxShape*>(_shape);
drawCube(box->getSize());
break;
}
case dynamics::Shape::CYLINDER: {
//FIXME: We are not in a glut instance
dynamics::CylinderShape* cylinder = static_cast<dynamics::CylinderShape*>(_shape);
drawCylinder(cylinder->getRadius(), cylinder->getHeight());
break;
}
case dynamics::Shape::ELLIPSOID: {
//FIXME: We are not in a glut instance
dynamics::EllipsoidShape* ellipsoid = static_cast<dynamics::EllipsoidShape*>(_shape);
drawEllipsoid(ellipsoid->getSize());
break;
}
case dynamics::Shape::PLANE: {
dterr << "PLANE shape is not supported yet." << std::endl;
break;
}
case dynamics::Shape::MESH: {
glDisable(GL_COLOR_MATERIAL); // Use mesh colors to draw
dynamics::MeshShape* mesh = static_cast<dynamics::MeshShape*>(_shape);
if(!mesh)
break;
else if(mesh->getDisplayList())
drawList(mesh->getDisplayList());
else
drawMesh(mesh->getScale(), mesh->getMesh());
break;
}
case dynamics::Shape::SOFT_MESH: {
// Do nothing
break;
}
case dynamics::Shape::LINE_SEGMENT: {
dynamics::LineSegmentShape* lineSegments =
static_cast<dynamics::LineSegmentShape*>(_shape);
drawLineSegments(lineSegments->getVertices(),
lineSegments->getConnections());
}
}
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}