void simLoop (int pause) { static bool DumpInfo=true; const dReal ss[3] = {0.02,0.02,0.02}; dContactGeom contacts[8]; int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom)); //fprintf(stdout,"Contact Count %d\n",contactcount); const dReal* pos; const dReal* R; dsSetTexture (DS_WOOD); pos = dGeomGetPosition (geoms[0]); R = dGeomGetRotation (geoms[0]); dsSetColor (0.6f,0.6f,1); dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); pos = dGeomGetPosition (geoms[1]); R = dGeomGetRotation (geoms[1]); dsSetColor (0.4f,1,1); dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); /*if (show_contacts) */ dMatrix3 RI; dRSetIdentity (RI); dsSetColor (1.0f,0,0); for(int i=0;i<contactcount;++i) { if(DumpInfo) { //DumpInfo=false; fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n", i, contacts[i].normal[0], contacts[i].normal[1], contacts[i].normal[2], contacts[i].depth); } dsDrawBox (contacts[i].pos,RI,ss); } if(DumpInfo) DumpInfo=false; }
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 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); } }
void simLoop (int pause) { int contactcount; const dReal ss[3] = {0.02,0.02,0.02}; dContactGeom contacts[8]; if(geoms==convex) contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom)); else contactcount = dCollideBoxBox(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom)); //fprintf(stdout,"Contact Count %d\n",contactcount); const dReal* pos; const dReal* R; dsSetTexture (DS_WOOD); pos = dGeomGetPosition (geoms[0]); R = dGeomGetRotation (geoms[0]); dsSetColor (0.6f,0.6f,1); dsSetDrawMode(drawmode); dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); dsSetDrawMode(DS_POLYFILL); pos = dGeomGetPosition (geoms[1]); R = dGeomGetRotation (geoms[1]); dsSetColor (0.4f,1,1); dsSetDrawMode(drawmode); dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); dsSetDrawMode(DS_POLYFILL); /*if (show_contacts) */ dMatrix3 RI; dRSetIdentity (RI); dsSetColor (1.0f,0,0); for(int i=0;i<contactcount;++i) { if(DumpInfo) { //DumpInfo=false; fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f Pos %f %f %f ", i, contacts[i].normal[0], contacts[i].normal[1], contacts[i].normal[2], contacts[i].depth, contacts[i].pos[0], contacts[i].pos[1], contacts[i].pos[2]); if(contacts[i].g1==geoms[0]) { fprintf(stdout,"Geoms 1 2\n"); } else { fprintf(stdout,"Geoms 2 1\n"); } } dsDrawBox (contacts[i].pos,RI,ss); } if(DumpInfo) DumpInfo=false; }