Test for cylinder vs sphere, by Bram Stolk
#include <ode/odeconfig.h> #include <assert.h> #ifdef HAVE_UNISTD_H #include <unistd.h> #endif #include <ode/ode.h> #include <drawstuff/drawstuff.h> #include "texturepath.h" #ifdef _MSC_VER #pragma warning( disable:4244 4305 ) // 型変換に伴う精度喪失の警告を抑制する (VC++) #endif
// 動力学と衝突検出のオブジェクト( chassis, 3 wheels, environment ) static dWorldID world; static dSpaceID space; static dBodyID cylbody; static dGeomID cylgeom; static dBodyID sphbody; static dGeomID sphgeom; static dJointGroupID contactgroup; static bool show_contacts = true; #define CYLRADIUS 0.6 #define CYLLENGTH 2.0 #define SPHERERADIUS 0.5
// 適切な描画関数を選択する #ifdef dDOUBLE #define dsDrawBox dsDrawBoxD #define dsDrawLine dsDrawLineD #endif
// スペースの2つのオブジェクトが衝突する可能性があるときに、 // dSpaceCollideによって呼び出される static void nearCallback( void *data, dGeomID o1, dGeomID o2 ) { assert( o1 ); assert( o2 ); if( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) { fprintf( stderr, "testing space %p %p\n", o1, o2 ); // 他のスペースの衝突検出をする dSpaceCollide2( o1, o2, data, &nearCallback ); // Note we do not want to test intersections within a space, // only between spaces. return; } const int N = 32; dContact contact[ N ]; int n = dCollide( o1, o2, N, &( contact[ 0 ].geom ), sizeof( dContact ) ); if( n > 0 ) { for( int i = 0; i < n; i++ ) { contact[ i ].surface.mode = 0; contact[ i ].surface.mu = 50.0; // 接触ジョイントを作成する dJointID c = dJointCreateContact( world, contactgroup, &contact[ i ] ); dJointAttach( c, dGeomGetBody( contact[ i ].geom.g1 ), dGeomGetBody( contact[ i ].geom.g2 ) ); if( show_contacts ) { // 接触点を描画する dMatrix3 RI; dRSetIdentity( RI ); const dReal ss[ 3 ] = { 0.12, 0.12, 0.12 }; dsSetColorAlpha( 0, 0, 1, 0.5 ); dsDrawBox( contact[ i ].geom.pos, RI, ss ); dReal *pos = contact[ i ].geom.pos; // 接触した位置 dReal depth = contact[ i ].geom.depth; // ジオメトリ同士がめり込んだ大きさ dReal *norm = contact[ i ].geom.normal; // 接触した面の方向 dReal endp[ 3 ] = { pos[ 0 ] + depth * norm[ 0 ], pos[ 1 ] + depth * norm[ 1 ], pos[ 2 ] + depth * norm[ 2 ] }; dsSetColorAlpha( 1, 1, 1, 1 ); dsDrawLine( contact[ i ].geom.pos, endp ); } } } }
// シミュレーションを開始する static void start() { dAllocateODEDataForThread( dAllocateMaskAll ); // 視点を設定する static float xyz[ 3 ] = { -8, -9, 3 }; static float hpr[ 3 ] = { 45.0000f, -27.5000f, 0.0000f }; dsSetViewpoint( xyz, hpr ); }
// キー押下時に呼び出される static void command( int cmd ) { switch( cmd ) { case ' ': break; } }
// シミュレーション ループ static void simLoop( int pause ) { // 衝突するボディを検出する dSpaceCollide( space, 0, &nearCallback ); if( !pause ) { // シミュレーションのステップを進める dWorldQuickStep( world, 0.01 ); // 100 Hz } // ジョイント グループに含まれるすべてのジョイントを破棄する dJointGroupEmpty( contactgroup ); dsSetColorAlpha( 1, 1, 0, 0.5 ); // 円柱を描画する const dReal *CPos = dBodyGetPosition( cylbody ); const dReal *CRot = dBodyGetRotation( cylbody ); float cpos[ 3 ] = { CPos[ 0 ], CPos[ 1 ], CPos[ 2 ] }; float crot[ 12 ] = { CRot[ 0 ], CRot[ 1 ], CRot[ 2 ], CRot[ 3 ], CRot[ 4 ], CRot[ 5 ], CRot[ 6 ], CRot[ 7 ], CRot[ 8 ], CRot[ 9 ], CRot[ 10 ], CRot[ 11 ] }; dsDrawCylinder( cpos, crot, CYLLENGTH, CYLRADIUS ); // 球を描画する const dReal *SPos = dBodyGetPosition( sphbody ); const dReal *SRot = dBodyGetRotation( sphbody ); float spos[ 3 ] = { SPos[ 0 ], SPos[ 1 ], SPos[ 2 ] }; float srot[ 12 ] = { SRot[ 0 ], SRot[ 1 ], SRot[ 2 ], SRot[ 3 ], SRot[ 4 ], SRot[ 5 ], SRot[ 6 ], SRot[ 7 ], SRot[ 8 ], SRot[ 9 ], SRot[ 10 ], SRot[ 11 ] }; dsDrawSphere( spos, srot, SPHERERADIUS ); }
int main( int argc, char **argv ) { dMass m; // drawstuffの設定をする 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, -9.8 ); dWorldSetQuickStepNumIterations( world, 32 ); dCreatePlane( space, 0, 0, 1, 0.0 ); // 円柱のボディを作成する cylbody = dBodyCreate( world ); dQuaternion q; #if 0 dQFromAxisAndAngle( q, 1, 0, 0, M_PI * 0.5 ); #else // dQFromAxisAndAngle( q, 1, 0, 0, M_PI * 1.0 ); dQFromAxisAndAngle( q, 1, 0, 0, M_PI * -0.77 ); #endif dBodySetQuaternion( cylbody, q ); dMassSetCylinder( &m, 1.0, 3, CYLRADIUS, CYLLENGTH ); dBodySetMass( cylbody, &m ); // 円柱のジオメトリを作成する cylgeom = dCreateCylinder( 0, CYLRADIUS, CYLLENGTH ); dGeomSetBody( cylgeom, cylbody ); dBodySetPosition( cylbody, 0, 0, 3 ); dSpaceAdd( space, cylgeom ); // 球のボディを作成する sphbody = dBodyCreate( world ); dMassSetSphere( &m, 1, SPHERERADIUS ); dBodySetMass( sphbody, &m ); // 球のジオメトリを作成する sphgeom = dCreateSphere( 0, SPHERERADIUS ); dGeomSetBody( sphgeom, sphbody ); dBodySetPosition( sphbody, 0, 0, 5.5 ); dSpaceAdd( space, sphgeom ); // シミュレーションを開始する dsSimulationLoop( argc, argv, 352, 288, &fn ); dJointGroupEmpty( contactgroup ); dJointGroupDestroy( contactgroup ); dGeomDestroy( sphgeom ); dGeomDestroy( cylgeom ); dSpaceDestroy( space ); dWorldDestroy( world ); dCloseODE(); return 0; }