//---------------------------------------------------------------------------
// fromRotationMatrix
//
// Setup the Euler angles, given a rotation matrix.
//
// See 10.6.2 for more information.
void fromRotationMatrix(RotationMatrix m)
{
// Extract sin(pitch) from m23.
float sp = -m.M23;
// Check for Gimbel lock
if (Math.Abs(sp) > 9.99999f) {
// Looking straight up or down
pitch = MathUtil.kPiOver2 * sp;
// Compute heading, slam bank to zero
heading = (float)Math.Atan2(-m.M31, m.M11);
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
heading = (float)Math.Atan2(m.M13, m.M33);
pitch = (float)Math.Asin(sp);
bank = (float)Math.Atan2(m.M21, m.M22);
}
}
public void setupParentToLocal(Vector3 pos, RotationMatrix orient)
{
// Copy the rotation portion of the matrix. We can copy the
// elements directly (without transposing) according
// to the layout as commented in RotationMatrix.cpp
m11 = orient.M11; m12 = orient.M12; m13 = orient.M13;
m21 = orient.M21; m22 = orient.M22; m23 = orient.M23;
m31 = orient.M31; m32 = orient.M32; m33 = orient.M33;
// Now set the translation portion. Normally, we would
// translate by the negative of the position to translate
// from world to inertial space. However, we must correct
// for the fact that the rotation occurs "first." So we
// must rotate the translation portion. This is the same
// as create a translation matrix T to translate by -pos,
// and a rotation matrix R, and then creating the matrix
// as the concatenation of TR
tx = -(pos.X*m11 + pos.Y*m21 + pos.Z*m31);
ty = -(pos.X*m12 + pos.Y*m22 + pos.Z*m32);
tz = -(pos.X*m13 + pos.Y*m23 + pos.Z*m33);
}
public void setupLocalToParent(Vector3 pos, RotationMatrix orient)
{
// Copy the rotation portion of the matrix. According to
// the comments in RotationMatrix.cpp, the rotation matrix
// is "normally" an inertial->object matrix, which is
// parent->local. We want a local->parent rotation, so we
// must transpose while copying
m11 = orient.M11; m12 = orient.M21; m13 = orient.M31;
m21 = orient.M12; m22 = orient.M22; m23 = orient.M32;
m31 = orient.M13; m32 = orient.M23; m33 = orient.M33;
// Now set the translation portion. Translation happens "after"
// the 3x3 portion, so we can simply copy the position
// field directly
tx = pos.X; ty = pos.Y; tz = pos.Z;
}
//---------------------------------------------------------------------------
// setupParentToLocal
//
// Setup the matrix to perform a parent -> local transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a world -> object matrix.
// To perform this transformation, we would normally FIRST transform
// from world to inertial space, and then rotate from inertial space into
// object space. However, out 4x3 matrix always translates last. So
// we think about creating two matrices T and R, and then concatonating
// M = TR.
//
// We allow the orientation to be specified using either euler angles,
// or a RotationMatrix
public void setupParentToLocal(Vector3 pos, EulerAngles orient)
{
// Create a rotation matrix.
RotationMatrix orientMatrix = new RotationMatrix();
orientMatrix.setup(orient);
// Setup the 4x3 matrix.
setupParentToLocal(pos, orientMatrix);
}
//---------------------------------------------------------------------------
// setupLocalToParent
//
// Setup the matrix to perform a local -> parent transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a object -> world matrix.
// As an example, the transformation in this case is straightforward. We
// first rotate from object space into inertial space, then we translate
// into world space.
//
// We allow the orientation to be specified using either euler angles,
// or a RotationMatrix
public void setupLocalToParent(Vector3 pos, EulerAngles orient)
{
// Create a rotation matrix.
RotationMatrix orientMatrix = new RotationMatrix();
orientMatrix.setup(orient);
// Setup the 4x3 matrix. Note: if we were really concerned with
// speed, we could create the matrix directly into these variables,
// without using the temporary RotationMatrix object. This would
// save us a function call and a few copy operations.
setupLocalToParent(pos, orientMatrix);
}