//---------------------------------------------------------------------------
// inverse
//
// Compute the inverse of a matrix. We use the classical adjoint divided
// by the determinant method.
//
// See 9.2.1 for more info.
Matrix4x3 inverse(Matrix4x3 m)
{
// Compute the determinant
float det = determinant(m);
// If we're singular, then the determinant is zero and there's
// no inverse
Trace.Assert(Math.Abs(det) > 0.000001f, "error");
// Compute one over the determinant, so we divide once and
// can *multiply* per element
float oneOverDet = 1.0f / det;
// Compute the 3x3 portion of the inverse, by
// dividing the adjoint by the determinant
Matrix4x3 r = new Matrix4x3();
r.M11 = (m.M22*m.m33 - m.M23*m.M32) * oneOverDet;
r.M12 = (m.M13*m.m32 - m.M12*m.M33) * oneOverDet;
r.M13 = (m.M12*m.m23 - m.M13*m.M22) * oneOverDet;
r.M21 = (m.M23*m.m31 - m.M21*m.M33) * oneOverDet;
r.M22 = (m.M11*m.m33 - m.M13*m.M31) * oneOverDet;
r.M23 = (m.M13*m.m21 - m.M11*m.M23) * oneOverDet;
r.M31 = (m.M21*m.m32 - m.M22*m.M31) * oneOverDet;
r.M32 = (m.M12*m.m31 - m.M11*m.M32) * oneOverDet;
r.M33 = (m.M11*m.m22 - m.M12*m.M21) * oneOverDet;
// Compute the translation portion of the inverse
r.tx = -(m.TX*r.M11 + m.TY*r.M21 + m.TZ*r.M31);
r.ty = -(m.TX*r.M12 + m.TY*r.M22 + m.TZ*r.M32);
r.tz = -(m.TX*r.M13 + m.TY*r.M23 + m.TZ*r.M33);
// Return it. Ouch - involves a copy constructor call. If speed
// is critical, we may need a seperate function which places the
// result where we want it...
return r;
}
//---------------------------------------------------------------------------
// fromWorldToObjectMatrix
//
// Setup the Euler angles, given a world->object transformation matrix.
//
// The matrix is assumed to be orthogonal. The translation portion is
// ignored.
//
// See 10.6.2 for more information.
void fromWorldToObjectMatrix(Matrix4x3 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);
}
}
//---------------------------------------------------------------------------
// getTranslation
//
// Return the translation row of the matrix in vector form
Vector3 getTranslation(Matrix4x3 m)
{
return new Vector3(m.TX, m.TY, m.TZ);
}
//---------------------------------------------------------------------------
// getPositionFromParentToLocalMatrix
//
// Extract the position of an object given a parent -> local transformation
// matrix (such as a world -> object matrix)
//
// We assume that the matrix represents a rigid transformation. (No scale,
// skew, or mirroring)
Vector3 getPositionFromParentToLocalMatrix(Matrix4x3 m)
{
// Multiply negative translation value by the
// transpose of the 3x3 portion. By using the transpose,
// we assume that the matrix is orthogonal. (This function
// doesn't really make sense for non-rigid transformations...)
return new Vector3(
-(m.TX*m.M11 + m.TY*m.M12 + m.TZ*m.M13),
-(m.TX*m.M21 + m.TY*m.M22 + m.TZ*m.M23),
-(m.TX*m.M31 + m.TY*m.M32 + m.TZ*m.M33)
);
}
//---------------------------------------------------------------------------
// getPositionFromLocalToParentMatrix
//
// Extract the position of an object given a local -> parent transformation
// matrix (such as an object -> world matrix)
Vector3 getPositionFromLocalToParentMatrix(Matrix4x3 m)
{
// Position is simply the translation portion
return new Vector3(m.TX, m.TY, m.TZ);
}
//---------------------------------------------------------------------------
// determinant
//
// Compute the determinant of the 3x3 portion of the matrix.
//
// See 9.1.1 for more info.
float determinant(Matrix4x3 m)
{
return
m.M11 * (m.M22*m.M33 - m.M23*m.M32)
+ m.M12 * (m.M23*m.M31 - m.M21*m.M33)
+ m.M13 * (m.M21*m.M32 - m.M22*m.M31);
}
//---------------------------------------------------------------------------
// Matrix4x3 * Matrix4x3
//
// Matrix concatenation. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//
// See 7.1.6
public static Matrix4x3 operator *(Matrix4x3 a, Matrix4x3 b)
{
Matrix4x3 r = new Matrix4x3();
// Compute the upper 3x3 (linear transformation) portion
r.M11 = a.M11*b.M11 + a.M12*b.M21 + a.M13*b.M31;
r.M12 = a.M11*b.M12 + a.M12*b.M22 + a.M13*b.M32;
r.M13 = a.M11*b.M13 + a.M12*b.M23 + a.M13*b.M33;
r.M21 = a.M21*b.M11 + a.M22*b.M21 + a.M23*b.M31;
r.M22 = a.M21*b.M12 + a.M22*b.M22 + a.M23*b.M32;
r.M23 = a.M21*b.M13 + a.M22*b.M23 + a.M23*b.M33;
r.M31 = a.M31*b.M11 + a.M32*b.M21 + a.M33*b.M31;
r.M32 = a.M31*b.M12 + a.M32*b.M22 + a.M33*b.M32;
r.M33 = a.M31*b.M13 + a.M32*b.M23 + a.M33*b.M33;
// Compute the translation portion
r.TX = a.TX*b.M11 + a.TY*b.M21 + a.TZ*b.M31 + b.TX;
r.TY = a.TX*b.M12 + a.TY*b.M22 + a.TZ*b.M32 + b.TY;
r.TZ = a.TX*b.M13 + a.TY*b.M23 + a.TZ*b.M33 + b.TZ;
// Return it. Ouch - involves a copy constructor call. If speed
// is critical, we may need a seperate function which places the
// result where we want it...
return r;
}