IoObject *IoDrawStuff_dsDrawTriangle(IoDrawStuff *self, IoObject *locals, IoMessage *m) { float *pos; float *R; float *v0; float *v1; float *v2; int solid; //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); other = IoMessage_locals_vectorArgAt_(m, locals, 1); v0 = IoSeq_floatPointerOfLength_(other, 3); other = IoMessage_locals_vectorArgAt_(m, locals, 2); v1 = IoSeq_floatPointerOfLength_(other, 3); other = IoMessage_locals_vectorArgAt_(m, locals, 3); v2 = IoSeq_floatPointerOfLength_(other, 3); solid = IoMessage_locals_intArgAt_(m, locals, 4); } dsDrawTriangle(pos, R, v0, v1, v2, solid); return self; }
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: {} } }
static void simLoop (int pause) { dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); if (!pause) dWorldStep (world,0.05); //if (!pause) dWorldStepFast (world,0.05, 1); // 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); } } /*{ for (int i = 1; i < IndexCount; i++) { dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]); } }*/ {const dReal* Pos = dGeomGetPosition(TriMesh); const dReal* Rot = dGeomGetRotation(TriMesh); {for (int i = 0; i < IndexCount / 3; i++){ const dVector3& v0 = Vertices[Indices[i * 3 + 0]]; const dVector3& v1 = Vertices[Indices[i * 3 + 1]]; const dVector3& v2 = Vertices[Indices[i * 3 + 2]]; dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0); }}} if (Ray){ dVector3 Origin, Direction; dGeomRayGet(Ray, Origin, Direction); dReal Length = dGeomRayGetLength(Ray); 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); dsDrawLine(Origin, End); } }
static void simLoop (int pause) { double simstep = 0.001; // 1ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf(dt/simstep); // fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps); for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldQuickStep (world, simstep); dJointGroupEmpty (contactgroup); } dsSetColor (1,1,1); 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, RADIUS ); // draw world trimesh dsSetColor(0.4,0.7,0.9); dsSetTexture (DS_NONE); const dReal* Pos = dGeomGetPosition(world_mesh); //dIASSERT(dVALIDVEC3(Pos)); float pos[3] = { Pos[0], Pos[1], Pos[2] }; const dReal* Rot = dGeomGetRotation(world_mesh); //dIASSERT(dVALIDMAT3(Rot)); float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] }; int numi = sizeof(world_indices) / sizeof(int); for (int i=0; i<numi/3; i++) { int i0 = world_indices[i*3+0]; int i1 = world_indices[i*3+1]; int i2 = world_indices[i*3+2]; float *v0 = world_vertices+i0*3; float *v1 = world_vertices+i1*3; float *v2 = world_vertices+i2*3; dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw } }
static void simLoop (int pause) { dsSetColor (0,0,2); dSpaceCollide (space,0,&nearCallback); #if 1 // What is this for??? - Bram if (!pause) { for (int i=0; i<num; i++) for (int j=0; j < GPB; j++) if (obj[i].geom[j]) if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) setCurrentTransform(obj[i].geom[j]); setCurrentTransform(TriMesh1); setCurrentTransform(TriMesh2); } #endif //if (!pause) dWorldStep (world,0.05); if (!pause) dWorldQuickStep (world,0.05); for (int j = 0; j < dSpaceGetNumGeoms(space); j++){ dSpaceGetGeom(space, j); } // 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 (obj[i].geom[j]) { if (i==selected) { dsSetColor (0,0.7,1); } else if (! dBodyIsEnabled (obj[i].body)) { dsSetColor (1,0,0); } else { dsSetColor (1,1,0); } if (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; 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); } } } } dTriIndex* Indices = (dTriIndex*)::Indices; {const dReal* Pos = dGeomGetPosition(TriMesh1); const dReal* Rot = dGeomGetRotation(TriMesh1); {for (int i = 0; i < IndexCount / 3; i++){ const dReal v[9] = { // explicit conversion from float to dReal Vertices[Indices[i * 3 + 0] * 3 + 0], Vertices[Indices[i * 3 + 0] * 3 + 1], Vertices[Indices[i * 3 + 0] * 3 + 2], Vertices[Indices[i * 3 + 1] * 3 + 0], Vertices[Indices[i * 3 + 1] * 3 + 1], Vertices[Indices[i * 3 + 1] * 3 + 2], Vertices[Indices[i * 3 + 2] * 3 + 0], Vertices[Indices[i * 3 + 2] * 3 + 1], Vertices[Indices[i * 3 + 2] * 3 + 2] }; dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0); }}} {const dReal* Pos = dGeomGetPosition(TriMesh2); const dReal* Rot = dGeomGetRotation(TriMesh2); {for (int i = 0; i < IndexCount / 3; i++){ const dReal v[9] = { // explicit conversion from float to dReal Vertices[Indices[i * 3 + 0] * 3 + 0], Vertices[Indices[i * 3 + 0] * 3 + 1], Vertices[Indices[i * 3 + 0] * 3 + 2], Vertices[Indices[i * 3 + 1] * 3 + 0], Vertices[Indices[i * 3 + 1] * 3 + 1], Vertices[Indices[i * 3 + 1] * 3 + 2], Vertices[Indices[i * 3 + 2] * 3 + 0], Vertices[Indices[i * 3 + 2] * 3 + 1], Vertices[Indices[i * 3 + 2] * 3 + 2] }; dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1); }}} }
static void simLoop (int pause) { double simstep = 0.005; // 5ms simulation steps double dt = dsElapsedTime(); int nrofsteps = (int) ceilf(dt/simstep); for (int i=0; i<nrofsteps && !pause; i++) { dSpaceCollide (space,0,&nearCallback); dWorldQuickStep (world, simstep); dJointGroupEmpty (contactgroup); } dsSetColor (1,1,1); #ifdef BOX const dReal *BPos = dBodyGetPosition(boxbody); const dReal *BRot = dBodyGetRotation(boxbody); float bpos[3] = {BPos[0], BPos[1], BPos[2]}; float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] }; float sides[3] = {BOXSZ, BOXSZ, BOXSZ}; dsDrawBox ( bpos, brot, sides ); // single precision #endif #ifdef CYL const dReal *CPos = dGeomGetPosition(cylgeom); const dReal *CRot = dGeomGetRotation(cylgeom); 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 ( // dBodyGetPosition(cylbody), // dBodyGetRotation(cylbody), cpos, crot, WHEELW, RADIUS ); // single precision #endif // draw world trimesh dsSetColor(0.7,0.7,0.4); dsSetTexture (DS_NONE); const dReal* Pos = dGeomGetPosition(world_mesh); float pos[3] = { Pos[0], Pos[1], Pos[2] }; const dReal* Rot = dGeomGetRotation(world_mesh); float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] }; int numi = sizeof(world_indices) / sizeof(dTriIndex); for (int i=0; i<numi/3; i++) { int i0 = world_indices[i*3+0]; int i1 = world_indices[i*3+1]; int i2 = world_indices[i*3+2]; float *v0 = world_vertices+i0*3; float *v1 = world_vertices+i1*3; float *v2 = world_vertices+i2*3; dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw } }
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); } }
// 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); } }