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
41 using 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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
InvertPRMatrix(Matrix4x4 & dest,const Matrix4x4 & src)54 ICEMATHS_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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
CoFactor(udword row,udword col) const80 float 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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Determinant() const93 float 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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Invert()104 Matrix4x4& 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
136