四元数与欧拉角

欧拉角到四元数的转换

公式：

struct Quaternion
{
    double w, x, y, z;
};
Quaternion ToQuaternion(float yaw, float pitch, float roll,float q[])) // yaw (Z), pitch (Y), roll (X)
{
    float cy = cos(yaw * 0.5);
    float sy = sin(yaw * 0.5);
    float cp = cos(pitch * 0.5);
    float sp = sin(pitch * 0.5);
    float cr = cos(roll * 0.5);
    float sr = sin(roll * 0.5);
    q[3] = cy * cp * cr + sy * sp * sr; // w
    q[0] = cy * cp * sr - sy * sp * cr; // x
    q[1] = sy * cp * sr + cy * sp * cr; // y
    q[2] = sy * cp * cr - cy * sp * sr; // z

    return q;
}

参考文章