#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <vector>
#define MAX_CONTACTS 4
#ifdef dDOUBLE
# define dsDrawSphere dsDrawSphereD
# define dsDrawBox dsDrawBoxD
# define dsDrawCapsule dsDrawCapsuleD
# define dsDrawCylinder dsDrawCylinderD
#endif
struct GeomDataT {
dReal color[4];
GeomDataT() {
color[0] = color[1] = color[2] = color[3] = 1.0;
}
void setColor( const dReal r, const dReal g, const dReal b, const dReal a=1.0 ) {
color[0] = r; color[1] = g; color[2] = b; color[3] = a;
}
const dReal* getColor() {
return color;
}
};
static dWorld *world;
static dSpace *space;
static dJointGroup contactgroup;
static std::vector<dBody*> body_vec;
static std::vector<dGeom*> geom_vec;
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
dBodyID b1 = dGeomGetBody( o1 );
dBodyID b2 = dGeomGetBody( o2 );
if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
return;
dContact contact[MAX_CONTACTS];
for ( int i=0; i<MAX_CONTACTS; i++ )
{
//contact[i].surface.mode = dContactBounce | dContactSoftERP | dContactSoftCFM;
contact[i].surface.mode = dContactBounce | dContactApprox1;
contact[i].surface.mu = 1.0; // dInfinity;
contact[i].surface.bounce = 0.8;
contact[i].surface.soft_erp = 0.2;
contact[i].surface.soft_cfm = 0.00001;
}
int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof( dContact ) );
if ( numc > 0 )
{
for ( int i=0; i<numc; i++ )
{
dJointID c = dJointCreateContact( *world, contactgroup, contact+i );
dJointAttach( c, b1, b2 );
}
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] = { -7.0f, 0.0f, 5.0f };
static float hpr[3] = { 0.f, -15.f, 0.f };
dAllocateODEDataForThread(dAllocateMaskAll);
dsSetViewpoint( xyz, hpr );
}
static void drawGeom( const dGeom* g )
{
if (!g) return;
GeomDataT* data = (GeomDataT*)g->getData();
if (data) {
const dReal* color = data->getColor();
dsSetColorAlpha( color[0], color[1], color[2], color[3] );
} else {
dsSetColor( 1.0, 1.0, 0.0 );
}
switch (g->getClass()) {
case dSphereClass:
{
dsDrawSphere( dGeomGetPosition( *g ), dGeomGetRotation( *g ), ((dSphere*)g)->getRadius() );
}
break;
case dBoxClass:
{
dVector3 len;
((dBox*)g)->getLengths( len );
dsDrawBox( dGeomGetPosition( *g ), dGeomGetRotation( *g ), len );
}
break;
case dCapsuleClass:
{
dReal radius, length;
((dCapsule*)g)->getParams( &radius, &length );
dsDrawCapsule( dGeomGetPosition( *g ), dGeomGetRotation( *g ), length, radius );
}
break;
case dCylinderClass:
{
dReal radius, length;
((dCylinder*)g)->getParams( &radius, &length );
dsDrawCylinder( dGeomGetPosition( *g ), dGeomGetRotation( *g ), length, radius );
}
break;
//case dPlaneClass:
//case dRayClass:
}
}
// simulation loop
static void simLoop( int pause )
{
space->collide( 0, &nearCallback );
if (!pause)
{
world->step( 0.005 );
}
dJointGroupEmpty( contactgroup );
/*
printf( "x=%.5f, y=%.5f, z=%.5f\n", pos[0], pos[1], pos[2] );
*/
for (unsigned i=0; i<geom_vec.size(); i++) {
drawGeom( geom_vec[i] );
}
}
int main( int argc, char* argv[] )
{
std::vector<GeomDataT*> data_vec;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE2(0);
world = new dWorld();
world->setGravity( 0, 0, -10.0f );
world->setERP( 0.5 );
world->setCFM( 1e-8f );
//world->setLinearDamping( 0.00001f );
//world->setAngularDamping( 0.0001f );
space = new dHashSpace( 0 );
dPlane *floor = new dPlane(*space, 0, 0, 1, 0 );
dJointGroupEmpty (contactgroup);
{ // Sphere
dReal pos[3] = { 0.0, 0.0, 5.0 };
dReal radius = 0.5;
dBody *body = new dBody( *world );
dMass m;
m.setSphereTotal( 1.0, radius );
body->setMass( &m );
body->setPosition( pos[0], pos[1], pos[2] );
dGeom *geom = new dSphere( *space, radius );
geom->setBody( *body );
GeomDataT* data = new GeomDataT;
data->setColor( 1.0, 1.0, 0.0 );
geom->setData( (GeomDataT*)data );
body_vec.push_back( body );
geom_vec.push_back( geom );
data_vec.push_back( data );
}
{ // Box
dReal pos[3] = { 0.0, 2.0, 5.0 };
dReal size[3] = { 1.0, 1.0, 1.0 };
dBody *body = new dBody( *world );
dMass m;
m.setBoxTotal( 1.0, size[0], size[1], size[2] );
body->setMass( &m );
body->setPosition( pos[0], pos[1], pos[2] );
dGeom *geom = new dBox( *space, size[0], size[1], size[2] );
geom->setBody( *body );
GeomDataT* data = new GeomDataT;
data->setColor( 1.0, 1.0, 1.0 );
geom->setData( (GeomDataT*)data );
data_vec.push_back( data );
body_vec.push_back( body );
geom_vec.push_back( geom );
}
{ // Capsule
dReal pos[3] = { 2.0, 0.0, 5.0 };
dReal radius = 0.5;
dReal length = 1.0;
dBody *body = new dBody( *world );
dMass m;
m.setCapsuleTotal( 1.0, 1, radius, length );
body->setMass( &m );
body->setPosition( pos[0], pos[1], pos[2] );
dGeom *geom = new dCapsule( *space, radius, length );
geom->setBody( *body );
GeomDataT* data = new GeomDataT;
data->setColor( 1.0, 0.0, 1.0 );
geom->setData( (GeomDataT*)data );
data_vec.push_back( data );
body_vec.push_back( body );
geom_vec.push_back( geom );
}
{ // Cylinder
dReal pos[3] = { 2.0, 2.0, 5.0 };
dReal radius = 0.5;
dReal length = 1.0;
dBody *body = new dBody( *world );
dMass m;
m.setCylinder( 1.0, 1, radius, length );
body->setMass( &m );
body->setPosition( pos[0], pos[1], pos[2] );
dGeom *geom = new dCylinder( *space, radius, length );
geom->setBody( *body );
GeomDataT* data = new GeomDataT;
data->setColor( 0.0, 0.0, 1.0 );
geom->setData( (GeomDataT*)data );
data_vec.push_back( data );
body_vec.push_back( body );
geom_vec.push_back( geom );
}
//////////////////////////////////////////////////////////////////
// simulation
//////////////////////////////////////////////////////////////////
dsSimulationLoop( argc, argv, 320, 240, &fn );
//////////////////////////////////////////////////////////////////
// destroy
//////////////////////////////////////////////////////////////////
contactgroup.empty();
for (unsigned i=0; i<body_vec.size(); i++) {
delete body_vec[i];
}
for (unsigned i=0; i<geom_vec.size(); i++) {
delete geom_vec[i];
}
for (unsigned i=0; i<data_vec.size(); i++) {
delete data_vec[i];
}
delete floor;
delete space;
delete world;
return 0;
}