機械学習基礎理論独習

誤りがあればご指摘いただけると幸いです。数式が整うまで少し時間かかります。リンクフリーです。

勉強ログです。リンクフリーです
目次へ戻る

JavaScript による回転姿勢変換

コンピュータグラフィックス

はじめに

基本的にglMatrixを使います。
glMatrixに無いものを作成しました。
「回転行列」と「オイラー角」と「四元数」を相互変換します。
なお、オイラー角はABCのタイプ(回転軸がすべて異なるタイプ)のみサポートしております。
第1軸と第3軸が一致する場合のタイプはサポートしておりませんので、ご了承ください。

先に便利な関数を作っておく

回転行列作成時にメソッドを呼ぶ回数を1回にするために以下の「回転行列作成」関数を作成します。
mat4.fromRotationを呼び出すだけです。

// Mat4 rotation
function mat4Rotation(angle, vector) {
    const m = mat4.create();
    mat4.fromRotation(m, angle, vector);
    return m;
}

4x4行列を3x3行列に変換(観点成分のみ抽出)時メソッドを呼ぶ回数を1回にするために以下の「回転行列作成関数」を作成します。
mat3.fromMat4を呼び出すだけです。

// Mat4 to mat3
function fromMat4ToMat3(m4) {
    const m = mat3.create();
    mat3.fromMat4(m, m4);
    return m;
}

回転行列を四元数に変換

quat.fromMat3を呼び出すだけです。

// Mat3 to quat
function fromMat3ToQuat(m) {
    const q = quat.create();
    quat.fromMat3(q, m);
    return q;
}

四元数を回転行列に変換

mat3.fromQuatを呼び出すだけです。

// Quat to mat3
function fromQuatToMat3(q) {
    const m = mat3.create();
    mat3.fromQuat(m, q);
    return m;
}

オイラー角を回転行列に変換

orderで指定した順番で、行列を掛けるだけです。この処理はglMatrixに存在しないため、自作です。

// Euler to mat4
function fromEulerToMat4(x, y, z, order = 'zyx') {
    if(order[0] === order[1] || order[0] === order[2] || order[1] === order[2]) {
        throw 'invalid paramters.';
    }
    const m4 = mat4.create();
    mat4.identity(m4);
    const m = { 
        x: mat4Rotation(x, [1, 0, 0]), 
        y: mat4Rotation(y, [0, 1, 0]), 
        z: mat4Rotation(z, [0, 0, 1]) 
    };
    [].forEach.call(order, s => { mat4.multiply(m4, m4, m[s]); });
    return m4;
}

オイラー角を四元数に変換

角度をラジアンから度に直して、quat.fromEulerを呼んでいます。

// Euler to quat
function fromEulerToQuat(x, y, z, order = 'zyx') {
    if(order[0] === order[1] || order[0] === order[2] || order[1] === order[2]) {
        throw 'invalid paramters.'
    }
    const toDegree = 180 / Math.PI;
    const q = quat.create();
    quat.fromEuler(q, x * toDegree, y * toDegree, z * toDegree, order);  // radian to degree
    return q;
}

回転行列をオイラー角に変換

こちらを参考にしました。
ジンバルロックがかかるかどうかは、種別に応じてある値が 0 に近いかどうかで判断しています。

// Mat3 to euler
function fromMat3ToEuler(m, order = 'zyx', epsilon = 1e-6) {
    if(order[0] === order[1] || order[0] === order[2] || order[1] === order[2]) {
        throw 'invalid paramters.';
    }
    if(order === 'xyz') {
        const y = Math.asin(m[6]);
        const cy = Math.cos(y);
        const isCosYNotZero = Math.abs(cy) > epsilon; // cos(theta_y) !== 0                
        const x = isCosYNotZero ? Math.atan(-m[7] / m[8]) : Math.atan(m[5] / m[4]);
        const z = isCosYNotZero ? Math.atan(-m[3] / m[0]) : 0;
        return [ x, y, z, ];
    } else if(order === 'xzy') {
        const z = Math.asin(-m[3]);
        const cz = Math.cos(z);
        const isCosZNotZero = Math.abs(cz) > epsilon; // cos(theta_z) !== 0                
        const x = isCosZNotZero ? Math.atan(m[5] / m[4]) : Math.atan(-m[7] / m[8]);
        const y = isCosZNotZero ? Math.atan(m[6] / m[0]) : 0;
        return [ x, y, z, ];
    } else if(order === 'yxz') {
        const x = Math.asin(-m[7]);
        const cx = Math.cos(x);
        const isCosZNotZero = Math.abs(cx) > epsilon; // cos(theta_x) !== 0                
        const y = isCosZNotZero ? Math.atan(m[6] / m[8]) : Math.atan(-m[2] / m[0]);
        const z = isCosZNotZero ? Math.atan(m[1] / m[4]) : 0;
        return [ x, y, z, ];
    } else if(order === 'yzx') {
        const z = Math.asin(m[1]);
        const cz = Math.cos(z);
        const isCosZNotZero = Math.abs(cz) > epsilon; // cos(theta_z) !== 0                
        const x = isCosZNotZero ? Math.atan(-m[7] / m[4]) : 0;
        const y = isCosZNotZero ? Math.atan(-m[2] / m[0]) : Math.atan(m[6] / m[8]);
        return [ x, y, z, ];
    } else if(order === 'zxy') {
        const x = Math.asin(m[5]);
        const cx = Math.cos(x);
        const isCosXNotZero = Math.abs(cx) > epsilon; // cos(theta_x) !== 0                
        const y = isCosXNotZero ? Math.atan(-m[2] / m[8]) : 0;
        const z = isCosXNotZero ? Math.atan(-m[3] / m[4]) : Math.atan(m[1] / m[0]);
        return [ x, y, z, ];
    } else if(order === 'zyx') {
        const y = Math.asin(-m[2]);
        const cy = Math.cos(y);
        const isCosYNotZero = Math.abs(cy) > epsilon; // cos(theta_y) !== 0
        const x = isCosYNotZero ? Math.atan(m[5] / m[8]) : 0;
        const z = isCosYNotZero ? Math.atan(m[1] / m[0]) : Math.atan(-m[3] / m[4]);
        return [ x, y, z, ];
    } else {
        throw 'invalid paramters.';
    }
}

四元数をオイラー角に変換

四元数を回転行列に変換し、その回転行列をオイラー角に変換します。

// Quat to euler
function fromQuatToEuler(q, order) {
    const m = fromQuatToMat3(q);
    return fromMat3ToEuler(m, order);
}

最後に

プログラムを書いて気付いたのですが、quat.fromEulerメソッドって角度がラジアンではなく度なんですね。ハマりました。
あと、glMatrixに今回私が作成したfromMat3ToEulerみたいな関数があってもよいのになって思いました。

参考リンク

回転行列、クォータニオン(四元数)、オイラー角の相互変換
オイラー角に潜む5つの罠

目次へ戻る