Open Dynamics Engine 入門
【5日目】デモ「こま」

物理シミュレータ ODE（Open Dynamics Engine）の入門編の５日目です。 ODEに付属のデモプログラムをいじって、より深く理解していきます。 ただ、今回のサンプルプログラムでは、各種のパラメータなどを設定する際に、ODEのAPIを利用せずに、それぞれの型のオブジェクトにメンバ関数を利用しています。 慣れると、VisualC++などで利用できるIntellisenseが使えるので、こちらのほうがプログラミングし易くなりそうです。

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

デモ「こま」

ODEで用意されているシリンダー型とカプセル型の剛体でこまを表現しています。
2つのこまの違いは、姿勢制御モードのある・なしです。

キーボードでの操作もできます。
「(スペース)」：リスタート
「A」：軸にトルクを与える（軸が傾いている時は逆効果）
「T」：衝突判定を行った結果、衝突計算が行われている場所を表示する
「1」：dWorldExportDIF関数を利用して、内部情報をdifファイルとして保存する

プログラムソース

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

#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
#define dsDrawConvex dsDrawConvexD
#endif

bool write_world = false;
bool show_contacts = false;
dWorld *world;     //dWorld型のポインタを宣言
dBody *top1, *top2;//dBody型のポインタを宣言
dSpace *space;     //dSpace型のポインタを宣言
dJointGroup contactgroup;

const dReal pinradius = 0.05;
const dReal pinlength = 1.5;
const dReal topradius = 1.0;
const dReal toplength = 0.25;
const dReal topmass = 1.0;

#define MAX_CONTACTS 4

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
    // for drawing the contact points
    dMatrix3 RI;          //3×3行列の変数RIを宣言
    dRSetIdentity (RI);   //行列RIを単位行列にセットする
    const dReal ss[3] = {0.1,0.1,0.1};

    int i;
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);

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

    for (i=0; i<numc; i++) {
        contact[i].surface.mode = dContactApprox1;
        contact[i].surface.mu = 2;

        dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
        dJointAttach (c,b1,b2);
        if (show_contacts)
            dsDrawBox (contact[i].geom.pos, RI, ss);
    }
}


// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {4.777f, -2.084f, 2.18f};
  static float hpr[3] = {153.0f, -14.5f, 0.0f};
  dsSetViewpoint (xyz,hpr);
  printf ("SPACE to reset\n");
  printf ("A to tilt the tops.\n");
  printf ("T to toggle showing the contact points.\n");
  printf ("1 to save the current state to 'state.dif'.\n");
}
char locase (char c) //大文字を小文字に変換
{
  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
  else return c;
  return c;
}

// called when a key pressed
static void reset();//関数のプロトタイプ宣言
static void tilt(); 

static void command (int cmd)
{
    cmd = locase (cmd);
    if (cmd == ' ')
    {
        reset();       
    }
    else if (cmd == 'a') {
        tilt();
    }
    else if (cmd == 't') {
        show_contacts = !show_contacts;
    }
    else if (cmd == '1') {
        write_world = true;
    }
}

// simulation loop
static void simLoop (int pause)
{
    dsSetColor (0,0,2);
    space->collide(0,&nearCallback);
    if (!pause)
        //world->quickStep(0.02);
        world->step(0.02);
    if (write_world) {
        FILE *f = fopen ("state.dif","wt");
        if (f) {
            dWorldExportDIF (*world,f,"X");
            fclose (f);
        }
        write_world = false;
    }

    // remove all contact joints
    dJointGroupEmpty (contactgroup);

    dsSetTexture (DS_WOOD);
    dsSetColor (1,0.5f,0);
    dsDrawCylinder(top1->getPosition(),
                    top1->getRotation(),
                    toplength, topradius);
    dsDrawCapsule(top1->getPosition(),
                    top1->getRotation(),
                    pinlength, pinradius);
    dsSetColor (0.5f,1,0);
    dsDrawCylinder(top2->getPosition(),
                    top2->getRotation(),
                    toplength, topradius);
    dsDrawCapsule(top2->getPosition(),
                    top2->getRotation(),
                    pinlength, pinradius);
}

static void reset()
{
    dMatrix3 R;
    dRSetIdentity(R);

    top1->setRotation(R);
    top2->setRotation(R);

    top1->setPosition(0.8f, -2, 2);
    top2->setPosition(0.8f, 2, 2);
    
    top1->setAngularVel(0,0,5);
    top2->setAngularVel(0,0,5);
    
    top1->setLinearVel(0,0.2f,0);
    top2->setLinearVel(0,0.2f,0);
}

