6. 衝突検出

ゲーム開発やロボットの研究者にも使われているオープンソースの物理計算エンジンODE (Open Dynamics Engine、オープンダイナミクスエンジン）を学ぶODE初級講座の第６回目です。

今回は衝突検出機能について勉強します。前回勉強したようにODEでは動力学計算と衝突検出計算が別々に実装されています。動力学計算をするためには、ワールドworldをdWorldCreate()で生成し、その中に剛体 bodyを生成し、dWorldStep()で動力学計算をしましたね。

ジオメトリ

ジオメトリとは物体の形状という意味で、下図の左からsphere（球）、box（直方体）、cylinder（円柱）、capsule（カプセル）などの種類があります。

衝突検出に関するAPI
- dSpaceID dHashSpaceCreate(0) 衝突計算用スペースを生成し、そのID（識別子）を返す。
- dGeomID dCreatePlane(dSpaceID space ,dReal a, dReal b, dReal c, dReal d) spaceにax+by+cz=dの平面ジオメトリを生成する。
- dGeomID dCreateSphere(dSpaceID space, dReal r) spaceに半径rの球ジオメトリを生成する。
- void dGeomSetBody(dGeomID geom, dBodyID body) 物体の2つの属性であるジオメトリgeomと剛体bodyを関連づける。
- dJointGroupID dJointGroupCreate(0) 接触点のグループを格納するジョイントグループを生成し、そのIDを返す。
- void dJointGroupEmpty(dJointGroupID) 接触点が格納されているジョイントグループを空にする。

以下にソースコードを示します。前回のプログラムと違うところだけコメントを入れています。

さて、ソースコードをさっそく読みましょう。なお、前回のsample1 と全く同じstart関数やprepDrawStuff関数は省略しています。

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

static dWorldID world;
static dSpaceID space;
static dGeomID  ground;
static dJointGroupID contactgroup;
static int flag = 0;
dsFunctions fn;

const dReal   radius = 0.2;
const dReal   mass   = 1.0;

typedef struct {
  dBodyID body;
  dGeomID geom;
} MyObject;
MyObject ball;

static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
  const int N = 10;
  dContact contact[N];

  int isGround = ((ground == o1) || (ground == o2));

  int n =  dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));

  if (isGround)  {
    if (n >= 1) flag = 1;
    else        flag = 0;
    for (int i = 0; i < n; i++) {
      contact[i].surface.mode = dContactBounce;
      contact[i].surface.mu   = dInfinity;
      contact[i].surface.bounce     = 0.0; // (0.0~1.0) restitution parameter
      contact[i].surface.bounce_vel = 0.0; // minimum incoming velocity for bounce
      dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
      dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
    }
  }
}

static void simLoop (int pause)
{
  const dReal *pos,*R;

  flag = 0;
  dSpaceCollide(space,0,&nearCallback);

  dWorldStep(world,0.01);

  dJointGroupEmpty(contactgroup);

  if (flag == 0) dsSetColor(1.0, 0.0, 0.0);
  else           dsSetColor(0.0, 0.0, 1.0);
  pos = dBodyGetPosition(ball.body);
  R   = dBodyGetRotation(ball.body);
  dsDrawSphere(pos,R,radius);
}

int main (int argc, char *argv[])
{
  dReal x0 = 0.0, y0 = 0.0, z0 = 2.0;
  dMass m1;

  prepDrawStuff();

  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate(0);
  contactgroup = dJointGroupCreate(0);

  dWorldSetGravity(world,0,0,-0.5);

  // Create a ground
  ground = dCreatePlane(space,0,0,1,0);

  // Create a ball
  ball.body = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetSphereTotal(&m1,mass,radius);
  dBodySetMass(ball.body,&m1);
  dBodySetPosition(ball.body, x0, y0, z0);

  ball.geom = dCreateSphere(space,radius);
  dGeomSetBody(ball.geom,ball.body);

  dsSimulationLoop (argc,argv,352,288,&fn);

  dWorldDestroy (world);
  dCloseODE();

  return 0;
}

オブジェクトも動力学計算用のbody(ボディ）の他に衝突検出計算用にgeom（ジオメトリ）を設定する必要があるのでMyObject 構造体でそれらをメンバとして定義していますね。

main関数の中で玉オブジェクトのgeomをdCreateSphere()で作り、dGeomSetBodyでbodyとgeomを関連付けていますのでオブジェクトの位置と姿勢はbodyだけで設定すればOKです。これをしないと幽体離脱現象に陥ってしまいます。

衝突検出関数dSpaceCollideはシミュレーションの各ステップで実行されるsimLoop関数の中で呼び出されています。注意する点としては、必ずsimLoopの一番始めで呼び出してください。これを後のほうにもっていくと玉が地面を突き抜けてしまいますよ。dSpaceCollide ではコールバック関数nearCallbackを呼び出しています。31行目で接触する可能性のある２つの物体のうち、どちらかが地面groundなら isGroundをtrueにセットします。32行目のdCollide()の戻り値は接触点数です。

29行目のif (isGround)文は，衝突する可能性のある物体のうちどちらかが地面なら30行目から43行目までの処理をします．つまり，地面との衝突以外は考えていません．地面以外との衝突を考慮する場合は，このif文を削除してください．

３0行目で地面でかつ接触点数が１以上なら、地面と接触したことを示すflagを１にセットしています。

一方、衝突検出計算をするためには、スペースspaceをdHashSpaceCreate()で生成し、その中に剛体bodyに対応するジオメトリ geometryを生成しなければなりません。以下のサンプルプログラムでは、85行目で球に対応するジオメトリをdCreateSphere()で生成しています。86行目で、動力学計算の対象となるボディball.bodyと衝突検出計算の対象となるball.geomを結びつけています。これで ODEの物体が完成です。

次に、simLoop関数の中で、dSpaceCollide()を呼び出します。このAPIは衝突しそうな２つのジオメトリが発生したら、それらを nearCallback関数に渡します。nearCallback関数の引数o1, o2が衝突する可能性のある２つのジオメトリです。nearCallback関数では、接触点を算出したり、接触点の性質などを設定します。　なお、注意しなければいけないことは、引数o1、o2は接触する可能性があるだけで、実際に接触しているかどうかよくわからないことです。それを知るためにはdCollide()の戻り値、つまり、接触点数が１以上かどうかチェックすれば良いのです。

また、71行目にあるように接触点の集まりが格納される入れ物をdJointGroupCreate()で生成し、シミュレーションループで毎回それをdJointGroupEmpty()を使って空にしなければなりません。サンプルプログラムでは52行目でやっています。これを忘れると１ステップ前の接触点達が悪さをしますのでお忘れなく！

では、次のサンプルプログラム６をダウンロードして実行しよう。

サンプルプログラム６

練　習

上の例ではボール１個を落下させましたが，多くのボールを落下させるプログラムを書いてみましょう．回答例は次からダウンロードできます。

サンプルプログラム６マルチ

では、また次回！