void glDrawShape(ors::Shape *s) {
//set name (for OpenGL selection)
glPushName((s->index <<2) | 1);
glColor(s->color[0], s->color[1], s->color[2], orsDrawAlpha);
double scale=.33*(s->size[0]+s->size[1]+s->size[2] + 2.*s->size[3]); //some scale
if(!scale) scale=1.;
scale*=.3;
double GLmatrix[16];
s->X.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
if(!orsDrawShapes) {
glDrawAxes(scale);
glColor(0, 0, .5);
glDrawSphere(.1*scale);
}
if(orsDrawShapes) {
switch(s->type) {
case ors::noneST: break;
case ors::boxST:
if(orsDrawMeshes && s->mesh.V.N) s->mesh.glDraw();
else glDrawBox(s->size[0], s->size[1], s->size[2]);
break;
case ors::sphereST:
if(orsDrawMeshes && s->mesh.V.N) s->mesh.glDraw();
else glDrawSphere(s->size[3]);
break;
case ors::cylinderST:
if(orsDrawMeshes && s->mesh.V.N) s->mesh.glDraw();
else glDrawCylinder(s->size[3], s->size[2]);
break;
case ors::cappedCylinderST:
if(orsDrawMeshes && s->mesh.V.N) s->mesh.glDraw();
else glDrawCappedCylinder(s->size[3], s->size[2]);
break;
case ors::markerST:
if(orsDrawMarkers){
glDrawDiamond(s->size[0]/5., s->size[0]/5., s->size[0]/5.); glDrawAxes(s->size[0]);
}
break;
case ors::meshST:
CHECK(s->mesh.V.N, "mesh needs to be loaded to draw mesh object");
s->mesh.glDraw();
break;
case ors::pointCloudST:
CHECK(s->mesh.V.N, "mesh needs to be loaded to draw point cloud object");
glDrawPointCloud(s->mesh.V, NoArr);
break;
default: HALT("can't draw that geom yet");
}
}
if(orsDrawZlines) {
glColor(0, .7, 0);
glBegin(GL_LINES);
glVertex3d(0., 0., 0.);
glVertex3d(0., 0., -s->X.pos.z);
glEnd();
}
glColor(1,1,1);
if(orsDrawBodyNames && s->body) glDrawText(s->body->name, 0, 0, 0);
glPopName();
}
void drawInit(void*){
glStandardLight(NULL);
glDrawAxes(1.);
glColor(1.,.5,0.);
}
/// GL routine to draw a ors::KinematicWorld
void ors::KinematicWorld::glDraw() {
uint i=0;
ors::Transformation f;
double GLmatrix[16];
glPushMatrix();
//bodies
if(orsDrawBodies) for(Shape *s: shapes) {
glDrawShape(s);
i++;
if(orsDrawLimit && i>=orsDrawLimit) break;
}
//joints
if(orsDrawJoints) for(Joint *e: joints) {
//set name (for OpenGL selection)
glPushName((e->index <<2) | 2);
double s=e->A.pos.length()+e->B.pos.length(); //some scale
s*=.25;
//from body to joint
f=e->from->X;
f.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
glColor(1, 1, 0);
//glDrawSphere(.1*s);
glBegin(GL_LINES);
glVertex3f(0, 0, 0);
glVertex3f(e->A.pos.x, e->A.pos.y, e->A.pos.z);
glEnd();
//joint frame A
f.appendTransformation(e->A);
f.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
glDrawAxes(s);
glColor(1, 0, 0);
glRotatef(90, 0, 1, 0); glDrawCylinder(.05*s, .3*s); glRotatef(-90, 0, 1, 0);
//joint frame B
f.appendTransformation(e->Q);
f.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
glDrawAxes(s);
//from joint to body
glColor(1, 0, 1);
glBegin(GL_LINES);
glVertex3f(0, 0, 0);
glVertex3f(e->B.pos.x, e->B.pos.y, e->B.pos.z);
glEnd();
glTranslatef(e->B.pos.x, e->B.pos.y, e->B.pos.z);
//glDrawSphere(.1*s);
glPopName();
i++;
if(orsDrawLimit && i>=orsDrawLimit) break;
}
//proxies
if(orsDrawProxies) for(Proxy *proxy: proxies) {
glLoadIdentity();
if(!proxy->colorCode) glColor(.75,.75,.75);
else glColor(proxy->colorCode);
glBegin(GL_LINES);
glVertex3dv(proxy->posA.p());
glVertex3dv(proxy->posB.p());
glEnd();
ors::Transformation f;
f.pos=proxy->posA;
f.rot.setDiff(ors::Vector(0, 0, 1), proxy->posA-proxy->posB);
f.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
glDisable(GL_CULL_FACE);
glDrawDisk(.02);
glEnable(GL_CULL_FACE);
f.pos=proxy->posB;
f.getAffineMatrixGL(GLmatrix);
glLoadMatrixd(GLmatrix);
glDrawDisk(.02);
}
glPopMatrix();
}
