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);
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce | dContactApprox1; //dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
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;
}
bool CPHMovementControl:: ActivateBoxDynamic(DWORD id,int num_it/*=8*/,int num_steps/*5*/,float resolve_depth/*=0.01f*/)
{
bool character_exist=CharacterExist();
if(character_exist&&trying_times[id]!=u32(-1))
{
Fvector dif;dif.sub(trying_poses[id],cast_fv(dBodyGetPosition(m_character->get_body())));
if(Device.dwTimeGlobal-trying_times[id]<500&&dif.magnitude()<0.05f)
return false;
}
if(!m_character||m_character->PhysicsRefObject()->PPhysicsShell())return false;
DWORD old_id=BoxID();
bool character_disabled=character_exist && !m_character->IsEnabled();
if(character_exist&&id==old_id)return true;
if(!character_exist)
{
CreateCharacter();
}
//m_PhysicMovementControl->ActivateBox(id);
m_character->CPHObject::activate();
ph_world->Freeze();
UnFreeze();
saved_callback=ObjectContactCallback();
SetOjectContactCallback(TestDepthCallback);
SetFootCallBack(TestFootDepthCallback);
max_depth=0.f;
//////////////////////////////////pars///////////////////////////////////////////
// int num_it=8;
// int num_steps=5;
// float resolve_depth=0.01f;
if(!character_exist)
{
num_it=20;
num_steps=1;
resolve_depth=0.1f;
}
///////////////////////////////////////////////////////////////////////
float fnum_it=float(num_it);
float fnum_steps=float(num_steps);
float fnum_steps_r=1.f/fnum_steps;
Fvector vel;
Fvector pos;
GetCharacterVelocity(vel);
GetCharacterPosition(pos);
//const Fbox& box =Box();
float pass= character_exist ? _abs(Box().getradius()-boxes[id].getradius()) : boxes[id].getradius();
float max_vel=pass/2.f/fnum_it/fnum_steps/fixed_step;
float max_a_vel=M_PI/8.f/fnum_it/fnum_steps/fixed_step;
dBodySetForce(GetBody(),0.f,0.f,0.f);
dBodySetLinearVel(GetBody(),0.f,0.f,0.f);
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
CVelocityLimiter vl(GetBody(),max_vel,max_vel);
max_vel=1.f/fnum_it/fnum_steps/fixed_step;
bool ret=false;
m_character->SwitchOFFInitContact();
vl.Activate();
vl.l_limit*=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
////////////////////////////////////
for(int m=0;30>m;++m)
{
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
max_depth=0.f;
ph_world->Step();
if(max_depth < resolve_depth)
{
break;
}
ph_world->CutVelocity(max_vel,max_a_vel);
}
vl.l_limit/=(fnum_it*fnum_steps/5.f);
vl.y_limit=vl.l_limit;
/////////////////////////////////////
for(int m=0;num_steps>m;++m)
{
float param =fnum_steps_r*(1+m);
InterpolateBox(id,param);
ret=false;
for(int i=0;num_it>i;++i){
max_depth=0.f;
Calculate(Fvector().set(0,0,0),Fvector().set(1,0,0),0,0,0,0);
EnableCharacter();
m_character->ApplyForce(0,ph_world->Gravity()*m_character->Mass(),0);
ph_world->Step();
ph_world->CutVelocity(max_vel,max_a_vel);
if(max_depth < resolve_depth)
{
ret=true;
break;
}
}
if(!ret) break;
}
m_character->SwitchInInitContact();
vl.Deactivate();
ph_world->UnFreeze();
if(!ret)
{
if(!character_exist)DestroyCharacter();
else if(character_disabled)m_character->Disable();
ActivateBox(old_id);
SetVelocity(vel);
dBodyID b=GetBody();
if(b)
{
dMatrix3 R;
dRSetIdentity (R);
dBodySetAngularVel(b,0.f,0.f,0.f);
dBodySetRotation(b,R);
}
SetPosition(pos);
//Msg("can not activate!");
}
else
{
ActivateBox(id);
//Msg("activate!");
}
SetOjectContactCallback(saved_callback);
SetVelocity(vel);
saved_callback=0;
if(!ret&&character_exist)
{
trying_times[id]=Device.dwTimeGlobal;
trying_poses[id].set(cast_fv(dBodyGetPosition(m_character->get_body())));
}
else
{
trying_times[id]=u32(-1);
}
return ret;
}
static void command (int cmd)
{
size_t i;
int k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y')
{
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
if (obj[i].body) {
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 + platpos[0],
dRandReal()*2-1 + platpos[1],
dRandReal()+2 + platpos[2]);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dBodySetPosition (obj[i].body,
platpos[0],
platpos[1],
platpos[2]+2);
dRSetIdentity (R);
}
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]);
}
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]);
}
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);
}
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 == ' ') {
mov_time = 0;
}
else if (cmd == 'm') {
mov_type = mov_type==1 ? 2 : 1;
mov_time = 0;
}
}
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);
}
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);
}
}
// 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);
}
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
int main (int argc, char **argv)
{
printf("ODE configuration: %s\n", dGetConfiguration());
// Is trimesh support built into this ODE?
g_allow_trimesh = dCheckConfiguration( "ODE_EXT_trimesh" );
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.05);
dWorldSetCFM (world,1e-5);
dWorldSetAutoDisableFlag (world,1);
dWorldSetContactMaxCorrectingVel (world,0.1);
dWorldSetContactSurfaceLayer (world,0.001);
memset (obj,0,sizeof(obj));
#if 1
dWorldSetAutoDisableAverageSamplesCount( world, 1 );
#endif
// base plane to catch overspill
dCreatePlane( space, 0, 0, 1, 0 );
// our heightfield floor
dHeightfieldDataID heightid = dGeomHeightfieldDataCreate();
// Create an finite heightfield.
dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback,
HFIELD_WIDTH, HFIELD_DEPTH, HFIELD_WSTEP, HFIELD_DSTEP,
REAL( 1.0 ), REAL( 0.0 ), REAL( 0.0 ), 0 );
// Give some very bounds which, while conservative,
// makes AABB computation more accurate than +/-INF.
dGeomHeightfieldDataSetBounds( heightid, REAL( -4.0 ), REAL( +6.0 ) );
gheight = dCreateHeightfield( space, heightid, 1 );
dVector3 pos;
pos[ 0 ] = 0;
pos[ 1 ] = 0;
pos[ 2 ] = 0;
// Rotate so Z is up, not Y (which is the default orientation)
dMatrix3 R;
dRSetIdentity( R );
dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );
// Place it.
dGeomSetRotation( gheight, R );
dGeomSetPosition( gheight, pos[0], pos[1], pos[2] );
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
// destroy heightfield data, because _we_ own it not ODE
dGeomHeightfieldDataDestroy( heightid );
dCloseODE();
return 0;
}
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;
}
}
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)
{
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
//----> 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_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);
}
}
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);
}
}
int main (int argc, char **argv)
{
dMass m;
dMatrix3 R;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-9.8);
dWorldSetQuickStepNumIterations (world, 64);
// Create a static world using a triangle mesh that we can collide with.
int numv = sizeof(world_vertices)/(3*sizeof(float));
int numi = sizeof(world_indices)/ sizeof(dTriIndex);
printf("numv=%d, numi=%d\n", numv, numi);
dTriMeshDataID Data = dGeomTriMeshDataCreate();
// fprintf(stderr,"Building Single Precision Mesh\n");
dGeomTriMeshDataBuildSingle
(
Data,
world_vertices,
3 * sizeof(float),
numv,
world_indices,
numi,
3 * sizeof(dTriIndex)
);
world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
dGeomTriMeshEnableTC(world_mesh, dSphereClass, false);
dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);
dGeomSetPosition(world_mesh, 0, 0, 0.5);
dRSetIdentity(R);
//dIASSERT(dVALIDMAT3(R));
dGeomSetRotation (world_mesh, R);
//float sx=0.0, sy=3.40, sz=6.80;
(void)world_normals; // get rid of compiler warning
sphbody = dBodyCreate (world);
dMassSetSphere (&m,1,RADIUS);
dBodySetMass (sphbody,&m);
sphgeom = dCreateSphere(0, RADIUS);
dGeomSetBody (sphgeom,sphbody);
reset_ball();
dSpaceAdd (space, sphgeom);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
// Causes segm violation? Why?
// (because dWorldDestroy() destroys body connected to geom; must call first!)
dGeomDestroy(sphgeom);
dGeomDestroy (world_mesh);
dJointGroupEmpty (contactgroup);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
