static void simLoop (int pause)
{
if (!pause) {
static double angle = 0;
angle += 0.05;
body[NUM-1].addForce (0,0,1.5*(sin(angle)+1.0));
space.collide (0,&nearCallback);
world.step (0.05);
// remove all contact joints
contactgroup.empty();
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<NUM; i++)
dsDrawBox (body[i].getPosition(),body[i].getRotation(),sides);
}
int main (int argc, char **argv)
{
// 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;
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
dInitODE();
world = new dWorld();
world->setGravity(0,0,-0.5f);
world->setCFM(1e-5f);
world->setLinearDamping(0.00001f);
world->setAngularDamping(0.0001f);
space = new dSimpleSpace(0);
dPlane *floor = new dPlane(*space, 0,0,1,0);
top1 = new dBody(*world);
top2 = new dBody(*world);
dMass m;
m.setCylinderTotal(1, 3, topradius, toplength);
top1->setMass(m);
top2->setMass(m);
dGeom *g1, *g2, *pin1, *pin2;
g1 = new dCylinder(*space, topradius, toplength);
g1->setBody(*top1);
g2 = new dCylinder(*space, topradius, toplength);
g2->setBody(*top2);
pin1 = new dCapsule(*space, pinradius, pinlength);
pin1->setBody(*top1);
pin2 = new dCapsule(*space, pinradius, pinlength);
pin2->setBody(*top2);
top2->setGyroscopicMode(false);
reset();
// run simulation
dsSimulationLoop (argc,argv,512,384,&fn);
delete g1;
delete g2;
delete pin1;
delete pin2;
delete floor;
contactgroup.empty();
delete top1;
delete top2;
delete space;
delete world;
dCloseODE();
}