Open Dynamics Engine 入門
【4日目】デモ「いもむし」

物理シミュレータ ODE（Open Dynamics Engine）の入門編の４日目です。 ４日目ODEに付属のデモプログラムをいじって、より深く理解していきます。
これまでの履歴 ■基本形１：【1日目】球の描画と衝突判定
■基本形２：【2日目】オブジェクトのジョイント
■デモ１：【3日目】デモ「カードタワー」

デモ「いもむし」

任意のボディに力を加えることで、運動させることができます。

ちょっと改造

いもむしの数を増やします。 さらに、デフォルトで用意されているテクスチャを適用します。

プログラムソース

以下のプログラムソースは、C:/ode-0.11.1/ode\demo/demo_chain1.c を利用させていただいております。

#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
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule dsDrawCapsuleD
#endif

/* select correct drawing functions */
/* some constants */
#define NUM 10      //いもむしの長さ
#define NN 5        //いもむしの数
#define SIDE (0.2)    /* side length of a box */
#define MASS (1.0)    /* mass of a box */
#define RADIUS (0.1732f)  /* sphere radius */

/* dynamics and collision objects */
static dWorldID world;
static dSpaceID space;
static dBodyID body[NN][NUM];
static dJointID joint[NN][NUM-1];
static dJointGroupID contactgroup;
static dGeomID sphere[NN][NUM];

/* this is called by dSpaceCollide when two objects in space are
 * potentially colliding.
*/
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  /* exit without doing anything if the two bodies are connected by a joint */
  dBodyID b1,b2;
  dContact contact;
  b1 = dGeomGetBody(o1);
  b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnected (b1,b2)) return; //接続されているボディ同士は、衝突計算を行わない
  contact.surface.mode = 0;
  contact.surface.mu = 1;
  contact.surface.mu2 = 0;
  if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) {
    dJointID c = dJointCreateContact (world,contactgroup,&contact);
    dJointAttach (c,b1,b2);
  }
}

/* start simulation - set viewpoint */
static void start()
{
  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
  //dAllocateODEDataForThread(dAllocateMaskAll);
  dsSetViewpoint (xyz,hpr);
}

/* simulation loop */
static void simLoop (int pause)
{
  int i;
  if (!pause) {
    for(int j=0; j<NN; j++){
    static double angle = 0;
    angle += 0.01;
    dBodyAddForce (body[j][NUM-1],0,0,5.0*(sin(angle*(j+1))+1.0)); //ボディに力を加える
    dSpaceCollide (space,0,&nearCallback);
    dWorldStep (world,0.05);
    /* remove all contact joints */
    dJointGroupEmpty (contactgroup);
    }
  }
  for(int j=0; j<NN; j++){
    for (i=0; i<NUM; i++){
      switch (j) {//いもむし用テクスチャ
        case  0:
          dsSetTexture (DS_CHECKERED);
        break;  
        case  1:
          dsSetTexture (DS_GROUND);
        break;  
        case  2:
          dsSetTexture (DS_SKY);
        break;
        case  3:
          dsSetTexture (DS_WOOD);
        break;
        default: dsSetColor (1, 1, 0);
      }
      dsDrawSphere (dBodyGetPosition(body[j][i]), dBodyGetRotation(body[j][i]),RADIUS);
    }
  }
}

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

  /* 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_TEXTURE_PATH;

  /* create world */
  dInitODE();
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dCreatePlane (space,0,0,1,0);
  for (int j=0; j<NN; j++ ){
    contactgroup = dJointGroupCreate (0);
    for (i=0; i<NUM; i++) {
      body[j][i] = dBodyCreate (world);
      k = i*SIDE;
      dBodySetPosition (body[j][i],k-j*0.1,k,k + 0.4 + j*1.0);
      dMassSetBox (&m,1,SIDE,SIDE,SIDE);
      dMassAdjust (&m,MASS);
      dBodySetMass (body[j][i],&m);
      sphere[j][i] = dCreateSphere (space,RADIUS);
      dGeomSetBody (sphere[j][i],body[j][i]);
    }
    for (i=0; i<(NUM-1); i++) {
      joint[j][i] = dJointCreateBall (world,0);    //ボール＆ソケットジョイントIDの取得
      dJointAttach (joint[j][i],body[j][i],body[j][i+1]);//ジョイントの生成
      k = (i+0.5)*SIDE;
      dJointSetBallAnchor (joint[j][i],k-j*0.1,k,k+0.4 + j*1.0); //軸の位置をセットする
    }
  }
  /* run simulation */
  dsSimulationLoop (argc,argv,WindowWidth,WindowHeight,&fn);
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

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

2つの物体同士の衝突を計算するコールバック関数 nearCallback関数 内で、 接続している物体同士の衝突計算を行わないようにするために必要なコード。

dGeomID o1, o2; //ジオメトリID
dBodyID b1, b2; //ボディID
b1 = dGeomGetBody(o1); //ジオメトリIDと関連付けられているボディIDを取得する
b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return; //接続されているボディ同士は、衝突計算を行わない

「dAreConnected (b1,b2)」は、ボディID変数「b1」と「b2」をもつ物体が接続されている場合には「true」、 接続されていない場合は「False」となります。 そのため、もし接続されている場合には「retuen」が実行されるので、nearCallback関数後半部分にある衝突計算が実行されません。