ODEで学ぶC言語　[Step5: 構造体]

ODEで学ぶC言語のStep5です．今回は構造体を練習しましょう．構造体の概要については既にわかっているものとし，サンプルコードを示すことにより具体的な使い方を学びます．

ODEでは衝突検出機能を使用するには衝突検出用のスペース(space）と動力学計算用のワールド(world)の２つを生成する必要があります。それに伴い，物体に２つの属性，つまり，動力学計算の対象となるボディ(剛体，body)と衝突検出計算の対象となるジオメトリ(geometry)を持たせています。なお，剛体のことを英語でRigid Bodyといい，Geometryとは幾何学的な形状という意味です。

ボディとジオメトリが別々に実装され，別々のワールドやスペースに存在するために，両者を関連付ける必要があります．以下，物体の作り方を説明します．

(1) ボディの作り方

ボディは以下の順番に従って作ります．

1. ボディの生成 dBodyID　dBodyCreate(dWorldID world);
   ボディをワールドworld内に生成し，ボディのIDを返します．
2. 質量パラメータの初期化 void　dMassSetZero(dMass *mass);
   質量，重心位置，慣性モーメントなどが入っている質量パラメータmassを初期化します．
3. 質量パラメータの計算 void　dMassSet***Total(dMass *mass, dReal total_mass, … );
   ***にはボディの種類(Shpere, Box, Cylinder, Capsuleなど）が入ります．massは質量パラメータ，total_massはボディの全質量．なお，引き数はボディの種類によって違うので省略しています．詳細はAPI集をご覧ください．
4. ボディに質量パラメータを設定 void　dBodySetMass(dBodyID body, const dMass *mass);
   ボディbodyに質量パラメータmassを設定します．
5. ボディの位置を設定 void dBodySetPosition(dBodyID body, dReal x, dReal y, dReal z);
   ボディbodyを絶対座標系(x,y,z)に設定します．
6. ボディの姿勢を設定　void dBodySetRotation(dBodyID, const dMatrix3 R);
   ボディbodyの姿勢を回転行列Rに設定します．

(2) ジオメトリの作り方

次にジオメトリの作り方を紹介します．ボディと比較すると簡単です．ジオメトリの種類に対応した以下のAPIを呼び出すだけです．

• dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz);

space で指定されたスペースにx,y,z 軸に沿った長さlx,ly,lz の直方体ジオメトリを生成し，そのID 番号を返す．直方体ジオメトリの参照点はその重心である．

• dGeomID dCreateCapsule(dSpaceID space, dReal r, dReal l);

space で指定されたスペースに半径r，長さl のカプセルジオメトリを生成し，そのID 番号を返す．カプセルは普通の円柱の両端に半球を被せたようなものである．この特徴は衝突検出の内部コードを拘束かつ正確にすることができる．引数のlength には半球のキャップを入れない．キャップの半径は円柱の半径radius と同じである．

• dGeomID dCreateCylinder(dSpaceID space, dReal r, dReal l);

スペースspaceに半径r，長さl の円柱ジオメトリを生成し，そのID 番号を返す．

• dGeomID dCreateSphere(dSpaceID space, dReal r);

半径rの球ジオメトリを生成し，そのID 番号を返す．

• dGeomID dCreatePlane(dSpaceID space, dReal a, dReal b, dReal c, dReal d);

平面ジオメトリを与えられた引数により生成し，そのID 番号を返す．引数space が0 でなければ，そのspace に平面ジオメトリを挿入する．引数a,b,c,d は平面の方程式　ax+by+cz = d のパラメータである．平面の法線ベクトルは(a,b,c) であり長さは１でなければならない．平面ジオメトリは設置不可能(non-placeable)，つまり位置や姿勢を定義できない特別なジオメトリで常に絶対座標系で定義しなければならない．つまり，平面オブジェクトは常に静的な環境の一部として使われることを仮定している．

（３）　ボディとジオメトリの対応付け

• void dGeomSetBody(dGeomID geom, dBodyID body);

  ジオメトリgeomをボディbodyに関連付けます．

（4）　衝突検出計算

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

次に、simLoop関数の中で、dSpaceCollide()を呼び出します。このAPIは衝突しそうな２つのジオメトリが発生したら、それらを nearCallback関数に渡します。ここではnearCallback関数の本体は示しませんが，この関数で、接触点を算出したり、接触点の性質などを設定します。

また、69行目にあるように接触点の集まりが格納される入れ物をdJointGroupCreate()で生成し、シミュレーションループで毎回そ れを31行目のようにdJointGroupEmpty()を使って空にしなければなりません。

• 衝突検出に関する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)
    接触点が格納されているジョイントグループを空にする。

