杂文（25）轴角转旋转矩阵；旋转矩阵转四元数（nodejs实现）

最后更新于2021年9月29日 10:04:21

参见：Maths - Rotation conversions

写在前面：下面我自己实现的轴角转旋转矩阵是基于右手系！基于右手系！基于右手系！如需要左手系，复制粘贴时请自行谷歌并做处理。
矩阵旋转公式备忘（左手坐标系和右手坐标系）

一些公共函数：

var mathjs = require('mathjs')

var d2r = function(degree) { // degree to radian
    return degree / 180 * Math.PI
}

轴角转旋转矩阵：

function rotateAxis(angle, derivedAxis) {
    const sinX = mathjs.sin(d2r(angle));
    const cosX = mathjs.cos(d2r(angle));
    let rotation;
    if(derivedAxis === 'x') {
        rotation = mathjs.matrix([
            [1,    0,    0],
            [0, cosX,-sinX],
            [0, sinX, cosX],
        ]);
    }
    else if (derivedAxis === 'y') {
        rotation = mathjs.matrix([
            [cosX, 0, sinX],
            [0,    1,    0],
            [-sinX,0, cosX],
        ]);
    }
    else {
        rotation = mathjs.matrix([
            [cosX,-sinX, 0],
            [sinX, cosX, 0],
            [0,    0,    1],
        ]);
    }
    return rotation;
}

更新，增加一轴角转旋转矩阵通用函数，这个是基于一个已经归一化（normalize）了的标量进行角度旋转，更通用，更短：

function rotateAxisFree(angle, scalar) {
    const sinX = mathjs.sin(d2r(angle));
    const cosX = mathjs.cos(d2r(angle));
    const t = 1 - cosX;
    const x = scalar[0], y =scalar[1], z = scalar[2];
    const tx = t * x, ty = t * y;

    let rotation;
    rotation = mathjs.matrix([
        [tx * x + cosX,     tx * y - sinX * z, tx * z + sinX * y],
        [tx * y + sinX * z, ty * y + cosX,     ty * z - sinX * x],
        [tx * z - sinX * y, ty * z + sinX * x, t * z * z + cosX ]
    ]);

    return rotation;
}

旋转矩阵转四元数：

/*
* @ele Enumerate the elements in the rotate matrix
* @trace The sum of the diagonal elements
* @S Denominator
* @qw, qx, qy, qz Four elements of quaternion
 */
function rotMat2Quat(rotation) {
    let ele = {
        m00 : rotation.get([0,0]),
        m01 : rotation.get([0,1]),
        m02 : rotation.get([0,2]),
        m10 : rotation.get([1,0]),
        m11 : rotation.get([1,1]),
        m12 : rotation.get([1,2]),
        m20 : rotation.get([2,0]),
        m21 : rotation.get([2,1]),
        m22 : rotation.get([2,2]),
    }
    const trace = ele.m00 + ele.m11 + ele.m22;
    let S;
    let qw, qx, qy, qz;

    if(trace > 0) {
        S = mathjs.sqrt(trace + 1) * 2; // S = 4 * qw
        qw = 0.25 * S;
        qx = (ele.m21 - ele.m12) / S;
        qy = (ele.m02 - ele.m20) / S;
        qz = (ele.m10 - ele.m01) / S;
    }
    else if((ele.m00 > ele.m11) && (ele.m00 > ele.m22)) {
        S = mathjs.sqrt(1 + ele.m00 - ele.m11 - ele.m22) * 2; // S = 4 * qx
        qw = (ele.m21 - ele.m12) / S;
        qx = 0.25 * S;
        qy = (ele.m01 + ele.m10) / S;
        qz = (ele.m02 + ele.m20) / S;
    }
    else if (ele.m11 > ele.m22) {
        S = mathjs.sqrt(1 + ele.m11 - ele.m00 - ele.m22) * 2; // S = 4 * qy
        qw = (ele.m02 - ele.m20) / S;
        qx = (ele.m01 + ele.m10) / S;
        qy = 0.25 * S;
        qz = (ele.m12 + ele.m21) / S;
    }
    else {
        S = mathjs.sqrt(1 + ele.m22 - ele.m00 - ele.m11) * 2; // S = 4 * qz
        qw = (ele.m10 - ele.m01) / S;
        qx = (ele.m02 + ele.m20) / S;
        qy = (ele.m12 + ele.m21) / S;
        qz = 0.25 * S;
    }

    return mathjs.matrix([qw, qx, qy, qz]);
}

简单的使用实例：

const rotation = mathjs.multiply(rotateAxis(data.latitude - 90, 'y'), rotateAxis(-1 * data.longitude, 'z'));
const quaternion = rotMat2Quat(rotation);

如有错误请指正，磕头了嘭嘭嘭。

更新：使用three.js的函数可以凑合使用进行各种坐标系转换，例如：
三维空间旋转和Three.JS中的实现

let threeMat = new three.Matrix4()
var yourRes = threeMat.makeRotationAxis(new three.Vector3(0,1,0), d2r(180));
console.log('Your function: ', yourRes);
// 生成4x4的旋转矩阵

threejs进行矩阵操作非常垃圾，有条件的话还是自己写吧。
threejs在进行一些矩阵乘法等操作时函数的分类非常不合理，非常难用。进行多次旋转时，输出做的很迷惑，完全脱离正常人的思维，看不懂在干什么。传值传引用传的一塌糊涂，一不小心就新值改变旧值。
两个matrix3x3 不能相乘，只能使用4x4相乘再转换为3x3，设计这部分的人你是萨比？？
🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡🤡

2021年8月24日 16:43:28： threejs的源码还是值得一看的，至少算法是对的。