ODE/地面との衝突 - bambooflow Note

地面との衝突

次に、地面と物体の衝突をしたいと思います。

実行ファイルgroundを実行したとき

はじめの一歩では、物体生成を行いましたが、重力を設けた場合物体は地面をすり抜けて永遠に落下してしまいました。

物体が地面から下へ落ちるのを防ぐためには、次の作業が必要となります。

1. スペース(衝突空間)の作成
2. スペースに地面作成
3. スペースに衝突物体作成
4. 衝突検出の実行

順に簡単に説明したいと思います。

スペース(衝突空間)の作成

ODEで衝突を行うためには、スペース(space)の作成が必要となります。スペースは物体同士が衝突するための空間のようなもので、質量などを持つ世界(world)とは並列的な関係があります(たぶん)。

スペースの作成は以下で行っています。

   9 static dSpaceID space; // スペースID

  98   space = dHashSpaceCreate( 0 );  // スペース(衝突空間)生成 

スペースの生成方法には幾つか種類があってdHashSpaceCreateのほかに、dSimpleSpaceCreateや
dQuadTreeSpaceCreateがあります。
違いは精度とスピードであります。ここでは速度と精度の面からdHashSpaceCreateを用いています。必要があればほかの生成方法も試してみてください。

ちなみに、私はよく違いを理解できていないので適当に使ってたりしてます。

地面の作成

次に地面の生成を行います。
地面の生成にはdCreatePlane関数を使用します。

 100   dCreatePlane( space, 0, 0, 1, 0 );         // 地面生成

第1引数には、作成したdSpaceIDをセットします。また、どこを地面にするかを設定するために第2〜4引数に値を設定します。
0,0,1,0と設定した場合は,(0*x + 0*y + 1*z = 0)の関係からz=0となり、zが0の平面が生成されることになります。

衝突物体の作成

次に衝突物体の生成を行います。
衝突のための物体のIDとしてdGeomIDを使用します。
dBodyIDでは物体に質量などを持たせることができましたが、大きさは設定できませんでした。

ここでは、剛体に対して大きさをもたせることが可能となります。

  14 static dGeomID geom;   // 剛体のGeomID

 108   geom = dCreateBox( space, box_sizes[0], box_sizes[1], box_sizes[2] );
 109                                   // スペースに大きさbox_sizesの立方体を生成
 110   dGeomSetBody( geom, box );      // geomとboxの関係づけ

108行目のdCreateBoxで一辺の大きさがbox_sizesの立方体を生成しています。
そして、110行目でdBodyIDのboxとdGeomIDのgeomを関係づけています。

どういうことかというと、geomとboxの位置や方位を一致させます。
たとえば、物体の位置を取得する方法はこの場合2つあって、

void dBodyGetPosition( body, x, y, z );

と

void dGeomGetPosition( geom, x, y, z );

です。

もともと世界(world)とスペース(space)という別の空間にあるbodyとgeomなので、関係を持たせない場合は位置はそれぞればらばらです。しかし、dGeomSetBodyで関係づけることでそれぞれのx, y, zの値は一致するはずです。

まぁそんな感じです。

衝突検出

衝突の検出を行わせるには、結構面倒でいろいろと下ごしらえが必要となります。
衝突の検出でもっとも重要となるのがnearCallBackという関数です。
関数名はユーザが勝手につけられるのですが、まぁnearCallbackという関数名を変更する必要はないでしょう。

はじめに、衝突の概念を説明したいと思います。
衝突を簡単に説明すると、物体同士を衝突させるという状態は、ODEでは接触ジョイントによって接続するという考え方になります。
ちょっとイメージしにくいところもありますが、衝突(接触)はジョイントの一種であるということを理解することが重要です。

衝突発生までの流れを説明すると、

1. 2つの物体の接近を検出
2. その2つの物体が衝突対象であるか判断
3. その2つの物体の衝突検出
4. 衝突が検出された場合に衝突を生成・発生(接触ジョイント生成・接続)
5. 衝突の初期化(ジョイントの破棄)

以下に、衝突検出に関係する部分を抜き出しています。

  10 static dJointGroupID contactgroup;
  11                        // 衝突用のジョイントグループ

  99   contactgroup = dJointGroupCreate( 0 );     // 衝突用のジョイントグループ作成

はじめに接触ジョイント用のジョイントグループを生成します。
これは接触ジョイントの生成と破棄のための入れ物として使用されます。

   7 #define MAX_CONTACTS 4 // 最大の衝突検出可能数

  22 // 衝突検出用関数
  23 static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
  24 {
  25   dBodyID b1 = dGeomGetBody( o1 ); // 物体1
  26   dBodyID b2 = dGeomGetBody( o2 ); // 物体2
  27
  28   if ( b1 && b2 && dAreConnectedExcluding( b1, b2, dJointTypeContact ) )
  29     return; // 衝突対象でない物体の衝突ははずす
  30
  31   dContact contact[MAX_CONTACTS];
  32   for ( int i=0; i<MAX_CONTACTS; i++ )
  33   {
  34     // 物体同士の接触時のパラメータ設定
  35     contact[i].surface.mode = dContactBounce | dContactSoftCFM;
  36     contact[i].surface.mu = dInfinity;   // 摩擦係数
  37     contact[i].surface.bounce = 0.1;     // 反発係数
  38     contact[i].surface.soft_cfm = 0.01;  // CFM設定
  39   }
  40
  41   // 衝突検出
  42   int numc = dCollide( o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof( dContact ) );
  43   if ( numc > 0 )
  44   {
  45     for ( int i=0; i<numc; i++ )
  46     {
  47       // 衝突の発生
  48       dJointID c = dJointCreateContact( world, contactgroup, contact+i );
  49                                  // 接触ジョイント生成
  50       dJointAttach( c, b1, b2 ); // 接触ジョイント接続
  51     }
  52   }
  53 }

