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