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;
また、STEP4まではdmLoop関数の引数にstep関数やcommand関数を渡していましたが、ここでは関数のポインタを要素に持つ構造体dmFunctionsを定義します。今まではdmLoopに渡す関数が増えるとその数だけ引数が多くなってしまいましたが、この構造体だけを引数として渡せば良いのでプログラムがすっきりします。
typedef struct
{
void (*start)(); // 初期化関数
void (*step) (int pause); // ステップ関数
void (*command) (int cmd); // キー関数
} dmFunctions;
○ 姿勢の変更
物体の姿勢を変更するためには次の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軸まわりのトルク
○ 高速なシミュレーション
前回のサンプルではシミュレーションのステップ関数としてdmWorldStep()を使いましたが，ここではより高速なdmWorldQuickStep()を使います．ただし，dmWorldStep()と比較して精度が悪くなります．
○ シミュレーションのリセット
rまたはRキーを押すと，resetSim関数が呼ばれてシミュレーションがリセットされます．resetSimの中身はシミュレーションループが１回以上呼び出されたときにdmInit関数を呼び出して初期化し，ドミノを再度生成しています．
○ ソースコード
/* step5 ドミノ倒し 2011-01-20 */
#include "dm5.h"
#define DOMINO_NUM 21
static int STEPS = 0; // シミュレーションのステップ数
double red[3] = {1.3, 0.0, 0.0}; // 赤色
dmObject domino[DOMINO_NUM]; // ドミノ
dmFunctions dmf; // 描画関数の構造体
/*** シミュレーションループ ***/
void simLoop(int pause)
{
int i;
dmWorldQuickStep(); // シミュレーションを１ステップ進める(高速版)
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;
}
}
/*** カメラの位置と姿勢設定 ***/
void setCamera()
{
float x = 5.0, y = 0.0, z = 1.0; // カメラの位置
float roll = 0, pitch = 0, yaw = -180; // カメラの方向[°]
dmSetCamera(x,y,z,roll,pitch,yaw); // カメラの設定
}
/*** 描画用構造体の設定 ***/
void setDraw()
{
dmf.start = &setCamera;
dmf.step = &simLoop;
dmf.command = &command;
}
/*** main関数 ***/
int main()
{
resetSim(0); // シミュレーションのリセット
setDraw(); // 描画関数の設定
dmLoop(800, 600, &dmf); // ウインドウの幅，高, ループ関数，コマンド関数
dmClose(); // 終了
return 0;
}
ホームワーク
- サンプルコードをここからダウンロードして実行しよう．
- １０００ステップ経過したら自動的にドミノが立ち上がるようにプログラムを変更しなさい。
- １０００ステップ毎にドミノ倒しが無限に続くようにプログラムを変更しなさい。
- ドミノを１辺６ｍの正方形上に配置して、ドミノ倒しをしなさい。
- ドミノを半径３ｍの円上に配置して、ドミノ倒しをしなさい。
コメント