// simulation loop static void simLoop (int pause) { const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3; //@a sphere dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1 dsSetSphereQuality(3); // set sphere quality. 3 is enough pos1 = dBodyGetPosition(sphere.body); // get a body position R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix dsDrawSphere(pos1,R1,radius); // draw a sphere // a cylinder dsSetColorAlpha (0,1,0,1); pos2 = dBodyGetPosition(cylinder.body); R2 = dBodyGetRotation(cylinder.body); dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder // a capsule dsSetColorAlpha (1,1,1,1); pos2 = dBodyGetPosition(capsule.body); R2 = dBodyGetRotation(capsule.body); dsDrawCapsule(pos2,R2,length,radius); // draw a capsule // a box dsSetColorAlpha (0,0,1,1); pos3 = dBodyGetPosition(box.body); R3 = dBodyGetRotation(box.body); dsDrawBox(pos3,R3,sides); // draw a box // a ray dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9}; dsDrawLine(posA,posB); // draw a ray }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!draw_geom){ return; } if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
/*** ロボットアームの描画 ***/ void drawArm() { dReal r,length; for (int i = 0; i < NUM; i++ ) { // カプセルの描画 dGeomCapsuleGetParams(rlink[i].geom, &r,&length); dsDrawCapsule(dBodyGetPosition(rlink[i].body), dBodyGetRotation(rlink[i].body),length,r); } }
static void simLoop (int pause) { dsSetColor (0,0,2); space->collide(0,&nearCallback); if (!pause) //world->quickStep(0.02); world->step(0.02); if (write_world) { FILE *f = fopen ("state.dif","wt"); if (f) { dWorldExportDIF (*world,f,"X"); fclose (f); } write_world = false; } // remove all contact joints dJointGroupEmpty (contactgroup); dsSetTexture (DS_WOOD); dsSetColor (1,0.5f,0); dsDrawCylinder(top1->getPosition(), top1->getRotation(), toplength, topradius); dsDrawCapsule(top1->getPosition(), top1->getRotation(), pinlength, pinradius); dsSetColor (0.5f,1,0); dsDrawCylinder(top2->getPosition(), top2->getRotation(), toplength, topradius); dsDrawCapsule(top2->getPosition(), top2->getRotation(), pinlength, pinradius); }
/*** ロボットアームの描画 ***/ void drawArm() { dReal r,length; dReal box_length[3] = {0.4, 0.4, 0.4}; for (int i = 0; i < NUM-1; i++ ) { // カプセルの描画 dGeomCapsuleGetParams(rlink[i].geom, &r,&length); dsDrawCapsule(dBodyGetPosition(rlink[i].body), dBodyGetRotation(rlink[i].body),length,r); } dGeomBoxGetLengths(rlink[NUM-1].geom, box_length); dsDrawBox(dBodyGetPosition(rlink[NUM-1].body), dBodyGetRotation(rlink[NUM-1].body), box_length); }
static void simLoop (int pause) { const dReal *pos1,*R1; dsSetColorAlpha (1,1,1,1); dSpaceCollide (space,0,&nearCallback); dJointGroupEmpty (contactgroup); dWorldStep(world,0.01); pos1 = dBodyGetPosition(capsule); R1 = dBodyGetRotation(capsule); dsDrawCapsule(pos1,R1,length,radius); // draw a capsule }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dConvexClass) { //dVector3 sides={0.50,0.50,0.50}; dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void dmDraw(dmObject *obj) /*** 物体の描画 ***/ { if (!obj->geom) return; obj->p = (double *) dGeomGetPosition(obj->geom); obj->R = (double *) dGeomGetRotation(obj->geom); dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b) // printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]); int type = dGeomGetClass(obj->geom); switch (type) { case dBoxClass: { dVector3 sides; dGeomBoxGetLengths(obj->geom,sides); dsDrawBox(obj->p,obj->R,sides); } break; case dSphereClass: dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom)); break; case dCapsuleClass: { dReal radius,length; dGeomCapsuleGetParams(obj->geom,&radius,&length); dsDrawCapsule(obj->p,obj->R,length,radius); } break; case dCylinderClass: { dReal radius,length; dGeomCylinderGetParams(obj->geom,&radius,&length); dsDrawCylinder(obj->p,obj->R,length,radius); } break; default: printf("Bad geometry type \n"); } }
IoObject *IoDrawStuff_dsDrawCapsule(IoDrawStuff *self, IoObject *locals, IoMessage *m) { float *pos; float *R; float length; float radius; //IoSeq_assertIsVector(self, locals, m); { IoSeq *other = IoMessage_locals_vectorArgAt_(m, locals, 0); pos = IoSeq_floatPointerOfLength_(other, 3); other = IoMessage_locals_vectorArgAt_(m, locals, 1); R = IoSeq_floatPointerOfLength_(other, 12); length = IoMessage_locals_floatArgAt_(m, locals, 2); radius = IoMessage_locals_floatArgAt_(m, locals, 3); } dsDrawCapsule(pos, R, length, radius); return self; }
static void simLoop(int pause) { const dReal *pos1, *R1, *pos2, *R2; dSpaceCollide(space, 0, &nearCallback); // add, Write this first dWorldStep(world, 0.01); dJointGroupEmpty(contactgroup); // add dsSetColor(1.0, 0.0, 0.0); // draw a ball pos1 = dBodyGetPosition(ball.body); R1 = dBodyGetRotation(ball.body); dsDrawSphere(pos1, R1, ball.radius); // draw a ccylinder pos2 = dBodyGetPosition(pole.body); R2 = dBodyGetRotation(pole.body); dsDrawCapsule(pos2, R2, pole.length, pole.radius); }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } //<---- Convex Object else if (type == dConvexClass) { #if 0 dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); #else dsDrawConvex(pos,R, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_body) { dBodyID body = dGeomGetBody(g); if (body) { const dReal *bodypos = dBodyGetPosition (body); const dReal *bodyr = dBodyGetRotation (body); dReal bodySides[3] = { 0.1, 0.1, 0.1 }; dsSetColorAlpha(0,1,0,1); dsDrawBox(bodypos,bodyr,bodySides); } } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
// simulation loop static void simLoop (int pause) { const dReal *rot; dVector3 ax; dReal l=0; switch (joint->getType() ) { case dJointTypeSlider : ( (dSliderJoint *) joint)->getAxis (ax); l = ( (dSliderJoint *) joint)->getPosition(); break; case dJointTypePiston : ( (dPistonJoint *) joint)->getAxis (ax); l = ( (dPistonJoint *) joint)->getPosition(); break; default: {} // keep the compiler happy } if (!pause) { double simstep = 0.01; // 1ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf (dt/simstep); if (!nrofsteps) nrofsteps = 1; for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldStep (world, simstep); dJointGroupEmpty (contactgroup); } update(); dReal radius, length; dsSetTexture (DS_WOOD); drawBox (geom[BODY2], 1,1,0); drawBox (geom[RECT], 0,0,1); if ( geom[BODY1] ) { const dReal *pos = dGeomGetPosition (geom[BODY1]); rot = dGeomGetRotation (geom[BODY1]); dsSetColor (0,0,1); dGeomCapsuleGetParams (geom[BODY1], &radius, &length); dsDrawCapsule (pos, rot, length, radius); } drawBox (geom[OBS], 1,0,1); // Draw the prismatic axis if ( geom[BODY1] ) { const dReal *pos = dGeomGetPosition (geom[BODY1]); rot = dGeomGetRotation (geom[BODY2]); dVector3 p; p[X] = pos[X] - l*ax[X]; p[Y] = pos[Y] - l*ax[Y]; p[Z] = pos[Z] - l*ax[Z]; dsSetColor (1,0,0); dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS); } if (joint->getType() == dJointTypePiston ) { dVector3 anchor; dJointGetPistonAnchor(joint->id(), anchor); // Draw the rotoide axis rot = dGeomGetRotation (geom[BODY2]); dsSetColor (1,0.5,0); dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS); dsSetColor (0,1,1); rot = dGeomGetRotation (geom[BODY1]); dsDrawSphere (anchor, rot, 1.5*RADIUS); } } }
void draw_all_objects (dSpaceID space) { int i, j; draw_all_objects_called = 1; if (!graphical_test) return; int n = dSpaceGetNumGeoms (space); // draw all contact points dsSetColor (0,1,1); dSpaceCollide (space,0,&nearCallback); // draw all rays for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); if (dGeomGetClass (g) == dRayClass) { dsSetColor (1,1,1); dVector3 origin,dir; dGeomRayGet (g,origin,dir); origin[2] += Z_OFFSET; dReal length = dGeomRayGetLength (g); for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j]; dsDrawLine (origin,dir); dsSetColor (0,0,1); dsDrawSphere (origin,dGeomGetRotation(g),0.01); } } // draw all other objects for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); dVector3 pos; if (dGeomGetClass (g) != dPlaneClass) { memcpy (pos,dGeomGetPosition(g),sizeof(pos)); pos[2] += Z_OFFSET; } switch (dGeomGetClass (g)) { case dSphereClass: { dsSetColorAlpha (1,0,0,0.8); dReal radius = dGeomSphereGetRadius (g); dsDrawSphere (pos,dGeomGetRotation(g),radius); break; } case dBoxClass: { dsSetColorAlpha (1,1,0,0.8); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,dGeomGetRotation(g),sides); break; } case dCapsuleClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,dGeomGetRotation(g),length,radius); break; } case dCylinderClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,dGeomGetRotation(g),length,radius); break; } case dPlaneClass: { dVector4 n; dMatrix3 R,sides; dVector3 pos2; dGeomPlaneGetParams (g,n); dRFromZAxis (R,n[0],n[1],n[2]); for (j=0; j<3; j++) pos[j] = n[j]*n[3]; pos[2] += Z_OFFSET; sides[0] = 2; sides[1] = 2; sides[2] = 0.001; dsSetColor (1,0,1); for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j]; dsDrawLine (pos,pos2); dsSetColorAlpha (1,0,1,0.8); dsDrawBox (pos,R,sides); break; } } } }
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (!g) return; if (dGeomIsSpace(g)) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else show_aabb = 0; if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void drawRobot_Nleg() { #ifndef NOVIZ if(visualize){ for(int segment = 0; segment < BODY_SEGMENTS; ++segment) { dReal r,length; dVector3 sides; // Drawing of body // Drawing of body dsSetTexture(1); dsSetColor(2.5,2.5,2.5); dGeomBoxGetLengths(torso[segment].geom,sides); dsDrawBox(dBodyGetPosition(torso[segment].body), dBodyGetRotation(torso[segment].body),sides); // Drawing oflegs dsSetTexture(0); dsSetColor(1.3,0.0,0.0); for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++ ) { dGeomCapsuleGetParams(leg[segment][i][j].geom, &r,&length); if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body), dBodyGetRotation(leg[segment][i][j].body),0.5*length,1.2*r); else dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body), dBodyGetRotation(leg[segment][i][j].body),length,r); } } } // int middleBodySegment = BODY_SEGMENTS/2; // cerr <<dBodyGetPosition(torso.body)[0]<<" " <<dBodyGetPosition(torso.body)[1]<< " " << dBodyGetPosition(torso.body)[2]<<endl; //FROM SIDE // hpr[0] = 90.0f; // hpr[1] = 0.0f;//-1.2f; // hpr[2] = 0.0f; // point of view[m] // xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]; // xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]-ceil(log2(BODY_SEGMENTS)); // xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°] //FROM BACK // hpr[0] = 1.0f; // hpr[1] = 0.0f;//-1.2f; // hpr[2] = 0.0f; // point of view[m] // xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-ceil(log2(BODY_SEGMENTS)); // xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]; // xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°] //FROM BELOW and BACK hpr[0] = 1.0f; hpr[1] = -1.2f; hpr[2] = 0.5f; // point of view[m] xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-BODY_SEGMENTS/2.0f; xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]; xyz[2] = 0.1; // gaze[°] dsSetViewpoint(xyz,hpr); } #endif }
/*** Drawing of robot ***/ void drawRobot() { #ifndef NOVIZ if(visualize){ dReal r,length; dVector3 sides; // Drawing of body dsSetTexture(1); dsSetColor(2.5,2.5,2.5); dGeomBoxGetLengths(torso[0].geom,sides); dsDrawBox(dBodyGetPosition(torso[0].body), dBodyGetRotation(torso[0].body),sides); // Drawing oflegs dsSetTexture(0); dsSetColor(1.3,0.0,0.0); for (int i = 0; i < num_legs; i++) { for (int j = 0; j < num_links; j++ ) { dGeomCapsuleGetParams(leg[0][i][j].geom, &r,&length); if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body), dBodyGetRotation(leg[0][i][j].body),0.5*length,1.2*r); else dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body), dBodyGetRotation(leg[0][i][j].body),length,r); } } // hpr[0] = 0.0f; // hpr[1] = -20.0f; // hpr[2] = 0.0f; // point of view[m] // //cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl; // xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f; // xyz[1] = dBodyGetPosition(torso[0].body)[1]; // xyz[2] = dBodyGetPosition(torso[0].body)[2]+0.05f; // gaze[°] hpr[0] = 1.0f; hpr[1] = -1.2f; hpr[2] = 0.5f; // point of view[m] // hpr[0] = dBodyGetRotation(torso[0].body)[0]; // hpr[1] = dBodyGetRotation(torso[0].body)[1]; // hpr[2] = dBodyGetRotation(torso[0].body)[2]; //cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl; xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f; xyz[1] = dBodyGetPosition(torso[0].body)[1]; xyz[2] = 0.1; //dBodyGetPosition(torso[0].body)[2]-0.15f; // gaze[°] //overrides to look from the other side of the robot //hpr[0] = -180.0f; //slides view to the right //xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f; // hpr[1] = -30.2f; //slides down/up // hpr[2] = 20.5f; // point of view[m] //different override for other corner xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f; xyz[1] = dBodyGetPosition(torso[0].body)[1]-.50; hpr[0] = -220.0f; //slides view to the right dsSetViewpoint(xyz,hpr); } #endif }
// copied from an OpenDE demo program //todo:pass trimesh as argument to this function void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm) { int i; if (!g) return; if (dGeomIsSpace(g)) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dTriMeshClass) { //dTriIndex* Indices = DISP.tmd[i].indices; const dReal* Pos = dGeomGetPosition(g); const dReal* Rot = dGeomGetRotation(g); for (int ii = 0; ii < (tm.indexSize/3); ii++) { const dReal v[9] = { // explicit conversion from float to dReal tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1], tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2] }; dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); } //std::cout<<"done once"<<std::endl; } else if (type == dRayClass) { dVector3 Origin, Direction; dGeomRayGet(g, Origin, Direction); dReal Length = dGeomRayGetLength(g); dVector3 End; End[0] = Origin[0] + (Direction[0] * Length); End[1] = Origin[1] + (Direction[1] * Length); End[2] = Origin[2] + (Direction[2] * Length); End[3] = Origin[3] + (Direction[3] * Length); double *ori = new double[3]; double *end = new double[4]; ori[0]=Origin[0]; ori[1]=Origin[1]; ori[2]=Origin[2]; end[0]=End[0]; end[1]=End[1]; end[2]=End[2]; end[3]=End[3]; dsDrawLine(ori, end); } else show_aabb = 0; if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
// copied from an OpenDE demo program void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { int i; if (g == nullptr) return; if (dGeomIsSpace(g) != 0) { displaySpace((dSpaceID)g); return; } int type = dGeomGetClass (g); if (type == dBoxClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dCylinderClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { if (pos == nullptr) pos = dGeomGetPosition (g); if (R == nullptr) R = dGeomGetRotation (g); dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } else show_aabb = 0; if (show_aabb != 0) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }