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 : /**
383 : * @brief Sets the matrix to its inverse
384 : *
385 : * sets the matrix values to the inverse of the provided matrix A*A^-1 = I
386 : * returns false if singular (or nearly singular to within tolerance of mindet)
387 : * or true if matrix was inverted successfully
388 : *
389 : * @param m a matrix4 object to be inverted and assigned to the object
390 : * @param mindet the minimum value at which matrices are considered
391 : *
392 : * @return true if the matrix was invertable
393 : */
394 4 : bool matrix4::invert(const matrix4 &m, double mindet)
395 : {
396 4 : double a1 = m.a.x, a2 = m.a.y, a3 = m.a.z, a4 = m.a.w,
397 4 : b1 = m.b.x, b2 = m.b.y, b3 = m.b.z, b4 = m.b.w,
398 4 : c1 = m.c.x, c2 = m.c.y, c3 = m.c.z, c4 = m.c.w,
399 4 : d1 = m.d.x, d2 = m.d.y, d3 = m.d.z, d4 = m.d.w,
400 4 : det1 = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4),
401 4 : det2 = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4),
402 4 : det3 = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4),
403 4 : det4 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4),
404 4 : det = a1*det1 + b1*det2 + c1*det3 + d1*det4;
405 :
406 4 : if(std::fabs(det) < mindet)
407 : {
408 2 : return false;
409 : }
410 :
411 2 : double invdet = 1/det;
412 :
413 2 : a.x = det1 * invdet;
414 2 : a.y = det2 * invdet;
415 2 : a.z = det3 * invdet;
416 2 : a.w = det4 * invdet;
417 :
418 2 : b.x = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * invdet;
419 2 : b.y = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * invdet;
420 2 : b.z = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * invdet;
421 2 : b.w = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * invdet;
422 :
423 2 : c.x = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * invdet;
424 2 : c.y = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * invdet;
425 2 : c.z = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * invdet;
426 2 : c.w = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * invdet;
427 :
428 2 : d.x = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * invdet;
429 2 : d.y = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * invdet;
430 2 : d.z = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * invdet;
431 2 : d.w = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * invdet;
432 :
433 2 : return true;
434 : }
435 :
436 2 : matrix4 matrix4::inverse(double mindet) const
437 : {
438 2 : matrix4 ret;
439 2 : if(ret.invert(*this, mindet))
440 : {
441 1 : return ret;
442 : }
443 : else
444 : {
445 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) };
446 : }
447 : }
448 :
449 140 : matrix4::matrix4() : a(0,0,0,0), b(0,0,0,0), c(0,0,0,0), d(0,0,0,0)
450 : {
451 140 : }
452 :
453 1 : matrix4::matrix4(const float *m) : a(m), b(m+4), c(m+8), d(m+12)
454 : {
455 1 : }
456 :
457 8 : matrix4::matrix4(const vec &a, const vec &b, const vec &c)
458 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)
459 : {
460 8 : }
461 :
462 31 : matrix4::matrix4(const vec4<float> &a, const vec4<float> &b, const vec4<float> &c, const vec4<float> &d)
463 31 : : a(a), b(b), c(c), d(d)
464 : {
465 31 : }
466 :
467 1 : matrix4::matrix4(const matrix4x3 &m)
468 1 : : a(m.a, 0), b(m.b, 0), c(m.c, 0), d(m.d, 1)
469 : {
470 1 : }
471 :
472 1 : matrix4::matrix4(const matrix3 &rot, const vec &trans)
473 1 : : a(rot.a, 0), b(rot.b, 0), c(rot.c, 0), d(trans, 1)
474 : {
475 1 : }
476 :
477 0 : void matrix4::transposedtransform(const plane &in, plane &out) const
478 : {
479 0 : out.x = in.dist(a);
480 0 : out.y = in.dist(b);
481 0 : out.z = in.dist(c);
482 0 : out.offset = in.dist(d);
483 0 : }
484 :
485 9 : void matrix4::mul(const matrix4 &x, const matrix3 &y)
486 : {
487 9 : a = vec4<float>(x.a).mul(y.a.x).madd(x.b, y.a.y).madd(x.c, y.a.z);
488 9 : b = vec4<float>(x.a).mul(y.b.x).madd(x.b, y.b.y).madd(x.c, y.b.z);
489 9 : c = vec4<float>(x.a).mul(y.c.x).madd(x.b, y.c.y).madd(x.c, y.c.z);
490 9 : d = x.d;
491 9 : }
492 :
493 9 : void matrix4::mul(const matrix3 &y)
494 : {
495 9 : mul(matrix4(*this), y);
496 9 : }
497 :
498 1 : void matrix4::mul(const matrix4 &x, const matrix4 &y)
499 : {
500 1 : mult<vec4<float>>(x, y);
501 1 : }
502 :
503 1 : void matrix4::mul(const matrix4 &y)
504 : {
505 1 : mult<vec4<float>>(matrix4(*this), y);
506 1 : }
507 :
508 1 : void matrix4::muld(const matrix4 &x, const matrix4 &y)
509 : {
510 1 : mult<vec4<double>>(x, y);
511 1 : }
512 :
513 1 : void matrix4::muld(const matrix4 &y)
514 : {
515 1 : mult<vec4<double>>(matrix4(*this), y);
516 1 : }
517 :
518 2 : void matrix4::rotate_around_x(float ck, float sk)
519 : {
520 2 : vec4<float> rb = vec4<float>(b).mul(ck).madd(c, sk),
521 2 : rc = vec4<float>(c).mul(ck).msub(b, sk);
522 2 : b = rb;
523 2 : c = rc;
524 2 : }
525 :
526 1 : void matrix4::rotate_around_x(float angle)
527 : {
528 1 : rotate_around_x(cosf(angle), std::sin(angle));
529 1 : }
530 :
531 1 : void matrix4::rotate_around_x(const vec2 &sc)
532 : {
533 1 : rotate_around_x(sc.x, sc.y);
534 1 : }
535 :
536 2 : void matrix4::rotate_around_y(float ck, float sk)
537 : {
538 2 : vec4<float> rc = vec4<float>(c).mul(ck).madd(a, sk),
539 2 : ra = vec4<float>(a).mul(ck).msub(c, sk);
540 2 : c = rc;
541 2 : a = ra;
542 2 : }
543 :
544 1 : void matrix4::rotate_around_y(float angle)
545 : {
546 1 : rotate_around_y(cosf(angle), std::sin(angle));
547 1 : }
548 :
549 1 : void matrix4::rotate_around_y(const vec2 &sc)
550 : {
551 1 : rotate_around_y(sc.x, sc.y);
552 1 : }
553 :
554 2 : void matrix4::rotate_around_z(float ck, float sk)
555 : {
556 2 : vec4<float> ra = vec4<float>(a).mul(ck).madd(b, sk),
557 2 : rb = vec4<float>(b).mul(ck).msub(a, sk);
558 2 : a = ra;
559 2 : b = rb;
560 2 : }
561 :
562 1 : void matrix4::rotate_around_z(float angle)
563 : {
564 1 : rotate_around_z(cosf(angle), std::sin(angle));
565 1 : }
566 :
567 1 : void matrix4::rotate_around_z(const vec2 &sc)
568 : {
569 1 : rotate_around_z(sc.x, sc.y);
570 1 : }
571 :
572 9 : void matrix4::rotate(float ck, float sk, const vec &axis)
573 : {
574 9 : matrix3 m;
575 9 : m.rotate(ck, sk, axis);
576 9 : mul(m);
577 9 : }
578 :
579 3 : void matrix4::rotate(float angle, const vec &dir)
580 : {
581 3 : rotate(cosf(angle), std::sin(angle), dir);
582 3 : }
583 :
584 3 : void matrix4::rotate(const vec2 &sc, const vec &dir)
585 : {
586 3 : rotate(sc.x, sc.y, dir);
587 3 : }
588 :
589 36 : void matrix4::identity()
590 : {
591 36 : a = vec4<float>(1, 0, 0, 0);
592 36 : b = vec4<float>(0, 1, 0, 0);
593 36 : c = vec4<float>(0, 0, 1, 0);
594 36 : d = vec4<float>(0, 0, 0, 1);
595 36 : }
596 :
597 1 : void matrix4::settranslation(const vec &v)
598 : {
599 1 : d.setxyz(v);
600 1 : }
601 :
602 1 : void matrix4::settranslation(float x, float y, float z)
603 : {
604 1 : d.x = x;
605 1 : d.y = y;
606 1 : d.z = z;
607 1 : }
608 :
609 0 : void matrix4::translate(const vec &p)
610 : {
611 0 : d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
612 0 : }
613 :
614 0 : void matrix4::translate(float x, float y, float z)
615 : {
616 0 : translate(vec(x, y, z));
617 0 : }
618 :
619 0 : void matrix4::translate(const vec &p, float scale)
620 : {
621 0 : translate(vec(p).mul(scale));
622 0 : }
623 :
624 3 : void matrix4::setscale(float x, float y, float z)
625 : {
626 3 : a.x = x;
627 3 : b.y = y;
628 3 : c.z = z;
629 3 : }
630 :
631 1 : void matrix4::setscale(const vec &v)
632 : {
633 1 : setscale(v.x, v.y, v.z);
634 1 : }
635 :
636 1 : void matrix4::setscale(float n)
637 : {
638 1 : setscale(n, n, n);
639 1 : }
640 :
641 3 : void matrix4::scale(float x, float y, float z)
642 : {
643 3 : a.mul(x);
644 3 : b.mul(y);
645 3 : c.mul(z);
646 3 : }
647 :
648 1 : void matrix4::scale(const vec &v)
649 : {
650 1 : scale(v.x, v.y, v.z);
651 1 : }
652 :
653 1 : void matrix4::scale(float n)
654 : {
655 1 : scale(n, n, n);
656 1 : }
657 :
658 2 : void matrix4::scalez(float k)
659 : {
660 2 : a.z *= k;
661 2 : b.z *= k;
662 2 : c.z *= k;
663 2 : d.z *= k;
664 2 : }
665 :
666 2 : void matrix4::jitter(float x, float y)
667 : {
668 2 : a.x += x * a.w;
669 2 : a.y += y * a.w;
670 2 : b.x += x * b.w;
671 2 : b.y += y * b.w;
672 2 : c.x += x * c.w;
673 2 : c.y += y * c.w;
674 2 : d.x += x * d.w;
675 2 : d.y += y * d.w;
676 2 : }
677 :
678 1 : void matrix4::transpose()
679 : {
680 : //swap upper triangular elements of row 'a'
681 1 : std::swap(a.y, b.x);
682 1 : std::swap(a.z, c.x);
683 1 : std::swap(a.w, d.x);
684 : //swap upper triangular elements of row 'b'
685 1 : std::swap(b.z, c.y);
686 1 : std::swap(b.w, d.y);
687 : //swap upper triangular element of row 'c'
688 1 : std::swap(c.w, d.z);
689 : //no upper triangular elements on row 'd'
690 1 : }
691 :
692 1 : void matrix4::transpose(const matrix4 &m)
693 : {
694 1 : a = vec4<float>(m.a.x, m.b.x, m.c.x, m.d.x);
695 1 : b = vec4<float>(m.a.y, m.b.y, m.c.y, m.d.y);
696 1 : c = vec4<float>(m.a.z, m.b.z, m.c.z, m.d.z);
697 1 : d = vec4<float>(m.a.w, m.b.w, m.c.w, m.d.w);
698 1 : }
699 :
700 2 : void matrix4::frustum(float left, float right, float bottom, float top, float znear, float zfar)
701 : {
702 2 : float width = right - left,
703 2 : height = top - bottom,
704 2 : zrange = znear - zfar;
705 2 : a = vec4<float>(2*znear/width, 0, 0, 0); //depth to width ratio
706 2 : b = vec4<float>(0, 2*znear/height, 0, 0); //depth to height ratio
707 2 : c = vec4<float>((right + left)/width, (top + bottom)/height, (zfar + znear)/zrange, -1); //offset from centered
708 2 : d = vec4<float>(0, 0, 2*znear*zfar/zrange, 0); //depth scale
709 2 : }
710 :
711 1 : void matrix4::perspective(float fovy, float aspect, float znear, float zfar)
712 : {
713 1 : float ydist = znear * std::tan(fovy/(2*RAD)),
714 1 : xdist = ydist * aspect;
715 1 : frustum(-xdist, xdist, -ydist, ydist, znear, zfar);
716 1 : }
717 :
718 3 : void matrix4::ortho(float left, float right, float bottom, float top, float znear, float zfar)
719 : {
720 3 : float width = right - left,
721 3 : height = top - bottom,
722 3 : zrange = znear - zfar;
723 3 : a = vec4<float>(2/width, 0, 0, 0);
724 3 : b = vec4<float>(0, 2/height, 0, 0);
725 3 : c = vec4<float>(0, 0, 2/zrange, 0);
726 3 : d = vec4<float>(-(right+left)/width, -(top+bottom)/height, (zfar+znear)/zrange, 1);
727 3 : }
728 :
729 1 : void matrix4::transform(const vec &in, vec &out) const
730 : {
731 1 : out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z)).add(vec(d));
732 1 : }
733 :
734 1 : void matrix4::transform(const vec4<float> &in, vec &out) const
735 : {
736 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));
737 1 : }
738 :
739 1 : void matrix4::transform(const vec &in, vec4<float> &out) const
740 : {
741 1 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).add(d);
742 1 : }
743 :
744 1 : void matrix4::transform(const vec4<float> &in, vec4<float> &out) const
745 : {
746 1 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).madd(d, in.w);
747 1 : }
748 :
749 4 : void matrix4::transformnormal(const vec &in, vec &out) const
750 : {
751 4 : out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z));
752 4 : }
753 :
754 3 : void matrix4::transformnormal(const vec &in, vec4<float> &out) const
755 : {
756 3 : out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z);
757 3 : }
758 :
759 3 : void matrix4::transposedtransform(const vec &in, vec &out) const
760 : {
761 3 : vec p = vec(in).sub(vec(d));
762 3 : out.x = a.dot3(p);
763 3 : out.y = b.dot3(p);
764 3 : out.z = c.dot3(p);
765 3 : }
766 :
767 3 : void matrix4::transposedtransformnormal(const vec &in, vec &out) const
768 : {
769 3 : out.x = a.dot3(in);
770 3 : out.y = b.dot3(in);
771 3 : out.z = c.dot3(in);
772 3 : }
773 :
774 1 : vec matrix4::gettranslation() const
775 : {
776 1 : return vec(d);
777 : }
778 :
779 2 : vec4<float> matrix4::rowx() const
780 : {
781 2 : return vec4<float>(a.x, b.x, c.x, d.x);
782 : }
783 :
784 2 : vec4<float> matrix4::rowy() const
785 : {
786 2 : return vec4<float>(a.y, b.y, c.y, d.y);
787 : }
788 :
789 2 : vec4<float> matrix4::rowz() const
790 : {
791 2 : return vec4<float>(a.z, b.z, c.z, d.z);
792 : }
793 :
794 2 : vec4<float> matrix4::roww() const
795 : {
796 2 : return vec4<float>(a.w, b.w, c.w, d.w);
797 : }
798 :
799 3 : vec2 matrix4::lineardepthscale() const
800 : {
801 3 : return vec2(d.w, -d.z).div(c.z*d.w - d.z*c.w);
802 : }
803 :
804 : // =============================================================================
805 : // matrix4x3 object
806 : // =============================================================================
807 :
808 67 : matrix4x3::matrix4x3() : a(0,0,0), b(0,0,0), c(0,0,0), d(0,0,0)
809 : {
810 67 : }
811 :
812 34 : matrix4x3::matrix4x3(const vec &a, const vec &b, const vec &c, const vec &d) : a(a), b(b), c(c), d(d)
813 : {
814 34 : }
815 :
816 9 : matrix4x3::matrix4x3(const matrix3 &rot, const vec &trans) : a(rot.a), b(rot.b), c(rot.c), d(trans)
817 : {
818 9 : }
819 :
820 2 : matrix4x3::matrix4x3(const dualquat &dq)
821 : {
822 2 : vec4<float> r = vec4<float>(dq.real).mul(1/dq.real.squaredlen()), rr = vec4<float>(r).mul(dq.real);
823 2 : r.mul(2);
824 2 : float xy = r.x*dq.real.y, xz = r.x*dq.real.z, yz = r.y*dq.real.z,
825 2 : wx = r.w*dq.real.x, wy = r.w*dq.real.y, wz = r.w*dq.real.z;
826 2 : a = vec(rr.w + rr.x - rr.y - rr.z, xy + wz, xz - wy);
827 2 : b = vec(xy - wz, rr.w + rr.y - rr.x - rr.z, yz + wx);
828 2 : c = vec(xz + wy, yz - wx, rr.w + rr.z - rr.x - rr.y);
829 4 : d = vec(-(dq.dual.w*r.x - dq.dual.x*r.w + dq.dual.y*r.z - dq.dual.z*r.y),
830 2 : -(dq.dual.w*r.y - dq.dual.x*r.z - dq.dual.y*r.w + dq.dual.z*r.x),
831 2 : -(dq.dual.w*r.z + dq.dual.x*r.y - dq.dual.y*r.x - dq.dual.z*r.w));
832 2 : }
833 :
834 4 : void matrix4x3::mul(float k)
835 : {
836 4 : a.mul(k);
837 4 : b.mul(k);
838 4 : c.mul(k);
839 4 : d.mul(k);
840 4 : }
841 :
842 3 : void matrix4x3::setscale(float x, float y, float z)
843 : {
844 3 : a.x = x;
845 3 : b.y = y;
846 3 : c.z = z;
847 3 : }
848 :
849 1 : void matrix4x3::setscale(const vec &v)
850 : {
851 1 : setscale(v.x, v.y, v.z);
852 1 : }
853 :
854 1 : void matrix4x3::setscale(float n)
855 : {
856 1 : setscale(n, n, n);
857 1 : }
858 :
859 3 : void matrix4x3::scale(float x, float y, float z)
860 : {
861 3 : a.mul(x);
862 3 : b.mul(y);
863 3 : c.mul(z);
864 3 : }
865 :
866 1 : void matrix4x3::scale(const vec &v)
867 : {
868 1 : scale(v.x, v.y, v.z);
869 1 : }
870 :
871 1 : void matrix4x3::scale(float n)
872 : {
873 1 : scale(n, n, n);
874 1 : }
875 :
876 1 : void matrix4x3::settranslation(const vec &p)
877 : {
878 1 : d = p;
879 1 : }
880 :
881 1 : void matrix4x3::settranslation(float x, float y, float z)
882 : {
883 1 : d = vec(x, y, z);
884 1 : }
885 :
886 4 : void matrix4x3::translate(const vec &p)
887 : {
888 4 : d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
889 4 : }
890 :
891 1 : void matrix4x3::translate(float x, float y, float z)
892 : {
893 1 : translate(vec(x, y, z));
894 1 : }
895 :
896 3 : void matrix4x3::translate(const vec &p, float scale)
897 : {
898 3 : translate(vec(p).mul(scale));
899 3 : }
900 :
901 1 : void matrix4x3::accumulate(const matrix4x3 &m, float k)
902 : {
903 1 : a.madd(m.a, k);
904 1 : b.madd(m.b, k);
905 1 : c.madd(m.c, k);
906 1 : d.madd(m.d, k);
907 1 : }
908 :
909 4 : void matrix4x3::normalize()
910 : {
911 4 : a.normalize();
912 4 : b.normalize();
913 4 : c.normalize();
914 4 : }
915 :
916 1 : void matrix4x3::lerp(const matrix4x3 &to, float t)
917 : {
918 1 : a.lerp(to.a, t);
919 1 : b.lerp(to.b, t);
920 1 : c.lerp(to.c, t);
921 1 : d.lerp(to.d, t);
922 1 : }
923 1 : void matrix4x3::lerp(const matrix4x3 &from, const matrix4x3 &to, float t)
924 : {
925 1 : a.lerp(from.a, to.a, t);
926 1 : b.lerp(from.b, to.b, t);
927 1 : c.lerp(from.c, to.c, t);
928 1 : d.lerp(from.d, to.d, t);
929 1 : }
930 :
931 47 : void matrix4x3::identity()
932 : {
933 47 : a = vec(1, 0, 0);
934 47 : b = vec(0, 1, 0);
935 47 : c = vec(0, 0, 1);
936 47 : d = vec(0, 0, 0);
937 47 : }
938 :
939 2 : void matrix4x3::mul(const matrix4x3 &m, const matrix4x3 &n)
940 : {
941 2 : a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
942 2 : b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
943 2 : c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
944 2 : d = vec(m.d).madd(m.a, n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
945 2 : }
946 :
947 1 : void matrix4x3::mul(const matrix4x3 &n)
948 : {
949 1 : mul(matrix4x3(*this), n);
950 1 : }
951 :
952 1 : void matrix4x3::mul(const matrix3 &m, const matrix4x3 &n)
953 : {
954 1 : a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
955 1 : b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
956 1 : c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
957 1 : d = vec(m.a).mul(n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
958 1 : }
959 :
960 1 : void matrix4x3::mul(const matrix3 &rot, const vec &trans, const matrix4x3 &n)
961 : {
962 1 : mul(rot, n);
963 1 : d.add(trans);
964 1 : }
965 :
966 3 : void matrix4x3::transpose()
967 : {
968 3 : d = vec(a.dot(d), b.dot(d), c.dot(d)).neg();
969 3 : std::swap(a.y, b.x);
970 3 : std::swap(a.z, c.x);
971 3 : std::swap(b.z, c.y);
972 3 : }
973 :
974 8 : void matrix4x3::transpose(const matrix4x3 &o)
975 : {
976 8 : a = vec(o.a.x, o.b.x, o.c.x);
977 8 : b = vec(o.a.y, o.b.y, o.c.y);
978 8 : c = vec(o.a.z, o.b.z, o.c.z);
979 8 : d = vec(o.a.dot(o.d), o.b.dot(o.d), o.c.dot(o.d)).neg();
980 8 : }
981 :
982 2 : void matrix4x3::transposemul(const matrix4x3 &m, const matrix4x3 &n)
983 : {
984 2 : vec t(m.a.dot(m.d), m.b.dot(m.d), m.c.dot(m.d));
985 2 : a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
986 2 : b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
987 2 : c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
988 2 : d = vec(m.a.dot(n.d), m.b.dot(n.d), m.c.dot(n.d)).sub(t);
989 2 : }
990 :
991 1 : void matrix4x3::multranspose(const matrix4x3 &m, const matrix4x3 &n)
992 : {
993 1 : vec t(n.a.dot(n.d), n.b.dot(n.d), n.c.dot(n.d));
994 1 : a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
995 1 : b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
996 1 : c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
997 1 : d = vec(m.d).msub(m.a, t.x).msub(m.b, t.y).msub(m.c, t.z);
998 1 : }
999 :
1000 8 : void matrix4x3::invert(const matrix4x3 &o)
1001 : {
1002 8 : vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
1003 8 : transpose(o);
1004 8 : a.mul(unscale);
1005 8 : b.mul(unscale);
1006 8 : c.mul(unscale);
1007 8 : d.mul(unscale);
1008 8 : }
1009 :
1010 6 : void matrix4x3::invert()
1011 : {
1012 6 : invert(matrix4x3(*this));
1013 6 : }
1014 :
1015 3 : void matrix4x3::rotate(float angle, const vec &d)
1016 : {
1017 3 : rotate(cosf(angle), std::sin(angle), d);
1018 3 : }
1019 :
1020 6 : void matrix4x3::rotate(float ck, float sk, const vec &axis)
1021 : {
1022 6 : matrix3 m;
1023 6 : m.rotate(ck, sk, axis);
1024 6 : *this = matrix4x3(m, vec(0, 0, 0));
1025 6 : }
1026 :
1027 2 : void matrix4x3::rotate_around_x(float ck, float sk)
1028 : {
1029 2 : vec rb = vec(b).mul(ck).madd(c, sk),
1030 2 : rc = vec(c).mul(ck).msub(b, sk);
1031 2 : b = rb;
1032 2 : c = rc;
1033 2 : }
1034 1 : void matrix4x3::rotate_around_x(float angle)
1035 : {
1036 1 : rotate_around_x(cosf(angle), std::sin(angle));
1037 1 : }
1038 :
1039 1 : void matrix4x3::rotate_around_x(const vec2 &sc)
1040 : {
1041 1 : rotate_around_x(sc.x, sc.y);
1042 1 : }
1043 :
1044 2 : void matrix4x3::rotate_around_y(float ck, float sk)
1045 : {
1046 2 : vec rc = vec(c).mul(ck).madd(a, sk),
1047 2 : ra = vec(a).mul(ck).msub(c, sk);
1048 2 : c = rc;
1049 2 : a = ra;
1050 2 : }
1051 1 : void matrix4x3::rotate_around_y(float angle)
1052 : {
1053 1 : rotate_around_y(cosf(angle), std::sin(angle));
1054 1 : }
1055 :
1056 1 : void matrix4x3::rotate_around_y(const vec2 &sc)
1057 : {
1058 1 : rotate_around_y(sc.x, sc.y);
1059 1 : }
1060 :
1061 2 : void matrix4x3::rotate_around_z(float ck, float sk)
1062 : {
1063 2 : vec ra = vec(a).mul(ck).madd(b, sk),
1064 2 : rb = vec(b).mul(ck).msub(a, sk);
1065 2 : a = ra;
1066 2 : b = rb;
1067 2 : }
1068 :
1069 1 : void matrix4x3::rotate_around_z(float angle)
1070 : {
1071 1 : rotate_around_z(cosf(angle), std::sin(angle));
1072 1 : }
1073 :
1074 1 : void matrix4x3::rotate_around_z(const vec2 &sc)
1075 : {
1076 1 : rotate_around_z(sc.x, sc.y);
1077 1 : }
1078 :
1079 3 : vec matrix4x3::transposedtransform(const vec &o) const
1080 : {
1081 3 : vec p = vec(o).sub(d);
1082 3 : return vec(a.dot(p), b.dot(p), c.dot(p));
1083 : }
1084 :
1085 3 : vec matrix4x3::transformnormal(const vec &o) const
1086 : {
1087 3 : return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
1088 : }
1089 :
1090 3 : vec matrix4x3::transposedtransformnormal(const vec &o) const
1091 : {
1092 3 : return vec(a.dot(o), b.dot(o), c.dot(o));
1093 : }
1094 :
1095 12 : vec matrix4x3::transform(const vec &o) const
1096 : {
1097 12 : return vec(d).madd(a, o.x).madd(b, o.y).madd(c, o.z);
1098 : }
1099 :
1100 3 : vec matrix4x3::transform(const vec2 &o) const
1101 : {
1102 3 : return vec(d).madd(a, o.x).madd(b, o.y);
1103 : }
1104 :
1105 1 : vec4<float> matrix4x3::rowx() const
1106 : {
1107 1 : return vec4<float>(a.x, b.x, c.x, d.x);
1108 : }
1109 :
1110 1 : vec4<float> matrix4x3::rowy() const
1111 : {
1112 1 : return vec4<float>(a.y, b.y, c.y, d.y);
1113 : }
1114 :
1115 1 : vec4<float> matrix4x3::rowz() const
1116 : {
1117 1 : return vec4<float>(a.z, b.z, c.z, d.z);
1118 : }
1119 :
1120 : //end matrix4x3
|
