四元数实现绕轴旋转

问题：已知单位方向向量v=(vx,vy,vz),求绕v旋转角度ω(逆时针为正)的旋转矩阵。

实现方法：

首先计算单位四元素：q = (q0, q1, q2, q3)

• q0 = cos(ω/2)
• q1 = vx*sin(ω/2)
• q2 = vy*sin(ω/2)
• q3 = vz*sin(ω/2)

接着，使用单位四元素计算旋转矩阵;

void UnitQuaternion2Rotation(const double* q, double* rot) {
    rot[0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    rot[1] = (q[1] * q[2] - q[0] * q[3]) * 2;
    rot[2] = (q[1] * q[3] + q[0] * q[2]) * 2;

    rot[3] = (q[2] * q[1] + q[0] * q[3]) * 2;
    rot[4] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3];
    rot[5] = (q[2] * q[3] - q[0] * q[1]) * 2;

    rot[6] = (q[3] * q[1] - q[0] * q[2]) * 2;
    rot[7] = (q[3] * q[2] + q[0] * q[1]) * 2;
    rot[8] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
}

参考：Closed-form solution of absolute orientation using unit quaternions