| 2 |
bj |
1 |
#include "Mat4x4f.h"
|
|
|
2 |
#include "Mat3x3f.h"
|
|
|
3 |
|
|
|
4 |
namespace CGLA {
|
|
|
5 |
|
|
|
6 |
namespace
|
|
|
7 |
{
|
|
|
8 |
|
|
|
9 |
/* Aux func. computes 3x3 determinants. */
|
|
|
10 |
inline float d3x3f( float a1, float a2, float a3,
|
|
|
11 |
float b1, float b2, float b3,
|
|
|
12 |
float c1, float c2, float c3 )
|
|
|
13 |
{
|
|
|
14 |
return determinant(Mat3x3f(Vec3f(a1,a2,a3),
|
|
|
15 |
Vec3f(b1,b2,b3),
|
|
|
16 |
Vec3f(c1,c2,c3)));
|
|
|
17 |
}
|
|
|
18 |
}
|
|
|
19 |
|
|
|
20 |
float determinant(const Mat4x4f& m )
|
|
|
21 |
{
|
|
|
22 |
float ans;
|
|
|
23 |
float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
|
|
|
24 |
|
|
|
25 |
/* assign to individual variable names to aid selecting */
|
|
|
26 |
/* correct elements */
|
|
|
27 |
|
|
|
28 |
a1 = m[0][0]; b1 = m[0][1];
|
|
|
29 |
c1 = m[0][2]; d1 = m[0][3];
|
|
|
30 |
|
|
|
31 |
a2 = m[1][0]; b2 = m[1][1];
|
|
|
32 |
c2 = m[1][2]; d2 = m[1][3];
|
|
|
33 |
|
|
|
34 |
a3 = m[2][0]; b3 = m[2][1];
|
|
|
35 |
c3 = m[2][2]; d3 = m[2][3];
|
|
|
36 |
|
|
|
37 |
a4 = m[3][0]; b4 = m[3][1];
|
|
|
38 |
c4 = m[3][2]; d4 = m[3][3];
|
|
|
39 |
|
|
|
40 |
ans =
|
|
|
41 |
a1 * d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4)
|
|
|
42 |
- b1 * d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4)
|
|
|
43 |
+ c1 * d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4)
|
|
|
44 |
- d1 * d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);
|
|
|
45 |
return ans;
|
|
|
46 |
}
|
|
|
47 |
|
|
|
48 |
|
|
|
49 |
Mat4x4f invert_affine(const Mat4x4f& this_mat)
|
|
|
50 |
{
|
|
|
51 |
// The following code has been copied from a gem in Graphics Gems II by
|
|
|
52 |
// Kevin Wu.
|
|
|
53 |
// The function is very fast, but it can only invert affine matrices. An
|
|
|
54 |
// exception NotAffine is thrown if the matrix is not affine, and another
|
|
|
55 |
// exception Singular is thrown if the matrix is singular.
|
|
|
56 |
|
|
|
57 |
Mat4x4f new_mat;
|
|
|
58 |
float det_1;
|
|
|
59 |
float pos, neg, temp;
|
|
|
60 |
|
|
|
61 |
|
|
|
62 |
if (!(is_tiny(this_mat[3][0]) &&
|
|
|
63 |
is_tiny(this_mat[3][1]) &&
|
|
|
64 |
is_tiny(this_mat[3][2]) &&
|
|
|
65 |
is_tiny(this_mat[3][3]-1.0f)))
|
|
|
66 |
throw(Mat4x4fNotAffine("Can only invert affine matrices"));
|
|
|
67 |
|
|
|
68 |
#define ACCUMULATE if (temp >= 0.0) pos += temp; else neg += temp
|
|
|
69 |
|
|
|
70 |
/*
|
|
|
71 |
* Calculate the determinant of submatrix A and determine if the
|
|
|
72 |
* the matrix is singular as limited by the float precision
|
|
|
73 |
* floating-point this_mat representation.
|
|
|
74 |
*/
|
|
|
75 |
|
|
|
76 |
pos = neg = 0.0;
|
|
|
77 |
temp = this_mat[0][0] * this_mat[1][1] * this_mat[2][2];
|
|
|
78 |
ACCUMULATE;
|
|
|
79 |
temp = this_mat[1][0] * this_mat[2][1] * this_mat[0][2];
|
|
|
80 |
ACCUMULATE;
|
|
|
81 |
temp = this_mat[2][0] * this_mat[0][1] * this_mat[1][2];
|
|
|
82 |
ACCUMULATE;
|
|
|
83 |
temp = -this_mat[2][0] * this_mat[1][1] * this_mat[0][2];
|
|
|
84 |
ACCUMULATE;
|
|
|
85 |
temp = -this_mat[1][0] * this_mat[0][1] * this_mat[2][2];
|
|
|
86 |
ACCUMULATE;
|
|
|
87 |
temp = -this_mat[0][0] * this_mat[2][1] * this_mat[1][2];
|
|
|
88 |
ACCUMULATE;
|
|
|
89 |
det_1 = pos + neg;
|
|
|
90 |
|
|
|
91 |
/* Is the submatrix A singular? */
|
|
|
92 |
if ((det_1 == 0.0) || (fabs(det_1 / (pos - neg)) < MINUTE))
|
|
|
93 |
{
|
|
|
94 |
/* Mat4x4f M has no inverse */
|
|
|
95 |
throw(Mat4x4fSingular("Tried to invert Singular matrix"));
|
|
|
96 |
}
|
|
|
97 |
|
|
|
98 |
else {
|
|
|
99 |
|
|
|
100 |
/* Calculate inverse(A) = adj(A) / det(A) */
|
|
|
101 |
det_1 = 1.0 / det_1;
|
|
|
102 |
new_mat[0][0] = ( this_mat[1][1] * this_mat[2][2] -
|
|
|
103 |
this_mat[2][1] * this_mat[1][2] )
|
|
|
104 |
* det_1;
|
|
|
105 |
new_mat[0][1] = - ( this_mat[0][1] * this_mat[2][2] -
|
|
|
106 |
this_mat[2][1] * this_mat[0][2] )
|
|
|
107 |
* det_1;
|
|
|
108 |
new_mat[0][2] = ( this_mat[0][1] * this_mat[1][2] -
|
|
|
109 |
this_mat[1][1] * this_mat[0][2] )
|
|
|
110 |
* det_1;
|
|
|
111 |
new_mat[1][0] = - ( this_mat[1][0] * this_mat[2][2] -
|
|
|
112 |
this_mat[2][0] * this_mat[1][2] )
|
|
|
113 |
* det_1;
|
|
|
114 |
new_mat[1][1] = ( this_mat[0][0] * this_mat[2][2] -
|
|
|
115 |
this_mat[2][0] * this_mat[0][2] )
|
|
|
116 |
* det_1;
|
|
|
117 |
new_mat[1][2] = - ( this_mat[0][0] * this_mat[1][2] -
|
|
|
118 |
this_mat[1][0] * this_mat[0][2] )
|
|
|
119 |
* det_1;
|
|
|
120 |
new_mat[2][0] = ( this_mat[1][0] * this_mat[2][1] -
|
|
|
121 |
this_mat[2][0] * this_mat[1][1] )
|
|
|
122 |
* det_1;
|
|
|
123 |
new_mat[2][1] = - ( this_mat[0][0] * this_mat[2][1] -
|
|
|
124 |
this_mat[2][0] * this_mat[0][1] )
|
|
|
125 |
* det_1;
|
|
|
126 |
new_mat[2][2] = ( this_mat[0][0] * this_mat[1][1] -
|
|
|
127 |
this_mat[1][0] * this_mat[0][1] )
|
|
|
128 |
* det_1;
|
|
|
129 |
|
|
|
130 |
/* Calculate -C * inverse(A) */
|
|
|
131 |
new_mat[0][3] = - ( this_mat[0][3] * new_mat[0][0] +
|
|
|
132 |
this_mat[1][3] * new_mat[0][1] +
|
|
|
133 |
this_mat[2][3] * new_mat[0][2] );
|
|
|
134 |
new_mat[1][3] = - ( this_mat[0][3] * new_mat[1][0] +
|
|
|
135 |
this_mat[1][3] * new_mat[1][1] +
|
|
|
136 |
this_mat[2][3] * new_mat[1][2] );
|
|
|
137 |
new_mat[2][3] = - ( this_mat[0][3] * new_mat[2][0] +
|
|
|
138 |
this_mat[1][3] * new_mat[2][1] +
|
|
|
139 |
this_mat[2][3] * new_mat[2][2] );
|
|
|
140 |
|
|
|
141 |
/* Fill in last column */
|
|
|
142 |
new_mat[3][0] = new_mat[3][1] = new_mat[3][2] = 0.0;
|
|
|
143 |
new_mat[3][3] = 1.0;
|
|
|
144 |
|
|
|
145 |
return new_mat;
|
|
|
146 |
|
|
|
147 |
}
|
|
|
148 |
|
|
|
149 |
#undef ACCUMULATE
|
|
|
150 |
}
|
|
|
151 |
|
|
|
152 |
|
|
|
153 |
|
|
|
154 |
|
|
|
155 |
Mat4x4f rotation_Mat4x4f(Axis axis, float angle)
|
|
|
156 |
{
|
|
|
157 |
Mat4x4f m;
|
|
|
158 |
|
|
|
159 |
switch(axis)
|
|
|
160 |
{
|
|
|
161 |
case XAXIS:
|
|
|
162 |
m[0][0] = 1.0;
|
|
|
163 |
m[1][1] = cos(angle);
|
|
|
164 |
m[1][2] = sin(angle);
|
|
|
165 |
m[2][1] = -sin(angle);
|
|
|
166 |
m[2][2] = cos(angle);
|
|
|
167 |
m[3][3] = 1.0;
|
|
|
168 |
break;
|
|
|
169 |
case YAXIS:
|
|
|
170 |
m[0][0] = cos(angle);
|
|
|
171 |
m[0][2] = -sin(angle);
|
|
|
172 |
m[2][0] = sin(angle);
|
|
|
173 |
m[2][2] = cos(angle);
|
|
|
174 |
m[1][1] = 1.0;
|
|
|
175 |
m[3][3] = 1.0;
|
|
|
176 |
break;
|
|
|
177 |
case ZAXIS:
|
|
|
178 |
m[0][0] = cos(angle);
|
|
|
179 |
m[0][1] = sin(angle);
|
|
|
180 |
m[1][0] = -sin(angle);
|
|
|
181 |
m[1][1] = cos(angle);
|
|
|
182 |
m[2][2] = 1.0;
|
|
|
183 |
m[3][3] = 1.0;
|
|
|
184 |
break;
|
|
|
185 |
}
|
|
|
186 |
|
|
|
187 |
return m;
|
|
|
188 |
}
|
|
|
189 |
|
|
|
190 |
Mat4x4f translation_Mat4x4f(const Vec3f& v)
|
|
|
191 |
{
|
|
|
192 |
Mat4x4f m;
|
|
|
193 |
|
|
|
194 |
m[0][0] = 1.0;
|
|
|
195 |
m[1][1] = 1.0;
|
|
|
196 |
m[2][2] = 1.0;
|
|
|
197 |
m[3][3] = 1.0;
|
|
|
198 |
|
|
|
199 |
m[0][3] = v[0];
|
|
|
200 |
m[1][3] = v[1];
|
|
|
201 |
m[2][3] = v[2];
|
|
|
202 |
|
|
|
203 |
return m;
|
|
|
204 |
}
|
|
|
205 |
|
|
|
206 |
Mat4x4f scaling_Mat4x4f(const Vec3f& v)
|
|
|
207 |
{
|
|
|
208 |
Mat4x4f m;
|
|
|
209 |
|
|
|
210 |
m[0][0] = v[0];
|
|
|
211 |
m[1][1] = v[1];
|
|
|
212 |
m[2][2] = v[2];
|
|
|
213 |
m[3][3] = 1.0;
|
|
|
214 |
|
|
|
215 |
return m;
|
|
|
216 |
}
|
|
|
217 |
|
|
|
218 |
Mat4x4f perspective_Mat4x4f(float d)
|
|
|
219 |
{
|
|
|
220 |
Mat4x4f m;
|
|
|
221 |
|
|
|
222 |
/* Eye at the origin, looking down the negative z axis */
|
|
|
223 |
|
|
|
224 |
m[0][0] = 1.0;
|
|
|
225 |
m[1][1] = 1.0;
|
|
|
226 |
m[2][2] = 1.0;
|
|
|
227 |
m[3][2] = -1.0/d;
|
|
|
228 |
|
|
|
229 |
return m;
|
|
|
230 |
}
|
|
|
231 |
|
|
|
232 |
|
|
|
233 |
Mat4x4f adjoint(const Mat4x4f& in)
|
|
|
234 |
{
|
|
|
235 |
float a1, a2, a3, a4, b1, b2, b3, b4;
|
|
|
236 |
float c1, c2, c3, c4, d1, d2, d3, d4;
|
|
|
237 |
|
|
|
238 |
/* assign to individual variable names to aid */
|
|
|
239 |
/* selecting correct values */
|
|
|
240 |
|
|
|
241 |
a1 = in[0][0]; b1 = in[0][1];
|
|
|
242 |
c1 = in[0][2]; d1 = in[0][3];
|
|
|
243 |
|
|
|
244 |
a2 = in[1][0]; b2 = in[1][1];
|
|
|
245 |
c2 = in[1][2]; d2 = in[1][3];
|
|
|
246 |
|
|
|
247 |
a3 = in[2][0]; b3 = in[2][1];
|
|
|
248 |
c3 = in[2][2]; d3 = in[2][3];
|
|
|
249 |
|
|
|
250 |
a4 = in[3][0]; b4 = in[3][1];
|
|
|
251 |
c4 = in[3][2]; d4 = in[3][3];
|
|
|
252 |
|
|
|
253 |
|
|
|
254 |
/* row column labeling reversed since we transpose rows & columns */
|
|
|
255 |
|
|
|
256 |
Mat4x4f out;
|
|
|
257 |
out[0][0] = d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4);
|
|
|
258 |
out[1][0] = - d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4);
|
|
|
259 |
out[2][0] = d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4);
|
|
|
260 |
out[3][0] = - d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);
|
|
|
261 |
|
|
|
262 |
out[0][1] = - d3x3f( b1, b3, b4, c1, c3, c4, d1, d3, d4);
|
|
|
263 |
out[1][1] = d3x3f( a1, a3, a4, c1, c3, c4, d1, d3, d4);
|
|
|
264 |
out[2][1] = - d3x3f( a1, a3, a4, b1, b3, b4, d1, d3, d4);
|
|
|
265 |
out[3][1] = d3x3f( a1, a3, a4, b1, b3, b4, c1, c3, c4);
|
|
|
266 |
|
|
|
267 |
out[0][2] = d3x3f( b1, b2, b4, c1, c2, c4, d1, d2, d4);
|
|
|
268 |
out[1][2] = - d3x3f( a1, a2, a4, c1, c2, c4, d1, d2, d4);
|
|
|
269 |
out[2][2] = d3x3f( a1, a2, a4, b1, b2, b4, d1, d2, d4);
|
|
|
270 |
out[3][2] = - d3x3f( a1, a2, a4, b1, b2, b4, c1, c2, c4);
|
|
|
271 |
|
|
|
272 |
out[0][3] = - d3x3f( b1, b2, b3, c1, c2, c3, d1, d2, d3);
|
|
|
273 |
out[1][3] = d3x3f( a1, a2, a3, c1, c2, c3, d1, d2, d3);
|
|
|
274 |
out[2][3] = - d3x3f( a1, a2, a3, b1, b2, b3, d1, d2, d3);
|
|
|
275 |
out[3][3] = d3x3f( a1, a2, a3, b1, b2, b3, c1, c2, c3);
|
|
|
276 |
|
|
|
277 |
return out;
|
|
|
278 |
}
|
|
|
279 |
|
|
|
280 |
|
|
|
281 |
Mat4x4f invert( const Mat4x4f& in)
|
|
|
282 |
{
|
|
|
283 |
float det = determinant( in );
|
|
|
284 |
if (is_tiny(det))
|
|
|
285 |
throw(Mat4x4fSingular("Tried to invert Singular matrix"));
|
|
|
286 |
Mat4x4f out = adjoint(in);
|
|
|
287 |
out/=det;
|
|
|
288 |
return out;
|
|
|
289 |
}
|
|
|
290 |
|
|
|
291 |
|
|
|
292 |
}
|