らりほーさんから回転行列のご質問があり，その回答も兼ねて久しぶりにODE (Open Dynamics Engine)講座を開講します．
回転行列は，基準の姿勢から現在の姿勢を行列で表したものです．回転行列は3行3列の行列ですが，ODEでは計算を高速化させるために，回転行列の各成分を要素数12個の１次元配列に格納しています．
回転行列のi行j列成分をrijとすると，回転行列の配列は以下で表すことができます(ODE本P103参照）．４番目、８番目、１２番目の要素は数字の０（ゼロ）です。
dReal R[12] = {r11, r12, r13, 0, r21, r22, r23, 0, r31, r32, r33,0};
なお、ODEの回転行列はdMatrix3型で、src/ode-バージョン番号/include/ode/common.hで次のように定義されています。
- typedef dReal dMatrix3[4*3] ;
つまり、要素数12個のdReal(ODEの実数型）型配列です。
これがわかるとロール(roll)・ピッチ(pitch)・ヨー(yaw)角をODEの回転行列から求めることができます．ロール・ピッチ・ヨー角の回転行列は，絶対座標系のx軸まわりにroll角度回転，次にy軸のまわりにpitch角度回転，最後にｚ軸のまわりにyaw角度回転させたものとなります．ここでは導出しませんが，次式で求めることができます（ODE本P149参照)
roll = atan2(r32, r33);
pitch = atan2(-r31, sqrt(r32*r32+r33*r33)) または atan2(-r31, -sqrt(r32*r32+r33*r33));
yaw = atan2(r21, r11);
これを使ったサンプルプログラムの主要部分を以下に示します．完全なプログラムのダウンロードはこちらから．
今回はこの辺で…
[cpp]
// roll, pitch, yawのサンプルプログラム
// rpy.cpp by Kosei Demura (2008-5-12)
dWorldID world; // 動力学計算用ワールド
dReal START_X = 0, START_Y = 0, START_Z=1.20; // 初期位置
dReal weight = 9.4; // ボックスの質量
dReal size[3] = {0.5,0.1,0.3}; // ボックスのサイズ
typedef struct { // MyObject構造体
dBodyID body; // ボディ(剛体)のID番号（動力学計算用）
dReal lx, ly, lz; // 長さ[m]
} MyObject;
MyObject box; // MyObject構造体
// ロール・ピッチ・ヨー角の計算 ODE本P150
static void rollPitchYaw()
{
const dReal *rot = dBodyGetRotation(box.body);
dReal r11,r12,r13,r21,r22,r23,r31,r32,r33;
dReal pitch, yaw, roll;
r11 = rot[0]; r12 = rot[1]; r13 = rot[2];
r21 = rot[4]; r22 = rot[5]; r23 = rot[6];
r31 = rot[8]; r32 = rot[9]; r33 = rot[10];
// ポインタ的表記では r11 = *(rot+0); r12 = *(rot+1); r13 = *(rot+2);
pitch = atan2(-r31, sqrt(r32*r32+r33*r33));
yaw = atan2(r21,r11);
roll = atan2(r32,r33);
printf("roll=%f pitch=%f yaw=%f r", 180.0*roll/M_PI, 180.0*pitch/M_PI, 180.0*yaw/M_PI);
}
// 絶対座標系x軸,ｙ軸,ｚ軸の順番で回転
static void rotation()
{
static dReal angle_x = 45, angle_y = 0, angle_z = 90;
const dReal *R0;
dMatrix3 R1, R2, R3, R10, R20, R30;
R0 = dBodyGetRotation(box.body);
dRFromAxisAndAngle(R1, 1, 0, 0, M_PI*angle_x/180.0); // x軸まわり
dRFromAxisAndAngle(R2, 0, 1, 0, M_PI*angle_y/180.0); // y軸まわり
dRFromAxisAndAngle(R3, 0, 0, 1, M_PI*angle_z/180.0); // z軸まわり
dMultiply0(R10, R1, R0, 3, 3, 3);
dMultiply0(R20, R2, R10, 3, 3, 3);
dMultiply0(R30, R3, R20, 3, 3, 3);
dBodySetRotation(box.body, R30);
}
static void simLoop(int pause)
{
rollPitchYaw();
dWorldStep(world,0.01);
dsSetColor(1.0,0.0,0.0);
dsDrawBox(dBodyGetPosition(box.body),
dBodyGetRotation(box.body),size);
}
// ボックスの生成
void makeBox()
{
dMass mass;
box.body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, weight, size[0], size[1], size[2]);
dBodySetMass(box.body, &mass);
dBodySetPosition(box.body,START_X,START_Y,START_Z);
}
int main(int argc, char **argv)
{
setDrawStuff();
world = dWorldCreate(); // 動力学計算ワールドの作成
makeBox(); // ボックスの作成
rotation(); // 回転
dsSimulationLoop(argc,argv,640,480,&fn); // シミュレーションループ
dWorldDestroy(world); // ワールドの破壊
return 0;
}
[/cpp]
コメント