ODE/テンプレート - bambooflow Note

ODEテンプレート

#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;
}