/lib/ode/ode_source/OPCODE/Ice/IceMatrix4x4.cpp

  1///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  2/**
  3 *	Contains code for 4x4 matrices.
  4 *	\file		IceMatrix4x4.cpp
  5 *	\author		Pierre Terdiman
  6 *	\date		April, 4, 2000
  7 */
  8///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  9
 10///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 11/**
 12 *	4x4 matrix.
 13 *	DirectX-compliant, ie row-column order, ie m[Row][Col].
 14 *	Same as:
 15 *	m11  m12  m13  m14	first row.
 16 *	m21  m22  m23  m24	second row.
 17 *	m31  m32  m33  m34	third row.
 18 *	m41  m42  m43  m44	fourth row.
 19 *	Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
 20 *	Stored in memory as m11 m12 m13 m14 m21...
 21 *
 22 *	Multiplication rules:
 23 *
 24 *	[x'y'z'1] = [xyz1][M]
 25 *
 26 *	x' = x*m11 + y*m21 + z*m31 + m41
 27 *	y' = x*m12 + y*m22 + z*m32 + m42
 28 *	z' = x*m13 + y*m23 + z*m33 + m43
 29 *	1' =     0 +     0 +     0 + m44
 30 *
 31 *	\class		Matrix4x4
 32 *	\author		Pierre Terdiman
 33 *	\version	1.0
 34 */
 35///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 36
 37///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 38// Precompiled Header
 39#include "Stdafx.h"
 40
 41using namespace IceMaths;
 42
 43///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 44/**
 45 *	Inverts a PR matrix. (which only contains a rotation and a translation)
 46 *	This is faster and less subject to FPU errors than the generic inversion code.
 47 *
 48 *	\relates	Matrix4x4
 49 *	\fn			InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
 50 *	\param		dest	[out] destination matrix
 51 *	\param		src		[in] source matrix
 52 */
 53///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 54ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
 55{
 56	dest.m[0][0] = src.m[0][0];
 57	dest.m[1][0] = src.m[0][1];
 58	dest.m[2][0] = src.m[0][2];
 59	dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
 60
 61	dest.m[0][1] = src.m[1][0];
 62	dest.m[1][1] = src.m[1][1];
 63	dest.m[2][1] = src.m[1][2];
 64	dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
 65
 66	dest.m[0][2] = src.m[2][0];
 67	dest.m[1][2] = src.m[2][1];
 68	dest.m[2][2] = src.m[2][2];
 69	dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
 70
 71	dest.m[0][3] = 0.0f;
 72	dest.m[1][3] = 0.0f;
 73	dest.m[2][3] = 0.0f;
 74	dest.m[3][3] = 1.0f;
 75}
 76
 77///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 78// Compute the cofactor of the Matrix at a specified location
 79///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 80float Matrix4x4::CoFactor(udword row, udword col) const
 81{
 82	return	 (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
 83				m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
 84				m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
 85			-  (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
 86				m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
 87				m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
 88}
 89
 90///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 91// Compute the determinant of the Matrix
 92///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 93float Matrix4x4::Determinant() const
 94{
 95	return	m[0][0] * CoFactor(0, 0) +
 96			m[0][1] * CoFactor(0, 1) +
 97			m[0][2] * CoFactor(0, 2) +
 98			m[0][3] * CoFactor(0, 3);
 99}
100
101///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102// Compute the inverse of the matrix
103///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104Matrix4x4& Matrix4x4::Invert()
105{
106	float Det = Determinant();
107	Matrix4x4 Temp;
108
109	if(fabsf(Det) < MATRIX4X4_EPSILON)
110		return	*this;		// The matrix is not invertible! Singular case!
111
112	float IDet = 1.0f / Det;
113
114	Temp.m[0][0] = CoFactor(0,0) * IDet;
115	Temp.m[1][0] = CoFactor(0,1) * IDet;
116	Temp.m[2][0] = CoFactor(0,2) * IDet;
117	Temp.m[3][0] = CoFactor(0,3) * IDet;
118	Temp.m[0][1] = CoFactor(1,0) * IDet;
119	Temp.m[1][1] = CoFactor(1,1) * IDet;
120	Temp.m[2][1] = CoFactor(1,2) * IDet;
121	Temp.m[3][1] = CoFactor(1,3) * IDet;
122	Temp.m[0][2] = CoFactor(2,0) * IDet;
123	Temp.m[1][2] = CoFactor(2,1) * IDet;
124	Temp.m[2][2] = CoFactor(2,2) * IDet;
125	Temp.m[3][2] = CoFactor(2,3) * IDet;
126	Temp.m[0][3] = CoFactor(3,0) * IDet;
127	Temp.m[1][3] = CoFactor(3,1) * IDet;
128	Temp.m[2][3] = CoFactor(3,2) * IDet;
129	Temp.m[3][3] = CoFactor(3,3) * IDet;
130
131	*this = Temp;
132
133	return	*this;
134}
135