(5) ソースコード

では，Step4のソースコードに衝突検出に関するコードを組み込んだソースコードを紹介します．この処理よりボールが地面をつき抜けず，地面ではずむようになります．

#include "dm5.h"

dWorldID world;  // 動力学の世界
dSpaceID space;  // 衝突検出用スペース
dGeomID  ground; // 地面
dJointGroupID contactgroup; // コンタクトグループ

typedef struct {
　　dBodyID body; // ボディのID
　　dGeomID geom; // ジオメトリのID
　　double *pos; // x, y, z　[m]
　　double *R;   // 回転行列 要素数4x3
　　double r, m; // 半径 [r], 質量 [kg]
　　float *color; // 色 r,g,b
} MyObject;

MyObject apple;

void dmDraw(MyObject obj) /*** 物体の描画 ***/
{
　dsSetColor(obj.color[0],obj.color[1],obj.color[2]);  // 色の設定(r,g,b)
  const double *pos = dBodyGetPosition(obj.body);  // 位置を取得
  const double *R   = dBodyGetRotation(obj.body);  // 姿勢を取得
  dsDrawSphere(pos,R,obj.r);                       // 球の描画
}

void simLoop(int pause)           /***  シミュレーションループ　***/
{
  dSpaceCollide(space,0,&nearCallback); // 衝突検出関数
  dWorldStep(world,0.01); // シミュレーションを１ステップ進める
  dJointGroupEmpty(contactgroup); // ジョイントグループを空にする

  dmDraw(apple); //  物体の描画
}

void dmSphereCreate(MyObject *obj,double p[3], double R[12],
	double m, double r, float *color)
{
　obj->m = m;
　obj->r = r;
　obj->pos = p;
　obj->R   = R;
　obj->color = color;

　dRSetIdentity(obj->R);
　obj->body = dBodyCreate(world);           // ボールの生成
　dMass mass;                               // 構造体massの宣言
  dMassSetZero(&mass);                      // 構造体massの初期化
  dMassSetSphereTotal(&mass,obj->m,obj->r); // 構造体massに質量を設定
  dBodySetMass(obj->body,&mass);            // ボールにmassを設定
  dBodySetPosition(obj->body,obj->pos[0],obj->pos[1],obj->pos[2]); // ボールの位置(x,y,z)を設定

  obj->geom = dCreateSphere(space,obj->r); // 球ジオメトリの生成
  dGeomSetBody(obj->geom, obj->body);      // ボディとジオメトリの関連付け
}

int main()         /*** main関数 ***/
{
  // リンゴの変数
　double p[3] = {0.0, 0.0, 2.0}; // 位置
　double R[12];   // 回転行列
　double m = 1.0; // 質量
　double r = 0.2; // 半径
  float  color[3] = {1.0, 0.0, 0.0}; // 色

  dInitODE(); // ODEの初期化
  world = dWorldCreate();               // 動力学用世界の創造
  space        = dHashSpaceCreate(0);   // 衝突用空間の創造
  contactgroup = dJointGroupCreate(0);  // ジョイントグループの生成
  ground = dCreatePlane(space,0,0,1,0); // 地面（平面ジオメトリ）の生成
  dWorldSetGravity(world,0,0,-0.2);     // 重力設定

  dRSetIdentity(R);  //回転行列を単位行列で初期化
  dmSphereCreate(&apple,p,R,m,r,color); // リンゴの生成

  dmLoop(800, 600);  // シミュレーションループ ウインドウの幅，高

  dSpaceDestroy(space); // 衝突用空間の破壊
  dWorldDestroy(world); // 動力学用世界の破壊
  dCloseODE();          // ODEの終了
  return 0;
}

(6)　ホームワーク

1. サンプルコードstep5-090714.zipをここからダウンロードして実行しよう！
2. step5.cppと同じフォルダにdm5.cppがあります．その中にnearCallback関数があり，反発係数を設定している箇所が47行目にあります．以下のように反発係数が1.0に設定されています．この値をいろいろ変更して挙動を観察してください．1.0より大きくするとどうなりますか？ 負にするとどうなるでしょうか？いずれも，現実にはありえない反発係数です．
3. contact[i].surface.bounce = 1.0;
4. サンプルプログラムは球を弾ませましたが，こんどは直方体を弾ませてください．
5. 直方体の初期姿勢を変え，少し斜めにして落下させ挙動を観察しましょう．
   • ヒント：姿勢を変えるにはODE本６７ページにあるdRFromAxisAndAngle()で回転軸のまわりを回転させたときの回転行列Rを取得し，それを初期姿勢で設定してください．