demo_chain1 (ODE 0.11.1)
#include <stdio.h>
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning(disable:4244 4305)
#endif
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif
#define NUM 10
#define SIDE (0.2)
#define MASS (1.0)
#define RADIUS (0.1732f)
static dWorldID world;
static dSpaceID space;
static dBodyID body[ NUM ];
static dJointID joint[ NUM - 1 ];
static dJointGroupID contactgroup;
static dGeomID sphere[ NUM ];
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
dBodyID b1, b2;
dContact contact;
b1 = dGeomGetBody( o1 );
b2 = dGeomGetBody( o2 );
if( b1 && b2 && dAreConnected( b1, b2 ) )
{
return;
}
contact.surface.mode = 0;
contact.surface.mu = 0.1;
contact.surface.mu2 = 0;
if( dCollide( o1, o2, 1, &contact.geom, sizeof( dContactGeom ) ) )
{
dJointID c = dJointCreateContact( world, contactgroup, &contact );
dJointAttach( c, b1, b2 );
}
}
static void start()
{
static float xyz[ 3 ] = { 2.1640f, - 1.3079f, 1.7600f };
static float hpr[ 3 ] = { 125.5000f, - 17.0000f, 0.0000f };
dAllocateODEDataForThread( dAllocateMaskAll );
dsSetViewpoint( xyz, hpr );
}
static void simLoop( int pause )
{
int i;
if( !pause )
{
static double angle = 0;
angle += 0.05;
dBodyAddForce(
body[ NUM - 1 ],
0, 0, 1.5 * ( sin( angle ) + 1.0 )
);
dSpaceCollide( space, 0, &nearCallback );
dWorldStep( world, 0.05 );
dJointGroupEmpty( contactgroup );
}
dsSetColor( 1, 1, 0 );
dsSetTexture( DS_WOOD );
for( i = 0; i < NUM; i++ )
{
dsDrawSphere(
dBodyGetPosition( body[ i ] ),
dBodyGetRotation( body[ i ] ),
RADIUS
);
}
}
main
int main( int argc, char **argv )
{
int i;
dReal k;
dMass m;
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = 0;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dInitODE2( 0 );
world = dWorldCreate();
space = dHashSpaceCreate( 0 );
contactgroup = dJointGroupCreate( 1000000 );
dWorldSetGravity( world, 0, 0, - 0.5 );
dCreatePlane( space, 0, 0, 1, 0 );
for( i = 0; i < NUM; i++ )
{
body[ i ] = dBodyCreate( world );
k = i * SIDE;
dBodySetPosition( body[ i ], k, k, k + 0.4 );
dMassSetBox( &m, 1, SIDE, SIDE, SIDE );
dMassAdjust( &m, MASS );
dBodySetMass( body[ i ], &m );
sphere[ i ] = dCreateSphere( space, RADIUS );
dGeomSetBody( sphere[ i ], body[ i ] );
}
for( i = 0; i < ( NUM - 1 ); i++ )
{
joint[ i ] = dJointCreateBall( world, 0 );
dJointAttach( joint[ i ], body[ i ], body[ i + 1 ] );
k = ( i + 0.5 ) * SIDE;
dJointSetBallAnchor( joint[ i ], k, k, k + 0.4 );
}
dsSimulationLoop( argc, argv, 352, 288, &fn );
dJointGroupDestroy( contactgroup );
dSpaceDestroy( space );
dWorldDestroy( world );
dCloseODE();
return 0;
}