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

Step5サンプルプログラム：ドミノ倒し

ODEで学ぶC言語2のStep5です．今回は構造体と物体へ力やトルクを加える方法，さらにシミュレーションのリセット法などを学びます．構造体の概要については既にわかっているものとし，サンプルコードを示すことにより具体的な使い方を学びます．

○　構造体

配列では同じ型しかまとめることができませんでしたが，構造体では違う型をまとめて扱うことができるのでシミュレーションなど物体に多くの属性がある場合に便利です．このサンプルプログラムでは物体をdmObjectという構造体で次のように表しています．dm5.hの１６行目に定義しています．今回の例はドミノ倒しなので，この構造体を元にドミノを21個生成しています．

typedef struct{
    dBodyID body; // ボディのID
    dGeomID geom; // ジオメトリのID
    const double *p; // x, y, z　[m]
    const double *R;   // 回転行列 要素数4x3
    double m; // 質量 [kg]
    double r,l; // 半径 [m], 長さ [m]
    const double *side; // サイズ　x,y,z
    const double *color; // 色 r,g,b
} dmObject;

○ 姿勢の変更

物体の姿勢を変更するためには次のAPIを使い回転行列Rの値を変更します．ここで，Rは回転行列が格納されている配列へのポインタ，ax, ay, axは回転軸ベクトル．angleは回転角度となります．なお，dだけで始まるAPIはODEのAPIです．

• dRFromAxisAndAngle(double　R[12], double  ax, doulbe  ay, double  az, double angle);

○　力，トルクの加え方

• void dmAddForce(dmObject *obj, double fx, double fy, double fz)
  • 物体objの重心に力(fx,fy,fz)を加える
• void dmAddTorque(dmObject *obj, double fx, double fy, double fz)
  • 物体objの重心にトルク(fx, fy, fz)を加える．fx，ｆｙ，ｆｚはそれぞれx, y, z軸まわりのトルク

○　高速なシミュレーション

前回のサンプルではシミュレーションのステップ関数としてdmSimStep()を使いましたが，ここではより高速なdmSimQuickStep()を使います．ただし，dmSimStep()と比較して精度が悪くなります．

○　シミュレーションのリセット

rまたはRキーを押すと，resetSim関数が呼ばれてシミュレーションがリセットされます．resetSimの中身はシミュレーションループが１回以上呼び出されたときにdmInit関数を呼び出して初期化し，ドミノを再度生成しています．

○　ソースコード

/* step5 ドミノ倒し 　*/
#include "dm5.h"
#define DOMINO_NUM  21

static int STEPS = 0;   // シミュレーションのステップ数
double red[3] = {1.3, 0.0, 0.0}; // 赤色
dmObject domino[DOMINO_NUM]; // ドミノ

void simLoop(int pause)           /***  シミュレーションループ　***/
{

    int i;
    double x = 5.0, y = 0, z = 1; // カメラの位置[m]
    double yaw = -180, pitch = 0, roll = 0; // カメラの姿勢　ヨー，ピッチ，ロール角[°]

    if (STEPS == 0) dmSetCamera(x,y,z,yaw,pitch,roll); // カメラの設定

    dmSimQuickStep(); // シミュレーションを１ステップ進める(高速版)

    for (i = 0; i < DOMINO_NUM; i++)
    {
        dmDraw(&domino[i]); //　壁の描画
    }
    STEPS++;
}

void resetSim(int n)
{
    double m = 0.1; // 質量
    double side[3] = {0.2, 0.05, 0.5}; // サイズ
    double R[12] = {1,0,0,0, 0,1,0,0, 0,0,1,0}; // 姿勢
    double p[DOMINO_NUM][3];  // 位置

    int i;

    // シミュレーションの終了
    if (STEPS != 0)
    {
        dmClose();
    }

    dmInit(); // 初期化

    // ドミノの生成
    for (i = 0; i < DOMINO_NUM; i++)
    {
        p[i][2] = 0.25; // ドミノ重心のz座標
        switch (n)
        {
        case 0:
            p[i][0] = 0;             // ドミノ重心のx座標
            p[i][1] = 0.3 * i -3.0;  // ドミノ重心のy座標
            break;
        case 1:
            p[i][0] = 0.3 * i -3.0;
            p[i][1] = p[i][0];
            // z軸周りにπ/4回転させた姿勢
            dRFromAxisAndAngle(R, 0, 0, 1, - M_PI/4);
            break;
        default:
            printf("Bad number \n");
            break;
        }
        dmCreateBox(&domino[i], p[i], R, m, side, red);
    }
}

void command(int cmd)
{
    switch (cmd)
    {
    case '1':
        resetSim(1);
        break;
    case 'r':
    case 'R':
        resetSim(0);
        break;
    case 'f':
    case 'F':
    {
        double fx = -1.0, fy = 0.0, fz = 0.0;
        dmAddTorque(&domino[0], fx, fy, fz); // x,y,z軸まわりにfx,fy,fzのトルクを付加
        break;
    }
    default:
        printf("Input r, R, f, F key \n");
        break;
    }
}

/*** main関数 ***/
int main()
{
    resetSim(0); // シミュレーションのリセット
    dmLoop(800, 600, simLoop, command);  // ウインドウの幅，高, ループ関数，コマンド関数
    dmClose(); // 終了

    return 0;
}

ホームワーク

1. サンプルコードstep5-100720.zipをここからダウンロードして実行しよう．
2. ドミノを X状に配置して，ドミノ倒しをしなさい.  解答例step5b-100726.zip
3. ドミノを半径２ｍの円上に配置して，ドミノ倒しをしなさい
4. 上の配置でドミノ倒しが無限に続くように，ドミノがひとりでに起き上がるようにしなさい．