I'm trying to learn ODE but I'm having a bit of trouble. In my simulation, I have a box which is supposed to just fall down and collide with my ground. It all works fairly well, but the box never comes to rest, instead it just sits there shaking violently and occasionally jumping up. I can't figure it out for the life of me. Here's the code I'm using:
dynamicsWorld = dWorldCreate();
dWorldSetGravity(dynamicsWorld, 0, -9.81, 0);
dWorldSetContactSurfaceLayer(dynamicsWorld, 0.001);
boxBody = dBodyCreate(dynamicsWorld);
dMass *boxmass = new dMass;
dMassSetBoxTotal(boxmass, 20, .5, .5, .5);
dBodySetMass(boxBody, boxmass);
dBodySetPosition(boxBody, 0, 10, 0);
dBodyAddTorque(boxBody, 200, 0, 200);
dynamicsSpace = dHashSpaceCreate(0);
boxGeom = dCreateBox(dynamicsSpace, .5, .5, .5);
dGeomSetBody(boxGeom, boxBody);
groundGeom = dCreateBox(dynamicsSpace, 10, .2, 10);
contactGroup = dJointGroupCreate(0);
dSpaceCollide(dynamicsSpace, 0, &nearCallback);
dWorldStep(dynamicsWorld, frameInterval);
dJointGroupEmpty(contactGroup);
void nearCallback (void *data, dGeomID o1, dGeomID o2) {
if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) {
// colliding a space with something
dSpaceCollide2 (o1,o2,data,&nearCallback);
// collide all geoms internal to the space(s)
if (dGeomIsSpace (o1))
dSpaceCollide ((dSpaceID)o1,data,&nearCallback);
if (dGeomIsSpace (o2))
dSpaceCollide ((dSpaceID)o2,data,&nearCallback);
}
else {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
// Don't do anything if the two bodies are connected
if (b2 && b2 && dAreConnected (b1,b2)) return;
// Assuming only one contact here...
dContact contact;
contact.surface.mode = dContactBounce;
contact.surface.bounce = 0.5;
contact.surface.mu = 0.4;
// etc...
// colliding two non-space geoms, so generate contact
// points between o1 and o2
if(dCollide (o1,o2,0,&contact.geom,sizeof(dContactGeom))) {
dJointID joint;
// add these contact points to the simulation
joint = dJointCreateContact(dynamicsWorld, contactGroup, &contact);
dJointAttach(joint,b1,b2);
}
}
}
