dungeon_crawler

view prototype/vmath/matrix.cc @ 1:96de911d05d4

find changesets by author, revision, files, or words in the commit message
started a rough prototype
author John Tsiombikas <nuclear@mutantstargoat.com>
date Thu, 28 Jun 2012 06:05:50 +0300
parents
children
line source
1 #include <cstdio>
2 #include <cmath>
3 #include "matrix.h"
4 #include "vector.h"
5 #include "quat.h"
6
7 using namespace std;
8
9 // ----------- Matrix3x3 --------------
10
11 Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
12
13 Matrix3x3::Matrix3x3()
14 {
15 *this = identity;
16 }
17
18 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,
19 scalar_t m21, scalar_t m22, scalar_t m23,
20 scalar_t m31, scalar_t m32, scalar_t m33)
21 {
22 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
23 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
24 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
25 }
26
27 Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
28 {
29 set_row_vector(ivec, 0);
30 set_row_vector(jvec, 1);
31 set_row_vector(kvec, 2);
32 }
33
34 Matrix3x3::Matrix3x3(const mat3_t cmat)
35 {
36 memcpy(m, cmat, sizeof(mat3_t));
37 }
38
39 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
40 {
41 for(int i=0; i<3; i++) {
42 for(int j=0; j<3; j++) {
43 m[i][j] = mat4x4[i][j];
44 }
45 }
46 }
47
48 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
49 {
50 Matrix3x3 res;
51 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
52 scalar_t *dest = res.m[0];
53
54 for(int i=0; i<9; i++) {
55 *dest++ = *op1++ + *op2++;
56 }
57 return res;
58 }
59
60 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
61 {
62 Matrix3x3 res;
63 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
64 scalar_t *dest = res.m[0];
65
66 for(int i=0; i<9; i++) {
67 *dest++ = *op1++ - *op2++;
68 }
69 return res;
70 }
71
72 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
73 {
74 Matrix3x3 res;
75 for(int i=0; i<3; i++) {
76 for(int j=0; j<3; j++) {
77 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
78 }
79 }
80 return res;
81 }
82
83 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
84 {
85 scalar_t *op1 = m1.m[0];
86 const scalar_t *op2 = m2.m[0];
87
88 for(int i=0; i<9; i++) {
89 *op1++ += *op2++;
90 }
91 }
92
93 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
94 {
95 scalar_t *op1 = m1.m[0];
96 const scalar_t *op2 = m2.m[0];
97
98 for(int i=0; i<9; i++) {
99 *op1++ -= *op2++;
100 }
101 }
102
103 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
104 {
105 Matrix3x3 res;
106 for(int i=0; i<3; i++) {
107 for(int j=0; j<3; j++) {
108 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j];
109 }
110 }
111 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
112 }
113
114 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
115 {
116 Matrix3x3 res;
117 const scalar_t *mptr = mat.m[0];
118 scalar_t *dptr = res.m[0];
119
120 for(int i=0; i<9; i++) {
121 *dptr++ = *mptr++ * scalar;
122 }
123 return res;
124 }
125
126 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
127 {
128 Matrix3x3 res;
129 const scalar_t *mptr = mat.m[0];
130 scalar_t *dptr = res.m[0];
131
132 for(int i=0; i<9; i++) {
133 *dptr++ = *mptr++ * scalar;
134 }
135 return res;
136 }
137
138 void operator *=(Matrix3x3 &mat, scalar_t scalar)
139 {
140 scalar_t *mptr = mat.m[0];
141
142 for(int i=0; i<9; i++) {
143 *mptr++ *= scalar;
144 }
145 }
146
147 void Matrix3x3::translate(const Vector2 &trans)
148 {
149 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
150 *this *= tmat;
151 }
152
153 void Matrix3x3::set_translation(const Vector2 &trans)
154 {
155 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
156 }
157
158 void Matrix3x3::rotate(scalar_t angle)
159 {
160 scalar_t cos_a = cos(angle);
161 scalar_t sin_a = sin(angle);
162 Matrix3x3 rmat( cos_a, -sin_a, 0,
163 sin_a, cos_a, 0,
164 0, 0, 1);
165 *this *= rmat;
166 }
167
168 void Matrix3x3::set_rotation(scalar_t angle)
169 {
170 scalar_t cos_a = cos(angle);
171 scalar_t sin_a = sin(angle);
172 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
173 }
174
175 void Matrix3x3::rotate(const Vector3 &euler_angles)
176 {
177 Matrix3x3 xrot, yrot, zrot;
178
179 xrot = Matrix3x3( 1, 0, 0,
180 0, cos(euler_angles.x), -sin(euler_angles.x),
181 0, sin(euler_angles.x), cos(euler_angles.x));
182
183 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
184 0, 1, 0,
185 -sin(euler_angles.y), 0, cos(euler_angles.y));
186
187 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
188 sin(euler_angles.z), cos(euler_angles.z), 0,
189 0, 0, 1);
190
191 *this *= xrot * yrot * zrot;
192 }
193
194 void Matrix3x3::set_rotation(const Vector3 &euler_angles)
195 {
196 Matrix3x3 xrot, yrot, zrot;
197
198 xrot = Matrix3x3( 1, 0, 0,
199 0, cos(euler_angles.x), -sin(euler_angles.x),
200 0, sin(euler_angles.x), cos(euler_angles.x));
201
202 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
203 0, 1, 0,
204 -sin(euler_angles.y), 0, cos(euler_angles.y));
205
206 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
207 sin(euler_angles.z), cos(euler_angles.z), 0,
208 0, 0, 1);
209
210 *this = xrot * yrot * zrot;
211 }
212
213 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
214 {
215 scalar_t sina = (scalar_t)sin(angle);
216 scalar_t cosa = (scalar_t)cos(angle);
217 scalar_t invcosa = 1-cosa;
218 scalar_t nxsq = axis.x * axis.x;
219 scalar_t nysq = axis.y * axis.y;
220 scalar_t nzsq = axis.z * axis.z;
221
222 Matrix3x3 xform;
223 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
224 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
225 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
226
227 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
228 xform.m[1][1] = nysq + (1-nysq) * cosa;
229 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
230
231 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
232 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
233 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
234
235 *this *= xform;
236 }
237
238 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
239 {
240 scalar_t sina = (scalar_t)sin(angle);
241 scalar_t cosa = (scalar_t)cos(angle);
242 scalar_t invcosa = 1-cosa;
243 scalar_t nxsq = axis.x * axis.x;
244 scalar_t nysq = axis.y * axis.y;
245 scalar_t nzsq = axis.z * axis.z;
246
247 reset_identity();
248 m[0][0] = nxsq + (1-nxsq) * cosa;
249 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
250 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
251 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
252 m[1][1] = nysq + (1-nysq) * cosa;
253 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
254 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
255 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
256 m[2][2] = nzsq + (1-nzsq) * cosa;
257 }
258
259 void Matrix3x3::scale(const Vector3 &scale_vec)
260 {
261 Matrix3x3 smat( scale_vec.x, 0, 0,
262 0, scale_vec.y, 0,
263 0, 0, scale_vec.z);
264 *this *= smat;
265 }
266
267 void Matrix3x3::set_scaling(const Vector3 &scale_vec)
268 {
269 *this = Matrix3x3( scale_vec.x, 0, 0,
270 0, scale_vec.y, 0,
271 0, 0, scale_vec.z);
272 }
273
274 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
275 {
276 m[0][col_index] = vec.x;
277 m[1][col_index] = vec.y;
278 m[2][col_index] = vec.z;
279 }
280
281 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
282 {
283 m[row_index][0] = vec.x;
284 m[row_index][1] = vec.y;
285 m[row_index][2] = vec.z;
286 }
287
288 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
289 {
290 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
291 }
292
293 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
294 {
295 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
296 }
297
298 void Matrix3x3::transpose()
299 {
300 Matrix3x3 tmp = *this;
301 for(int i=0; i<3; i++) {
302 for(int j=0; j<3; j++) {
303 m[i][j] = tmp[j][i];
304 }
305 }
306 }
307
308 Matrix3x3 Matrix3x3::transposed() const
309 {
310 Matrix3x3 res;
311 for(int i=0; i<3; i++) {
312 for(int j=0; j<3; j++) {
313 res[i][j] = m[j][i];
314 }
315 }
316 return res;
317 }
318
319 scalar_t Matrix3x3::determinant() const
320 {
321 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
322 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
323 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
324 }
325
326 Matrix3x3 Matrix3x3::inverse() const
327 {
328 // TODO: implement 3x3 inverse
329 return *this;
330 }
331
332 ostream &operator <<(ostream &out, const Matrix3x3 &mat)
333 {
334 for(int i=0; i<3; i++) {
335 char str[100];
336 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
337 out << str;
338 }
339 return out;
340 }
341
342
343
344 /* ----------------- Matrix4x4 implementation --------------- */
345
346 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
347
348 Matrix4x4::Matrix4x4()
349 {
350 *this = identity;
351 }
352
353 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
354 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
355 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
356 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
357 {
358 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
359 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
360 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
361 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
362 }
363
364 Matrix4x4::Matrix4x4(const mat4_t cmat)
365 {
366 memcpy(m, cmat, sizeof(mat4_t));
367 }
368
369 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
370 {
371 reset_identity();
372 for(int i=0; i<3; i++) {
373 for(int j=0; j<3; j++) {
374 m[i][j] = mat3x3[i][j];
375 }
376 }
377 }
378
379 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
380 {
381 Matrix4x4 res;
382 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
383 scalar_t *dest = res.m[0];
384
385 for(int i=0; i<16; i++) {
386 *dest++ = *op1++ + *op2++;
387 }
388 return res;
389 }
390
391 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
392 {
393 Matrix4x4 res;
394 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
395 scalar_t *dest = res.m[0];
396
397 for(int i=0; i<16; i++) {
398 *dest++ = *op1++ - *op2++;
399 }
400 return res;
401 }
402
403 /*
404 Matrix4x4 operator *(const Matrix4x4 &m1, const Matrix4x4 &m2) {
405 Matrix4x4 res;
406
407 for(int i=0; i<4; i++) {
408 for(int j=0; j<4; j++) {
409 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
410 }
411 }
412
413 return res;
414 }
415 */
416
417 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
418 {
419 scalar_t *op1 = m1.m[0];
420 const scalar_t *op2 = m2.m[0];
421
422 for(int i=0; i<16; i++) {
423 *op1++ += *op2++;
424 }
425 }
426
427 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
428 {
429 scalar_t *op1 = m1.m[0];
430 const scalar_t *op2 = m2.m[0];
431
432 for(int i=0; i<16; i++) {
433 *op1++ -= *op2++;
434 }
435 }
436
437 void operator *=(Matrix4x4 &m1, const Matrix4x4 &m2)
438 {
439 Matrix4x4 res;
440 for(int i=0; i<4; i++) {
441 for(int j=0; j<4; j++) {
442 res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j] + m1.m[i][3] * m2.m[3][j];
443 }
444 }
445 memcpy(m1.m, res.m, 16 * sizeof(scalar_t));
446 }
447
448 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
449 {
450 Matrix4x4 res;
451 const scalar_t *mptr = mat.m[0];
452 scalar_t *dptr = res.m[0];
453
454 for(int i=0; i<16; i++) {
455 *dptr++ = *mptr++ * scalar;
456 }
457 return res;
458 }
459
460 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
461 {
462 Matrix4x4 res;
463 const scalar_t *mptr = mat.m[0];
464 scalar_t *dptr = res.m[0];
465
466 for(int i=0; i<16; i++) {
467 *dptr++ = *mptr++ * scalar;
468 }
469 return res;
470 }
471
472 void operator *=(Matrix4x4 &mat, scalar_t scalar)
473 {
474 scalar_t *mptr = mat.m[0];
475
476 for(int i=0; i<16; i++) {
477 *mptr++ *= scalar;
478 }
479 }
480
481 void Matrix4x4::translate(const Vector3 &trans)
482 {
483 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
484 *this *= tmat;
485 }
486
487 void Matrix4x4::set_translation(const Vector3 &trans)
488 {
489 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
490 }
491
492 void Matrix4x4::rotate(const Vector3 &euler_angles)
493 {
494 Matrix3x3 xrot, yrot, zrot;
495
496 xrot = Matrix3x3( 1, 0, 0,
497 0, cos(euler_angles.x), -sin(euler_angles.x),
498 0, sin(euler_angles.x), cos(euler_angles.x));
499
500 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
501 0, 1, 0,
502 -sin(euler_angles.y), 0, cos(euler_angles.y));
503
504 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
505 sin(euler_angles.z), cos(euler_angles.z), 0,
506 0, 0, 1);
507
508 *this *= Matrix4x4(xrot * yrot * zrot);
509 }
510
511 void Matrix4x4::set_rotation(const Vector3 &euler_angles)
512 {
513 Matrix3x3 xrot, yrot, zrot;
514
515 xrot = Matrix3x3( 1, 0, 0,
516 0, cos(euler_angles.x), -sin(euler_angles.x),
517 0, sin(euler_angles.x), cos(euler_angles.x));
518
519 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
520 0, 1, 0,
521 -sin(euler_angles.y), 0, cos(euler_angles.y));
522
523 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
524 sin(euler_angles.z), cos(euler_angles.z), 0,
525 0, 0, 1);
526
527 *this = Matrix4x4(xrot * yrot * zrot);
528 }
529
530 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
531 {
532 scalar_t sina = (scalar_t)sin(angle);
533 scalar_t cosa = (scalar_t)cos(angle);
534 scalar_t invcosa = 1-cosa;
535 scalar_t nxsq = axis.x * axis.x;
536 scalar_t nysq = axis.y * axis.y;
537 scalar_t nzsq = axis.z * axis.z;
538
539 Matrix3x3 xform;
540 xform[0][0] = nxsq + (1-nxsq) * cosa;
541 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
542 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
543 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
544 xform[1][1] = nysq + (1-nysq) * cosa;
545 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
546 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
547 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
548 xform[2][2] = nzsq + (1-nzsq) * cosa;
549
550 *this *= Matrix4x4(xform);
551 }
552
553 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
554 {
555 scalar_t sina = (scalar_t)sin(angle);
556 scalar_t cosa = (scalar_t)cos(angle);
557 scalar_t invcosa = 1-cosa;
558 scalar_t nxsq = axis.x * axis.x;
559 scalar_t nysq = axis.y * axis.y;
560 scalar_t nzsq = axis.z * axis.z;
561
562 reset_identity();
563 m[0][0] = nxsq + (1-nxsq) * cosa;
564 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
565 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
566 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
567 m[1][1] = nysq + (1-nysq) * cosa;
568 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
569 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
570 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
571 m[2][2] = nzsq + (1-nzsq) * cosa;
572 }
573
574 void Matrix4x4::scale(const Vector4 &scale_vec)
575 {
576 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
577 0, scale_vec.y, 0, 0,
578 0, 0, scale_vec.z, 0,
579 0, 0, 0, scale_vec.w);
580 *this *= smat;
581 }
582
583 void Matrix4x4::set_scaling(const Vector4 &scale_vec)
584 {
585 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
586 0, scale_vec.y, 0, 0,
587 0, 0, scale_vec.z, 0,
588 0, 0, 0, scale_vec.w);
589 }
590
591 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
592 {
593 m[0][col_index] = vec.x;
594 m[1][col_index] = vec.y;
595 m[2][col_index] = vec.z;
596 m[3][col_index] = vec.w;
597 }
598
599 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
600 {
601 m[row_index][0] = vec.x;
602 m[row_index][1] = vec.y;
603 m[row_index][2] = vec.z;
604 m[row_index][3] = vec.w;
605 }
606
607 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
608 {
609 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
610 }
611
612 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
613 {
614 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
615 }
616
617 void Matrix4x4::transpose()
618 {
619 Matrix4x4 tmp = *this;
620 for(int i=0; i<4; i++) {
621 for(int j=0; j<4; j++) {
622 m[i][j] = tmp[j][i];
623 }
624 }
625 }
626
627 Matrix4x4 Matrix4x4::transposed() const
628 {
629 Matrix4x4 res;
630 for(int i=0; i<4; i++) {
631 for(int j=0; j<4; j++) {
632 res[i][j] = m[j][i];
633 }
634 }
635 return res;
636 }
637
638 scalar_t Matrix4x4::determinant() const
639 {
640 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
641 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
642 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
643
644 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
645 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
646 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
647
648 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
649 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
650 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
651
652 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
653 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
654 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
655
656 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
657 }
658
659
660 Matrix4x4 Matrix4x4::adjoint() const
661 {
662 Matrix4x4 coef;
663
664 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
665 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
666 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
667 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
668 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
669 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
670 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
671 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
672 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
673 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
674 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
675 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
676
677 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
678 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
679 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
680 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
681 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
682 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
683 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
684 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
685 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
686 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
687 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
688 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
689
690 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
691 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
692 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
693 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
694 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
695 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
696 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
697 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
698 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
699 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
700 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
701 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
702
703 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
704 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
705 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
706 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
707 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
708 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
709 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
710 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
711 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
712 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
713 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
714 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
715
716 coef.transpose();
717
718 for(int i=0; i<4; i++) {
719 for(int j=0; j<4; j++) {
720 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
721 if(i%2) coef.m[i][j] = -coef.m[i][j];
722 }
723 }
724
725 return coef;
726 }
727
728 Matrix4x4 Matrix4x4::inverse() const
729 {
730 Matrix4x4 adj = adjoint();
731
732 return adj * (1.0f / determinant());
733 }
734
735 ostream &operator <<(ostream &out, const Matrix4x4 &mat)
736 {
737 for(int i=0; i<4; i++) {
738 char str[100];
739 sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]);
740 out << str;
741 }
742 return out;
743 }