/lib/ode/ode_source/OPCODE/Ice/IceMatrix4x4.cpp
C++ | 135 lines | 63 code | 16 blank | 56 comment | 1 complexity | 90595b10e8077df3cacb235d3f829d39 MD5 | raw file
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