// 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 vmWishboneCar::drawLinkage(vm::WheelLoc loc) { // select wheel vmWishbone *snow; switch (loc) { case vm::WheelLoc::FR: snow= &frSuspension; break; case vm::WheelLoc::FL: snow= &flSuspension; break; case vm::WheelLoc::RR: snow= &rrSuspension; break; case vm::WheelLoc::RL: snow= &rlSuspension; break; default: break; } // setup linkage const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3; const dReal *pos4,*R4,*pos5,*R5; dsSetColorAlpha(0.0,0.0,1.0, 0.5); pos1 = dBodyGetPosition(snow->uplink.body); R1 = dBodyGetRotation(snow->uplink.body); dsDrawCylinderD(pos1,R1,snow->uplink.width,snow->uplink.radius); dsSetColor(0.0,1.0,0.0); pos2 = dBodyGetPosition(snow->hlink.body); R2 = dBodyGetRotation(snow->hlink.body); dsDrawCylinderD(pos2,R2,snow->hlink.width,snow->hlink.radius); dsSetColorAlpha(1.0,0.0,1.0, 0.5); pos3 = dBodyGetPosition(snow->lowlink.body); R3 = dBodyGetRotation(snow->lowlink.body); dsDrawCylinderD(pos3,R3,snow->lowlink.width,snow->lowlink.radius); // setup spring and damper dsSetColorAlpha(0.1,0.7,1.0, 0.5); pos4= dBodyGetPosition(snow->upstrut.body); R4= dBodyGetRotation(snow->upstrut.body); dsDrawCylinderD(pos4,R4,snow->upstrut.width,snow->upstrut.radius); dsSetColorAlpha(1.0,0.7,1.0, 0.5); pos5= dBodyGetPosition(snow->lowstrut.body); R5= dBodyGetRotation(snow->lowstrut.body); dsDrawCylinderD(pos5,R5,snow->lowstrut.width,snow->lowstrut.radius); }
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); } }
static void simLoop (int pause) { dSpaceCollide (space,0,&nearCallback); if (!pause) { dWorldQuickStep (world, 0.01); // 100 Hz } dJointGroupEmpty (contactgroup); dsSetColorAlpha (1,1,0,0.5); const dReal *CPos = dBodyGetPosition(cylbody); const dReal *CRot = dBodyGetRotation(cylbody); float cpos[3] = {CPos[0], CPos[1], CPos[2]}; float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] }; dsDrawCylinder ( cpos, crot, CYLLENGTH, CYLRADIUS ); // single precision const dReal *SPos = dBodyGetPosition(sphbody); const dReal *SRot = dBodyGetRotation(sphbody); float spos[3] = {SPos[0], SPos[1], SPos[2]}; float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] }; dsDrawSphere ( spos, srot, SPHERERADIUS ); // single precision }
//=========================================================================================== //! \brief 描画関数 void draw_world( void ) //=========================================================================================== { int i; dsSetColor (0,0.5,1); dsSetTexture (DS_WOOD); dReal rad, len; dReal sides[4]; dBox *blink; dCapsule *clink; dsSetColorAlpha (1.0, 0.0, 0.0, 0.6); i=0; blink=&LinkBase; blink->getLengths(sides); dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides); dsSetColorAlpha (0.0, 0.5, 1.0, 0.6); i=1; clink=&Link1; clink->getParams(&rad, &len); dsDrawCappedCylinder (body[i].getPosition(),body[i].getRotation(),len,rad); i=2; clink=&Link2; clink->getParams(&rad, &len); dsDrawCappedCylinder (body[i].getPosition(),body[i].getRotation(),len,rad); }
void drawGeom(dGeomID g) { int gclass = dGeomGetClass(g); const dReal *pos = dGeomGetPosition(g); const dReal *rot = dGeomGetRotation(g); switch (gclass) { case dSphereClass: dsSetColorAlpha(0, 0.75, 0.5, 0.5); dsSetTexture (DS_CHECKERED); dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); break; case dBoxClass: { dVector3 lengths; dsSetColorAlpha(1, 1, 0, 0.5); dsSetTexture (DS_WOOD); dGeomBoxGetLengths(g, lengths); dsDrawBox(pos, rot, lengths); break; } case dTriMeshClass: { int numi = dGeomTriMeshGetTriangleCount(g); for (int i=0; i<numi; ++i) { dVector3 v0, v1, v2; dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2); dsSetTexture (DS_WOOD); dsSetDrawMode(DS_WIREFRAME); dsSetColorAlpha(0, 0, 0, 1.0); dsDrawTriangle(pos, rot, v0, v1, v2, true); dsSetDrawMode(DS_POLYFILL); dsSetColorAlpha(1, 1, 0, 0.5); dsDrawTriangle(pos, rot, v0, v1, v2, true); } break; } default: {} } }
void DrawBox() { for(int i = 0; i < BOX_NUM; i++) { dsSetColorAlpha(0.3, 0.2, 0.1, 1.0); dsSetTexture(DS_WOOD); dsDrawBoxD(dBodyGetPosition(box[i].body),dBodyGetRotation(box[i].body), boxSize[i]); } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { assert(o1); assert(o2); if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) { fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2); // colliding a space with something dSpaceCollide2(o1,o2,data,&nearCallback); // Note we do not want to test intersections within a space, // only between spaces. return; } const int N = 32; dContact contact[N]; int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); if (n > 0) { for (int i=0; i<n; i++) { contact[i].surface.mode = 0; contact[i].surface.mu = 50.0; // was: dInfinity dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); if (show_contacts) { dMatrix3 RI; dRSetIdentity (RI); const dReal ss[3] = {0.12,0.12,0.12}; dsSetColorAlpha (0,0,1,0.5); dsDrawBox (contact[i].geom.pos,RI,ss); dReal *pos = contact[i].geom.pos; dReal depth = contact[i].geom.depth; dReal *norm = contact[i].geom.normal; dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]}; dsSetColorAlpha (1,1,1,1); dsDrawLine (contact[i].geom.pos, endp); } } } }
IoObject *IoDrawStuff_dsSetColorAlpha(IoDrawStuff *self, IoObject *locals, IoMessage *m) { float red = IoMessage_locals_intArgAt_(m, locals, 0); float green = IoMessage_locals_intArgAt_(m, locals, 1); float blue = IoMessage_locals_intArgAt_(m, locals, 2); float alpha = IoMessage_locals_intArgAt_(m, locals, 3); dsSetColorAlpha(red, green, blue, alpha); return self; }
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 vmWishboneCar::simDraw() { // draw suspension linkages drawLinkage(vm::FR); drawLinkage(vm::FL); drawLinkage(vm::RR); drawLinkage(vm::RL); // draw wheels dsSetColorAlpha(1.0,1.0,0.0,0.5); dsDrawCylinderD(dBodyGetPosition(frWheel.body),dBodyGetRotation(frWheel.body),frWheel.length,frWheel.radius); dsDrawCylinderD(dBodyGetPosition(flWheel.body),dBodyGetRotation(frWheel.body),flWheel.length,flWheel.radius); dsDrawCylinderD(dBodyGetPosition(rrWheel.body),dBodyGetRotation(rrWheel.body),rrWheel.length,rrWheel.radius); dsDrawCylinderD(dBodyGetPosition(rlWheel.body),dBodyGetRotation(rlWheel.body),rlWheel.length,rlWheel.radius); // draw chassis dsSetColorAlpha(1.0,0.0,0.0,0.5); // red dsDrawBoxD(dBodyGetPosition(chassis.body),dBodyGetRotation(chassis.body) ,chassis.sides); }
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 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); } }
// 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); } }
static void simLoop (int pause) { int i,j; dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); //if (!pause) dWorldStep (world,0.05); //if (!pause) dWorldQuickStep (world,0.05); if (!pause) dWorldStepFast1 (world,0.05, 5); if (write_world) { FILE *f = fopen ("state.dif","wt"); if (f) { dWorldExportDIF (world,f,"X"); fclose (f); } write_world = 0; } // remove all contact joints dJointGroupEmpty (contactgroup); const dReal* pReal = dGeomGetPosition( gheight ); const dReal* RReal = dGeomGetRotation( gheight ); // // Draw Heightfield // // Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode. int ox = (int) ( -HFIELD_WIDTH/2 ); int oz = (int) ( -HFIELD_DEPTH/2 ); // for ( int tx = -1; tx < 2; ++tx ) // for ( int tz = -1; tz < 2; ++tz ) { dsSetColorAlpha (0.5,1,0.5,0.5); dsSetTexture( DS_WOOD ); for ( int i = 0; i < HFIELD_WSTEP - 1; ++i ) for ( int j = 0; j < HFIELD_DSTEP - 1; ++j ) { dReal a[3], b[3], c[3], d[3]; a[ 0 ] = ox + ( i ) * HFIELD_WSAMP; a[ 1 ] = heightfield_callback( NULL, i, j ); a[ 2 ] = oz + ( j ) * HFIELD_DSAMP; b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; b[ 1 ] = heightfield_callback( NULL, i + 1, j ); b[ 2 ] = oz + ( j ) * HFIELD_DSAMP; c[ 0 ] = ox + ( i ) * HFIELD_WSAMP; c[ 1 ] = heightfield_callback( NULL, i, j + 1 ); c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP; d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 ); d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP; dsDrawTriangle( pReal, RReal, a, c, b, 1 ); dsDrawTriangle( pReal, RReal, b, c, d, 1 ); } } dsSetColor (1,1,0); dsSetTexture (DS_WOOD); for (i=0; i<num; i++) { for (j=0; j < GPB; j++) { if (i==selected) { dsSetColor (0,0.7,1); } else if (! dBodyIsEnabled (obj[i].body)) { dsSetColor (1,0.8,0); } else { dsSetColor (1,1,0); } if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass ) { dTriIndex* Indices = (dTriIndex*)::Indices; // assume all trimeshes are drawn as bunnies const dReal* Pos = dGeomGetPosition(obj[i].geom[j]); const dReal* Rot = dGeomGetRotation(obj[i].geom[j]); for (int ii = 0; ii < IndexCount / 3; ii++) { const dReal v[9] = { // explicit conversion from float to dReal Vertices[Indices[ii * 3 + 0] * 3 + 0], Vertices[Indices[ii * 3 + 0] * 3 + 1], Vertices[Indices[ii * 3 + 0] * 3 + 2], Vertices[Indices[ii * 3 + 1] * 3 + 0], Vertices[Indices[ii * 3 + 1] * 3 + 1], Vertices[Indices[ii * 3 + 1] * 3 + 2], Vertices[Indices[ii * 3 + 2] * 3 + 0], Vertices[Indices[ii * 3 + 2] * 3 + 1], Vertices[Indices[ii * 3 + 2] * 3 + 2] }; dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); } // tell the tri-tri collider the current transform of the trimesh -- // this is fairly important for good results. // Fill in the (4x4) matrix. dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ); p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0; p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0; p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0; p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1; // Flip to other matrix. obj[i].last_matrix_index = !obj[i].last_matrix_index; // Apply the 'other' matrix which is the oldest. dGeomTriMeshSetLastTransform( obj[i].geom[j], *(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) ); } else { drawGeom (obj[i].geom[j],0,0,show_aabb); } } } if ( show_aabb ) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (gheight,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 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); } }
static void simLoop (int pause) { dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); if (!pause) dWorldStep (world,0.05); dAASSERT(terrainY); dAASSERT(terrainZ); dsSetColor (0,1,0); dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY)); dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ)); if (show_aabb) { dReal aabb[6]; dGeomGetAABB (terrainY,aabb); dVector3 bbpos; int i; 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); dGeomGetAABB (terrainZ,aabb); for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; dsDrawBox (bbpos,RI,bbsides); } dsSetColor (1,1,0); // remove all contact joints dJointGroupEmpty (contactgroup); dsSetColor (1,1,0); dsSetTexture (DS_WOOD); for (int i=0; i<num; i++) { for (int j=0; j < GPB; j++) { if (i==selected) { dsSetColor (0,0.7,1); } else if (! dBodyIsEnabled (obj[i].body)) { dsSetColor (1,0,0); } else { dsSetColor (1,1,0); } drawGeom (obj[i].geom[j],0,0,show_aabb); } } }
// 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); } }
static void drawGeom(dGeomID geomID) { // printf("1%lu", (uint64_t)geomID); int gclass = dGeomGetClass(geomID); // printf("2\n"); const dReal *pos = NULL; const dReal *rot = NULL; bool canDrawJoints = false; ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID); if (nullptr != userObj) { dsSetColorAlpha(1, 1, 1, 1); // printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]); dsSetTexture (userObj->textureNum); dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]); } else { dsSetColorAlpha(1, 1, 0, 1); dsSetTexture (DS_WOOD); } switch (gclass) { case dSphereClass: if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID)); } canDrawJoints = true; break; case dBoxClass: { if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dVector3 lengths; dGeomBoxGetLengths(geomID, lengths); dsDrawBox(pos, rot, lengths); } canDrawJoints = true; break; } case dCylinderClass: { if (nullptr != userObj && userObj->visible) { pos = dGeomGetPosition(geomID); rot = dGeomGetRotation(geomID); dReal length; dReal radius; dGeomCylinderGetParams(geomID, &radius, &length); dsDrawCylinder(pos, rot, length, radius); } canDrawJoints = true; break; } default: break; } // printf("class: %d\n", gclass); if (canDrawJoints) { #ifdef DRAW_JOINTS_TOO dBodyID body = dGeomGetBody(geomID); int numJoints = dBodyGetNumJoints(body); for (int i = 0; i < numJoints; ++i) { dJointID joint = dBodyGetJoint(body, i); int jointClass = dJointGetType(joint); switch (jointClass) { case dJointTypeHinge: { dVector3 a11; dBodyID body1 = dJointGetBody(joint, 0); dBodyID body2 = dJointGetBody(joint, 1); if (body1 && body2) { const dReal* bodyPos1 = dBodyGetPosition(body1); const dReal* bodyPos2 = dBodyGetPosition(body2); dJointGetHingeAnchor(joint, a11); dsSetColor(1, 0, 0); dsDrawLine(a11, bodyPos1); dsDrawLine(a11, bodyPos2); dsSetColor(0, 1, 0); dsDrawLine(bodyPos1, bodyPos2); } } } } #endif } }