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