QQuaternion四元数类

利用四元数获得旋转矩阵

QMatrix4x4 fromQuaternion(QQuaternion &q)
{
    QMatrix4x4 mat ;
    mat.setToIdentity();
    float x = q.x();
    float y = q.y();
    float z = q.z();
    float w = q.scalar();

    float x2 = x * x;
    float xy = x * y;
    float xz = x * z;
    float y2 = y * y;
    float yz = y * z;
    float z2 = z * z;
    mat(0, 0) = 1 - 2 * (y2 + z2);
    mat(0, 1) = 2 * (xy - w * z);
    mat(0, 2) = 2 * (xz + w * y);
    mat(0, 3) = 0;
    mat(1, 0) = 2 * (xy + w * z);
    mat(1, 1) = 1 - 2 * (x2 + z2);
    mat(1, 2) = 2 * (yz - w * x);
    mat(1, 3) = 0;
    mat(2, 0) = 2 * (xz - w * y);
    mat(2, 1) = 2 * (yz + w * x);
    mat(2, 2) = 1 - 2 * (x2 + y2);
    mat(2, 3) = 0;


    return mat;
}

下面同时给出Eigen中的四元数类

Eigen::Matrix4f fromQuaternion(Eigen::Quaternion<float> &q)
{
    Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();

    float x = q.x();
    float y = q.y();
    float z = q.z();
    float w = q.w();

    float x2 = x * x;
    float xy = x * y;
    float xz = x * z;
    float y2 = y * y;
    float yz = y * z;
    float z2 = z * z;
    mat(0, 0) = 1 - 2 * (y2 + z2);
    mat(0, 1) = 2 * (xy - w * z);
    mat(0, 2) = 2 * (xz + w * y);
    mat(0, 3) = 0;
    mat(1, 0) = 2 * (xy + w * z);
    mat(1, 1) = 1 - 2 * (x2 + z2);
    mat(1, 2) = 2 * (yz - w * x);
    mat(1, 3) = 0;
    mat(2, 0) = 2 * (xz - w * y);
    mat(2, 1) = 2 * (yz + w * x);
    mat(2, 2) = 1 - 2 * (x2 + y2);
    mat(2, 3) = 0;
    return mat;
}

如果在Qt工程中那么就用第一个类，在非Qt中用Eigen，也可以自定义四元数类，如果想偷懒Qt和Eigen的四元数类就够用了。
其中四元数的累积，可以直接利用乘法，如下：

    Eigen::Quaternionf eq1(0.5, 1.0, 0.0, 0.0);
    Eigen::Quaternionf eq2(0.5, 1.0, 0.0, 0.0);
    Eigen::Quaternionf eq = multiquaternion(eq1, eq2);

QQuaternion 的用法和Eigen类似。