| 2 |
bj |
1 |
#include "ArithSqMat4x4Float.h"
|
|
|
2 |
#include "Mat4x4f.h"
|
| 113 |
jab |
3 |
#include "Mat4x4d.h"
|
| 2 |
bj |
4 |
|
| 324 |
jab |
5 |
namespace CGLA {
|
|
|
6 |
|
| 2 |
bj |
7 |
namespace
|
|
|
8 |
{
|
|
|
9 |
|
|
|
10 |
/* Aux func. computes 3x3 determinants. */
|
|
|
11 |
template<class T>
|
|
|
12 |
inline T d3x3f( T a0, T a1, T a2,
|
|
|
13 |
T b0, T b1, T b2,
|
|
|
14 |
T c0, T c1, T c2 )
|
|
|
15 |
{
|
|
|
16 |
return a0*b1*c2 +
|
|
|
17 |
a1*b2*c0 +
|
|
|
18 |
a2*b0*c1 -
|
|
|
19 |
a2*b1*c0 -
|
|
|
20 |
a0*b2*c1 -
|
|
|
21 |
a1*b0*c2 ;
|
|
|
22 |
}
|
|
|
23 |
}
|
|
|
24 |
|
|
|
25 |
|
|
|
26 |
|
|
|
27 |
template<class V, class M>
|
|
|
28 |
M invert_affine(const ArithSqMat4x4Float<V,M>& this_mat)
|
|
|
29 |
{
|
| 327 |
jab |
30 |
// The following com[3]e has been copied from a gem in Graphics Gems II by
|
| 2 |
bj |
31 |
// Kevin Wu.
|
|
|
32 |
// The function is very fast, but it can only invert affine matrices. An
|
|
|
33 |
// exception NotAffine is thrown if the matrix is not affine, and another
|
|
|
34 |
// exception Singular is thrown if the matrix is singular.
|
|
|
35 |
|
|
|
36 |
typedef typename M::ScalarType ScalarType;
|
|
|
37 |
|
|
|
38 |
M new_mat;
|
|
|
39 |
ScalarType det_1;
|
|
|
40 |
ScalarType pos, neg, temp;
|
|
|
41 |
|
|
|
42 |
|
|
|
43 |
if (!(is_tiny(this_mat[3][0]) &&
|
|
|
44 |
is_tiny(this_mat[3][1]) &&
|
|
|
45 |
is_tiny(this_mat[3][2]) &&
|
| 5 |
jab |
46 |
is_tiny(this_mat[3][3]-1.0)))
|
| 2 |
bj |
47 |
throw(Mat4x4fNotAffine("Can only invert affine matrices"));
|
|
|
48 |
|
|
|
49 |
#define ACCUMULATE if (temp >= 0.0) pos += temp; else neg += temp
|
|
|
50 |
|
|
|
51 |
/*
|
|
|
52 |
* Calculate the determinant of submatrix A and determine if the
|
|
|
53 |
* the matrix is singular as limited by the float precision
|
|
|
54 |
* floating-point this_mat representation.
|
|
|
55 |
*/
|
|
|
56 |
|
|
|
57 |
pos = neg = 0.0;
|
|
|
58 |
temp = this_mat[0][0] * this_mat[1][1] * this_mat[2][2];
|
|
|
59 |
ACCUMULATE;
|
|
|
60 |
temp = this_mat[1][0] * this_mat[2][1] * this_mat[0][2];
|
|
|
61 |
ACCUMULATE;
|
|
|
62 |
temp = this_mat[2][0] * this_mat[0][1] * this_mat[1][2];
|
|
|
63 |
ACCUMULATE;
|
|
|
64 |
temp = -this_mat[2][0] * this_mat[1][1] * this_mat[0][2];
|
|
|
65 |
ACCUMULATE;
|
|
|
66 |
temp = -this_mat[1][0] * this_mat[0][1] * this_mat[2][2];
|
|
|
67 |
ACCUMULATE;
|
|
|
68 |
temp = -this_mat[0][0] * this_mat[2][1] * this_mat[1][2];
|
|
|
69 |
ACCUMULATE;
|
|
|
70 |
det_1 = pos + neg;
|
|
|
71 |
|
|
|
72 |
/* Is the submatrix A singular? */
|
|
|
73 |
if ((det_1 == 0.0) || (fabs(det_1 / (pos - neg)) < MINUTE))
|
|
|
74 |
{
|
|
|
75 |
/* Mat4x4f M has no inverse */
|
|
|
76 |
throw(Mat4x4fSingular("Tried to invert Singular matrix"));
|
|
|
77 |
}
|
|
|
78 |
|
|
|
79 |
else {
|
|
|
80 |
|
|
|
81 |
/* Calculate inverse(A) = adj(A) / det(A) */
|
|
|
82 |
det_1 = 1.0 / det_1;
|
|
|
83 |
new_mat[0][0] = ( this_mat[1][1] * this_mat[2][2] -
|
|
|
84 |
this_mat[2][1] * this_mat[1][2] )
|
|
|
85 |
* det_1;
|
|
|
86 |
new_mat[0][1] = - ( this_mat[0][1] * this_mat[2][2] -
|
|
|
87 |
this_mat[2][1] * this_mat[0][2] )
|
|
|
88 |
* det_1;
|
|
|
89 |
new_mat[0][2] = ( this_mat[0][1] * this_mat[1][2] -
|
|
|
90 |
this_mat[1][1] * this_mat[0][2] )
|
|
|
91 |
* det_1;
|
|
|
92 |
new_mat[1][0] = - ( this_mat[1][0] * this_mat[2][2] -
|
|
|
93 |
this_mat[2][0] * this_mat[1][2] )
|
|
|
94 |
* det_1;
|
|
|
95 |
new_mat[1][1] = ( this_mat[0][0] * this_mat[2][2] -
|
|
|
96 |
this_mat[2][0] * this_mat[0][2] )
|
|
|
97 |
* det_1;
|
|
|
98 |
new_mat[1][2] = - ( this_mat[0][0] * this_mat[1][2] -
|
|
|
99 |
this_mat[1][0] * this_mat[0][2] )
|
|
|
100 |
* det_1;
|
|
|
101 |
new_mat[2][0] = ( this_mat[1][0] * this_mat[2][1] -
|
|
|
102 |
this_mat[2][0] * this_mat[1][1] )
|
|
|
103 |
* det_1;
|
|
|
104 |
new_mat[2][1] = - ( this_mat[0][0] * this_mat[2][1] -
|
|
|
105 |
this_mat[2][0] * this_mat[0][1] )
|
|
|
106 |
* det_1;
|
|
|
107 |
new_mat[2][2] = ( this_mat[0][0] * this_mat[1][1] -
|
|
|
108 |
this_mat[1][0] * this_mat[0][1] )
|
|
|
109 |
* det_1;
|
|
|
110 |
|
|
|
111 |
/* Calculate -C * inverse(A) */
|
|
|
112 |
new_mat[0][3] = - ( this_mat[0][3] * new_mat[0][0] +
|
|
|
113 |
this_mat[1][3] * new_mat[0][1] +
|
|
|
114 |
this_mat[2][3] * new_mat[0][2] );
|
|
|
115 |
new_mat[1][3] = - ( this_mat[0][3] * new_mat[1][0] +
|
|
|
116 |
this_mat[1][3] * new_mat[1][1] +
|
|
|
117 |
this_mat[2][3] * new_mat[1][2] );
|
|
|
118 |
new_mat[2][3] = - ( this_mat[0][3] * new_mat[2][0] +
|
|
|
119 |
this_mat[1][3] * new_mat[2][1] +
|
|
|
120 |
this_mat[2][3] * new_mat[2][2] );
|
|
|
121 |
|
|
|
122 |
/* Fill in last column */
|
|
|
123 |
new_mat[3][0] = new_mat[3][1] = new_mat[3][2] = 0.0;
|
|
|
124 |
new_mat[3][3] = 1.0;
|
|
|
125 |
|
|
|
126 |
return new_mat;
|
|
|
127 |
|
|
|
128 |
}
|
|
|
129 |
|
|
|
130 |
#undef ACCUMULATE
|
|
|
131 |
}
|
|
|
132 |
|
|
|
133 |
template<class V, class M>
|
|
|
134 |
M adjoint(const ArithSqMat4x4Float<V,M>& in)
|
|
|
135 |
{
|
| 5 |
jab |
136 |
double a1, a2, a3, a4, b1, b2, b3, b4;
|
|
|
137 |
double c1, c2, c3, c4, d1, d2, d3, d4;
|
| 2 |
bj |
138 |
|
|
|
139 |
/* assign to individual variable names to aid */
|
|
|
140 |
/* selecting correct values */
|
|
|
141 |
|
|
|
142 |
a1 = in[0][0]; b1 = in[0][1];
|
|
|
143 |
c1 = in[0][2]; d1 = in[0][3];
|
|
|
144 |
|
|
|
145 |
a2 = in[1][0]; b2 = in[1][1];
|
|
|
146 |
c2 = in[1][2]; d2 = in[1][3];
|
|
|
147 |
|
|
|
148 |
a3 = in[2][0]; b3 = in[2][1];
|
|
|
149 |
c3 = in[2][2]; d3 = in[2][3];
|
|
|
150 |
|
|
|
151 |
a4 = in[3][0]; b4 = in[3][1];
|
|
|
152 |
c4 = in[3][2]; d4 = in[3][3];
|
|
|
153 |
|
|
|
154 |
|
|
|
155 |
/* row column labeling reversed since we transpose rows & columns */
|
|
|
156 |
|
|
|
157 |
M out;
|
|
|
158 |
out[0][0] = d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4);
|
|
|
159 |
out[1][0] = - d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4);
|
|
|
160 |
out[2][0] = d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4);
|
|
|
161 |
out[3][0] = - d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);
|
|
|
162 |
|
|
|
163 |
out[0][1] = - d3x3f( b1, b3, b4, c1, c3, c4, d1, d3, d4);
|
|
|
164 |
out[1][1] = d3x3f( a1, a3, a4, c1, c3, c4, d1, d3, d4);
|
|
|
165 |
out[2][1] = - d3x3f( a1, a3, a4, b1, b3, b4, d1, d3, d4);
|
|
|
166 |
out[3][1] = d3x3f( a1, a3, a4, b1, b3, b4, c1, c3, c4);
|
|
|
167 |
|
|
|
168 |
out[0][2] = d3x3f( b1, b2, b4, c1, c2, c4, d1, d2, d4);
|
|
|
169 |
out[1][2] = - d3x3f( a1, a2, a4, c1, c2, c4, d1, d2, d4);
|
|
|
170 |
out[2][2] = d3x3f( a1, a2, a4, b1, b2, b4, d1, d2, d4);
|
|
|
171 |
out[3][2] = - d3x3f( a1, a2, a4, b1, b2, b4, c1, c2, c4);
|
|
|
172 |
|
|
|
173 |
out[0][3] = - d3x3f( b1, b2, b3, c1, c2, c3, d1, d2, d3);
|
|
|
174 |
out[1][3] = d3x3f( a1, a2, a3, c1, c2, c3, d1, d2, d3);
|
|
|
175 |
out[2][3] = - d3x3f( a1, a2, a3, b1, b2, b3, d1, d2, d3);
|
|
|
176 |
out[3][3] = d3x3f( a1, a2, a3, b1, b2, b3, c1, c2, c3);
|
|
|
177 |
|
|
|
178 |
return out;
|
|
|
179 |
}
|
|
|
180 |
|
|
|
181 |
|
|
|
182 |
template<class V, class M>
|
|
|
183 |
M invert(const ArithSqMat4x4Float<V,M>& in)
|
|
|
184 |
{
|
| 5 |
jab |
185 |
double det = determinant( in );
|
| 2 |
bj |
186 |
if (is_tiny(det))
|
|
|
187 |
throw(Mat4x4fSingular("Tried to invert Singular matrix"));
|
|
|
188 |
M out = adjoint(in);
|
|
|
189 |
out/=det;
|
|
|
190 |
return out;
|
|
|
191 |
}
|
|
|
192 |
|
|
|
193 |
|
|
|
194 |
template class ArithSqMat4x4Float<Vec4f, Mat4x4f>;
|
|
|
195 |
template Mat4x4f adjoint(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);
|
|
|
196 |
template Mat4x4f invert(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);
|
|
|
197 |
template Mat4x4f invert_affine(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);
|
|
|
198 |
|
| 112 |
jab |
199 |
template class ArithSqMat4x4Float<Vec4d, Mat4x4d>;
|
|
|
200 |
template Mat4x4d adjoint(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
|
|
|
201 |
template Mat4x4d invert(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
|
|
|
202 |
template Mat4x4d invert_affine(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
|
| 2 |
bj |
203 |
|
| 112 |
jab |
204 |
|
| 2 |
bj |
205 |
}
|