Open Dynamics Engine 入門
【６日目】円柱と球

物理シミュレータ ODE（Open Dynamics Engine）の入門編の６日目です。 ODEに付属のデモプログラムをいじって、より深く理解していきます。 今回のサンプルプログラムは単純ですが、ODEの有用な関数が多く使われているので、利用のしかたを確認することができます。

これまでの履歴
■基本形１：【1日目】球の描画と衝突判定
■基本形２：【2日目】オブジェクトのジョイント
■デモ１：【3日目】デモ「カードタワー」
■デモ２：【4日目】デモ「いもむし」
■デモ３：【5日目】デモ「こま」

デモ「円柱と球」

円柱と球を落下させるだけのシンプルなデモ。 オブジェクトの回転角度の制御のためのクォータニオンの使い方や、接触点と力の向きの表示の仕方など、細かなところで有用なODE関数の使い方が参考になります。

プログラムソース

以下のプログラムソースは、C:/ode-0.11.1/ode\demo/demo_cylvssphere.cpp をもとにしております。

#include <ode/odeconfig.h>
#include <assert.h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
int WindowWidth  = 480; //ウィンドウの幅
int WindowHeight = 320; //ウィンドウの高さ

#ifndef DRAWSTUFF_TEXTURE_PATH
#define DRAWSTUFF_TEXTURE_PATH "C:/ode-0.11.1/drawstuff/textures"
#endif

// dynamics and collision objects (chassis, 3 wheels, environment)
static dWorldID world;
static dSpaceID space;
static dBodyID cylbody; //動力学計算用円柱ボディIDの宣言
static dGeomID cylgeom; //動力学計算用円柱ボディIDの宣言
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


// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

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);
    // colliding a space with something
    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; // was: dInfinity
      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.1,0.1,0.1};
        dsSetColorAlpha (0,0,1,1.0);
        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,0,0,1);
        dsDrawLine (contact[i].geom.pos, endp);//衝突方向を線で表す
      }
    }
  }
}

// start simulation - set viewpoint
static void start(){
  static float xyz[3] = {-4,-5,3};
  static float hpr[3] = {45.0000f,-20.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}

// called when a key pressed
static void command (int cmd){
  switch (cmd) 
  {
    case ' ':
      break;
  }
}

// simulation loop
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
  ); // single precision

  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
  ); // single precision
}

int main (int argc, char **argv){
  dMass m;

  // setup pointers to drawstuff callback functions
  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;

  // create world
  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;                                   //回転用のクォータニオンの宣言
  dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);      //クォータニオンの生成
  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);

  // run simulation
  dsSimulationLoop (argc,argv,WindowWidth,WindowHeight,&fn);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);

  dGeomDestroy(sphgeom);
  dGeomDestroy (cylgeom);

  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

サンプルプログラムで理解したこと

任意の回転軸での回転：クォータニオンの利用

クォータニオンとは、実数軸と3つの虚数軸を基底とする４次元ベクトル空間内での数で、四元数とも呼ばれます。 コンピュータ・グラフィックスの世界で、物体の姿勢を制御する際に非常に有用であるとして、よく利用されます。 クォータニオンを利用することで任意の回転軸と回転角度を与えることで、オブジェクトの回転を表現することが簡単にできます。

  cylbody = dBodyCreate (world);                   //円柱ボディの生成
  dQuaternion q;                                   //回転用のクォータニオンの宣言
  dQFromAxisAndAngle (q,1,0,0, M_PI * -0.77);      //クォータニオンの生成
  dBodySetQuaternion (cylbody,q);                  //円柱ボディをクォータニオンを使って回転

上記の例では(1,0,0)軸で -0.77π 回転させるクォータニオンを生成しています。

nearCallback関数内での、衝突点と衝突方向ベクトルの取得

nearCallback関数は、衝突の可能性がある場合にコールバックされる関数で、 実際に衝突している場合には、その位置や衝突の向きなどを計算してくれます。

  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; // was: dInfinity
      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.1,0.1,0.1};
        dsSetColorAlpha (0,0,1,1.0);
        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,0,0,1);
        dsDrawLine (contact[i].geom.pos, endp);//衝突方向を線で表す
      }
    }
  }