数学関数 (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であり、これは

typedef dReal dMatrix3[ 4 * 3 ];

のように定義されるdReal型の配列です。

単位行列に初期化

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

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は、

typedef dReal dQuaternion[ 4 ];

のようにdReal型の配列として定義されています。

単位行列に初期化

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⁻¹ * qc
 
void dQMultiply1 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );
qa = qb * qc⁻¹
 
void dQMultiply2 (
    dQuaternion qa,
    const dQuaternion qb,
    const dQuaternion qc
    );
qa = qb⁻¹ * qc⁻¹
 
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  // クォータニオン
    );

または、

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

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

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

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

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))

のように定義されていることからわかるように、上記の関数に置き換えられます。

関連ページ