-
Notifications
You must be signed in to change notification settings - Fork 15
/
HMat44.cpp
97 lines (77 loc) · 2.8 KB
/
HMat44.cpp
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
#include "HMat44.h"
bool HMat44::InverseFastSelf(void)//
{
// 6*8+2*6 = 60 multiplications
// 2*1 = 2 divisions
FLOAT r0[2][2], r1[2][2], r2[2][2], r3[2][2];
FLOAT a, det, invDet;
FLOAT *mat = reinterpret_cast<FLOAT *>(this);
// r0 = m0.Inverse();
det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0];
if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
return false;
}
invDet = 1.0f / det;
r0[0][0] = mat[1*4+1] * invDet;
r0[0][1] = - mat[0*4+1] * invDet;
r0[1][0] = - mat[1*4+0] * invDet;
r0[1][1] = mat[0*4+0] * invDet;
// r1 = r0 * m1;
r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2];
r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3];
r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2];
r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3];
// r2 = m2 * r1;
r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0];
r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1];
r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0];
r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1];
// r3 = r2 - m3;
r3[0][0] = r2[0][0] - mat[2*4+2];
r3[0][1] = r2[0][1] - mat[2*4+3];
r3[1][0] = r2[1][0] - mat[3*4+2];
r3[1][1] = r2[1][1] - mat[3*4+3];
// r3.InverseSelf();
det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
return false;
}
invDet = 1.0f / det;
a = r3[0][0];
r3[0][0] = r3[1][1] * invDet;
r3[0][1] = - r3[0][1] * invDet;
r3[1][0] = - r3[1][0] * invDet;
r3[1][1] = a * invDet;
// r2 = m2 * r0;
r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0];
r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1];
r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0];
r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1];
// m2 = r3 * r2;
mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
// m0 = r0 - r1 * m2;
mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0];
mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1];
mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0];
mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1];
// m1 = r1 * r3;
mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
// m3 = -r3;
mat[2*4+2] = -r3[0][0];
mat[2*4+3] = -r3[0][1];
mat[3*4+2] = -r3[1][0];
mat[3*4+3] = -r3[1][1];
return true;
}
void HMat44::TransposeSelf()
{
for(int i = 0;i < 3;i++)
for(int j = i+1;j < 4;j++)
std::swap(this->mat[i][j],this->mat[i][j]);
}