demo_boxstack (ODE 0.11.1)
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"
#ifdef _MSC_VER
#pragma warning( disable:4244 4305 )
#endif
#include "icosahedron_geom.h"
dReal planes[] =
{
1.0f, 0.0f, 0.0f, 0.25f,
0.0f, 1.0f, 0.0f, 0.25f,
0.0f, 0.0f, 1.0f, 0.25f,
-1.0f, 0.0f, 0.0f, 0.25f,
0.0f, -1.0f, 0.0f, 0.25f,
0.0f, 0.0f, -1.0f, 0.25f,
};
const unsigned int planecount = 6;
dReal points[] =
{
0.25f, 0.25f, 0.25f,
-0.25f, 0.25f, 0.25f,
0.25f, -0.25f, 0.25f,
-0.25f, -0.25f, 0.25f,
0.25f, 0.25f, -0.25f,
-0.25f, 0.25f, -0.25f,
0.25f, -0.25f, -0.25f,
-0.25f, -0.25f, -0.25f,
};
const unsigned int pointcount = 8;
unsigned int polygons[] =
{
4, 0, 2, 6, 4,
4, 1, 0, 4, 5,
4, 0, 1, 3, 2,
4, 3, 1, 5, 7,
4, 2, 3, 7, 6,
4, 5, 4, 6, 7,
};
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#define dsDrawConvex dsDrawConvexD
#endif
#define NUM 100
#define DENSITY ( 5.0 )
#define GPB 3
#define MAX_CONTACTS 8
#define MAX_FEEDBACKNUM 20
#define GRAVITY REAL( 0.5 )
#define USE_GEOM_OFFSET 1
struct MyObject
{
dBodyID body;
dGeomID geom[ GPB ];
};
static int num = 0;
static int nextobj = 0;
static dWorldID world;
static dSpaceID space;
static MyObject obj[ NUM ];
static dJointGroupID contactgroup;
static int selected = -1;
static int show_aabb = 0;
static int show_contacts = 0;
static int random_pos = 1;
static int write_world = 0;
static int show_body = 0;
struct MyFeedback
{
dJointFeedback fb;
bool first;
};
static MyFeedback feedbacks[ MAX_FEEDBACKNUM ];
static int doFeedback = 0;
static int fbnum = 0;
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
int i;
dBodyID b1 = dGeomGetBody( o1 );
dBodyID b2 = dGeomGetBody( o2 );
if( b1 && b2
&& dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
{
return;
}
dContact contact[ MAX_CONTACTS ];
for( i = 0; i < MAX_CONTACTS; i++ )
{
contact[ i ].surface.mode = dContactBounce | dContactSoftCFM;
contact[ i ].surface.mu = dInfinity;
contact[ i ].surface.mu2 = 0;
contact[ i ].surface.bounce = 0.1;
contact[ i ].surface.bounce_vel = 0.1;
contact[ i ].surface.soft_cfm = 0.01;
}
int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[ 0 ].geom, sizeof( dContact ) );
if( numc )
{
dMatrix3 RI;
dRSetIdentity( RI );
const dReal ss[ 3 ] = { 0.02, 0.02, 0.02 };
for( i = 0; i < numc; i++ )
{
dJointID c = dJointCreateContact( world, contactgroup, contact + i );
dJointAttach( c, b1, b2 );
if( show_contacts )
{
dsDrawBox( contact[ i ].geom.pos, RI, ss );
}
if( doFeedback
&& ( b1 == obj[ selected ].body
|| b2 == obj[ selected ].body ) )
{
if( fbnum < MAX_FEEDBACKNUM )
{
feedbacks[ fbnum ].first = ( b1 == obj[ selected ].body );
dJointSetFeedback( c, &feedbacks[ fbnum++ ].fb );
}
else
{
fbnum++;
}
}
}
}
}
static void start()
{
dAllocateODEDataForThread( dAllocateMaskAll );
static float xyz[ 3 ] = { 2.1640f, -1.3079f, 1.7600f };
static float hpr[ 3 ] = { 125.5000f, -17.0000f, 0.0000f };
dsSetViewpoint( xyz, hpr );
printf( "To drop another object, press:\n" );
printf( " b for box.\n" );
printf( " s for sphere.\n" );
printf( " c for capsule.\n" );
printf( " y for cylinder.\n" );
printf( " v for a convex object.\n" );
printf( " x for a composite object.\n" );
printf( "To select an object, press space.\n" );
printf( "To disable the selected object, press d.\n" );
printf( "To enable the selected object, press e.\n" );
printf( "To dump transformation data for the selected object, press p.\n" );
printf( "To toggle showing the geom AABBs, press a.\n" );
printf( "To toggle showing the contact points, press t.\n" );
printf( "To toggle dropping from random position/orientation, press r.\n" );
printf( "To save the current state to 'state.dif', press 1.\n" );
printf( "To show joint feedbacks of selected object, press f.\n" );
}
char locase( char c )
{
if( c >= 'A' && c <= 'Z' )
{
return c - ( 'a' - 'A' );
}
else
{
return c;
}
}
static void command( int cmd )
{
size_t i;
int j, k;
dReal sides[ 3 ];
dMass m;
int setBody;
cmd = locase( cmd );
if( cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v' )
{
setBody = 0;
if( num < NUM )
{
i = num;
num++;
}
else
{
i = nextobj;
nextobj++;
if( nextobj >= num )
{
nextobj = 0;
}
dBodyDestroy( obj[ i ].body );
for( k = 0; k < GPB; k++ )
{
if( obj[ i ].geom[ k ] )
{
dGeomDestroy( obj[ i ].geom[ k ] );
}
}
memset( &obj[ i ], 0, sizeof( obj[ i ] ) );
}
obj[ i ].body = dBodyCreate( world );
for( k = 0; k < 3; k++ )
{
sides[ k ] = dRandReal() * 0.5 + 0.1;
}
dMatrix3 R;
if( random_pos )
{
dBodySetPosition(
obj[ i ].body,
dRandReal() * 2 - 1,
dRandReal() * 2 - 1,
dRandReal() + 2
);
dRFromAxisAndAngle(
R,
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 10.0 - 5.0
);
}
else
{
dReal maxheight = 0;
for( k = 0; k < num; k++ )
{
const dReal *pos = dBodyGetPosition( obj[ k ].body );
if( pos[ 2 ] > maxheight )
{
maxheight = pos[ 2 ];
}
}
dBodySetPosition( obj[ i ].body, 0, 0, maxheight + 1 );
dRSetIdentity( R );
}
dBodySetRotation( obj[ i ].body, R );
dBodySetData( obj[ i ].body, ( void* )i );
if( cmd == 'b' )
{
dMassSetBox( &m, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
obj[ i ].geom[ 0 ] = dCreateBox( space, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
}
else if( cmd == 'c' )
{
sides[ 0 ] *= 0.5;
dMassSetCapsule( &m, DENSITY, 3, sides[ 0 ], sides[ 1 ] );
obj[ i ].geom[ 0 ] = dCreateCapsule( space, sides[ 0 ], sides[ 1 ] );
}
else if( cmd == 'v' )
{
dMassSetBox( &m, DENSITY, 0.25, 0.25, 0.25 );
#if 0
obj[ i ].geom[ 0 ] = dCreateConvex(
space,
planes,
planecount,
points,
pointcount,
polygons
);
#else
obj[ i ].geom[ 0 ] = dCreateConvex(
space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons
);
#endif
}
else if( cmd == 'y' )
{
dMassSetCylinder( &m, DENSITY, 3, sides[ 0 ], sides[ 1 ] );
obj[ i ].geom[ 0 ] = dCreateCylinder( space, sides[ 0 ], sides[ 1 ] );
}
else if( cmd == 's' )
{
sides[ 0 ] *= 0.5;
dMassSetSphere( &m, DENSITY, sides[ 0 ] );
obj[ i ].geom[ 0 ] = dCreateSphere( space, sides[ 0 ] );
}
else if( cmd == 'x' && USE_GEOM_OFFSET )
{
setBody = 1;
dMass m2;
dMassSetZero( &m );
dReal dpos[ GPB ][ 3 ];
dMatrix3 drot[ GPB ];
for( j = 0; j < GPB; j++ )
{
for( k = 0; k < 3; k++ )
{
dpos[ j ][ k ] = dRandReal() * 0.3 - 0.15;
}
}
for( k = 0; k < GPB; k++ )
{
if( k == 0 )
{
dReal radius = dRandReal() * 0.25 + 0.05;
obj[ i ].geom[ k ] = dCreateSphere( space, radius );
dMassSetSphere( &m2, DENSITY, radius );
}
else if( k == 1 )
{
obj[ i ].geom[ k ] = dCreateBox( space, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
dMassSetBox( &m2, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
}
else
{
dReal radius = dRandReal() * 0.1 + 0.05;
dReal length = dRandReal() * 1.0 + 0.1;
obj[ i ].geom[ k ] = dCreateCapsule( space, radius, length );
dMassSetCapsule( &m2, DENSITY, 3, radius, length );
}
dRFromAxisAndAngle(
drot[ k ],
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 10.0 - 5.0
);
dMassRotate( &m2, drot[ k ] );
dMassTranslate( &m2, dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );
dMassAdd( &m, &m2 );
}
for( k = 0; k < GPB; k++ )
{
dGeomSetBody( obj[ i ].geom[ k ], obj[ i ].body );
dGeomSetOffsetPosition(
obj[ i ].geom[ k ],
dpos[ k ][ 0 ] - m.c[ 0 ],
dpos[ k ][ 1 ] - m.c[ 1 ],
dpos[ k ][ 2 ] - m.c[ 2 ]
);
dGeomSetOffsetRotation( obj[ i ].geom[ k ], drot[ k ] );
}
dMassTranslate( &m, -m.c[ 0 ], -m.c[ 1 ], -m.c[ 2 ] );
dBodySetMass( obj[ i ].body, &m );
}
else if( cmd == 'x' )
{
dGeomID g2[ GPB ];
dReal dpos[ GPB ][ 3 ];
dMass m2;
dMassSetZero( &m );
for( j = 0; j < GPB; j++ )
{
for( k = 0; k < 3; k++ )
{
dpos[ j ][ k ] = dRandReal() * 0.3 - 0.15;
}
}
for( k = 0; k < GPB; k++ )
{
obj[ i ].geom[ k ] = dCreateGeomTransform( space );
dGeomTransformSetCleanup( obj[ i ].geom[ k ], 1 );
if( k == 0 )
{
dReal radius = dRandReal() * 0.25 + 0.05;
g2[ k ] = dCreateSphere( 0, radius );
dMassSetSphere( &m2, DENSITY, radius );
}
else if( k == 1 )
{
g2[ k ] = dCreateBox( 0, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
dMassSetBox( &m2, DENSITY, sides[ 0 ], sides[ 1 ], sides[ 2 ] );
}
else
{
dReal radius = dRandReal() * 0.1 + 0.05;
dReal length = dRandReal() * 1.0 + 0.1;
g2[ k ] = dCreateCapsule( 0, radius, length );
dMassSetCapsule( &m2, DENSITY, 3, radius, length );
}
dGeomTransformSetGeom( obj[ i ].geom[ k ], g2[ k ] );
dMatrix3 Rtx;
dRFromAxisAndAngle(
Rtx,
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 2.0 - 1.0,
dRandReal() * 10.0 - 5.0
);
dGeomSetPosition( g2[ k ], dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );
dGeomSetRotation( g2[ k ], Rtx );
dMassRotate( &m2, Rtx );
dMassTranslate( &m2, dpos[ k ][ 0 ], dpos[ k ][ 1 ], dpos[ k ][ 2 ] );
dMassAdd( &m, &m2 );
}
for( k = 0; k < GPB; k++ )
{
dGeomSetPosition(
g2[ k ],
dpos[ k ][ 0 ] - m.c[ 0 ],
dpos[ k ][ 1 ] - m.c[ 1 ],
dpos[ k ][ 2 ] - m.c[ 2 ]
);
}
dMassTranslate( &m, -m.c[ 0 ], -m.c[ 1 ], -m.c[ 2 ] );
}
if( !setBody )
{
for( k = 0; k < GPB; k++ )
{
if( obj[ i ].geom[ k ] )
{
dGeomSetBody( obj[ i ].geom[ k ], obj[ i ].body );
}
}
}
dBodySetMass( obj[ i ].body, &m );
}
if( cmd == ' ' )
{
selected++;
if( selected >= num )
{
selected = 0;
}
if( selected < 0 )
{
selected = 0;
}
}
else if( cmd == 'd' && selected >= 0 && selected < num )
{
dBodyDisable( obj[ selected ].body );
}
else if( cmd == 'e' && selected >= 0 && selected < num )
{
dBodyEnable( obj[ selected ].body );
}
else if( cmd == 'a' )
{
show_aabb ^= 1;
}
else if( cmd == 't' )
{
show_contacts ^= 1;
}
else if( cmd == 'r' )
{
random_pos ^= 1;
}
else if( cmd == '1' )
{
write_world = 1;
}
else if( cmd == 'p' && selected >= 0 )
{
const dReal *pos = dGeomGetPosition( obj[ selected ].geom[ 0 ] );
const dReal *rot = dGeomGetRotation( obj[ selected ].geom[ 0 ] );
printf( "POSITION:\n\t[ %f, %f, %f ]\n\n", pos[ 0 ], pos[ 1 ], pos[ 2 ] );
printf( "ROTATION:\n\t[ %f, %f, %f, %f ]\n\t[ %f, %f, %f, %f ]\n\t[ %f, %f, %f, %f ]\n\n",
rot[ 0 ], rot[ 1 ], rot[ 2 ], rot[ 3 ],
rot[ 4 ], rot[ 5 ], rot[ 6 ], rot[ 7 ],
rot[ 8 ], rot[ 9 ], rot[ 10 ], rot[ 11 ]
);
}
else if( cmd == 'f' && selected >= 0 && selected < num )
{
if( dBodyIsEnabled( obj[ selected ].body ) )
{
doFeedback = 1;
}
}
}
void drawGeom( dGeomID g, const dReal *pos, const dReal *R, int show_aabb )
{
int i;
if( !g )
{
return;
}
if( !pos )
{
pos = dGeomGetPosition( g );
}
if( !R )
{
R = dGeomGetRotation( g );
}
int type = dGeomGetClass( g );
if( type == dBoxClass )
{
dVector3 sides;
dGeomBoxGetLengths( g, sides );
dsDrawBox( pos, R, sides );
}
else if( type == dSphereClass )
{
dsDrawSphere( pos, R, dGeomSphereGetRadius( g ) );
}
else if( type == dCapsuleClass )
{
dReal radius, length;
dGeomCapsuleGetParams( g, &radius, &length );
dsDrawCapsule( pos, R, length, radius );
}
else if( type == dConvexClass )
{
#if 0
dsDrawConvex(
pos,
R,
planes,
planecount,
points,
pointcount,
polygons
);
#else
dsDrawConvex(
pos,
R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons
);
#endif
}
else if( type == dCylinderClass )
{
dReal radius, length;
dGeomCylinderGetParams( g, &radius, &length );
dsDrawCylinder( pos, R, length, radius );
}
else if( type == dGeomTransformClass )
{
dGeomID g2 = dGeomTransformGetGeom( g );
const dReal *pos2 = dGeomGetPosition( g2 );
const dReal *R2 = dGeomGetRotation( g2 );
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331( actual_pos, R, pos2 );
actual_pos[ 0 ] += pos[ 0 ];
actual_pos[ 1 ] += pos[ 1 ];
actual_pos[ 2 ] += pos[ 2 ];
dMULTIPLY0_333( actual_R, R, R2 );
drawGeom( g2, actual_pos, actual_R, 0 );
}
if( show_body )
{
dBodyID body = dGeomGetBody( g );
if( body )
{
const dReal *bodypos = dBodyGetPosition( body );
const dReal *bodyr = dBodyGetRotation( body );
dReal bodySides[ 3 ] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha( 0, 1, 0, 1 );
dsDrawBox( bodypos, bodyr, bodySides );
}
}
if( show_aabb )
{
dReal aabb[ 6 ];
dGeomGetAABB( g, aabb );
dVector3 bbpos;
for( i = 0; i < 3; i++ )
{
bbpos[ i ] = 0.5 * ( aabb[ i * 2 ] + aabb[ i * 2 + 1 ] );
}
dVector3 bbsides;
for( i = 0; i < 3; i++ )
{
bbsides[ i ] = aabb[ i * 2 + 1 ] - aabb[ i * 2 ];
}
dMatrix3 RI;
dRSetIdentity( RI );
dsSetColorAlpha( 1, 0, 0, 0.5 );
dsDrawBox( bbpos, RI, bbsides );
}
}
static void simLoop( int pause )
{
dsSetColor( 0, 0, 2 );
dSpaceCollide( space, 0, &nearCallback );
if( !pause )
{
dWorldQuickStep( world, 0.02 );
}
if( write_world )
{
FILE *f = fopen( "state.dif", "wt" );
if( f )
{
dWorldExportDIF( world, f, "X" );
fclose( f );
}
write_world = 0;
}
if( doFeedback )
{
if( fbnum>MAX_FEEDBACKNUM )
{
printf( "joint feedback buffer overflow!\n" );
}
else
{
dVector3 sum = { 0, 0, 0 };
printf( "\n" );
for( int i = 0; i < fbnum; i++ )
{
dReal *f =
feedbacks[ i ].first ?
feedbacks[ i ].fb.f1 :
feedbacks[ i ].fb.f2;
printf( "%f %f %f\n", f[ 0 ], f[ 1 ], f[ 2 ] );
sum[ 0 ] += f[ 0 ];
sum[ 1 ] += f[ 1 ];
sum[ 2 ] += f[ 2 ];
}
printf( "Sum: %f %f %f\n", sum[ 0 ], sum[ 1 ], sum[ 2 ] );
dMass m;
dBodyGetMass( obj[ selected ].body, &m );
printf( "Object G=%f\n", GRAVITY * m.mass );
}
doFeedback = 0;
fbnum = 0;
}
dJointGroupEmpty( contactgroup );
dsSetColor( 1, 1, 0 );
dsSetTexture( DS_WOOD );
for( int i = 0; i < num; i++ )
{
for( int j = 0; j < GPB; j++ )
{
if( i == selected )
{
dsSetColor( 0, 0.7, 1 );
}
else if( ! dBodyIsEnabled( obj[ i ].body ) )
{
dsSetColor( 1, 0.8, 0 );
}
else
{
dsSetColor( 1, 1, 0 );
}
drawGeom( obj[ i ].geom[ j ], 0, 0, show_aabb );
}
}
}
main
int main( int argc, char **argv )
{
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;
dInitODE2( 0 );
world = dWorldCreate();
space = dHashSpaceCreate( 0 );
contactgroup = dJointGroupCreate( 0 );
dWorldSetGravity( world, 0, 0, -GRAVITY );
dWorldSetCFM( world, 1e-5 );
dWorldSetAutoDisableFlag( world, 1 );
#if 1
dWorldSetAutoDisableAverageSamplesCount( world, 10 );
#endif
dWorldSetLinearDamping( world, 0.00001 );
dWorldSetAngularDamping( world, 0.005 );
dWorldSetMaxAngularSpeed( world, 200 );
dWorldSetContactMaxCorrectingVel( world, 0.1 );
dWorldSetContactSurfaceLayer( world, 0.001 );
dCreatePlane( space, 0, 0, 1, 0 );
memset( obj, 0, sizeof( obj ) );
dsSimulationLoop( argc, argv, 352, 288, &fn );
dJointGroupDestroy( contactgroup );
dSpaceDestroy( space );
dWorldDestroy( world );
dCloseODE();
return 0;
}