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

domino

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

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

○ 構造体

配列では同じ型しかまとめることができませんでしたが,構造体では違う型をまとめて扱うことができるのでシミュレーションなど物体に多くの属性がある場合に便利です.このサンプルプログラムでは物体をdmObjectという構造体で次のように表しています.dm5.hの16行目に定義しています.今回の例はドミノ倒しなので,この構造体を元にドミノを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,fy,fzはそれぞれx, y, z軸まわりのトルク

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

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

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

rまたはRキーを押すと,resetSim関数が呼ばれてシミュレーションがリセットされます.resetSimの中身はシミュレーションループが1回以上呼び出されたときに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(); // シミュレーションを1ステップ進める(高速版)

    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. ドミノを半径2mの円上に配置して,ドミノ倒しをしなさい
  4. 上の配置でドミノ倒しが無限に続くように,ドミノがひとりでに起き上がるようにしなさい.
ode
スポンサーリンク
シェアする
demura.netをフォローする

コメント

タイトルとURLをコピーしました