static void tilt()
{
    top1->addTorque(0, 10, 0);
    top2->addTorque(0, 10, 0);
}

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 = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;


    // create world
    dInitODE();
    world = new dWorld(); //dWorld型のオブジェクトを生成（ODEの世界を生成）
    world->setGravity(0,0,-0.5f);     //重力の設定
    world->setCFM(1e-5f);             //CFMを設定
    world->setLinearDamping(0.00001f);//速度減衰率を設定 
    world->setAngularDamping(0.0001f);//角速度減衰率を設定
    
    space = new dSimpleSpace(0);//dSpace型のオブジェクトを生成（衝突空間の生成）

    dPlane *floor = new dPlane(*space, 0,0,1,0);//dPlane型のオブジェクトを生成（平面をspace衝突空間に生成）

    top1 = new dBody(*world); //dbody型のオブジェクトを生成（動力学計算用ボディをODE世界のworldに生成）
    top2 = new dBody(*world);

    dMass m; //dMass型のオブジェクト m を宣言
    m.setCylinderTotal(1, 3, topradius, toplength); //シリンダー型の質量をセット //void OdeMass::set_cylinder_total(float total_mass, int direction, float radius, float length);
    
    top1->setMass(m);//dbody型のオブジェクト top1 に
    top2->setMass(m);
    
    dGeom *g1, *g2, *pin1, *pin2;//dGeom型のポインタを宣言
    g1 = new dCylinder(*space, topradius, toplength); //dGeom型のポインタにシリンダーの新規アドレスをセット
    g1->setBody(*top1);                               //ボディとジオメトリとの関連付けを行う
    g2 = new dCylinder(*space, topradius, toplength);
    g2->setBody(*top2);
    pin1 = new dCapsule(*space, pinradius, pinlength);
    pin1->setBody(*top1);
    pin2 = new dCapsule(*space, pinradius, pinlength);
    pin2->setBody(*top2);
    
    top2->setGyroscopicMode(false); //姿勢制御モード
    
    reset();

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

    //dWorldDestroy (world) を利用しないで、手動でオブジェクトを捨てる
    delete g1;
    delete g2;
    delete pin1;
    delete pin2;
    delete floor;
    contactgroup.empty();
    delete top1;
    delete top2;
    delete space;
    delete world;
    dCloseODE();
}

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

姿勢制御モード

まずはじめに、姿勢制御モードについてですが、dBody型のオブジェクトにメンバ関数setGyroscopicModeで指定しています。

top2->setGyroscopicMode(false); //姿勢制御モード

計算の安定性を確保するために、デフォルトで姿勢制御モードは「オン」になっています。 こまのシミュレーションをするのであれば、当然「オフ」にしなければいけないでしょう。

dWorld型のメンバ関数

今回のサンプルでは、これまでのようにID型の変数を宣言して、ODEのAPIでパラメータを設定することを行っていません。 dWorld型のオブジェクトを生成し、メンバ関数を利用してパラメータを設定しています。

dWorld *world;     //dWorld型のオブジェクトをポインタとして生成
dSpace *space;     //dSpace型のオブジェクトをポインタとして生成

world = new dWorld(); //dWorld型のオブジェクトを生成（ODEの世界を生成）
world->setGravity(0,0,-0.5f);     //重力の設定
world->setCFM(1e-5f);             //CFMを設定
world->setLinearDamping(0.00001f);//速度減衰率を設定 
world->setAngularDamping(0.0001f);//角速度減衰率を設定

dBody型のメンバ関数

dBody *top1;//dBody型のオブジェクトをポインタとして生成

top1 = new dBody(*world); //dbody型のオブジェクトを生成（動力学計算用ボディをODE世界のworldに生成）
dMass m; //dMass型のオブジェクト m を宣言
m.setCylinderTotal(1, 3, topradius, toplength); //シリンダー型の質量をセット //void OdeMass::set_cylinder_total(float total_mass, int direction, float radius, float length);
top1->setMass(m);//dbody型のオブジェクト top1 に質量型のオブジェクトと関連付けを行う

dGeom型のメンバ関数

dGeom *g1;//dGeom型のオブジェクトをポインタとして生成

g1 = new dCylinder(*space, topradius, toplength); //dGeom型のポインタにシリンダーの新規アドレスをセット
g1->setBody(*top1);                               //ボディとジオメトリとの関連付けを行う

次回、このデモ「こま」を改造して遊んでみたいと思います。