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