1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85
| //---------------------------------------------------------------------------
// RotationMatrix::fromInertialToObjectQuaternion
//
// Setup the matrix, given a quaternion that performs an inertial->object
// rotation
//
// See 10.6.3
void RotationMatrix::fromInertialToObjectQuaternion(const Quaternion &q) {
// Fill in the matrix elements. This could possibly be
// optimized since there are many common subexpressions.
// We'll leave that up to the compiler...
m11 = 1.0f - 2.0f * (q.y*q.y + q.z*q.z);
m12 = 2.0f * (q.x*q.y + q.w*q.z);
m13 = 2.0f * (q.x*q.z - q.w*q.y);
m21 = 2.0f * (q.x*q.y - q.w*q.z);
m22 = 1.0f - 2.0f * (q.x*q.x + q.z*q.z);
m23 = 2.0f * (q.y*q.z + q.w*q.x);
m31 = 2.0f * (q.x*q.z + q.w*q.y);
m32 = 2.0f * (q.y*q.z - q.w*q.x);
m33 = 1.0f - 2.0f * (q.x*q.x + q.y*q.y);
}
//---------------------------------------------------------------------------
// RotationMatrix::fromObjectToInertialQuaternion
//
// Setup the matrix, given a quaternion that performs an object->inertial
// rotation
//
// See 10.6.3
void RotationMatrix::fromObjectToInertialQuaternion(const Quaternion &q) {
// Fill in the matrix elements. This could possibly be
// optimized since there are many common subexpressions.
// We'll leave that up to the compiler...
m11 = 1.0f - 2.0f * (q.y*q.y + q.z*q.z);
m12 = 2.0f * (q.x*q.y - q.w*q.z);
m13 = 2.0f * (q.x*q.z + q.w*q.y);
m21 = 2.0f * (q.x*q.y + q.w*q.z);
m22 = 1.0f - 2.0f * (q.x*q.x + q.z*q.z);
m23 = 2.0f * (q.y*q.z - q.w*q.x);
m31 = 2.0f * (q.x*q.z - q.w*q.y);
m32 = 2.0f * (q.y*q.z + q.w*q.x);
m33 = 1.0f - 2.0f * (q.x*q.x + q.y*q.y);
}
//---------------------------------------------------------------------------
// RotationMatrix::inertialToObject
//
// Rotate a vector from inertial to object space
Vector3 RotationMatrix::inertialToObject(const Vector3 &v) const {
// Perform the matrix multiplication in the "standard" way.
return Vector3(
m11*v.x + m21*v.y + m31*v.z,
m12*v.x + m22*v.y + m32*v.z,
m13*v.x + m23*v.y + m33*v.z
);
}
//---------------------------------------------------------------------------
// RotationMatrix::objectToInertial
//
// Rotate a vector from object to inertial space
Vector3 RotationMatrix::objectToInertial(const Vector3 &v) const {
// Multiply by the transpose
return Vector3(
m11*v.x + m12*v.y + m13*v.z,
m21*v.x + m22*v.y + m23*v.z,
m31*v.x + m32*v.y + m33*v.z
); |
Partager