vrshoot

view libs/vmath/matrix.cc @ 0:b2f14e535253

find changesets by author, revision, files, or words in the commit message
initial commit
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 01 Feb 2014 19:58:19 +0200
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 = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
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 // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
260 // article "Quaternion Calculus and Fast Animation".
261 // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
262 Quaternion Matrix3x3::get_rotation_quat() const
263 {
264 static const int next[3] = {1, 2, 0};
265
266 float quat[4];
267
268 scalar_t trace = m[0][0] + m[1][1] + m[2][2];
269 scalar_t root;
270
271 if(trace > 0.0f) {
272 // |w| > 1/2
273 root = sqrt(trace + 1.0f); // 2w
274 quat[0] = 0.5f * root;
275 root = 0.5f / root; // 1 / 4w
276 quat[1] = (m[2][1] - m[1][2]) * root;
277 quat[2] = (m[0][2] - m[2][0]) * root;
278 quat[3] = (m[1][0] - m[0][1]) * root;
279 } else {
280 // |w| <= 1/2
281 int i = 0;
282 if(m[1][1] > m[0][0]) {
283 i = 1;
284 }
285 if(m[2][2] > m[i][i]) {
286 i = 2;
287 }
288 int j = next[i];
289 int k = next[j];
290
291 root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
292 quat[i + 1] = 0.5f * root;
293 root = 0.5f / root;
294 quat[0] = (m[k][j] - m[j][k]) * root;
295 quat[j + 1] = (m[j][i] - m[i][j]) * root;
296 quat[k + 1] = (m[k][i] - m[i][k]) * root;
297 }
298 return Quaternion(quat[0], quat[1], quat[2], quat[3]);
299 }
300
301 void Matrix3x3::scale(const Vector3 &scale_vec)
302 {
303 Matrix3x3 smat( scale_vec.x, 0, 0,
304 0, scale_vec.y, 0,
305 0, 0, scale_vec.z);
306 *this *= smat;
307 }
308
309 void Matrix3x3::set_scaling(const Vector3 &scale_vec)
310 {
311 *this = Matrix3x3( scale_vec.x, 0, 0,
312 0, scale_vec.y, 0,
313 0, 0, scale_vec.z);
314 }
315
316 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
317 {
318 m[0][col_index] = vec.x;
319 m[1][col_index] = vec.y;
320 m[2][col_index] = vec.z;
321 }
322
323 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
324 {
325 m[row_index][0] = vec.x;
326 m[row_index][1] = vec.y;
327 m[row_index][2] = vec.z;
328 }
329
330 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
331 {
332 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
333 }
334
335 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
336 {
337 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
338 }
339
340 void Matrix3x3::transpose()
341 {
342 Matrix3x3 tmp = *this;
343 for(int i=0; i<3; i++) {
344 for(int j=0; j<3; j++) {
345 m[i][j] = tmp[j][i];
346 }
347 }
348 }
349
350 Matrix3x3 Matrix3x3::transposed() const
351 {
352 Matrix3x3 res;
353 for(int i=0; i<3; i++) {
354 for(int j=0; j<3; j++) {
355 res[i][j] = m[j][i];
356 }
357 }
358 return res;
359 }
360
361 scalar_t Matrix3x3::determinant() const
362 {
363 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
364 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
365 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
366 }
367
368 Matrix3x3 Matrix3x3::inverse() const
369 {
370 // TODO: implement 3x3 inverse
371 return *this;
372 }
373
374 ostream &operator <<(ostream &out, const Matrix3x3 &mat)
375 {
376 for(int i=0; i<3; i++) {
377 char str[100];
378 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
379 out << str;
380 }
381 return out;
382 }
383
384
385
386 /* ----------------- Matrix4x4 implementation --------------- */
387
388 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
389
390 Matrix4x4::Matrix4x4()
391 {
392 *this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
393 }
394
395 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
396 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
397 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
398 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
399 {
400 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
401 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
402 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
403 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
404 }
405
406 Matrix4x4::Matrix4x4(const mat4_t cmat)
407 {
408 memcpy(m, cmat, sizeof(mat4_t));
409 }
410
411 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
412 {
413 reset_identity();
414 for(int i=0; i<3; i++) {
415 for(int j=0; j<3; j++) {
416 m[i][j] = mat3x3[i][j];
417 }
418 }
419 }
420
421 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
422 {
423 Matrix4x4 res;
424 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
425 scalar_t *dest = res.m[0];
426
427 for(int i=0; i<16; i++) {
428 *dest++ = *op1++ + *op2++;
429 }
430 return res;
431 }
432
433 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
434 {
435 Matrix4x4 res;
436 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
437 scalar_t *dest = res.m[0];
438
439 for(int i=0; i<16; i++) {
440 *dest++ = *op1++ - *op2++;
441 }
442 return res;
443 }
444
445 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
446 {
447 scalar_t *op1 = m1.m[0];
448 const scalar_t *op2 = m2.m[0];
449
450 for(int i=0; i<16; i++) {
451 *op1++ += *op2++;
452 }
453 }
454
455 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
456 {
457 scalar_t *op1 = m1.m[0];
458 const scalar_t *op2 = m2.m[0];
459
460 for(int i=0; i<16; i++) {
461 *op1++ -= *op2++;
462 }
463 }
464
465 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
466 {
467 Matrix4x4 res;
468 const scalar_t *mptr = mat.m[0];
469 scalar_t *dptr = res.m[0];
470
471 for(int i=0; i<16; i++) {
472 *dptr++ = *mptr++ * scalar;
473 }
474 return res;
475 }
476
477 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
478 {
479 Matrix4x4 res;
480 const scalar_t *mptr = mat.m[0];
481 scalar_t *dptr = res.m[0];
482
483 for(int i=0; i<16; i++) {
484 *dptr++ = *mptr++ * scalar;
485 }
486 return res;
487 }
488
489 void operator *=(Matrix4x4 &mat, scalar_t scalar)
490 {
491 scalar_t *mptr = mat.m[0];
492
493 for(int i=0; i<16; i++) {
494 *mptr++ *= scalar;
495 }
496 }
497
498 void Matrix4x4::translate(const Vector3 &trans)
499 {
500 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
501 *this *= tmat;
502 }
503
504 void Matrix4x4::set_translation(const Vector3 &trans)
505 {
506 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
507 }
508
509 Vector3 Matrix4x4::get_translation() const
510 {
511 return Vector3(m[0][3], m[1][3], m[2][3]);
512 }
513
514 void Matrix4x4::rotate(const Vector3 &euler_angles)
515 {
516 Matrix3x3 xrot, yrot, zrot;
517
518 xrot = Matrix3x3( 1, 0, 0,
519 0, cos(euler_angles.x), -sin(euler_angles.x),
520 0, sin(euler_angles.x), cos(euler_angles.x));
521
522 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
523 0, 1, 0,
524 -sin(euler_angles.y), 0, cos(euler_angles.y));
525
526 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
527 sin(euler_angles.z), cos(euler_angles.z), 0,
528 0, 0, 1);
529
530 *this *= Matrix4x4(xrot * yrot * zrot);
531 }
532
533 void Matrix4x4::set_rotation(const Vector3 &euler_angles)
534 {
535 Matrix3x3 xrot, yrot, zrot;
536
537 xrot = Matrix3x3( 1, 0, 0,
538 0, cos(euler_angles.x), -sin(euler_angles.x),
539 0, sin(euler_angles.x), cos(euler_angles.x));
540
541 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
542 0, 1, 0,
543 -sin(euler_angles.y), 0, cos(euler_angles.y));
544
545 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
546 sin(euler_angles.z), cos(euler_angles.z), 0,
547 0, 0, 1);
548
549 *this = Matrix4x4(xrot * yrot * zrot);
550 }
551
552 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
553 {
554 scalar_t sina = (scalar_t)sin(angle);
555 scalar_t cosa = (scalar_t)cos(angle);
556 scalar_t invcosa = 1-cosa;
557 scalar_t nxsq = axis.x * axis.x;
558 scalar_t nysq = axis.y * axis.y;
559 scalar_t nzsq = axis.z * axis.z;
560
561 Matrix4x4 xform;
562 xform[0][0] = nxsq + (1-nxsq) * cosa;
563 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
564 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
565 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
566 xform[1][1] = nysq + (1-nysq) * cosa;
567 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
568 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
569 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
570 xform[2][2] = nzsq + (1-nzsq) * cosa;
571
572 *this *= xform;
573 }
574
575 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
576 {
577 scalar_t sina = (scalar_t)sin(angle);
578 scalar_t cosa = (scalar_t)cos(angle);
579 scalar_t invcosa = 1-cosa;
580 scalar_t nxsq = axis.x * axis.x;
581 scalar_t nysq = axis.y * axis.y;
582 scalar_t nzsq = axis.z * axis.z;
583
584 reset_identity();
585 m[0][0] = nxsq + (1-nxsq) * cosa;
586 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
587 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
588 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
589 m[1][1] = nysq + (1-nysq) * cosa;
590 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
591 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
592 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
593 m[2][2] = nzsq + (1-nzsq) * cosa;
594 }
595
596 void Matrix4x4::rotate(const Quaternion &quat)
597 {
598 *this *= quat.get_rotation_matrix();
599 }
600
601 void Matrix4x4::set_rotation(const Quaternion &quat)
602 {
603 *this = quat.get_rotation_matrix();
604 }
605
606 Quaternion Matrix4x4::get_rotation_quat() const
607 {
608 Matrix3x3 mat3 = *this;
609 return mat3.get_rotation_quat();
610 }
611
612 void Matrix4x4::scale(const Vector4 &scale_vec)
613 {
614 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
615 0, scale_vec.y, 0, 0,
616 0, 0, scale_vec.z, 0,
617 0, 0, 0, scale_vec.w);
618 *this *= smat;
619 }
620
621 void Matrix4x4::set_scaling(const Vector4 &scale_vec)
622 {
623 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
624 0, scale_vec.y, 0, 0,
625 0, 0, scale_vec.z, 0,
626 0, 0, 0, scale_vec.w);
627 }
628
629 Vector3 Matrix4x4::get_scaling() const
630 {
631 Vector3 vi = get_row_vector(0);
632 Vector3 vj = get_row_vector(1);
633 Vector3 vk = get_row_vector(2);
634
635 return Vector3(vi.length(), vj.length(), vk.length());
636 }
637
638 void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar)
639 {
640 float dx = right - left;
641 float dy = top - bottom;
642 float dz = zfar - znear;
643
644 float a = (right + left) / dx;
645 float b = (top + bottom) / dy;
646 float c = -(zfar + znear) / dz;
647 float d = -2.0 * zfar * znear / dz;
648
649 *this = Matrix4x4(2.0 * znear / dx, 0, a, 0,
650 0, 2.0 * znear / dy, b, 0,
651 0, 0, c, d,
652 0, 0, -1, 0);
653 }
654
655 void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
656 {
657 float f = 1.0f / tan(vfov * 0.5f);
658 float dz = znear - zfar;
659
660 reset_identity();
661
662 m[0][0] = f / aspect;
663 m[1][1] = f;
664 m[2][2] = (zfar + znear) / dz;
665 m[3][2] = -1.0f;
666 m[2][3] = 2.0f * zfar * znear / dz;
667 m[3][3] = 0.0f;
668 }
669
670 void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
671 {
672 float dx = right - left;
673 float dy = top - bottom;
674 float dz = zfar - znear;
675
676 reset_identity();
677
678 m[0][0] = 2.0 / dx;
679 m[1][1] = 2.0 / dy;
680 m[2][2] = -2.0 / dz;
681 m[0][3] = -(right + left) / dx;
682 m[1][3] = -(top + bottom) / dy;
683 m[2][3] = -(zfar + znear) / dz;
684 }
685
686 void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up)
687 {
688 Vector3 vk = (targ - pos).normalized();
689 Vector3 vj = up.normalized();
690 Vector3 vi = cross_product(vk, vj).normalized();
691 vj = cross_product(vi, vk);
692
693 *this = Matrix4x4(
694 vi.x, vi.y, vi.z, 0,
695 vj.x, vj.y, vj.z, 0,
696 -vk.x, -vk.y, -vk.z, 0,
697 0, 0, 0, 1);
698 translate(-pos);
699 }
700
701 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
702 {
703 m[0][col_index] = vec.x;
704 m[1][col_index] = vec.y;
705 m[2][col_index] = vec.z;
706 m[3][col_index] = vec.w;
707 }
708
709 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
710 {
711 m[row_index][0] = vec.x;
712 m[row_index][1] = vec.y;
713 m[row_index][2] = vec.z;
714 m[row_index][3] = vec.w;
715 }
716
717 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
718 {
719 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
720 }
721
722 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
723 {
724 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
725 }
726
727 void Matrix4x4::transpose()
728 {
729 Matrix4x4 tmp = *this;
730 for(int i=0; i<4; i++) {
731 for(int j=0; j<4; j++) {
732 m[i][j] = tmp[j][i];
733 }
734 }
735 }
736
737 Matrix4x4 Matrix4x4::transposed() const
738 {
739 Matrix4x4 res;
740 for(int i=0; i<4; i++) {
741 for(int j=0; j<4; j++) {
742 res[i][j] = m[j][i];
743 }
744 }
745 return res;
746 }
747
748 scalar_t Matrix4x4::determinant() const
749 {
750 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
751 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
752 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
753
754 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
755 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
756 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
757
758 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
759 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
760 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
761
762 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
763 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
764 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
765
766 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
767 }
768
769
770 Matrix4x4 Matrix4x4::adjoint() const
771 {
772 Matrix4x4 coef;
773
774 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
775 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
776 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
777 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
778 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
779 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
780 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
781 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
782 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
783 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
784 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
785 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
786
787 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
788 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
789 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
790 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
791 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
792 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
793 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
794 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
795 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
796 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
797 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
798 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
799
800 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
801 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
802 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
803 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
804 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
805 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
806 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
807 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
808 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
809 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
810 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
811 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
812
813 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
814 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
815 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
816 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
817 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
818 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
819 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
820 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
821 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
822 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
823 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
824 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
825
826 coef.transpose();
827
828 for(int i=0; i<4; i++) {
829 for(int j=0; j<4; j++) {
830 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
831 if(i%2) coef.m[i][j] = -coef.m[i][j];
832 }
833 }
834
835 return coef;
836 }
837
838 Matrix4x4 Matrix4x4::inverse() const
839 {
840 Matrix4x4 adj = adjoint();
841
842 return adj * (1.0f / determinant());
843 }
844
845 ostream &operator <<(ostream &out, const Matrix4x4 &mat)
846 {
847 for(int i=0; i<4; i++) {
848 char str[100];
849 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]);
850 out << str;
851 }
852 return out;
853 }