衝突検出用の関数nearCallbackを作成します。この関数は物体同士が接近したときに呼び出されます。衝突検出はかなり時間がかかる処理になるので物体同士が近付いたときのみ衝突検出を実行するようにしてCPUの負担を軽減させています。
nearCallback関数の第2、第3引数には、接近した2つの物体が渡されてきます。この関数内ではこの2つの物体に対して衝突検出を行わせます。

28行目では、衝突対象でない物体の場合は衝突検出を行わないようにしています。衝突対象でない物体とは、たとえば同じdBodyIDを持つ同一の物体同士の衝突です。

31〜39行目では、dContact contactに対して接触条件を設定しています。物体の接触時において摩擦係数や反発係数などを個別に設定することができます。
38行目でcontact[i].surface.soft_cfm = 0.01;と設定している部分があります。これは物体のやわからさを表現するものに相当するのですが、ERPやCFMの設定がないとODEはエラーを引き起こすことがあるので注意が必要です。

42行目のdCollide関数で、実際に衝突を検出しています。第1、第2引数に衝突検出させる物体のdGeomIDを設定します。第3引数に最大衝突検出数を設定します。返り値として2つの物体の衝突数が返ってきます。
検出された衝突のデータは第3引数のcontactに渡されます。

dCollide関数で検出した衝突数に対して、それぞれ接触ジョイントを生成・接続を行います。

  67   dSpaceCollide( space, 0, &nearCallback ); // 衝突検出

  75   dJointGroupEmpty( contactgroup ); // 接触の初期化

最後に、simLoop内の説明をします。
67行目のdSpaceCollide関数で物体同士が接近したときにnearCallbackを呼び出しています。
シミュレーションステップ後、75行目のdJointGroupEmptyで、生成した接触ジョイントの破棄を行い、次のステップに備えています。この作業を行わないと衝突はうまくできません。
また、simLoop内の関数の実行順番があります。間違った順番で実行するとうまくできません。

まとめ

衝突検出は、プログラムがかなり複雑になります。プログラムの流れを追うのはかなり大変かもしれません。
このプログラムでは、地面との衝突について説明しましたが少しプログラムに修正を入れるだけで他の物体との衝突をさせることが簡単にできます。
これからたくさん物体を生成する場合にもこのプログラムをテンプレートとして使用できると思います。

全ソースコード

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
 
#define MAX_CONTACTS 4 // 最大の衝突検出可能数
static dWorldID world; // 世界ID
static dSpaceID space; // スペースID
static dJointGroupID contactgroup;
                       // 衝突用のジョイントグループ
 
static dBodyID box;    // 剛体のBodyID
static dGeomID geom;   // 剛体のGeomID
dReal  box_sizes[3] = { 1.0, 1.0, 1.0 };
                       // 剛体のサイズ
 
#ifdef dDOUBLE
  #define dsDrawBox dsDrawBoxD
#endif
 
// 衝突検出用関数
static void nearCallback( void *data, dGeomID o1, dGeomID o2 )
{
  dBodyID b1 = dGeomGetBody( o1 ); // 物体1
  dBodyID b2 = dGeomGetBody( o2 ); // 物体2
 
  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 | dContactSoftCFM;
    contact[i].surface.mu = dInfinity;   // 摩擦係数
    contact[i].surface.bounce = 0.1;     // 反発係数
    contact[i].surface.soft_cfm = 0.01;  // CFM設定
  }
 
  // 衝突検出
  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] = { 0.f, 5.f, 3.f };
  static float hpr[3] = { -90.f, -15.f, 0.f };
 
  dsSetViewpoint( xyz, hpr ); // カメラ位置と方向設定
}
 
// simulation loop
static void simLoop( int pause )
{
  dSpaceCollide( space, 0, &nearCallback ); // 衝突検出
 
  // Ctl+p が押されたらifに入らない
  if (!pause)
  {
    dWorldStep( world, 0.01 ); // 世界を進める
  }
 
  dJointGroupEmpty( contactgroup ); // 接触の初期化
 
  // 剛体の表示
  dsSetColor( 1, 1, 0 ); // RGB Color
  dsDrawBox( dBodyGetPosition( box ),
             dBodyGetRotation( box ),
             box_sizes );
}
 
 
int main( int argc, char* argv[] )
{
  // 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";
 
  dInitODE();
 
  // create world
  world = dWorldCreate();         // 世界生成
  space = dHashSpaceCreate( 0 );  // スペース(衝突空間)生成
  contactgroup = dJointGroupCreate( 0 );     // 衝突用のジョイントグループ作成
  dCreatePlane( space, 0, 0, 1, 0 );         // 地面生成
  dWorldSetGravity( world, 0.0, 0.0, -9.8 ); // 世界に重力を生成(z方向に-9.8m/s/s)
 
  // create box
  box = dBodyCreate( world );     // 世界に剛体を作成
  dReal pos[3] = { 0.0, 0.0, 3.0 };
  dBodySetPosition( box, pos[0], pos[1], pos[2] );
                                  // 剛体の位置を設定
  geom = dCreateBox( space, box_sizes[0], box_sizes[1], box_sizes[2] );
                                  // スペースに大きさbox_sizesの立方体を生成
  dGeomSetBody( geom, box );      // geomとboxの関係づけ
 
  // starting simulation
  dsSimulationLoop( argc, argv, 320, 240, &fn );
                                  // シミュレーション開始(simLoopへ)
                                  // + ウィンドウサイズ設定
 
  dJointGroupDestroy( contactgroup ); // ジョイントグループの破棄
  dSpaceDestroy( space );         // スペースの破棄
  dWorldDestroy( world );         // 世界の破壊
  return 0;
}