demo_cylvssphere (ODE 0.11.1)

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

nearCallback

// スペースの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 );
      }
    }
  }
}

start

// シミュレーションを開始する

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

command

// キー押下時に呼び出される

static void command( int cmd )
{
  switch( cmd )
  {
    case ' ':
      break;
  }
}

simLoop

// シミュレーション ループ

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

main

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