Line data Source code
1 : #include <cstring>
2 : #include <functional>
3 : #include <stdexcept>
4 : #include <algorithm>
5 :
6 : #include <SDL.h>
7 : #include <zlib.h>
8 :
9 : #include "../libprimis-headers/tools.h"
10 : #include "../libprimis-headers/geom.h"
11 :
12 : #include "geomexts.h"
13 :
14 : /**
15 : * @file matrix.cpp
16 : * @brief Matrix math functions
17 : *
18 : * This file defines matrix3, matrix4, and matrix4x3's member functions.
19 : * The definitions of the classes themselves are in geom.h (one of the shared
20 : * header files)
21 : *
22 : */
23 :
24 : //needed for det3
25 :
26 : /**
27 : * @brief 2x2 matrix determinant
28 : *
29 : * Returns the determinant of a 2x2 matrix, provided a set of four doubles
30 : * (rather than as a matrix2 object).
31 : *
32 : * @param a top left element
33 : * @param b top right element
34 : * @param c bottom left element
35 : * @param d bottom right element
36 : *
37 : * @return determinant of the matrix
38 : */
39 120 : static double det2x2(double a, double b, double c, double d)
40 : {
41 120 : return a*d - b*c;
42 : }
43 :
44 : //needed to invert matrix below
45 : /**
46 : * @brief 3x3 matrix determinant
47 : *
48 : * Returns the determinant of a 3x3 matrix, provided a set of nine doubles
49 : * (rather than as a matrix3 object).
50 : *
51 : * @param a1 top left element
52 : * @param a2 top center element
53 : * @param a3 top right element
54 : * @param b1 middle left element
55 : * @param b2 middle center element
56 : * @param b3 middle right element
57 : * @param c1 bottom left element
58 : * @param c2 bottom center element
59 : * @param c3 bottom right element
60 : *
61 : * @return determinant of the matrix
62 : */
63 40 : static double det3x3(double a1, double a2, double a3,
64 : double b1, double b2, double b3,
65 : double c1, double c2, double c3)
66 : {
67 40 : return a1 * det2x2(b2, b3, c2, c3)
68 40 : - b1 * det2x2(a2, a3, c2, c3)
69 40 : + c1 * det2x2(a2, a3, b2, b3);
70 : }
71 :
72 : // =============================================================================
73 : // matrix3 (3x3) object
74 : // =============================================================================
75 :
76 519 : matrix3::matrix3() : a(0,0,0), b(0,0,0), c(0,0,0)
77 : {
78 519 : }
79 :
80 22 : matrix3::matrix3(const vec &a, const vec &b, const vec &c) : a(a), b(b), c(c)
81 : {
82 22 : }
83 :
84 4 : matrix3::matrix3(const quat &q)
85 : {
86 4 : float x = q.x, y = q.y, z = q.z, w = q.w,
87 4 : tx = 2*x, ty = 2*y, tz = 2*z,
88 4 : txx = tx*x, tyy = ty*y, tzz = tz*z,
89 4 : txy = tx*y, txz = tx*z, tyz = ty*z,
90 4 : twx = w*tx, twy = w*ty, twz = w*tz;
91 4 : a = vec(1 - (tyy + tzz), txy + twz, txz - twy);
92 4 : b = vec(txy - twz, 1 - (txx + tzz), tyz + twx);
93 4 : c = vec(txz + twy, tyz - twx, 1 - (txx + tyy));
94 4 : }
95 :
96 9 : matrix3::matrix3(float angle, const vec &axis)
97 : {
98 9 : rotate(angle, axis);
99 9 : }
100 :
101 4 : void matrix3::mul(const matrix3 &m, const matrix3 &n)
102 : {
103 4 : a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
104 4 : b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
105 4 : c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
106 4 : }
107 :
108 2 : void matrix3::mul(const matrix3 &n)
109 : {
110 2 : mul(matrix3(*this), n);
111 2 : }
112 :
113 2 : void matrix3::multranspose(const matrix3 &m, const matrix3 &n)
114 : {
115 2 : a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
116 2 : b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
117 2 : c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
118 2 : }
119 2 : void matrix3::multranspose(const matrix3 &n)
120 : {
121 2 : multranspose(matrix3(*this), n);
122 2 : }
123 :
124 2 : void matrix3::transposemul(const matrix3 &m, const matrix3 &n)
125 : {
126 2 : a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
127 2 : b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
128 2 : c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
129 2 : }
130 :
131 2 : void matrix3::transposemul(const matrix3 &n)
132 : {
133 2 : transposemul(matrix3(*this), n);
134 2 : }
135 :
136 1 : void matrix3::transpose()
137 : {
138 1 : std::swap(a.y, b.x); std::swap(a.z, c.x);
139 1 : std::swap(b.z, c.y);
140 1 : }
141 :
142 4 : void matrix3::transpose(const matrix3 &m)
143 : {
144 4 : a = vec(m.a.x, m.b.x, m.c.x);
145 4 : b = vec(m.a.y, m.b.y, m.c.y);
146 4 : c = vec(m.a.z, m.b.z, m.c.z);
147 4 : }
148 :
149 4 : void matrix3::invert(const matrix3 &o)
150 : {
151 4 : vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
152 4 : transpose(o);
153 4 : a.mul(unscale);
154 4 : b.mul(unscale);
155 4 : c.mul(unscale);
156 4 : }
157 :
158 2 : void matrix3::invert()
159 : {
160 2 : invert(matrix3(*this));
161 2 : }
162 :
163 2 : void matrix3::normalize()
164 : {
165 2 : a.normalize();
166 2 : b.normalize();
167 2 : c.normalize();
168 2 : }
169 :
170 1 : void matrix3::scale(float k)
171 : {
172 1 : a.mul(k);
173 1 : b.mul(k);
174 1 : c.mul(k);
175 1 : }
176 :
177 9 : void matrix3::rotate(float angle, const vec &axis)
178 : {
179 9 : rotate(cosf(angle), std::sin(angle), axis);
180 9 : }
181 :
182 24 : void matrix3::rotate(float ck, float sk, const vec &axis)
183 : {
184 24 : a = vec(axis.x*axis.x*(1-ck)+ck, axis.x*axis.y*(1-ck)+axis.z*sk, axis.x*axis.z*(1-ck)-axis.y*sk);
185 24 : b = vec(axis.x*axis.y*(1-ck)-axis.z*sk, axis.y*axis.y*(1-ck)+ck, axis.y*axis.z*(1-ck)+axis.x*sk);
186 24 : c = vec(axis.x*axis.z*(1-ck)+axis.y*sk, axis.y*axis.z*(1-ck)-axis.x*sk, axis.z*axis.z*(1-ck)+ck);
187 24 : }
188 :
189 9 : void matrix3::setyaw(float ck, float sk)
190 : {
191 9 : a = vec(ck, sk, 0);
192 9 : b = vec(-sk, ck, 0);
193 9 : c = vec(0, 0, 1);
194 9 : }
195 :
196 9 : void matrix3::setyaw(float angle)
197 : {
198 9 : setyaw(cosf(angle), std::sin(angle));
199 9 : }
200 :
201 10 : float matrix3::trace() const
202 : {
203 10 : return a.x + b.y + c.z;
204 : }
205 :
206 7 : bool matrix3::calcangleaxis(float tr, float &angle, vec &axis, float threshold) const
207 : {
208 7 : if(tr <= -1)
209 : {
210 2 : if(a.x >= b.y && a.x >= c.z)
211 : {
212 2 : float r = 1 + a.x - b.y - c.z;
213 2 : if(r <= threshold)
214 : {
215 0 : return false;
216 : }
217 2 : r = sqrtf(r);
218 2 : axis.x = 0.5f*r;
219 2 : axis.y = b.x/r;
220 2 : axis.z = c.x/r;
221 2 : }
222 0 : else if(b.y >= c.z)
223 : {
224 0 : float r = 1 + b.y - a.x - c.z;
225 0 : if(r <= threshold)
226 : {
227 0 : return false;
228 : }
229 0 : r = sqrtf(r);
230 0 : axis.y = 0.5f*r;
231 0 : axis.x = b.x/r;
232 0 : axis.z = c.y/r;
233 : }
234 : else
235 : {
236 0 : float r = 1 + b.y - a.x - c.z;
237 0 : if(r <= threshold)
238 : {
239 0 : return false;
240 : }
241 0 : r = sqrtf(r);
242 0 : axis.z = 0.5f*r;
243 0 : axis.x = c.x/r;
244 0 : axis.y = c.y/r;
245 : }
246 2 : angle = M_PI;
247 : }
248 5 : else if(tr >= 3)
249 : {
250 2 : axis = vec(0, 0, 1);
251 2 : angle = 0;
252 : }
253 : else
254 : {
255 3 : axis = vec(b.z - c.y, c.x - a.z, a.y - b.x);
256 3 : float r = axis.squaredlen();
257 3 : if(r <= threshold)
258 : {
259 0 : return false;
260 : }
261 3 : axis.mul(1/sqrtf(r));
262 3 : angle = acosf(0.5f*(tr - 1));
263 : }
264 7 : return true;
265 : }
266 :
267 1 : bool matrix3::calcangleaxis(float &angle, vec &axis, float threshold) const
268 : {
269 1 : return calcangleaxis(trace(), angle, axis, threshold);
270 : }
271 :
272 6 : vec matrix3::transform(const vec &o) const
273 : {
274 6 : return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
275 : }
276 :
277 9 : vec matrix3::transposedtransform(const vec &o) const
278 : {
279 9 : return vec(a.dot(o), b.dot(o), c.dot(o));
280 : }
281 :
282 4 : vec matrix3::abstransform(const vec &o) const
283 : {
284 4 : return vec(a).mul(o.x).abs().add(vec(b).mul(o.y).abs()).add(vec(c).mul(o.z).abs());
285 : }
286 :
287 4 : vec matrix3::abstransposedtransform(const vec &o) const
288 : {
289 4 : return vec(a.absdot(o), b.absdot(o), c.absdot(o));
290 : }
291 :
292 59 : void matrix3::identity()
293 : {
294 59 : a = vec(1, 0, 0);
295 59 : b = vec(0, 1, 0);
296 59 : c = vec(0, 0, 1);
297 59 : }
298 :
299 2 : void matrix3::rotate_around_x(float ck, float sk)
300 : {
301 2 : vec rb = vec(b).mul(ck).madd(c, sk),
302 2 : rc = vec(c).mul(ck).msub(b, sk);
303 2 : b = rb;
304 2 : c = rc;
305 2 : }
306 :
307 1 : void matrix3::rotate_around_x(float angle)
308 : {
309 1 : rotate_around_x(cosf(angle), std::sin(angle));
310 1 : }
311 :
312 1 : void matrix3::rotate_around_x(const vec2 &sc)
313 : {
314 1 : rotate_around_x(sc.x, sc.y);
315 1 : }
316 :
317 2 : void matrix3::rotate_around_y(float ck, float sk)
318 : {
319 2 : vec rc = vec(c).mul(ck).madd(a, sk),
320 2 : ra = vec(a).mul(ck).msub(c, sk);
321 2 : c = rc;
322 2 : a = ra;
323 2 : }
324 :
325 1 : void matrix3::rotate_around_y(float angle)
326 : {
327 1 : rotate_around_y(cosf(angle), std::sin(angle));
328 1 : }
329 :
330 1 : void matrix3::rotate_around_y(const vec2 &sc)
331 : {
332 1 : rotate_around_y(sc.x, sc.y);
333 1 : }
334 :
335 3 : void matrix3::rotate_around_z(float ck, float sk)
336 : {
337 3 : vec ra = vec(a).mul(ck).madd(b, sk),
338 3 : rb = vec(b).mul(ck).msub(a, sk);
339 3 : a = ra;
340 3 : b = rb;
341 3 : }
342 :
343 1 : void matrix3::rotate_around_z(float angle)
344 : {
345 1 : rotate_around_z(cosf(angle), std::sin(angle));
346 1 : }
347 :
348 2 : void matrix3::rotate_around_z(const vec2 &sc)
349 : {
350 2 : rotate_around_z(sc.x, sc.y);
351 2 : }
352 :
353 3 : vec matrix3::transform(const vec2 &o) const
354 : {
355 3 : return vec(a).mul(o.x).madd(b, o.y);
356 : }
357 :
358 3 : vec matrix3::transposedtransform(const vec2 &o) const
359 : {
360 3 : return vec(a.dot2(o), b.dot2(o), c.dot2(o));
361 : }
362 :
363 1 : vec matrix3::rowx() const
364 : {
365 1 : return vec(a.x, b.x, c.x);
366 : }
367 :
368 1 : vec matrix3::rowy() const
369 : {
370 1 : return vec(a.y, b.y, c.y);
371 : }
372 :
373 1 : vec matrix3::rowz() const
374 : {
375 1 : return vec(a.z, b.z, c.z);
376 : }
377 :
378 : // =============================================================================
379 : // matrix4 (4x4) object
380 : // =============================================================================
381 :
382 : /* invert()
383 : *
384 : * sets the matrix values to the inverse of the provided matrix A*A^-1 = I
385 : * returns false if singular (or nearly singular to within tolerance of mindet)
386 : * or true if matrix was inverted successfully
387 : *
388 : * &m: a matrix4 object to be inverted and assigned to the object
389 : * mindet: the minimum value at which matrices are considered
390 : */
391 4 : bool matrix4::invert(const matrix4 &m, double mindet)
392 : {
393 4 : double a1 = m.a.x, a2 = m.a.y, a3 = m.a.z, a4 = m.a.w,
394 4 : b1 = m.b.x, b2 = m.b.y, b3 = m.b.z, b4 = m.b.w,
395 4 : c1 = m.c.x, c2 = m.c.y, c3 = m.c.z, c4 = m.c.w,
396 4 : d1 = m.d.x, d2 = m.d.y, d3 = m.d.z, d4 = m.d.w,
397 4 : det1 = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4),
398 4 : det2 = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4),
399 4 : det3 = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4),
400 4 : det4 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4),
401 4 : det = a1*det1 + b1*det2 + c1*det3 + d1*det4;
402 :
403 4 : if(std::fabs(det) < mindet)
404 : {
405 2 : return false;
406 : }
407 :
408 2 : double invdet = 1/det;
409 :
410 2 : a.x = det1 * invdet;
411 2 : a.y = det2 * invdet;
412 2 : a.z = det3 * invdet;
413 2 : a.w = det4 * invdet;
414 :
415 2 : b.x = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * invdet;
416 2 : b.y = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * invdet;
417 2 : b.z = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * invdet;
418 2 : b.w = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * invdet;
419 :
420 2 : c.x = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * invdet;
421 2 : c.y = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * invdet;
422 2 : c.z = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * invdet;
423 2 : c.w = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * invdet;
424 :
425 2 : d.x = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * invdet;
426 2 : d.y = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * invdet;
427 2 : d.z = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * invdet;
428 2 : d.w = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * invdet;
429 :
430 2 : return true;
431 : }
432 :
433 2 : matrix4 matrix4::inverse(double mindet) const
434 : {
435 2 : matrix4 ret;
436 2 : if(ret.invert(*this, mindet))
437 : {
438 1 : return ret;
439 : }
440 : else
441 : {
442 1 : return { vec4<float>(0,0,0,0), vec4<float>(0,0,0,0), vec4<float>(0,0,0,0), vec4<float>(0,0,0,0) };
443 : }
444 : }
445 :
446 140 : matrix4::matrix4() : a(0,0,0,0), b(0,0,0,0), c(0,0,0,0), d(0,0,0,0)
447 : {
448 140 : }
449 :
450 1 : matrix4::matrix4(const float *m) : a(m), b(m+4), c(m+8), d(m+12)
451 : {
452 1 : }
453 :
454 8 : matrix4::matrix4(const vec &a, const vec &b, const vec &c)
455 8 : : a(a.x, b.x, c.x, 0), b(a.y, b.y, c.y, 0), c(a.z, b.z, c.z, 0), d(0, 0, 0, 1)
456 : {
457 8 : }
458 :
459 31 : matrix4::matrix4(const vec4<float> &a, const vec4<float> &b, const vec4<float> &c, const vec4<float> &d)
460 31 : : a(a), b(b), c(c), d(d)
461 : {
462 31 : }
463 :
464 1 : matrix4::matrix4(const matrix4x3 &m)
465 1 : : a(m.a, 0), b(m.b, 0), c(m.c, 0), d(m.d, 1)
466 : {
467 1 : }
468 :
469 1 : matrix4::matrix4(const matrix3 &rot, const vec &trans)
470 1 : : a(rot.a, 0), b(rot.b, 0), c(rot.c, 0), d(trans, 1)
471 : {
472 1 : }
473 :
474 0 : void matrix4::transposedtransform(const plane &in, plane &out) const
475 : {
476 0 : out.x = in.dist(a);
477 0 : out.y = in.dist(b);
478 0 : out.z = in.dist(c);
479 0 : out.offset = in.dist(d);
480 0 : }
481 :
482 9 : void matrix4::mul(const matrix4 &x, const matrix3 &y)
483 : {
484 9 : a = vec4<float>(x.a).mul(y.a.x).madd(x.b, y.a.y).madd(x.c, y.a.z);
485 9 : b = vec4<float>(x.a).mul(y.b.x).madd(x.b, y.b.y).madd(x.c, y.b.z);
486 9 : c = vec4<float>(x.a).mul(y.c.x).madd(x.b, y.c.y).madd(x.c, y.c.z);
487 9 : d = x.d;
488 9 : }
489 :
490 9 : void matrix4::mul(const matrix3 &y)
491 : {
492 9 : mul(matrix4(*this), y);
493 9 : }
494 :
495 1 : void matrix4::mul(const matrix4 &x, const matrix4 &y)
496 : {
497 1 : mult<vec4<float>>(x, y);
498 1 : }
499 :
500 1 : void matrix4::mul(const matrix4 &y)
501 : {
502 1 : mult<vec4<float>>(matrix4(*this), y);
503 1 : }
504 :
505 1 : void matrix4::muld(const matrix4 &x, const matrix4 &y)
506 : {
507 1 : mult<vec4<double>>(x, y);
508 1 : }
509 :
510 1 : void matrix4::muld(const matrix4 &y)
511 : {
512 1 : mult<vec4<double>>(matrix4(*this), y);
513 1 : }
514 :
515 2 : void matrix4::rotate_around_x(float ck, float sk)
516 : {
517 2 : vec4<float> rb = vec4<float>(b).mul(ck).madd(c, sk),
518 2 : rc = vec4<float>(c).mul(ck).msub(b, sk);
519 2 : b = rb;
520 2 : c = rc;
521 2 : }
522 :
523 1 : void matrix4::rotate_around_x(float angle)
524 : {
525 1 : rotate_around_x(cosf(angle), std::sin(angle));
526 1 : }
527 :
528 1 : void matrix4::rotate_around_x(const vec2 &sc)
529 : {
530 1 : rotate_around_x(sc.x, sc.y);
531 1 : }
532 :
533 2 : void matrix4::rotate_around_y(float ck, float sk)
534 : {
535 2 : vec4<float> rc = vec4<float>(c).mul(ck).madd(a, sk),
536 2 : ra = vec4<float>(a).mul(ck).msub(c, sk);
537 2 : c = rc;
538 2 : a = ra;
539 2 : }
540 :
541 1 : void matrix4::rotate_around_y(float angle)
542 : {
543 1 : rotate_around_y(cosf(angle), std::sin(angle));
544 1 : }
545 :
546 1 : void matrix4::rotate_around_y(const vec2 &sc)
547 : {
548 1 : rotate_around_y(sc.x, sc.y);
549 1 : }
550 :
551 2 : void matrix4::rotate_around_z(float ck, float sk)
552 : {
553 2 : vec4<float> ra = vec4<float>(a).mul(ck).madd(b, sk),
554 2 : rb = vec4<float>(b).mul(ck).msub(a, sk);
555 2 : a = ra;
556 2 : b = rb;
557 2 : }
558 :
559 1 : void matrix4::rotate_around_z(float angle)
560 : {
561 1 : rotate_around_z(cosf(angle), std::sin(angle));
562 1 : }
563 :
564 1 : void matrix4::rotate_around_z(const vec2 &sc)
565 : {
566 1 : rotate_around_z(sc.x, sc.y);
567 1 : }
568 :
569 9 : void matrix4::rotate(float ck, float sk, const vec &axis)
570 : {
571 9 : matrix3 m;
572 9 : m.rotate(ck, sk, axis);
573 9 : mul(m);
574 9 : }
575 :
576 3 : void matrix4::rotate(float angle, const vec &dir)
577 : {
578 3 : rotate(cosf(angle), std::sin(angle), dir);
579 3 : }
580 :
581 3 : void matrix4::rotate(const vec2 &sc, const vec &dir)
582 : {
583 3 : rotate(sc.x, sc.y, dir);
584 3 : }
585 :
586 36 : void matrix4::identity()
587 : {
588 36 : a = vec4<float>(1, 0, 0, 0);
589 36 : b = vec4<float>(0, 1, 0, 0);
590 36 : c = vec4<float>(0, 0, 1, 0);
591 36 : d = vec4<float>(0, 0, 0, 1);
592 36 : }
593 :
594 1 : void matrix4::settranslation(const vec &v)
595 : {
596 1 : d.setxyz(v);
597 1 : }
598 :
599 1 : void matrix4::settranslation(float x, float y, float z)
600 : {
601 1 : d.x = x;
602 1 : d.y = y;
603 1 : d.z = z;
604 1 : }
605 :
606 0 : void matrix4::translate(const vec &p)
607 : {
608 0 : d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
609 0 : }
610 :
611 0 : void matrix4::translate(float x, float y, float z)
612 : {
613 0 : translate(vec(x, y, z));
614 0 : }
615 :
616 0 : void matrix4::translate(const vec &p, float scale)
617 : {
618 0 : translate(vec(p).mul(scale));
619 0 : }
620 :
621 3 : void matrix4::setscale(float x, float y, float z)
622 : {
623 3 : a.x = x;
624 3 : b.y = y;
625 3 : c.z = z;
626 3 : }
627 :
628 1 : void matrix4::setscale(const vec &v)
629 : {
630 1 : setscale(v.x, v.y, v.z);
631 1 : }
632 :
633 1 : void matrix4::setscale(float n)
634 : {
635 1 : setscale(n, n, n);
636 1 : }
637 :
638 3 : void matrix4::scale(float x, float y, float z)
639 : {
640 3 : a.mul(x);
641 3 : b.mul(y);
642 3 : c.mul(z);
643 3 : }
644 :
645 1 : void matrix4::scale(const vec &v)
646 : {
647 1 : scale(v.x, v.y, v.z);
648 1 : }
649 :
650 1 : void matrix4::scale(float n)
651 : {
652 1 : scale(n, n, n);
653 1 : }
654 :
655 2 : void matrix4::scalez(float k)
656 : {
657 2 : a.z *= k;
658 2 : b.z *= k;
659 2 : c.z *= k;
660 2 : d.z *= k;
661 2 : }
662 :
663 2 : void matrix4::jitter(float x, float y)
664 : {
665 2 : a.x += x * a.w;
666 2 : a.y += y * a.w;
667 2 : b.x += x * b.w;
668 2 : b.y += y * b.w;
669 2 : c.x += x * c.w;
670 2 : c.y += y * c.w;
671 2 : d.x += x * d.w;
672 2 : d.y += y * d.w;
673 2 : }
674 :
675 1 : void matrix4::transpose()
676 : {
677 : //swap upper triangular elements of row 'a'
678 1 : std::swap(a.y, b.x);
679 1 : std::swap(a.z, c.x);
680 1 : std::swap(a.w, d.x);
681 : //swap upper triangular elements of row 'b'
682 1 : std::swap(b.z, c.y);
683 1 : std::swap(b.w, d.y);
684 : //swap upper triangular element of row 'c'
685 1 : std::swap(c.w, d.z);
686 : //no upper triangular elements on row 'd'
687 1 : }
688 :
689 1 : void matrix4::transpose(const matrix4 &m)
690 : {
691 1 : a = vec4<float>(m.a.x, m.b.x, m.c.x, m.d.x);
692 1 : b = vec4<float>(m.a.y, m.b.y, m.c.y, m.d.y);
693 1 : c = vec4<float>(m.a.z, m.b.z, m.c.z, m.d.z);
694 1 : d = vec4<float>(m.a.w, m.b.w, m.c.w, m.d.w);
695 1 : }
696 :
697 2 : void matrix4::frustum(float left, float right, float bottom, float top, float znear, float zfar)
698 : {
699 2 : float width = right - left,
700 2 : height = top - bottom,
701 2 : zrange = znear - zfar;
702 2 : a = vec4<float>(2*znear/width, 0, 0, 0); //depth to width ratio
703 2 : b = vec4<float>(0, 2*znear/height, 0, 0); //depth to height ratio
704 2 : c = vec4<float>((right + left)/width, (top + bottom)/height, (zfar + znear)/zrange, -1); //offset from centered
705 2 : d = vec4<float>(0, 0, 2*znear*zfar/zrange, 0); //depth scale
706 2 : }
707 :
708 1 : void matrix4::perspective(float fovy, float aspect, float znear, float zfar)
709 : {
710 1 : float ydist = znear * std::tan(fovy/(2*RAD)),
711 1 : xdist = ydist * aspect;
712 1 : frustum(-xdist, xdist, -ydist, ydist, znear, zfar);
713 1 : }
714 :
715 3 : void matrix4::ortho(float left, float right, float bottom, float top, float znear, float zfar)
716 : {
717 3 : float width = right - left,
718 3 : height = top - bottom,
719 3 : zrange = znear - zfar;
720 3 : a = vec4<float>(2/width, 0, 0, 0);
721 3 : b = vec4<float>(0, 2/height, 0, 0);
722 3 : c = vec4<float>(0, 0, 2/zrange, 0);
723 3 : d = vec4<float>(-(right+left)/width, -(top+bottom)/height, (zfar+znear)/zrange, 1);
724 3 : }
725 :
726 1 : void matrix4::transform(const vec &in, vec &out) const
727 : {
728 1 : out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z)).add(vec(d));
729 1 : }
730 :
731 1 : void matrix4::transform(const vec4<float> &in, vec &out) const
732 : {
733 1 : out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z)).add(vec(d).mul(in.w));
734 1 : }
735 :
736 1 : void matrix4::transform(const vec &in, vec4<float> &out) const
737 : {
738 1 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).add(d);
739 1 : }
740 :
741 1 : void matrix4::transform(const vec4<float> &in, vec4<float> &out) const
742 : {
743 1 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).madd(d, in.w);
744 1 : }
745 :
746 4 : void matrix4::transformnormal(const vec &in, vec &out) const
747 : {
748 4 : out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z));
749 4 : }
750 :
751 3 : void matrix4::transformnormal(const vec &in, vec4<float> &out) const
752 : {
753 3 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z);
754 3 : }
755 :
756 3 : void matrix4::transposedtransform(const vec &in, vec &out) const
757 : {
758 3 : vec p = vec(in).sub(vec(d));
759 3 : out.x = a.dot3(p);
760 3 : out.y = b.dot3(p);
761 3 : out.z = c.dot3(p);
762 3 : }
763 :
764 3 : void matrix4::transposedtransformnormal(const vec &in, vec &out) const
765 : {
766 3 : out.x = a.dot3(in);
767 3 : out.y = b.dot3(in);
768 3 : out.z = c.dot3(in);
769 3 : }
770 :
771 1 : vec matrix4::gettranslation() const
772 : {
773 1 : return vec(d);
774 : }
775 :
776 2 : vec4<float> matrix4::rowx() const
777 : {
778 2 : return vec4<float>(a.x, b.x, c.x, d.x);
779 : }
780 :
781 2 : vec4<float> matrix4::rowy() const
782 : {
783 2 : return vec4<float>(a.y, b.y, c.y, d.y);
784 : }
785 :
786 2 : vec4<float> matrix4::rowz() const
787 : {
788 2 : return vec4<float>(a.z, b.z, c.z, d.z);
789 : }
790 :
791 2 : vec4<float> matrix4::roww() const
792 : {
793 2 : return vec4<float>(a.w, b.w, c.w, d.w);
794 : }
795 :
796 3 : vec2 matrix4::lineardepthscale() const
797 : {
798 3 : return vec2(d.w, -d.z).div(c.z*d.w - d.z*c.w);
799 : }
800 :
801 : // =============================================================================
802 : // matrix4x3 object
803 : // =============================================================================
804 :
805 67 : matrix4x3::matrix4x3() : a(0,0,0), b(0,0,0), c(0,0,0), d(0,0,0)
806 : {
807 67 : }
808 :
809 34 : matrix4x3::matrix4x3(const vec &a, const vec &b, const vec &c, const vec &d) : a(a), b(b), c(c), d(d)
810 : {
811 34 : }
812 :
813 9 : matrix4x3::matrix4x3(const matrix3 &rot, const vec &trans) : a(rot.a), b(rot.b), c(rot.c), d(trans)
814 : {
815 9 : }
816 :
817 2 : matrix4x3::matrix4x3(const dualquat &dq)
818 : {
819 2 : vec4<float> r = vec4<float>(dq.real).mul(1/dq.real.squaredlen()), rr = vec4<float>(r).mul(dq.real);
820 2 : r.mul(2);
821 2 : float xy = r.x*dq.real.y, xz = r.x*dq.real.z, yz = r.y*dq.real.z,
822 2 : wx = r.w*dq.real.x, wy = r.w*dq.real.y, wz = r.w*dq.real.z;
823 2 : a = vec(rr.w + rr.x - rr.y - rr.z, xy + wz, xz - wy);
824 2 : b = vec(xy - wz, rr.w + rr.y - rr.x - rr.z, yz + wx);
825 2 : c = vec(xz + wy, yz - wx, rr.w + rr.z - rr.x - rr.y);
826 4 : d = vec(-(dq.dual.w*r.x - dq.dual.x*r.w + dq.dual.y*r.z - dq.dual.z*r.y),
827 2 : -(dq.dual.w*r.y - dq.dual.x*r.z - dq.dual.y*r.w + dq.dual.z*r.x),
828 2 : -(dq.dual.w*r.z + dq.dual.x*r.y - dq.dual.y*r.x - dq.dual.z*r.w));
829 2 : }
830 :
831 4 : void matrix4x3::mul(float k)
832 : {
833 4 : a.mul(k);
834 4 : b.mul(k);
835 4 : c.mul(k);
836 4 : d.mul(k);
837 4 : }
838 :
839 3 : void matrix4x3::setscale(float x, float y, float z)
840 : {
841 3 : a.x = x;
842 3 : b.y = y;
843 3 : c.z = z;
844 3 : }
845 :
846 1 : void matrix4x3::setscale(const vec &v)
847 : {
848 1 : setscale(v.x, v.y, v.z);
849 1 : }
850 :
851 1 : void matrix4x3::setscale(float n)
852 : {
853 1 : setscale(n, n, n);
854 1 : }
855 :
856 3 : void matrix4x3::scale(float x, float y, float z)
857 : {
858 3 : a.mul(x);
859 3 : b.mul(y);
860 3 : c.mul(z);
861 3 : }
862 :
863 1 : void matrix4x3::scale(const vec &v)
864 : {
865 1 : scale(v.x, v.y, v.z);
866 1 : }
867 :
868 1 : void matrix4x3::scale(float n)
869 : {
870 1 : scale(n, n, n);
871 1 : }
872 :
873 1 : void matrix4x3::settranslation(const vec &p)
874 : {
875 1 : d = p;
876 1 : }
877 :
878 1 : void matrix4x3::settranslation(float x, float y, float z)
879 : {
880 1 : d = vec(x, y, z);
881 1 : }
882 :
883 4 : void matrix4x3::translate(const vec &p)
884 : {
885 4 : d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
886 4 : }
887 :
888 1 : void matrix4x3::translate(float x, float y, float z)
889 : {
890 1 : translate(vec(x, y, z));
891 1 : }
892 :
893 3 : void matrix4x3::translate(const vec &p, float scale)
894 : {
895 3 : translate(vec(p).mul(scale));
896 3 : }
897 :
898 1 : void matrix4x3::accumulate(const matrix4x3 &m, float k)
899 : {
900 1 : a.madd(m.a, k);
901 1 : b.madd(m.b, k);
902 1 : c.madd(m.c, k);
903 1 : d.madd(m.d, k);
904 1 : }
905 :
906 4 : void matrix4x3::normalize()
907 : {
908 4 : a.normalize();
909 4 : b.normalize();
910 4 : c.normalize();
911 4 : }
912 :
913 1 : void matrix4x3::lerp(const matrix4x3 &to, float t)
914 : {
915 1 : a.lerp(to.a, t);
916 1 : b.lerp(to.b, t);
917 1 : c.lerp(to.c, t);
918 1 : d.lerp(to.d, t);
919 1 : }
920 1 : void matrix4x3::lerp(const matrix4x3 &from, const matrix4x3 &to, float t)
921 : {
922 1 : a.lerp(from.a, to.a, t);
923 1 : b.lerp(from.b, to.b, t);
924 1 : c.lerp(from.c, to.c, t);
925 1 : d.lerp(from.d, to.d, t);
926 1 : }
927 :
928 47 : void matrix4x3::identity()
929 : {
930 47 : a = vec(1, 0, 0);
931 47 : b = vec(0, 1, 0);
932 47 : c = vec(0, 0, 1);
933 47 : d = vec(0, 0, 0);
934 47 : }
935 :
936 2 : void matrix4x3::mul(const matrix4x3 &m, const matrix4x3 &n)
937 : {
938 2 : a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
939 2 : b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
940 2 : c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
941 2 : d = vec(m.d).madd(m.a, n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
942 2 : }
943 :
944 1 : void matrix4x3::mul(const matrix4x3 &n)
945 : {
946 1 : mul(matrix4x3(*this), n);
947 1 : }
948 :
949 1 : void matrix4x3::mul(const matrix3 &m, const matrix4x3 &n)
950 : {
951 1 : a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
952 1 : b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
953 1 : c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
954 1 : d = vec(m.a).mul(n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
955 1 : }
956 :
957 1 : void matrix4x3::mul(const matrix3 &rot, const vec &trans, const matrix4x3 &n)
958 : {
959 1 : mul(rot, n);
960 1 : d.add(trans);
961 1 : }
962 :
963 3 : void matrix4x3::transpose()
964 : {
965 3 : d = vec(a.dot(d), b.dot(d), c.dot(d)).neg();
966 3 : std::swap(a.y, b.x);
967 3 : std::swap(a.z, c.x);
968 3 : std::swap(b.z, c.y);
969 3 : }
970 :
971 8 : void matrix4x3::transpose(const matrix4x3 &o)
972 : {
973 8 : a = vec(o.a.x, o.b.x, o.c.x);
974 8 : b = vec(o.a.y, o.b.y, o.c.y);
975 8 : c = vec(o.a.z, o.b.z, o.c.z);
976 8 : d = vec(o.a.dot(o.d), o.b.dot(o.d), o.c.dot(o.d)).neg();
977 8 : }
978 :
979 2 : void matrix4x3::transposemul(const matrix4x3 &m, const matrix4x3 &n)
980 : {
981 2 : vec t(m.a.dot(m.d), m.b.dot(m.d), m.c.dot(m.d));
982 2 : a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
983 2 : b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
984 2 : c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
985 2 : d = vec(m.a.dot(n.d), m.b.dot(n.d), m.c.dot(n.d)).sub(t);
986 2 : }
987 :
988 1 : void matrix4x3::multranspose(const matrix4x3 &m, const matrix4x3 &n)
989 : {
990 1 : vec t(n.a.dot(n.d), n.b.dot(n.d), n.c.dot(n.d));
991 1 : a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
992 1 : b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
993 1 : c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
994 1 : d = vec(m.d).msub(m.a, t.x).msub(m.b, t.y).msub(m.c, t.z);
995 1 : }
996 :
997 8 : void matrix4x3::invert(const matrix4x3 &o)
998 : {
999 8 : vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
1000 8 : transpose(o);
1001 8 : a.mul(unscale);
1002 8 : b.mul(unscale);
1003 8 : c.mul(unscale);
1004 8 : d.mul(unscale);
1005 8 : }
1006 :
1007 6 : void matrix4x3::invert()
1008 : {
1009 6 : invert(matrix4x3(*this));
1010 6 : }
1011 :
1012 3 : void matrix4x3::rotate(float angle, const vec &d)
1013 : {
1014 3 : rotate(cosf(angle), std::sin(angle), d);
1015 3 : }
1016 :
1017 6 : void matrix4x3::rotate(float ck, float sk, const vec &axis)
1018 : {
1019 6 : matrix3 m;
1020 6 : m.rotate(ck, sk, axis);
1021 6 : *this = matrix4x3(m, vec(0, 0, 0));
1022 6 : }
1023 :
1024 2 : void matrix4x3::rotate_around_x(float ck, float sk)
1025 : {
1026 2 : vec rb = vec(b).mul(ck).madd(c, sk),
1027 2 : rc = vec(c).mul(ck).msub(b, sk);
1028 2 : b = rb;
1029 2 : c = rc;
1030 2 : }
1031 1 : void matrix4x3::rotate_around_x(float angle)
1032 : {
1033 1 : rotate_around_x(cosf(angle), std::sin(angle));
1034 1 : }
1035 :
1036 1 : void matrix4x3::rotate_around_x(const vec2 &sc)
1037 : {
1038 1 : rotate_around_x(sc.x, sc.y);
1039 1 : }
1040 :
1041 2 : void matrix4x3::rotate_around_y(float ck, float sk)
1042 : {
1043 2 : vec rc = vec(c).mul(ck).madd(a, sk),
1044 2 : ra = vec(a).mul(ck).msub(c, sk);
1045 2 : c = rc;
1046 2 : a = ra;
1047 2 : }
1048 1 : void matrix4x3::rotate_around_y(float angle)
1049 : {
1050 1 : rotate_around_y(cosf(angle), std::sin(angle));
1051 1 : }
1052 :
1053 1 : void matrix4x3::rotate_around_y(const vec2 &sc)
1054 : {
1055 1 : rotate_around_y(sc.x, sc.y);
1056 1 : }
1057 :
1058 2 : void matrix4x3::rotate_around_z(float ck, float sk)
1059 : {
1060 2 : vec ra = vec(a).mul(ck).madd(b, sk),
1061 2 : rb = vec(b).mul(ck).msub(a, sk);
1062 2 : a = ra;
1063 2 : b = rb;
1064 2 : }
1065 :
1066 1 : void matrix4x3::rotate_around_z(float angle)
1067 : {
1068 1 : rotate_around_z(cosf(angle), std::sin(angle));
1069 1 : }
1070 :
1071 1 : void matrix4x3::rotate_around_z(const vec2 &sc)
1072 : {
1073 1 : rotate_around_z(sc.x, sc.y);
1074 1 : }
1075 :
1076 3 : vec matrix4x3::transposedtransform(const vec &o) const
1077 : {
1078 3 : vec p = vec(o).sub(d);
1079 3 : return vec(a.dot(p), b.dot(p), c.dot(p));
1080 : }
1081 :
1082 3 : vec matrix4x3::transformnormal(const vec &o) const
1083 : {
1084 3 : return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
1085 : }
1086 :
1087 3 : vec matrix4x3::transposedtransformnormal(const vec &o) const
1088 : {
1089 3 : return vec(a.dot(o), b.dot(o), c.dot(o));
1090 : }
1091 :
1092 12 : vec matrix4x3::transform(const vec &o) const
1093 : {
1094 12 : return vec(d).madd(a, o.x).madd(b, o.y).madd(c, o.z);
1095 : }
1096 :
1097 3 : vec matrix4x3::transform(const vec2 &o) const
1098 : {
1099 3 : return vec(d).madd(a, o.x).madd(b, o.y);
1100 : }
1101 :
1102 1 : vec4<float> matrix4x3::rowx() const
1103 : {
1104 1 : return vec4<float>(a.x, b.x, c.x, d.x);
1105 : }
1106 :
1107 1 : vec4<float> matrix4x3::rowy() const
1108 : {
1109 1 : return vec4<float>(a.y, b.y, c.y, d.y);
1110 : }
1111 :
1112 1 : vec4<float> matrix4x3::rowz() const
1113 : {
1114 1 : return vec4<float>(a.z, b.z, c.z, d.z);
1115 : }
1116 :
1117 : //end matrix4x3
|
