//---------------------------------------------------------------------------
// operator *
//
// Quaternion cross product, which concatonates multiple angular
// displacements. The order of multiplication, from left to right,
// corresponds to the order that the angular displacements are
// applied. This is backwards from the *standard* definition of
// quaternion multiplication. See section 10.4.8 for the rationale
// behind this deviation from the standard.
public static Quaternion operator *(Quaternion b, Quaternion a)
{
Quaternion result = new Quaternion();
result.w = b.W*a.W - b.W*a.X - b.Y*a.Y - b.Z*a.Z;
result.x = b.W*a.X + b.X*a.W + b.Z*a.Y - b.Y*a.Z;
result.y = b.W*a.Y + b.Y*a.W + b.X*a.Z - b.Z*a.X;
result.z = b.W*a.Z + b.Z*a.W + b.Y*a.X - b.X*a.Y;
return result;
}
//---------------------------------------------------------------------------
// RotationMatrix::fromInertialToObjectQuaternion
//
// Setup the matrix, given a quaternion that performs an inertial->object
// rotation
//
// See 10.6.3
public void fromInertialToObjectQuaternion(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);
}
//---------------------------------------------------------------------------
// conjugate
//
// Compute the quaternion conjugate. This is the quaternian
// with the opposite rotation as the original quaternian. See 10.4.7
public Quaternion conjugate(Quaternion q)
{
Quaternion result = new Quaternion();
// Same rotation amount
result.w = q.w;
// Opposite axis of rotation
result.x = -q.x;
result.y = -q.y;
result.z = -q.z;
// Return it
return result;
}
//---------------------------------------------------------------------------
// fromObjectToInertialQuaternion
//
// Setup the Euler angles, given an object->inertial rotation quaternion
//
// See 10.6.6 for more information.
void fromObjectToInertialQuaternion(Quaternion q)
{
// Extract sin(pitch)
float sp = -2.0f * (q.Y*q.Z - q.W*q.X);
// Check for Gimbel lock, giving slight tolerance for numerical imprecision
if (Math.Abs(sp) > 0.9999f) {
// Looking straight up or down
pitch = MathUtil.kPiOver2 * sp;
// Compute heading, slam bank to zero
heading = (float)Math.Atan2(-q.X*q.Z + q.W*q.Y, 0.5f - q.Y*q.Y - q.Z*q.Z);
bank = 0.0f;
} else {
// Compute angles. We don't have to use the "safe" asin
// function because we already checked for range errors when
// checking for Gimbel lock
pitch = (float)Math.Asin(sp);
heading = (float)Math.Atan2(q.X*q.Z + q.W*q.Y, 0.5f - q.X*q.X - q.Y*q.Y);
bank = (float)Math.Atan2(q.X*q.Y + q.W*q.Z, 0.5f - q.X*q.X - q.Z*q.Z);
}
}
//---------------------------------------------------------------------------
// fromQuaternion
//
// Setup the matrix to perform a rotation, given the angular displacement
// in quaternion form.
//
// The translation portion is reset.
//
// See 10.6.3 for more info.
public void fromQuaternion(Quaternion q)
{
// Compute a few values to optimize common subexpressions
float ww = 2.0f * q.W;
float xx = 2.0f * q.X;
float yy = 2.0f * q.Y;
float zz = 2.0f * q.Z;
// Set the matrix elements. There is still a little more
// opportunity for optimization due to the many common
// subexpressions. We'll let the compiler handle that...
m11 = 1.0f - yy*q.Y - zz*q.Z;
m12 = xx*q.Y + ww*q.Z;
m13 = xx*q.Z - ww*q.X;
m21 = xx*q.Y - ww*q.Z;
m22 = 1.0f - xx*q.X - zz*q.Z;
m23 = yy*q.Z + ww*q.X;
m31 = xx*q.Z + ww*q.Y;
m32 = yy*q.Z - ww*q.X;
m33 = 1.0f - xx*q.X - yy*q.Y;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// slerp
//
// Spherical linear interpolation.
//
// See 10.4.13
public Quaternion slerp(Quaternion q0, Quaternion q1, float t)
{
// Check for out-of range parameter and return edge points if so
if (t <= 0.0f) return q0;
if (t >= 1.0f) return q1;
// Compute "cosine of angle between quaternions" using dot product
float cosOmega = dotProduct(q0, q1);
// If negative dot, use -q1. Two quaternions q and -q
// represent the same rotation, but may produce
// different slerp. We chose q or -q to rotate using
// the acute angle.
float q1w = q1.w;
float q1x = q1.x;
float q1y = q1.y;
float q1z = q1.z;
if (cosOmega < 0.0f) {
q1w = -q1w;
q1x = -q1x;
q1y = -q1y;
q1z = -q1z;
cosOmega = -cosOmega;
}
// We should have two unit quaternions, so dot should be <= 1.0
Trace.Assert(cosOmega < 1.1f, "error");
// Compute interpolation fraction, checking for quaternions
// almost exactly the same
float k0, k1;
if (cosOmega > 0.9999f) {
// Very close - just use linear interpolation,
// which will protect againt a divide by zero
k0 = 1.0f-t;
k1 = t;
} else {
// Compute the sin of the angle using the
// trig identity sin^2(omega) + cos^2(omega) = 1
float sinOmega = (float)Math.Sqrt(1.0f - cosOmega*cosOmega);
// Compute the angle from its sin and cosine
float omega = (float)Math.Atan2(sinOmega, cosOmega);
// Compute inverse of denominator, so we only have
// to divide once
float oneOverSinOmega = 1.0f / sinOmega;
// Compute interpolation parameters
k0 = (float)Math.Sin((1.0f - t) * omega) * oneOverSinOmega;
k1 = (float)Math.Sin(t * omega) * oneOverSinOmega;
}
// Interpolate
Quaternion result = new Quaternion();
result.x = k0*q0.x + k1*q1x;
result.y = k0*q0.y + k1*q1y;
result.z = k0*q0.z + k1*q1z;
result.w = k0*q0.w + k1*q1w;
// Return it
return result;
}
//---------------------------------------------------------------------------
// pow
//
// Quaternion exponentiation.
//
// See 10.4.12
public Quaternion pow(Quaternion q, float exponent)
{
// Check for the case of an identity quaternion.
// This will protect against divide by zero
if (Math.Abs(q.w) > .9999f) {
return q;
}
// Extract the half angle alpha (alpha = theta/2)
float alpha = (float)Math.Acos(q.w);
// Compute new alpha value
float newAlpha = alpha * exponent;
// Compute new w value
Quaternion result = new Quaternion();
result.w = (float)Math.Cos(newAlpha);
// Compute new xyz values
float mult = (float)Math.Sin(newAlpha) / (float)Math.Sin(alpha);
result.x = q.x * mult;
result.y = q.y * mult;
result.z = q.z * mult;
// Return it
return result;
}
/////////////////////////////////////////////////////////////////////////////
//
// Nonmember functions
//
/////////////////////////////////////////////////////////////////////////////
//---------------------------------------------------------------------------
// dotProduct
//
// Quaternion dot product. We use a nonmember function so we can
// pass quaternion expressions as operands without having "funky syntax"
//
// See 10.4.10
public float dotProduct(Quaternion a, Quaternion b)
{
return a.W*b.W + a.X*b.X + a.Y*b.Y + a.Z*b.Z;
}