四元数转矩阵
void fromInertialToObjectQuaternion(const Quaternion &q);
void fromObjectToInertialQuaternion(const Quaternion &q);
void RotationMatrix::fromInertialToObjectQuaternion(const Quaternion &q)
{
m11 = 1.0f - 2.0f*q.y*q.y - 2.0f*q.z*q.z;
m12 = 2.0f * q.x*q.y + 2.0f * q.w*q.z;
m13 = 2.0f * q.x*q.z - 2.0f * q.w*q.y;
m21 = 2.0f * q.x*q.y - 2.0f * q.w*q.z;
m22 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
m23 = 2.0f * q.y*q.z + 2.0f * q.w*q.x;
m31 = 2.0f * q.x*q.z + 2.0f * q.w*q.y;
m32 = 2.0f * q.y*q.z - 2.0f * q.w*q.z;
m33 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
}
//正交矩阵 逆=转置
void RotationMatrix::fromObjectToInertialQuaternion(const Quaternion &q)
{
m11 = 1.0f - 2.0f*q.y*q.y - 2.0f*q.z*q.z;
m21 = 2.0f * q.x*q.y + 2.0f * q.w*q.z;
m31 = 2.0f * q.x*q.z - 2.0f * q.w*q.y;
m12 = 2.0f * q.x*q.y - 2.0f * q.w*q.z;
m22 = 1.0f - 2.0f * q.x*q.x - 2.0