数学関数 (Math functions)

回転行列、クオータニオンの主な関数
  回転行列 クオータニオン
単位行列 dRSetIdentity dQSetIdentity
軸まわりに回転 dRFromAxisAndAngle dQFromAxisAndAngle
相互変換 dRfromQ dQfromR
※ それぞれの関数名は、回転行列はdR、クオータニオンはdQから始まる。

ベクトルと行列 (Vector、Matrix)

ベクトルと行列の型は、次のようにdReal型の配列として定義されています。

ベクトル

typedef dReal dVector3[ 4 ];
typedef dReal dVector4[ 4 ];

行列

typedef dReal dMatrix3[ 4 * 3 ];
typedef dReal dMatrix4[ 4 * 4 ];
typedef dReal dMatrix6[ 8 * 6 ];

すべての要素に指定値を設定します。

void dSetValue ( dReal *a, int n, dReal value );

零ベクトル、零行列 (zero vector、zero matrix)

すべての要素に0を設定します。

void dSetZero ( dReal *a, int n );

内積 (dot product)

2つのn×1ベクトルの内積を求めます。

dReal dDot ( const dReal *a, const dReal *b, int n );

積 (product)

式 行列の大きさ 関数
A B C
A = B * C
 p * r p * q q * r 
void dMultiply0 (
    dReal *A,            // 行列A
    const dReal *B,      // 行列B
    const dReal *C,      // 行列C
    int p, int q, int r  // 行列の大きさ
    );
A = tB * C
 p * r q * p q * r 
void dMultiply1 (
    dReal *A,            // 行列A
    const dReal *B,      // 行列B
    const dReal *C,      // 行列C
    int p, int q, int r  // 行列の大きさ
    );
A = B * tC
 p * r p * q r * q 
void dMultiply2 (
    dReal *A,            // 行列A
    const dReal *B,      // 行列B
    const dReal *C,      // 行列C
    int p, int q, int r  // 行列の大きさ
    );

正規化 (normalize)

ベクトルを正規化し、単位ベクトルに変換します。

void dNormalize3 ( dVector3 a );
void dNormalize4 ( dVector4 a );

回転行列 (Rotation Matrix)

回転行列の型はdMatrix3であり、これは次のように定義されるdReal型の配列です。

typedef dReal dMatrix3[ 4 * 3 ];

単位行列に初期化

単位行列 (回転していない状態) に設定します。

void dRSetIdentity ( dMatrix3 R );

軸まわりに回転

軸ベクトル (ax、ay、az) まわりに角度angle[rad]回転したときの、回転行列Rを求めます。

void dRFromAxisAndAngle (
    dMatrix3 R,
    dReal ax, dReal ay, dReal az,      // 軸ベクトル
    dReal angle                        // 回転角[rad]
    );

オイラー角から作成

オイラー角 (Euler angle) から回転行列Rを作成します。

void dRFromEulerAngles (
    dMatrix3 R,
    dReal phi, dReal theta, dReal psi    // オイラー角
    );

単位ベクトルから作成

2つのベクトルから回転行列Rを作成します。それぞれのベクトルが作成された回転行列のx軸、y軸となります。よってベクトルは単位ベクトルで、互いに直交している必要があります。

void dRFrom2Axes (
    dMatrix3 R,
    dReal ax, dReal ay, dReal az,        // ベクトルa
    dReal bx, dReal by, dReal bz         // ベクトルb
    );

？

void dRFromZAxis (
    dMatrix3 R,
    dReal ax, dReal ay, dReal az         // ？
    );

クオータニオン (Quaternion)

クオータニオンの型dQuaternionは、次のようにdReal型の配列として定義されています。

typedef dReal dQuaternion[ 4 ];

単位行列に初期化

void dQSetIdentity ( dQuaternion q );

軸まわりに回転

軸ベクトル (ax、ay、az) まわりに角度angle[rad]回転したときの、クオータニオンqを求めます。

void dQFromAxisAndAngle (
    dQuaternion q,
    dReal ax, dReal ay, dReal az,   // 軸ベクトル
    dReal angle                     // 回転角[rad]
    );

クオータニオンによる回転

クオータニオンの積算を行います。これによりクオータニオンqaを、qb回転した後にqc回転させられます。

式 関数
qa = qb * qc
 
void dQMultiply0 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );
qa = qb-1 * qc
 
void dQMultiply1 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );
qa = qb * qc-1
 
void dQMultiply2 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );
qa = qb-1 * qc-1
 
void dQMultiply3 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );

クオータニオンの時間微分

dDQfromW()で、クオータニオンを時間微分した値dq/dtを求められます。

void dDQfromW (
    dReal dq[ 4 ],       // 結果を格納する4元ベクトル
    const dVector3 w,    // 角ベト
    const dQuaternion q  // クオータニオ 
    );

または次のようなマクロが定義されているため、dWtoDQ()でも同様に求めらま。

#define dWtoDQ(w,q,dq)    dDQfromW((dq),(w),(q))

回転行列とクオータニオンの相互変換

クオータニオンを回転行列に変換します。

void dRfromQ ( dMatrix3 R, const dQuaternion q );

回転行列をクオータニオンに変換します。

void dQfromR ( dQuaternion q, const dMatrix3 R );

dQtoR()、dRtoQ()は互換性のために残されている関数で、それぞれ次に示すように上記のに置換れます。

#define dQtoR(q,R)    dRfromQ((R),(q))
#define dRtoQ(R,q)    dQfromR((q),(R))

関連ページ