■　クォータニオン(四元数)を使用して座標を回転させる方法の勉強メモです。クォータニオン(四元数)の基礎についてはWikipediaなどで勉強してください。 このページでは座標やベクトルを回転させる方法のみを記述します。三次元のクォータニオンは
q = w + xi + yj + zk
のように定義されています。クォータニオン型の定義は下記ようです。

// クォータニオン型  
typedef struct {
    double   w;
    double   x;
    double   y;
    double   z;
} Quaternion;

■　下記の関数で座標或はベクトル(x,y,z)からクォータニオン型の変数を作成します。

Quaternion Coordinate2Quaternion(double x, double y, double z)
{
    Quaternion q;
    q.w = 0.0;
    q.x = x;
    q.y = y;
    q.z = z;
    return q;
}

■　下記の関数でクォータニオン乗算
(w1 + x1i + y1j + z1k)(w2 + x2i + y2j + z2k)

Quaternion QuaternionMultiplication(Quaternion left, Quaternion right) {
    Quaternion quat;
    double d0, d1, d2, d3;

    d0 = left.w * right.w;
    d1 = -left.x * right.x;
    d2 = -left.y * right.y;
    d3 = -left.z * right.z;
    quat.w = d0 + d1 + d2 + d3;

    d0 = left.w * right.x;
    d1 = right.w * left.x;
    d2 = left.y * right.z;
    d3 = -left.z * right.y;
    quat.x = d0 + d1 + d2 + d3;

    d0 = left.w * right.y;
    d1 = right.w * left.y;
    d2 = left.z * right.x;
    d3 = -left.x * right.z;
    quat.y = d0 + d1 + d2 + d3;

    d0 = left.w * right.z;
    d1 = right.w * left.z;
    d2 = left.x * right.y;
    d3 = -left.y * right.x;
    quat.z = d0 + d1 + d2 + d3;
    return quat;
}

■　上記の掛け算を行列で表現できます。この表現方法は場合によって非常に便利です。

■　下記の関数で回転軸のベクトルと回転角度からクォータニオン型の変数qを作成します。 座標やベクトルを回転させるためのクォータニオンは単位四元数でなければなりません。そうではないと、回転後のベクトルの長さが変わってしまいます。

Quaternion Rotational2Quaternion(double angle, double x, double y, double z) {
    Quaternion quat = { 0.0, 0.0, 0.0, 0.0 };
    double norm;

    norm = x * x + y * y + z * z;
    if (norm == 0.0) {
        return quat;
    }
    // 単位ベクトルにする（重要）
    norm = 1.0 / sqrt(norm);
    x *= norm;
    y *= norm;
    z *= norm;
    // 単位ベクトルからクォータニオンを作ると、単位四元数が作れるはず
    double sin_a = sin(angle / 2.0);
    quat.w = cos(angle / 2.0);
    quat.x = x * sin_a;
    quat.y = y * sin_a;
    quat.z = z * sin_a;
    return quat;
}

■　座標やベクトルを回転させるために、回転クォータニオンの共役クォータニオンの作成が必要です。

Quaternion QuaternionConjugate(Quaternion quat)
{
    Quaternion q;
    q.w = quat.w;
    q.x = -quat.x;
    q.y = -quat.y;
    q.z = -quat.z;
    return q;
}

■　回転処理は、回転クォータニオン(Q)　×　座標クォータニオン(P)　×　回転クォータニオンの共役(Q^*)です。二回のクォータニオン掛け算です。P2は回転後の座標です。

Qは単位四元数でなければなりません！

■　例を使って座標の回転処理をしてみます。図のように、XY平面上の青色線（ベクトル:1, 1, 0）を回転軸として、180°で回転して、Y座標上の点（0, 5, 0）をX軸上の(5, 0, 0)まで移します。またY軸を回転軸として、-90°で回転して、Z座標軸上の点（0, 0, 5）に移します。

void test()
{
    double d2r = atan(1.0) / 45.0;
    // 座標(0,5,0)のクォータニオン作成
    Quaternion p = Coordinate2Quaternion(0.0, 5.0, 0.0);

    // XY平面上の軸で180°回転クォータニオン作成      
    Quaternion q = Rotational2Quaternion(180 * d2r, 1.0, 1.0, 0.0);
    // 回転クォータニオンの共役クォータニオン作成      
    Quaternion qc = QuaternionConjugate(q);

    // 回転処理
    p = QuaternionMultiplication(q, p);
    p = QuaternionMultiplication(p, qc);
    // 結果を見る
    cout << fixed << p.x << ", " << p.y << ", " << p.z << endl;

    // Y軸で-90°回転クォータニオン作成      
    q = Rotational2Quaternion(- 90 * d2r, 0.0, 1.0, 0.0);
    // 回転クォータニオンの共役クォータニオン作成      
    qc = QuaternionConjugate(q);

    // 回転処理
    p = QuaternionMultiplication(q, p);
    p = QuaternionMultiplication(p, qc);
    // 結果を見る
    cout << fixed << p.x << ", " << p.y << ", " << p.z << endl;
    return;
}

結果は下記のようです。

■　複数の回転のための回転クォータニオンは、個別回転クォータニオンを乗算させた結果となります。例えば、q1で回転してq2で回転するとすれば、トータルの回転クォータニオンqはq2xq1となります。

Quaternion QuaternionRotation2Times(Quaternion q1, Quaternion q2)
{
    Quaternion q;
    q = QuaternionMultiplication(q2, q1);
    return q;
}