3dphotoshoot

view libs/vmath/matrix.cc @ 13:8cfe061ddd5f

find changesets by author, revision, files, or words in the commit message
fixed the assets directories in the android makefile
author John Tsiombikas <nuclear@member.fsf.org>
date Sun, 07 Jun 2015 02:29:10 +0300
parents
children
line source
1 /*
2 libvmath - a vector math library
3 Copyright (C) 2004-2015 John Tsiombikas <nuclear@member.fsf.org>
4
5 This program is free software: you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as published
7 by the Free Software Foundation, either version 3 of the License, or
8 (at your option) any later version.
9
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14
15 You should have received a copy of the GNU Lesser General Public License
16 along with this program. If not, see <http://www.gnu.org/licenses/>.
17 */
18 #include <cstdio>
19 #include <cmath>
20 #include "matrix.h"
21 #include "vector.h"
22 #include "quat.h"
23
24 using namespace std;
25
26 // ----------- Matrix3x3 --------------
27
28 Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
29
30 Matrix3x3::Matrix3x3()
31 {
32 *this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
33 }
34
35 Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13,
36 scalar_t m21, scalar_t m22, scalar_t m23,
37 scalar_t m31, scalar_t m32, scalar_t m33)
38 {
39 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13;
40 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23;
41 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33;
42 }
43
44 Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec)
45 {
46 set_row_vector(ivec, 0);
47 set_row_vector(jvec, 1);
48 set_row_vector(kvec, 2);
49 }
50
51 Matrix3x3::Matrix3x3(const mat3_t cmat)
52 {
53 memcpy(m, cmat, sizeof(mat3_t));
54 }
55
56 Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4)
57 {
58 for(int i=0; i<3; i++) {
59 for(int j=0; j<3; j++) {
60 m[i][j] = mat4x4[i][j];
61 }
62 }
63 }
64
65 Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2)
66 {
67 Matrix3x3 res;
68 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
69 scalar_t *dest = res.m[0];
70
71 for(int i=0; i<9; i++) {
72 *dest++ = *op1++ + *op2++;
73 }
74 return res;
75 }
76
77 Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2)
78 {
79 Matrix3x3 res;
80 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
81 scalar_t *dest = res.m[0];
82
83 for(int i=0; i<9; i++) {
84 *dest++ = *op1++ - *op2++;
85 }
86 return res;
87 }
88
89 Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2)
90 {
91 Matrix3x3 res;
92 for(int i=0; i<3; i++) {
93 for(int j=0; j<3; j++) {
94 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];
95 }
96 }
97 return res;
98 }
99
100 void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2)
101 {
102 scalar_t *op1 = m1.m[0];
103 const scalar_t *op2 = m2.m[0];
104
105 for(int i=0; i<9; i++) {
106 *op1++ += *op2++;
107 }
108 }
109
110 void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2)
111 {
112 scalar_t *op1 = m1.m[0];
113 const scalar_t *op2 = m2.m[0];
114
115 for(int i=0; i<9; i++) {
116 *op1++ -= *op2++;
117 }
118 }
119
120 void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2)
121 {
122 Matrix3x3 res;
123 for(int i=0; i<3; i++) {
124 for(int j=0; j<3; j++) {
125 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];
126 }
127 }
128 memcpy(m1.m, res.m, 9 * sizeof(scalar_t));
129 }
130
131 Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar)
132 {
133 Matrix3x3 res;
134 const scalar_t *mptr = mat.m[0];
135 scalar_t *dptr = res.m[0];
136
137 for(int i=0; i<9; i++) {
138 *dptr++ = *mptr++ * scalar;
139 }
140 return res;
141 }
142
143 Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat)
144 {
145 Matrix3x3 res;
146 const scalar_t *mptr = mat.m[0];
147 scalar_t *dptr = res.m[0];
148
149 for(int i=0; i<9; i++) {
150 *dptr++ = *mptr++ * scalar;
151 }
152 return res;
153 }
154
155 void operator *=(Matrix3x3 &mat, scalar_t scalar)
156 {
157 scalar_t *mptr = mat.m[0];
158
159 for(int i=0; i<9; i++) {
160 *mptr++ *= scalar;
161 }
162 }
163
164 void Matrix3x3::translate(const Vector2 &trans)
165 {
166 Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
167 *this *= tmat;
168 }
169
170 void Matrix3x3::set_translation(const Vector2 &trans)
171 {
172 *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1);
173 }
174
175 void Matrix3x3::rotate(scalar_t angle)
176 {
177 scalar_t cos_a = cos(angle);
178 scalar_t sin_a = sin(angle);
179 Matrix3x3 rmat( cos_a, -sin_a, 0,
180 sin_a, cos_a, 0,
181 0, 0, 1);
182 *this *= rmat;
183 }
184
185 void Matrix3x3::set_rotation(scalar_t angle)
186 {
187 scalar_t cos_a = cos(angle);
188 scalar_t sin_a = sin(angle);
189 *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1);
190 }
191
192 void Matrix3x3::rotate(const Vector3 &euler_angles)
193 {
194 Matrix3x3 xrot, yrot, zrot;
195
196 xrot = Matrix3x3( 1, 0, 0,
197 0, cos(euler_angles.x), -sin(euler_angles.x),
198 0, sin(euler_angles.x), cos(euler_angles.x));
199
200 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
201 0, 1, 0,
202 -sin(euler_angles.y), 0, cos(euler_angles.y));
203
204 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
205 sin(euler_angles.z), cos(euler_angles.z), 0,
206 0, 0, 1);
207
208 *this *= xrot * yrot * zrot;
209 }
210
211 void Matrix3x3::set_rotation(const Vector3 &euler_angles)
212 {
213 Matrix3x3 xrot, yrot, zrot;
214
215 xrot = Matrix3x3( 1, 0, 0,
216 0, cos(euler_angles.x), -sin(euler_angles.x),
217 0, sin(euler_angles.x), cos(euler_angles.x));
218
219 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
220 0, 1, 0,
221 -sin(euler_angles.y), 0, cos(euler_angles.y));
222
223 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
224 sin(euler_angles.z), cos(euler_angles.z), 0,
225 0, 0, 1);
226
227 *this = xrot * yrot * zrot;
228 }
229
230 void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle)
231 {
232 scalar_t sina = (scalar_t)sin(angle);
233 scalar_t cosa = (scalar_t)cos(angle);
234 scalar_t invcosa = 1-cosa;
235 scalar_t nxsq = axis.x * axis.x;
236 scalar_t nysq = axis.y * axis.y;
237 scalar_t nzsq = axis.z * axis.z;
238
239 Matrix3x3 xform;
240 xform.m[0][0] = nxsq + (1-nxsq) * cosa;
241 xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
242 xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
243
244 xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
245 xform.m[1][1] = nysq + (1-nysq) * cosa;
246 xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
247
248 xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
249 xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
250 xform.m[2][2] = nzsq + (1-nzsq) * cosa;
251
252 *this *= xform;
253 }
254
255 void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle)
256 {
257 scalar_t sina = (scalar_t)sin(angle);
258 scalar_t cosa = (scalar_t)cos(angle);
259 scalar_t invcosa = 1-cosa;
260 scalar_t nxsq = axis.x * axis.x;
261 scalar_t nysq = axis.y * axis.y;
262 scalar_t nzsq = axis.z * axis.z;
263
264 reset_identity();
265 m[0][0] = nxsq + (1-nxsq) * cosa;
266 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
267 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
268 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
269 m[1][1] = nysq + (1-nysq) * cosa;
270 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
271 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
272 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
273 m[2][2] = nzsq + (1-nzsq) * cosa;
274 }
275
276 // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
277 // article "Quaternion Calculus and Fast Animation".
278 // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
279 Quaternion Matrix3x3::get_rotation_quat() const
280 {
281 static const int next[3] = {1, 2, 0};
282
283 float quat[4];
284
285 scalar_t trace = m[0][0] + m[1][1] + m[2][2];
286 scalar_t root;
287
288 if(trace > 0.0f) {
289 // |w| > 1/2
290 root = sqrt(trace + 1.0f); // 2w
291 quat[0] = 0.5f * root;
292 root = 0.5f / root; // 1 / 4w
293 quat[1] = (m[2][1] - m[1][2]) * root;
294 quat[2] = (m[0][2] - m[2][0]) * root;
295 quat[3] = (m[1][0] - m[0][1]) * root;
296 } else {
297 // |w| <= 1/2
298 int i = 0;
299 if(m[1][1] > m[0][0]) {
300 i = 1;
301 }
302 if(m[2][2] > m[i][i]) {
303 i = 2;
304 }
305 int j = next[i];
306 int k = next[j];
307
308 root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f);
309 quat[i + 1] = 0.5f * root;
310 root = 0.5f / root;
311 quat[0] = (m[k][j] - m[j][k]) * root;
312 quat[j + 1] = (m[j][i] - m[i][j]) * root;
313 quat[k + 1] = (m[k][i] - m[i][k]) * root;
314 }
315 return Quaternion(quat[0], quat[1], quat[2], quat[3]);
316 }
317
318 void Matrix3x3::scale(const Vector3 &scale_vec)
319 {
320 Matrix3x3 smat( scale_vec.x, 0, 0,
321 0, scale_vec.y, 0,
322 0, 0, scale_vec.z);
323 *this *= smat;
324 }
325
326 void Matrix3x3::set_scaling(const Vector3 &scale_vec)
327 {
328 *this = Matrix3x3( scale_vec.x, 0, 0,
329 0, scale_vec.y, 0,
330 0, 0, scale_vec.z);
331 }
332
333 void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index)
334 {
335 m[0][col_index] = vec.x;
336 m[1][col_index] = vec.y;
337 m[2][col_index] = vec.z;
338 }
339
340 void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index)
341 {
342 m[row_index][0] = vec.x;
343 m[row_index][1] = vec.y;
344 m[row_index][2] = vec.z;
345 }
346
347 Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const
348 {
349 return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]);
350 }
351
352 Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const
353 {
354 return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]);
355 }
356
357 void Matrix3x3::transpose()
358 {
359 Matrix3x3 tmp = *this;
360 for(int i=0; i<3; i++) {
361 for(int j=0; j<3; j++) {
362 m[i][j] = tmp[j][i];
363 }
364 }
365 }
366
367 Matrix3x3 Matrix3x3::transposed() const
368 {
369 Matrix3x3 res;
370 for(int i=0; i<3; i++) {
371 for(int j=0; j<3; j++) {
372 res[i][j] = m[j][i];
373 }
374 }
375 return res;
376 }
377
378 scalar_t Matrix3x3::determinant() const
379 {
380 return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) -
381 m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) +
382 m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
383 }
384
385 Matrix3x3 Matrix3x3::inverse() const
386 {
387 // TODO: implement 3x3 inverse
388 return *this;
389 }
390
391 /*ostream &operator <<(ostream &out, const Matrix3x3 &mat)
392 {
393 for(int i=0; i<3; i++) {
394 char str[100];
395 sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]);
396 out << str;
397 }
398 return out;
399 }*/
400
401
402
403 /* ----------------- Matrix4x4 implementation --------------- */
404
405 Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
406
407 Matrix4x4::Matrix4x4()
408 {
409 *this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
410 }
411
412 Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14,
413 scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24,
414 scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34,
415 scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44)
416 {
417 m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14;
418 m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24;
419 m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34;
420 m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44;
421 }
422
423 Matrix4x4::Matrix4x4(const mat4_t cmat)
424 {
425 memcpy(m, cmat, sizeof(mat4_t));
426 }
427
428 Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3)
429 {
430 reset_identity();
431 for(int i=0; i<3; i++) {
432 for(int j=0; j<3; j++) {
433 m[i][j] = mat3x3[i][j];
434 }
435 }
436 }
437
438 Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2)
439 {
440 Matrix4x4 res;
441 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
442 scalar_t *dest = res.m[0];
443
444 for(int i=0; i<16; i++) {
445 *dest++ = *op1++ + *op2++;
446 }
447 return res;
448 }
449
450 Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2)
451 {
452 Matrix4x4 res;
453 const scalar_t *op1 = m1.m[0], *op2 = m2.m[0];
454 scalar_t *dest = res.m[0];
455
456 for(int i=0; i<16; i++) {
457 *dest++ = *op1++ - *op2++;
458 }
459 return res;
460 }
461
462 void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2)
463 {
464 scalar_t *op1 = m1.m[0];
465 const scalar_t *op2 = m2.m[0];
466
467 for(int i=0; i<16; i++) {
468 *op1++ += *op2++;
469 }
470 }
471
472 void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2)
473 {
474 scalar_t *op1 = m1.m[0];
475 const scalar_t *op2 = m2.m[0];
476
477 for(int i=0; i<16; i++) {
478 *op1++ -= *op2++;
479 }
480 }
481
482 Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar)
483 {
484 Matrix4x4 res;
485 const scalar_t *mptr = mat.m[0];
486 scalar_t *dptr = res.m[0];
487
488 for(int i=0; i<16; i++) {
489 *dptr++ = *mptr++ * scalar;
490 }
491 return res;
492 }
493
494 Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat)
495 {
496 Matrix4x4 res;
497 const scalar_t *mptr = mat.m[0];
498 scalar_t *dptr = res.m[0];
499
500 for(int i=0; i<16; i++) {
501 *dptr++ = *mptr++ * scalar;
502 }
503 return res;
504 }
505
506 void operator *=(Matrix4x4 &mat, scalar_t scalar)
507 {
508 scalar_t *mptr = mat.m[0];
509
510 for(int i=0; i<16; i++) {
511 *mptr++ *= scalar;
512 }
513 }
514
515 void Matrix4x4::translate(const Vector3 &trans)
516 {
517 Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
518 *this *= tmat;
519 }
520
521 void Matrix4x4::set_translation(const Vector3 &trans)
522 {
523 *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1);
524 }
525
526 Vector3 Matrix4x4::get_translation() const
527 {
528 return Vector3(m[0][3], m[1][3], m[2][3]);
529 }
530
531 void Matrix4x4::rotate(const Vector3 &euler_angles)
532 {
533 Matrix3x3 xrot, yrot, zrot;
534
535 xrot = Matrix3x3( 1, 0, 0,
536 0, cos(euler_angles.x), -sin(euler_angles.x),
537 0, sin(euler_angles.x), cos(euler_angles.x));
538
539 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
540 0, 1, 0,
541 -sin(euler_angles.y), 0, cos(euler_angles.y));
542
543 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
544 sin(euler_angles.z), cos(euler_angles.z), 0,
545 0, 0, 1);
546
547 *this *= Matrix4x4(xrot * yrot * zrot);
548 }
549
550 void Matrix4x4::set_rotation(const Vector3 &euler_angles)
551 {
552 Matrix3x3 xrot, yrot, zrot;
553
554 xrot = Matrix3x3( 1, 0, 0,
555 0, cos(euler_angles.x), -sin(euler_angles.x),
556 0, sin(euler_angles.x), cos(euler_angles.x));
557
558 yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y),
559 0, 1, 0,
560 -sin(euler_angles.y), 0, cos(euler_angles.y));
561
562 zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0,
563 sin(euler_angles.z), cos(euler_angles.z), 0,
564 0, 0, 1);
565
566 *this = Matrix4x4(xrot * yrot * zrot);
567 }
568
569 void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle)
570 {
571 scalar_t sina = (scalar_t)sin(angle);
572 scalar_t cosa = (scalar_t)cos(angle);
573 scalar_t invcosa = 1-cosa;
574 scalar_t nxsq = axis.x * axis.x;
575 scalar_t nysq = axis.y * axis.y;
576 scalar_t nzsq = axis.z * axis.z;
577
578 Matrix4x4 xform;
579 xform[0][0] = nxsq + (1-nxsq) * cosa;
580 xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
581 xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
582 xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
583 xform[1][1] = nysq + (1-nysq) * cosa;
584 xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
585 xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
586 xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
587 xform[2][2] = nzsq + (1-nzsq) * cosa;
588
589 *this *= xform;
590 }
591
592 void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle)
593 {
594 scalar_t sina = (scalar_t)sin(angle);
595 scalar_t cosa = (scalar_t)cos(angle);
596 scalar_t invcosa = 1-cosa;
597 scalar_t nxsq = axis.x * axis.x;
598 scalar_t nysq = axis.y * axis.y;
599 scalar_t nzsq = axis.z * axis.z;
600
601 reset_identity();
602 m[0][0] = nxsq + (1-nxsq) * cosa;
603 m[0][1] = axis.x * axis.y * invcosa - axis.z * sina;
604 m[0][2] = axis.x * axis.z * invcosa + axis.y * sina;
605 m[1][0] = axis.x * axis.y * invcosa + axis.z * sina;
606 m[1][1] = nysq + (1-nysq) * cosa;
607 m[1][2] = axis.y * axis.z * invcosa - axis.x * sina;
608 m[2][0] = axis.x * axis.z * invcosa - axis.y * sina;
609 m[2][1] = axis.y * axis.z * invcosa + axis.x * sina;
610 m[2][2] = nzsq + (1-nzsq) * cosa;
611 }
612
613 void Matrix4x4::rotate(const Quaternion &quat)
614 {
615 *this *= quat.get_rotation_matrix();
616 }
617
618 void Matrix4x4::set_rotation(const Quaternion &quat)
619 {
620 *this = quat.get_rotation_matrix();
621 }
622
623 Quaternion Matrix4x4::get_rotation_quat() const
624 {
625 Matrix3x3 mat3 = *this;
626 return mat3.get_rotation_quat();
627 }
628
629 void Matrix4x4::scale(const Vector4 &scale_vec)
630 {
631 Matrix4x4 smat( scale_vec.x, 0, 0, 0,
632 0, scale_vec.y, 0, 0,
633 0, 0, scale_vec.z, 0,
634 0, 0, 0, scale_vec.w);
635 *this *= smat;
636 }
637
638 void Matrix4x4::set_scaling(const Vector4 &scale_vec)
639 {
640 *this = Matrix4x4( scale_vec.x, 0, 0, 0,
641 0, scale_vec.y, 0, 0,
642 0, 0, scale_vec.z, 0,
643 0, 0, 0, scale_vec.w);
644 }
645
646 Vector3 Matrix4x4::get_scaling() const
647 {
648 Vector3 vi = get_row_vector(0);
649 Vector3 vj = get_row_vector(1);
650 Vector3 vk = get_row_vector(2);
651
652 return Vector3(vi.length(), vj.length(), vk.length());
653 }
654
655 void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar)
656 {
657 float dx = right - left;
658 float dy = top - bottom;
659 float dz = zfar - znear;
660
661 float a = (right + left) / dx;
662 float b = (top + bottom) / dy;
663 float c = -(zfar + znear) / dz;
664 float d = -2.0 * zfar * znear / dz;
665
666 *this = Matrix4x4(2.0 * znear / dx, 0, a, 0,
667 0, 2.0 * znear / dy, b, 0,
668 0, 0, c, d,
669 0, 0, -1, 0);
670 }
671
672 void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar)
673 {
674 float f = 1.0f / tan(vfov * 0.5f);
675 float dz = znear - zfar;
676
677 reset_identity();
678
679 m[0][0] = f / aspect;
680 m[1][1] = f;
681 m[2][2] = (zfar + znear) / dz;
682 m[3][2] = -1.0f;
683 m[2][3] = 2.0f * zfar * znear / dz;
684 m[3][3] = 0.0f;
685 }
686
687 void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar)
688 {
689 float dx = right - left;
690 float dy = top - bottom;
691 float dz = zfar - znear;
692
693 reset_identity();
694
695 m[0][0] = 2.0 / dx;
696 m[1][1] = 2.0 / dy;
697 m[2][2] = -2.0 / dz;
698 m[0][3] = -(right + left) / dx;
699 m[1][3] = -(top + bottom) / dy;
700 m[2][3] = -(zfar + znear) / dz;
701 }
702
703 void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up)
704 {
705 Vector3 vk = (targ - pos).normalized();
706 Vector3 vj = up.normalized();
707 Vector3 vi = cross_product(vk, vj).normalized();
708 vj = cross_product(vi, vk);
709
710 *this = Matrix4x4(
711 vi.x, vi.y, vi.z, 0,
712 vj.x, vj.y, vj.z, 0,
713 -vk.x, -vk.y, -vk.z, 0,
714 0, 0, 0, 1);
715 translate(-pos);
716 }
717
718 void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index)
719 {
720 m[0][col_index] = vec.x;
721 m[1][col_index] = vec.y;
722 m[2][col_index] = vec.z;
723 m[3][col_index] = vec.w;
724 }
725
726 void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index)
727 {
728 m[row_index][0] = vec.x;
729 m[row_index][1] = vec.y;
730 m[row_index][2] = vec.z;
731 m[row_index][3] = vec.w;
732 }
733
734 Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const
735 {
736 return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]);
737 }
738
739 Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const
740 {
741 return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]);
742 }
743
744 void Matrix4x4::transpose()
745 {
746 Matrix4x4 tmp = *this;
747 for(int i=0; i<4; i++) {
748 for(int j=0; j<4; j++) {
749 m[i][j] = tmp[j][i];
750 }
751 }
752 }
753
754 Matrix4x4 Matrix4x4::transposed() const
755 {
756 Matrix4x4 res;
757 for(int i=0; i<4; i++) {
758 for(int j=0; j<4; j++) {
759 res[i][j] = m[j][i];
760 }
761 }
762 return res;
763 }
764
765 scalar_t Matrix4x4::determinant() const
766 {
767 scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
768 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
769 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
770
771 scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
772 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
773 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
774
775 scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
776 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
777 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
778
779 scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
780 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
781 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
782
783 return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14;
784 }
785
786
787 Matrix4x4 Matrix4x4::adjoint() const
788 {
789 Matrix4x4 coef;
790
791 coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
792 (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
793 (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
794 coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
795 (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
796 (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
797 coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
798 (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
799 (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
800 coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
801 (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
802 (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
803
804 coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
805 (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) +
806 (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2]));
807 coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) -
808 (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
809 (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2]));
810 coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) -
811 (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) +
812 (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
813 coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) -
814 (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) +
815 (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1]));
816
817 coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
818 (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) +
819 (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2]));
820 coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) -
821 (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
822 (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2]));
823 coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) -
824 (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) +
825 (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
826 coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) -
827 (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) +
828 (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1]));
829
830 coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
831 (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) +
832 (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]));
833 coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) -
834 (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
835 (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2]));
836 coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) -
837 (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) +
838 (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
839 coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) -
840 (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) +
841 (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1]));
842
843 coef.transpose();
844
845 for(int i=0; i<4; i++) {
846 for(int j=0; j<4; j++) {
847 coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j];
848 if(i%2) coef.m[i][j] = -coef.m[i][j];
849 }
850 }
851
852 return coef;
853 }
854
855 Matrix4x4 Matrix4x4::inverse() const
856 {
857 Matrix4x4 adj = adjoint();
858
859 return adj * (1.0f / determinant());
860 }
861
862 /*
863 ostream &operator <<(ostream &out, const Matrix4x4 &mat)
864 {
865 for(int i=0; i<4; i++) {
866 char str[100];
867 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]);
868 out << str;
869 }
870 return out;
871 }
872 */