ODE/オブジェクトの位置と姿勢 - ずくなしのーと

オブジェクトの位置と姿勢

3D空間にODEのオブジェクトを配置する際に、困らないようにいろいろとメモします。

drawstuffの座標系

まずは、drawstuffの座標系についてです。
サンプルプログラムを作る際はdrawstuffに非常にお世話になるのでおさえておきます。

drawstuffの空間定義では、Z軸が上方向になります。
また、地面に４角錐マークがありますが、
赤マークがX軸のプラス方向 を示し、
青マークがY軸のプラス方向を示します。
また、マークとマークの間隔は1.0になります。

ちなみに、上図のカメラ設定は次の通りです。
カメラ位置を(x,y,z)=(0,-5,3)とし、カメラ向きをy軸プラス方向(90度)に少し下向き(-15度)としています。

static void start()
{
  static float xyz[3] = { 0.f, -5.f, 3.f };
  static float hpr[3] = { 90.f, -15.f, 0.f };
  dsSetViewpoint( xyz, hpr ); // カメラ位置と方向設定
}
 

ただし、この設定だと逆光になっちゃうので、
逆光がいやな場合は-5.f => 5.f, 90.f => -90.f にすると見やすくなります。

位置

位置の設定は次のAPIを使用します。

// dBodyID位置設定
void dBodySetPosition(dBodyID body, dReal x, dReal y, dReal z);
 
// dGeomID位置設定
void dGeomSetPosition(dGeomID body, dReal x, dReal y, dReal z);
 

もし、dGeomSetBodyを使ってdBodyIDとdGeomIDが関連づけられている場合は、dBodySetPositionとdGeomSetPositionは同じ動作になります。
なので、dBodyIDとdGeomIDが関連づけられている状態で、dBodySetPositionで設定してからdGeomSetPositionで設定するとdBodySetPositionで設定した位置は上書きされます。逆の順番も同様です。

姿勢

姿勢制御する場合は、次のAPIを使用します。

// dBodyIDの姿勢
void dBodySetRotation (dBodyID, const dMatrix3 R);
void dBodySetQuaternion (dBodyID, const dQuaternion q);
 
// dGeomIDの姿勢
void dGeomSetRotation (dGeomID, const dMatrix3 R);
void dGeomSetQuaternion (dGeomID, const dQuaternion q);
 

このとき、引数として渡すRもしくはqはマトリックスになります。
マトリックスを素のままでは扱いづらいので、次のサポートAPIが用意されています。

// ax,ay,azで回転させる軸を設定し、回転角度angleをラジアンで指定する
void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle);
// オイラーで指定する
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi);
// (ax,ay,az)から(bx,by,bz)の方向に回転させる
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
// 四元数でax,ay,azを軸に回転角度angleをラジアンで指定する
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, dReal angle);
 

dQFromAxisAndAngleによる初期姿勢の設定例

マトリックスを掛け合わせることで、x軸、y軸、z軸のそれぞれの回転を合成することができます。

• 無回転

dQuaternion q, q1, q2, q3, q4;
dQFromAxisAndAngle(q1, 1.0, 0.0, 0.0, 0.0*M_PI/180.0); // x-axis
dQFromAxisAndAngle(q2, 0.0, 1.0, 0.0, 0.0*M_PI/180.0); // y-axis
dQFromAxisAndAngle(q3, 0.0, 0.0, 1.0, 0.0*M_PI/180.0); // z-axis
dQMultiply0(q4, q1, q2);
dQMultiply0(q, q3, q4);
dBodySetQuaternion(m_body, q);
 

• x軸に対して30度回転

dQuaternion q, q1, q2, q3, q4;
dQFromAxisAndAngle(q1, 1.0, 0.0, 0.0, 30.0*M_PI/180.0); // x-axis
dQFromAxisAndAngle(q2, 0.0, 1.0, 0.0, 0.0*M_PI/180.0); // y-axis
dQFromAxisAndAngle(q3, 0.0, 0.0, 1.0, 0.0*M_PI/180.0); // z-axis
dQMultiply0(q4, q1, q2);
dQMultiply0(q, q3, q4);
dBodySetQuaternion(m_body, q);
 

• y軸に対して30度回転

dQuaternion q, q1, q2, q3, q4;
dQFromAxisAndAngle(q1, 1.0, 0.0, 0.0, 0.0*M_PI/180.0); // x-axis
dQFromAxisAndAngle(q2, 0.0, 1.0, 0.0, 30.0*M_PI/180.0); // y-axis
dQFromAxisAndAngle(q3, 0.0, 0.0, 1.0, 0.0*M_PI/180.0); // z-axis
dQMultiply0(q4, q1, q2);
dQMultiply0(q, q3, q4);
dBodySetQuaternion(m_body, q);
 

• z軸に対して30度回転

dQuaternion q, q1, q2, q3, q4;
dQFromAxisAndAngle(q1, 1.0, 0.0, 0.0, 0.0*M_PI/180.0); // x-axis
dQFromAxisAndAngle(q2, 0.0, 1.0, 0.0, 0.0*M_PI/180.0); // y-axis
dQFromAxisAndAngle(q3, 0.0, 0.0, 1.0, 30.0*M_PI/180.0); // z-axis
dQMultiply0(q4, q1, q2);
dQMultiply0(q, q3, q4);
dBodySetQuaternion(m_body, q);
 

• x軸、y軸、z軸に対してそれぞれ30度回転

dQuaternion q, q1, q2, q3, q4;
dQFromAxisAndAngle(q1, 1.0, 0.0, 0.0, 30.0*M_PI/180.0); // x-axis
dQFromAxisAndAngle(q2, 0.0, 1.0, 0.0, 30.0*M_PI/180.0); // y-axis
dQFromAxisAndAngle(q3, 0.0, 0.0, 1.0, 30.0*M_PI/180.0); // z-axis
dQMultiply0(q4, q1, q2);
dQMultiply0(q, q3, q4);
dBodySetQuaternion(m_body, q);