void start() { // adjust the starting viewpoint a bit float xyz[3],hpr[3]; dsGetViewpoint (xyz,hpr); hpr[0] += 7; dsSetViewpoint (xyz,hpr); geoms[0]=dCreateConvex (space, planes, planecount, points, pointcount, polygons); dGeomSetPosition (geoms[0],0,0,0.25); geoms[1]=dCreateConvex (space, planes, planecount, points, pointcount, polygons); dGeomSetPosition (geoms[1],0.25,0.25,0.70); }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' /* || cmd == 'l' */) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } /* // cylinder option not yet implemented else if (cmd == 'l') { sides[1] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } */ else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } else if (cmd == 'y') { sides[1] *= 0.5; dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm') { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(obj[i].geom[0], new_tmdata); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; int setBody; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') { setBody = 0; if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRSetIdentity (R); //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } //<---- Convex Object else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); #if 0 obj[i].geom[0] = dCreateConvex (space, planes, planecount, points, pointcount, polygons); #else obj[i].geom[0] = dCreateConvex (space, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (cmd == 'y') { dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x' && USE_GEOM_OFFSET) { setBody = 1; // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for encapsulated geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule (space,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate (&m2,drot[k]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody (obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition (obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass (obj[i].body,&m); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // Translation *after* rotation dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<GPB; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } if (!setBody) for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } else if (cmd == 'p'&& selected >= 0) { const dReal* pos = dGeomGetPosition(obj[selected].geom[0]); const dReal* rot = dGeomGetRotation(obj[selected].geom[0]); printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]); printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n", rot[0],rot[1],rot[2],rot[3], rot[4],rot[5],rot[6],rot[7], rot[8],rot[9],rot[10],rot[11]); } else if (cmd == 'f' && selected >= 0 && selected < num) { if (dBodyIsEnabled(obj[selected].body)) doFeedback = 1; } }
void start() { // adjust the starting viewpoint a bit float xyz[3],hpr[3]; dsGetViewpoint (xyz,hpr); hpr[0] += 7; dsSetViewpoint (xyz,hpr); convex[0]=dCreateConvex (space, planes, planecount, points, pointcount, polygons); convex[1]=dCreateConvex (space, planes, planecount, points, pointcount, polygons); boxes[0]=dCreateBox(space,0.5,0.5,0.5); boxes[1]=dCreateBox(space,0.5,0.5,0.5); geoms=convex; dMatrix3 m1 = { 1,0,0,0,0,1,0,0,0,0,1,0 }; dMatrix3 m2 = { 1,0,0,0,0,1,0,0,0,0,1,0 }; #if 0 dGeomSetPosition (convex[0], 0.0, 0.0, 0.25); dGeomSetPosition (convex[1], geom1pos[0], geom1pos[1], geom1pos[2]); dQtoR (geom0quat, m1); dGeomSetRotation (convex[0],m1); dQtoR (geom1quat, m2); dGeomSetRotation (convex[1],m2); dGeomSetPosition (boxes[0], 0.0, 0.0, 0.25); dGeomSetPosition (boxes[1], geom1pos[0], geom1pos[1], geom1pos[2]); dQtoR (geom0quat, m1); dGeomSetRotation (boxes[0],m1); dQtoR (geom1quat, m2); dGeomSetRotation (boxes[1],m2); #else dGeomSetPosition (convex[0], fixed_pos_0[0], fixed_pos_0[1], fixed_pos_0[2]); dGeomSetPosition (convex[1], fixed_pos_1[0], fixed_pos_1[1], fixed_pos_1[2]); dGeomSetRotation (convex[0],fixed_rot_0); dGeomSetRotation (convex[1],fixed_rot_1); dGeomSetPosition (boxes[0], fixed_pos_0[0], fixed_pos_0[1], fixed_pos_0[2]); dGeomSetPosition (boxes[1], fixed_pos_1[0], fixed_pos_1[1], fixed_pos_1[2]); dGeomSetRotation (boxes[0],fixed_rot_0); dGeomSetRotation (boxes[1],fixed_rot_1); #endif }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; cmd = locase (cmd); // // Geom Creation // if ( cmd == 'b' || cmd == 's' || cmd == 'c' || ( cmd == 'm' && g_allow_trimesh ) || cmd == 'x' || cmd == 'y' || cmd == 'v' ) { if ( num < NUM ) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, (dRandReal()-0.5)*HFIELD_WIDTH*0.75, (dRandReal()-0.5)*HFIELD_DEPTH*0.75, dRandReal() + 2 ); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,maxheight+1,0); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } //<---- Convex Object else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex (space, planes, planecount, points, pointcount, polygons); } //----> Convex Object else if (cmd == 'y') { dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm' && g_allow_trimesh) { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, &Indices[0], IndexCount, 3 * sizeof(dTriIndex)); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<2; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } // // Control Commands // if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } }
// called when a key pressed static void command( int cmd ) { int i,k; dReal sides[3]; dMass m; cmd = locase( cmd ); if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' ) { if ( num < NUM ) { i = num; num++; } else { i = nextobj; nextobj++; if ( nextobj >= num ) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy( obj[i].body ); for ( k=0; k < GPB; k++ ) { if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] ); } memset( &obj[i],0,sizeof( obj[i] ) ); } obj[i].body = dBodyCreate( world ); for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if ( random_pos ) { dBodySetPosition( obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 ); dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 ); } else { dReal maxheight = 0; for ( k=0; k<num; k++ ) { const dReal *pos = dBodyGetPosition( obj[k].body ); if ( pos[2] > maxheight ) maxheight = pos[2]; } dBodySetPosition( obj[i].body, 0,0,maxheight+1 ); dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 ); } dBodySetRotation( obj[i].body,R ); dBodySetData( obj[i].body,( void* )( size_t )i ); if ( cmd == 'b' ) { dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] ); obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] ); } else if ( cmd == 'c' ) { sides[0] *= 0.5; dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] ); obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] ); } else if ( cmd == 's' ) { sides[0] *= 0.5; dMassSetSphere( &m,DENSITY,sides[0] ); obj[i].geom[0] = dCreateSphere( space,sides[0] ); } else if ( cmd == 'v' ) { obj[i].geom[0] = dCreateConvex( space, convexBunnyPlanes, convexBunnyPlaneCount, convexBunnyPoints, convexBunnyPointCount, convexBunnyPolygons ); /// Use equivalent TriMesh to set mass dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount, ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) ); dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 ); dMassSetTrimesh( &m, DENSITY, triMesh ); dGeomDestroy( triMesh ); dGeomTriMeshDataDestroy( new_tmdata ); printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] ); dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] ); dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] ); } for ( k=0; k < GPB; k++ ) { if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body ); } dBodySetMass( obj[i].body,&m ); } if ( cmd == ' ' ) { selected++; if ( selected >= num ) selected = 0; if ( selected < 0 ) selected = 0; } else if ( cmd == 'd' && selected >= 0 && selected < num ) { dBodyDisable( obj[selected].body ); } else if ( cmd == 'e' && selected >= 0 && selected < num ) { dBodyEnable( obj[selected].body ); } else if ( cmd == 'a' ) { show_aabb ^= 1; } else if ( cmd == 't' ) { show_contacts ^= 1; } else if ( cmd == 'r' ) { random_pos ^= 1; } }