OpenVDB: openvdb/math/Mat4.h Source File

OpenVDB 9.0.0

Mat4.h
Go to the documentation of this file.
1// Copyright Contributors to the OpenVDB Project
2// SPDX-License-Identifier: MPL-2.0
3
4#ifndef OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
5#define OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
6
7#include <openvdb/Exceptions.h>
8#include <openvdb/Platform.h>
9#include "Math.h"
10#include "Mat3.h"
11#include "Vec3.h"
12#include "Vec4.h"
13#include <algorithm> // for std::copy(), std::swap()
14#include <cassert>
15#include <iomanip>
16#include <cmath>
17
18
19namespace openvdb {
20 OPENVDB_USE_VERSION_NAMESPACE
21namespace OPENVDB_VERSION_NAME {
22namespace math {
23
24template<typename T> class Vec4;
25
26
27/// @class Mat4 Mat4.h
28/// @brief 4x4 -matrix class.
29template<typename T>
30class Mat4: public Mat<4, T>
31{
32public:
33 /// Data type held by the matrix.
34 using value_type = T;
35 using ValueType = T;
36 using MyBase = Mat<4, T>;
37
38 /// Trivial constructor, the matrix is NOT initialized
39#if OPENVDB_ABI_VERSION_NUMBER >= 8
40 /// @note destructor, copy constructor, assignment operator and
41 /// move constructor are left to be defined by the compiler (default)
42 Mat4() = default;
43#else
44 Mat4() {}
45
46 /// Copy constructor
47 Mat4(const Mat<4, T> &m)
48 {
49 for (int i = 0; i < 4; ++i) {
50 for (int j = 0; j < 4; ++j) {
51 MyBase::mm[i*4 + j] = m[i][j];
52 }
53 }
54 }
55#endif
56
57 /// Constructor given array of elements, the ordering is in row major form:
58 /** @verbatim
59 a[ 0] a[1] a[ 2] a[ 3]
60 a[ 4] a[5] a[ 6] a[ 7]
61 a[ 8] a[9] a[10] a[11]
62 a[12] a[13] a[14] a[15]
63 @endverbatim */
64 template<typename Source>
65 Mat4(Source *a)
66 {
67 for (int i = 0; i < 16; i++) {
68 MyBase::mm[i] = static_cast<T>(a[i]);
69 }
70 }
71
72 /// Constructor given array of elements, the ordering is in row major form:
73 /** @verbatim
74 a b c d
75 e f g h
76 i j k l
77 m n o p
78 @endverbatim */
79 template<typename Source>
80 Mat4(Source a, Source b, Source c, Source d,
81 Source e, Source f, Source g, Source h,
82 Source i, Source j, Source k, Source l,
83 Source m, Source n, Source o, Source p)
84 {
85 MyBase::mm[ 0] = static_cast<T>(a);
86 MyBase::mm[ 1] = static_cast<T>(b);
87 MyBase::mm[ 2] = static_cast<T>(c);
88 MyBase::mm[ 3] = static_cast<T>(d);
89
90 MyBase::mm[ 4] = static_cast<T>(e);
91 MyBase::mm[ 5] = static_cast<T>(f);
92 MyBase::mm[ 6] = static_cast<T>(g);
93 MyBase::mm[ 7] = static_cast<T>(h);
94
95 MyBase::mm[ 8] = static_cast<T>(i);
96 MyBase::mm[ 9] = static_cast<T>(j);
97 MyBase::mm[10] = static_cast<T>(k);
98 MyBase::mm[11] = static_cast<T>(l);
99
100 MyBase::mm[12] = static_cast<T>(m);
101 MyBase::mm[13] = static_cast<T>(n);
102 MyBase::mm[14] = static_cast<T>(o);
103 MyBase::mm[15] = static_cast<T>(p);
104 }
105
106 /// Construct matrix from rows or columns vectors (defaults to rows
107 /// for historical reasons)
108 template<typename Source>
109 Mat4(const Vec4<Source> &v1, const Vec4<Source> &v2,
110 const Vec4<Source> &v3, const Vec4<Source> &v4, bool rows = true)
111 {
112 if (rows) {
113 this->setRows(v1, v2, v3, v4);
114 } else {
115 this->setColumns(v1, v2, v3, v4);
116 }
117 }
118
119 /// Conversion constructor
120 template<typename Source>
121 explicit Mat4(const Mat4<Source> &m)
122 {
123 const Source *src = m.asPointer();
124
125 for (int i=0; i<16; ++i) {
126 MyBase::mm[i] = static_cast<T>(src[i]);
127 }
128 }
129
130 /// Predefined constant for identity matrix
131 static const Mat4<T>& identity() {
132 static const Mat4<T> sIdentity = Mat4<T>(
133 1, 0, 0, 0,
134 0, 1, 0, 0,
135 0, 0, 1, 0,
136 0, 0, 0, 1
137 );
138 return sIdentity;
139 }
140
141 /// Predefined constant for zero matrix
142 static const Mat4<T>& zero() {
143 static const Mat4<T> sZero = Mat4<T>(
144 0, 0, 0, 0,
145 0, 0, 0, 0,
146 0, 0, 0, 0,
147 0, 0, 0, 0
148 );
149 return sZero;
150 }
151
152 /// Set ith row to vector v
153 void setRow(int i, const Vec4<T> &v)
154 {
155 // assert(i>=0 && i<4);
156 int i4 = i * 4;
157 MyBase::mm[i4+0] = v[0];
158 MyBase::mm[i4+1] = v[1];
159 MyBase::mm[i4+2] = v[2];
160 MyBase::mm[i4+3] = v[3];
161 }
162
163 /// Get ith row, e.g. Vec4f v = m.row(1);
164 Vec4<T> row(int i) const
165 {
166 // assert(i>=0 && i<3);
167 return Vec4<T>((*this)(i,0), (*this)(i,1), (*this)(i,2), (*this)(i,3));
168 }
169
170 /// Set jth column to vector v
171 void setCol(int j, const Vec4<T>& v)
172 {
173 // assert(j>=0 && j<4);
174 MyBase::mm[ 0+j] = v[0];
175 MyBase::mm[ 4+j] = v[1];
176 MyBase::mm[ 8+j] = v[2];
177 MyBase::mm[12+j] = v[3];
178 }
179
180 /// Get jth column, e.g. Vec4f v = m.col(0);
181 Vec4<T> col(int j) const
182 {
183 // assert(j>=0 && j<4);
184 return Vec4<T>((*this)(0,j), (*this)(1,j), (*this)(2,j), (*this)(3,j));
185 }
186
187 /// Alternative indexed reference to the elements
188 /// Note that the indices are row first and column second.
189 /// e.g. m(0,0) = 1;
190 T& operator()(int i, int j)
191 {
192 // assert(i>=0 && i<4);
193 // assert(j>=0 && j<4);
194 return MyBase::mm[4*i+j];
195 }
196
197 /// Alternative indexed constant reference to the elements,
198 /// Note that the indices are row first and column second.
199 /// e.g. float f = m(1,0);
200 T operator()(int i, int j) const
201 {
202 // assert(i>=0 && i<4);
203 // assert(j>=0 && j<4);
204 return MyBase::mm[4*i+j];
205 }
206
207 /// Set the rows of this matrix to the vectors v1, v2, v3, v4
208 void setRows(const Vec4<T> &v1, const Vec4<T> &v2,
209 const Vec4<T> &v3, const Vec4<T> &v4)
210 {
211 MyBase::mm[ 0] = v1[0];
212 MyBase::mm[ 1] = v1[1];
213 MyBase::mm[ 2] = v1[2];
214 MyBase::mm[ 3] = v1[3];
215
216 MyBase::mm[ 4] = v2[0];
217 MyBase::mm[ 5] = v2[1];
218 MyBase::mm[ 6] = v2[2];
219 MyBase::mm[ 7] = v2[3];
220
221 MyBase::mm[ 8] = v3[0];
222 MyBase::mm[ 9] = v3[1];
223 MyBase::mm[10] = v3[2];
224 MyBase::mm[11] = v3[3];
225
226 MyBase::mm[12] = v4[0];
227 MyBase::mm[13] = v4[1];
228 MyBase::mm[14] = v4[2];
229 MyBase::mm[15] = v4[3];
230 }
231
232 /// Set the columns of this matrix to the vectors v1, v2, v3, v4
233 void setColumns(const Vec4<T> &v1, const Vec4<T> &v2,
234 const Vec4<T> &v3, const Vec4<T> &v4)
235 {
236 MyBase::mm[ 0] = v1[0];
237 MyBase::mm[ 1] = v2[0];
238 MyBase::mm[ 2] = v3[0];
239 MyBase::mm[ 3] = v4[0];
240
241 MyBase::mm[ 4] = v1[1];
242 MyBase::mm[ 5] = v2[1];
243 MyBase::mm[ 6] = v3[1];
244 MyBase::mm[ 7] = v4[1];
245
246 MyBase::mm[ 8] = v1[2];
247 MyBase::mm[ 9] = v2[2];
248 MyBase::mm[10] = v3[2];
249 MyBase::mm[11] = v4[2];
250
251 MyBase::mm[12] = v1[3];
252 MyBase::mm[13] = v2[3];
253 MyBase::mm[14] = v3[3];
254 MyBase::mm[15] = v4[3];
255 }
256
257 // Set this matrix to zero
258 void setZero()
259 {
260 MyBase::mm[ 0] = 0;
261 MyBase::mm[ 1] = 0;
262 MyBase::mm[ 2] = 0;
263 MyBase::mm[ 3] = 0;
264 MyBase::mm[ 4] = 0;
265 MyBase::mm[ 5] = 0;
266 MyBase::mm[ 6] = 0;
267 MyBase::mm[ 7] = 0;
268 MyBase::mm[ 8] = 0;
269 MyBase::mm[ 9] = 0;
270 MyBase::mm[10] = 0;
271 MyBase::mm[11] = 0;
272 MyBase::mm[12] = 0;
273 MyBase::mm[13] = 0;
274 MyBase::mm[14] = 0;
275 MyBase::mm[15] = 0;
276 }
277
278 /// Set this matrix to identity
279 void setIdentity()
280 {
281 MyBase::mm[ 0] = 1;
282 MyBase::mm[ 1] = 0;
283 MyBase::mm[ 2] = 0;
284 MyBase::mm[ 3] = 0;
285
286 MyBase::mm[ 4] = 0;
287 MyBase::mm[ 5] = 1;
288 MyBase::mm[ 6] = 0;
289 MyBase::mm[ 7] = 0;
290
291 MyBase::mm[ 8] = 0;
292 MyBase::mm[ 9] = 0;
293 MyBase::mm[10] = 1;
294 MyBase::mm[11] = 0;
295
296 MyBase::mm[12] = 0;
297 MyBase::mm[13] = 0;
298 MyBase::mm[14] = 0;
299 MyBase::mm[15] = 1;
300 }
301
302
303 /// Set upper left to a Mat3
304 void setMat3(const Mat3<T> &m)
305 {
306 for (int i = 0; i < 3; i++)
307 for (int j=0; j < 3; j++)
308 MyBase::mm[i*4+j] = m[i][j];
309 }
310
311 Mat3<T> getMat3() const
312 {
313 Mat3<T> m;
314
315 for (int i = 0; i < 3; i++)
316 for (int j = 0; j < 3; j++)
317 m[i][j] = MyBase::mm[i*4+j];
318
319 return m;
320 }
321
322 /// Return the translation component
323 Vec3<T> getTranslation() const
324 {
325 return Vec3<T>(MyBase::mm[12], MyBase::mm[13], MyBase::mm[14]);
326 }
327
328 void setTranslation(const Vec3<T> &t)
329 {
330 MyBase::mm[12] = t[0];
331 MyBase::mm[13] = t[1];
332 MyBase::mm[14] = t[2];
333 }
334
335 /// Assignment operator
336 template<typename Source>
337 const Mat4& operator=(const Mat4<Source> &m)
338 {
339 const Source *src = m.asPointer();
340
341 // don't suppress warnings when assigning from different numerical types
342 std::copy(src, (src + this->numElements()), MyBase::mm);
343 return *this;
344 }
345
346 /// Return @c true if this matrix is equivalent to @a m within a tolerance of @a eps.
347 bool eq(const Mat4 &m, T eps=1.0e-8) const
348 {
349 for (int i = 0; i < 16; i++) {
350 if (!isApproxEqual(MyBase::mm[i], m.mm[i], eps))
351 return false;
352 }
353 return true;
354 }
355
356 /// Negation operator, for e.g. m1 = -m2;
357 Mat4<T> operator-() const
358 {
359 return Mat4<T>(
360 -MyBase::mm[ 0], -MyBase::mm[ 1], -MyBase::mm[ 2], -MyBase::mm[ 3],
361 -MyBase::mm[ 4], -MyBase::mm[ 5], -MyBase::mm[ 6], -MyBase::mm[ 7],
362 -MyBase::mm[ 8], -MyBase::mm[ 9], -MyBase::mm[10], -MyBase::mm[11],
363 -MyBase::mm[12], -MyBase::mm[13], -MyBase::mm[14], -MyBase::mm[15]
364 );
365 } // trivial
366
367 /// Multiply each element of this matrix by @a scalar.
368 template <typename S>
369 const Mat4<T>& operator*=(S scalar)
370 {
371 MyBase::mm[ 0] *= scalar;
372 MyBase::mm[ 1] *= scalar;
373 MyBase::mm[ 2] *= scalar;
374 MyBase::mm[ 3] *= scalar;
375
376 MyBase::mm[ 4] *= scalar;
377 MyBase::mm[ 5] *= scalar;
378 MyBase::mm[ 6] *= scalar;
379 MyBase::mm[ 7] *= scalar;
380
381 MyBase::mm[ 8] *= scalar;
382 MyBase::mm[ 9] *= scalar;
383 MyBase::mm[10] *= scalar;
384 MyBase::mm[11] *= scalar;
385
386 MyBase::mm[12] *= scalar;
387 MyBase::mm[13] *= scalar;
388 MyBase::mm[14] *= scalar;
389 MyBase::mm[15] *= scalar;
390 return *this;
391 }
392
393 /// Add each element of the given matrix to the corresponding element of this matrix.
394 template <typename S>
395 const Mat4<T> &operator+=(const Mat4<S> &m1)
396 {
397 const S* s = m1.asPointer();
398
399 MyBase::mm[ 0] += s[ 0];
400 MyBase::mm[ 1] += s[ 1];
401 MyBase::mm[ 2] += s[ 2];
402 MyBase::mm[ 3] += s[ 3];
403
404 MyBase::mm[ 4] += s[ 4];
405 MyBase::mm[ 5] += s[ 5];
406 MyBase::mm[ 6] += s[ 6];
407 MyBase::mm[ 7] += s[ 7];
408
409 MyBase::mm[ 8] += s[ 8];
410 MyBase::mm[ 9] += s[ 9];
411 MyBase::mm[10] += s[10];
412 MyBase::mm[11] += s[11];
413
414 MyBase::mm[12] += s[12];
415 MyBase::mm[13] += s[13];
416 MyBase::mm[14] += s[14];
417 MyBase::mm[15] += s[15];
418
419 return *this;
420 }
421
422 /// Subtract each element of the given matrix from the corresponding element of this matrix.
423 template <typename S>
424 const Mat4<T> &operator-=(const Mat4<S> &m1)
425 {
426 const S* s = m1.asPointer();
427
428 MyBase::mm[ 0] -= s[ 0];
429 MyBase::mm[ 1] -= s[ 1];
430 MyBase::mm[ 2] -= s[ 2];
431 MyBase::mm[ 3] -= s[ 3];
432
433 MyBase::mm[ 4] -= s[ 4];
434 MyBase::mm[ 5] -= s[ 5];
435 MyBase::mm[ 6] -= s[ 6];
436 MyBase::mm[ 7] -= s[ 7];
437
438 MyBase::mm[ 8] -= s[ 8];
439 MyBase::mm[ 9] -= s[ 9];
440 MyBase::mm[10] -= s[10];
441 MyBase::mm[11] -= s[11];
442
443 MyBase::mm[12] -= s[12];
444 MyBase::mm[13] -= s[13];
445 MyBase::mm[14] -= s[14];
446 MyBase::mm[15] -= s[15];
447
448 return *this;
449 }
450
451 /// Multiply this matrix by the given matrix.
452 template <typename S>
453 const Mat4<T> &operator*=(const Mat4<S> &m1)
454 {
455 Mat4<T> m0(*this);
456
457 const T* s0 = m0.asPointer();
458 const S* s1 = m1.asPointer();
459
460 for (int i = 0; i < 4; i++) {
461 int i4 = 4 * i;
462 MyBase::mm[i4+0] = static_cast<T>(s0[i4+0] * s1[ 0] +
463 s0[i4+1] * s1[ 4] +
464 s0[i4+2] * s1[ 8] +
465 s0[i4+3] * s1[12]);
466
467 MyBase::mm[i4+1] = static_cast<T>(s0[i4+0] * s1[ 1] +
468 s0[i4+1] * s1[ 5] +
469 s0[i4+2] * s1[ 9] +
470 s0[i4+3] * s1[13]);
471
472 MyBase::mm[i4+2] = static_cast<T>(s0[i4+0] * s1[ 2] +
473 s0[i4+1] * s1[ 6] +
474 s0[i4+2] * s1[10] +
475 s0[i4+3] * s1[14]);
476
477 MyBase::mm[i4+3] = static_cast<T>(s0[i4+0] * s1[ 3] +
478 s0[i4+1] * s1[ 7] +
479 s0[i4+2] * s1[11] +
480 s0[i4+3] * s1[15]);
481 }
482 return *this;
483 }
484
485 /// @return transpose of this
486 Mat4 transpose() const
487 {
488 return Mat4<T>(
489 MyBase::mm[ 0], MyBase::mm[ 4], MyBase::mm[ 8], MyBase::mm[12],
490 MyBase::mm[ 1], MyBase::mm[ 5], MyBase::mm[ 9], MyBase::mm[13],
491 MyBase::mm[ 2], MyBase::mm[ 6], MyBase::mm[10], MyBase::mm[14],
492 MyBase::mm[ 3], MyBase::mm[ 7], MyBase::mm[11], MyBase::mm[15]
493 );
494 }
495
496
497 /// @return inverse of this
498 /// @throw ArithmeticError if singular
499 Mat4 inverse(T tolerance = 0) const
500 {
501 //
502 // inv [ A | b ] = [ E | f ] A: 3x3, b: 3x1, c': 1x3 d: 1x1
503 // [ c' | d ] [ g' | h ]
504 //
505 // If A is invertible use
506 //
507 // E = A^-1 + p*h*r
508 // p = A^-1 * b
509 // f = -p * h
510 // g' = -h * c'
511 // h = 1 / (d - c'*p)
512 // r' = c'*A^-1
513 //
514 // Otherwise use gauss-jordan elimination
515 //
516
517 //
518 // We create this alias to ourself so we can easily use own subscript
519 // operator.
520 const Mat4<T>& m(*this);
521
522 T m0011 = m[0][0] * m[1][1];
523 T m0012 = m[0][0] * m[1][2];
524 T m0110 = m[0][1] * m[1][0];
525 T m0210 = m[0][2] * m[1][0];
526 T m0120 = m[0][1] * m[2][0];
527 T m0220 = m[0][2] * m[2][0];
528
529 T detA = m0011 * m[2][2] - m0012 * m[2][1] - m0110 * m[2][2]
530 + m0210 * m[2][1] + m0120 * m[1][2] - m0220 * m[1][1];
531
532 bool hasPerspective =
533 (!isExactlyEqual(m[0][3], T(0.0)) ||
534 !isExactlyEqual(m[1][3], T(0.0)) ||
535 !isExactlyEqual(m[2][3], T(0.0)) ||
536 !isExactlyEqual(m[3][3], T(1.0)));
537
538 T det;
539 if (hasPerspective) {
540 det = m[0][3] * det3(m, 1,2,3, 0,2,1)
541 + m[1][3] * det3(m, 2,0,3, 0,2,1)
542 + m[2][3] * det3(m, 3,0,1, 0,2,1)
543 + m[3][3] * detA;
544 } else {
545 det = detA * m[3][3];
546 }
547
548 Mat4<T> inv;
549 bool invertible;
550
551 if (isApproxEqual(det,T(0.0),tolerance)) {
552 invertible = false;
553
554 } else if (isApproxEqual(detA,T(0.0),T(1e-8))) {
555 // det is too small to rely on inversion by subblocks
556 invertible = m.invert(inv, tolerance);
557
558 } else {
559 invertible = true;
560 detA = 1.0 / detA;
561
562 //
563 // Calculate A^-1
564 //
565 inv[0][0] = detA * ( m[1][1] * m[2][2] - m[1][2] * m[2][1]);
566 inv[0][1] = detA * (-m[0][1] * m[2][2] + m[0][2] * m[2][1]);
567 inv[0][2] = detA * ( m[0][1] * m[1][2] - m[0][2] * m[1][1]);
568
569 inv[1][0] = detA * (-m[1][0] * m[2][2] + m[1][2] * m[2][0]);
570 inv[1][1] = detA * ( m[0][0] * m[2][2] - m0220);
571 inv[1][2] = detA * ( m0210 - m0012);
572
573 inv[2][0] = detA * ( m[1][0] * m[2][1] - m[1][1] * m[2][0]);
574 inv[2][1] = detA * ( m0120 - m[0][0] * m[2][1]);
575 inv[2][2] = detA * ( m0011 - m0110);
576
577 if (hasPerspective) {
578 //
579 // Calculate r, p, and h
580 //
581 Vec3<T> r;
582 r[0] = m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
583 + m[3][2] * inv[2][0];
584 r[1] = m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
585 + m[3][2] * inv[2][1];
586 r[2] = m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
587 + m[3][2] * inv[2][2];
588
589 Vec3<T> p;
590 p[0] = inv[0][0] * m[0][3] + inv[0][1] * m[1][3]
591 + inv[0][2] * m[2][3];
592 p[1] = inv[1][0] * m[0][3] + inv[1][1] * m[1][3]
593 + inv[1][2] * m[2][3];
594 p[2] = inv[2][0] * m[0][3] + inv[2][1] * m[1][3]
595 + inv[2][2] * m[2][3];
596
597 T h = m[3][3] - p.dot(Vec3<T>(m[3][0],m[3][1],m[3][2]));
598 if (isApproxEqual(h,T(0.0),tolerance)) {
599 invertible = false;
600
601 } else {
602 h = 1.0 / h;
603
604 //
605 // Calculate h, g, and f
606 //
607 inv[3][3] = h;
608 inv[3][0] = -h * r[0];
609 inv[3][1] = -h * r[1];
610 inv[3][2] = -h * r[2];
611
612 inv[0][3] = -h * p[0];
613 inv[1][3] = -h * p[1];
614 inv[2][3] = -h * p[2];
615
616 //
617 // Calculate E
618 //
619 p *= h;
620 inv[0][0] += p[0] * r[0];
621 inv[0][1] += p[0] * r[1];
622 inv[0][2] += p[0] * r[2];
623 inv[1][0] += p[1] * r[0];
624 inv[1][1] += p[1] * r[1];
625 inv[1][2] += p[1] * r[2];
626 inv[2][0] += p[2] * r[0];
627 inv[2][1] += p[2] * r[1];
628 inv[2][2] += p[2] * r[2];
629 }
630 } else {
631 // Equations are much simpler in the non-perspective case
632 inv[3][0] = - (m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
633 + m[3][2] * inv[2][0]);
634 inv[3][1] = - (m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
635 + m[3][2] * inv[2][1]);
636 inv[3][2] = - (m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
637 + m[3][2] * inv[2][2]);
638 inv[0][3] = 0.0;
639 inv[1][3] = 0.0;
640 inv[2][3] = 0.0;
641 inv[3][3] = 1.0;
642 }
643 }
644
645 if (!invertible) OPENVDB_THROW(ArithmeticError, "Inversion of singular 4x4 matrix");
646 return inv;
647 }
648
649
650 /// Determinant of matrix
651 T det() const
652 {
653 const T *ap;
654 Mat3<T> submat;
655 T det;
656 T *sp;
657 int i, j, k, sign;
658
659 det = 0;
660 sign = 1;
661 for (i = 0; i < 4; i++) {
662 ap = &MyBase::mm[ 0];
663 sp = submat.asPointer();
664 for (j = 0; j < 4; j++) {
665 for (k = 0; k < 4; k++) {
666 if ((k != i) && (j != 0)) {
667 *sp++ = *ap;
668 }
669 ap++;
670 }
671 }
672
673 det += T(sign) * MyBase::mm[i] * submat.det();
674 sign = -sign;
675 }
676
677 return det;
678 }
679
680 /// Sets the matrix to a matrix that translates by v
681 static Mat4 translation(const Vec3d& v)
682 {
683 return Mat4(
684 T(1), T(0), T(0), T(0),
685 T(0), T(1), T(0), T(0),
686 T(0), T(0), T(1), T(0),
687 T(v.x()), T(v.y()),T(v.z()), T(1));
688 }
689
690 /// Sets the matrix to a matrix that translates by v
691 template <typename T0>
692 void setToTranslation(const Vec3<T0>& v)
693 {
694 MyBase::mm[ 0] = 1;
695 MyBase::mm[ 1] = 0;
696 MyBase::mm[ 2] = 0;
697 MyBase::mm[ 3] = 0;
698
699 MyBase::mm[ 4] = 0;
700 MyBase::mm[ 5] = 1;
701 MyBase::mm[ 6] = 0;
702 MyBase::mm[ 7] = 0;
703
704 MyBase::mm[ 8] = 0;
705 MyBase::mm[ 9] = 0;
706 MyBase::mm[10] = 1;
707 MyBase::mm[11] = 0;
708
709 MyBase::mm[12] = v.x();
710 MyBase::mm[13] = v.y();
711 MyBase::mm[14] = v.z();
712 MyBase::mm[15] = 1;
713 }
714
715 /// Left multiples by the specified translation, i.e. Trans * (*this)
716 template <typename T0>
717 void preTranslate(const Vec3<T0>& tr)
718 {
719 Vec3<T> tmp(tr.x(), tr.y(), tr.z());
720 Mat4<T> Tr = Mat4<T>::translation(tmp);
721
722 *this = Tr * (*this);
723
724 }
725
726 /// Right multiplies by the specified translation matrix, i.e. (*this) * Trans
727 template <typename T0>
728 void postTranslate(const Vec3<T0>& tr)
729 {
730 Vec3<T> tmp(tr.x(), tr.y(), tr.z());
731 Mat4<T> Tr = Mat4<T>::translation(tmp);
732
733 *this = (*this) * Tr;
734
735 }
736
737
738 /// Sets the matrix to a matrix that scales by v
739 template <typename T0>
740 void setToScale(const Vec3<T0>& v)
741 {
742 this->setIdentity();
743 MyBase::mm[ 0] = v.x();
744 MyBase::mm[ 5] = v.y();
745 MyBase::mm[10] = v.z();
746 }
747
748 // Left multiples by the specified scale matrix, i.e. Sc * (*this)
749 template <typename T0>
750 void preScale(const Vec3<T0>& v)
751 {
752 MyBase::mm[ 0] *= v.x();
753 MyBase::mm[ 1] *= v.x();
754 MyBase::mm[ 2] *= v.x();
755 MyBase::mm[ 3] *= v.x();
756
757 MyBase::mm[ 4] *= v.y();
758 MyBase::mm[ 5] *= v.y();
759 MyBase::mm[ 6] *= v.y();
760 MyBase::mm[ 7] *= v.y();
761
762 MyBase::mm[ 8] *= v.z();
763 MyBase::mm[ 9] *= v.z();
764 MyBase::mm[10] *= v.z();
765 MyBase::mm[11] *= v.z();
766 }
767
768
769
770 // Right multiples by the specified scale matrix, i.e. (*this) * Sc
771 template <typename T0>
772 void postScale(const Vec3<T0>& v)
773 {
774
775 MyBase::mm[ 0] *= v.x();
776 MyBase::mm[ 1] *= v.y();
777 MyBase::mm[ 2] *= v.z();
778
779 MyBase::mm[ 4] *= v.x();
780 MyBase::mm[ 5] *= v.y();
781 MyBase::mm[ 6] *= v.z();
782
783 MyBase::mm[ 8] *= v.x();
784 MyBase::mm[ 9] *= v.y();
785 MyBase::mm[10] *= v.z();
786
787 MyBase::mm[12] *= v.x();
788 MyBase::mm[13] *= v.y();
789 MyBase::mm[14] *= v.z();
790
791 }
792
793
794 /// @brief Sets the matrix to a rotation about the given axis.
795 /// @param axis The axis (one of X, Y, Z) to rotate about.
796 /// @param angle The rotation angle, in radians.
797 void setToRotation(Axis axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
798
799 /// @brief Sets the matrix to a rotation about an arbitrary axis
800 /// @param axis The axis of rotation (cannot be zero-length)
801 /// @param angle The rotation angle, in radians.
802 void setToRotation(const Vec3<T>& axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
803
804 /// @brief Sets the matrix to a rotation that maps v1 onto v2 about the cross
805 /// product of v1 and v2.
806 void setToRotation(const Vec3<T>& v1, const Vec3<T>& v2) {*this = rotation<Mat4<T> >(v1, v2);}
807
808
809 /// @brief Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
810 /// @param axis The axis (one of X, Y, Z) of rotation.
811 /// @param angle The clock-wise rotation angle, in radians.
812 void preRotate(Axis axis, T angle)
813 {
814 T c = static_cast<T>(cos(angle));
815 T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
816
817 switch (axis) {
818 case X_AXIS:
819 {
820 T a4, a5, a6, a7;
821
822 a4 = c * MyBase::mm[ 4] - s * MyBase::mm[ 8];
823 a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 9];
824 a6 = c * MyBase::mm[ 6] - s * MyBase::mm[10];
825 a7 = c * MyBase::mm[ 7] - s * MyBase::mm[11];
826
827
828 MyBase::mm[ 8] = s * MyBase::mm[ 4] + c * MyBase::mm[ 8];
829 MyBase::mm[ 9] = s * MyBase::mm[ 5] + c * MyBase::mm[ 9];
830 MyBase::mm[10] = s * MyBase::mm[ 6] + c * MyBase::mm[10];
831 MyBase::mm[11] = s * MyBase::mm[ 7] + c * MyBase::mm[11];
832
833 MyBase::mm[ 4] = a4;
834 MyBase::mm[ 5] = a5;
835 MyBase::mm[ 6] = a6;
836 MyBase::mm[ 7] = a7;
837 }
838 break;
839
840 case Y_AXIS:
841 {
842 T a0, a1, a2, a3;
843
844 a0 = c * MyBase::mm[ 0] + s * MyBase::mm[ 8];
845 a1 = c * MyBase::mm[ 1] + s * MyBase::mm[ 9];
846 a2 = c * MyBase::mm[ 2] + s * MyBase::mm[10];
847 a3 = c * MyBase::mm[ 3] + s * MyBase::mm[11];
848
849 MyBase::mm[ 8] = -s * MyBase::mm[ 0] + c * MyBase::mm[ 8];
850 MyBase::mm[ 9] = -s * MyBase::mm[ 1] + c * MyBase::mm[ 9];
851 MyBase::mm[10] = -s * MyBase::mm[ 2] + c * MyBase::mm[10];
852 MyBase::mm[11] = -s * MyBase::mm[ 3] + c * MyBase::mm[11];
853
854
855 MyBase::mm[ 0] = a0;
856 MyBase::mm[ 1] = a1;
857 MyBase::mm[ 2] = a2;
858 MyBase::mm[ 3] = a3;
859 }
860 break;
861
862 case Z_AXIS:
863 {
864 T a0, a1, a2, a3;
865
866 a0 = c * MyBase::mm[ 0] - s * MyBase::mm[ 4];
867 a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 5];
868 a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 6];
869 a3 = c * MyBase::mm[ 3] - s * MyBase::mm[ 7];
870
871 MyBase::mm[ 4] = s * MyBase::mm[ 0] + c * MyBase::mm[ 4];
872 MyBase::mm[ 5] = s * MyBase::mm[ 1] + c * MyBase::mm[ 5];
873 MyBase::mm[ 6] = s * MyBase::mm[ 2] + c * MyBase::mm[ 6];
874 MyBase::mm[ 7] = s * MyBase::mm[ 3] + c * MyBase::mm[ 7];
875
876 MyBase::mm[ 0] = a0;
877 MyBase::mm[ 1] = a1;
878 MyBase::mm[ 2] = a2;
879 MyBase::mm[ 3] = a3;
880 }
881 break;
882
883 default:
884 assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
885 }
886 }
887
888
889 /// @brief Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
890 /// @param axis The axis (one of X, Y, Z) of rotation.
891 /// @param angle The clock-wise rotation angle, in radians.
892 void postRotate(Axis axis, T angle)
893 {
894 T c = static_cast<T>(cos(angle));
895 T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
896
897
898
899 switch (axis) {
900 case X_AXIS:
901 {
902 T a2, a6, a10, a14;
903
904 a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 1];
905 a6 = c * MyBase::mm[ 6] - s * MyBase::mm[ 5];
906 a10 = c * MyBase::mm[10] - s * MyBase::mm[ 9];
907 a14 = c * MyBase::mm[14] - s * MyBase::mm[13];
908
909
910 MyBase::mm[ 1] = c * MyBase::mm[ 1] + s * MyBase::mm[ 2];
911 MyBase::mm[ 5] = c * MyBase::mm[ 5] + s * MyBase::mm[ 6];
912 MyBase::mm[ 9] = c * MyBase::mm[ 9] + s * MyBase::mm[10];
913 MyBase::mm[13] = c * MyBase::mm[13] + s * MyBase::mm[14];
914
915 MyBase::mm[ 2] = a2;
916 MyBase::mm[ 6] = a6;
917 MyBase::mm[10] = a10;
918 MyBase::mm[14] = a14;
919 }
920 break;
921
922 case Y_AXIS:
923 {
924 T a2, a6, a10, a14;
925
926 a2 = c * MyBase::mm[ 2] + s * MyBase::mm[ 0];
927 a6 = c * MyBase::mm[ 6] + s * MyBase::mm[ 4];
928 a10 = c * MyBase::mm[10] + s * MyBase::mm[ 8];
929 a14 = c * MyBase::mm[14] + s * MyBase::mm[12];
930
931 MyBase::mm[ 0] = c * MyBase::mm[ 0] - s * MyBase::mm[ 2];
932 MyBase::mm[ 4] = c * MyBase::mm[ 4] - s * MyBase::mm[ 6];
933 MyBase::mm[ 8] = c * MyBase::mm[ 8] - s * MyBase::mm[10];
934 MyBase::mm[12] = c * MyBase::mm[12] - s * MyBase::mm[14];
935
936 MyBase::mm[ 2] = a2;
937 MyBase::mm[ 6] = a6;
938 MyBase::mm[10] = a10;
939 MyBase::mm[14] = a14;
940 }
941 break;
942
943 case Z_AXIS:
944 {
945 T a1, a5, a9, a13;
946
947 a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 0];
948 a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 4];
949 a9 = c * MyBase::mm[ 9] - s * MyBase::mm[ 8];
950 a13 = c * MyBase::mm[13] - s * MyBase::mm[12];
951
952 MyBase::mm[ 0] = c * MyBase::mm[ 0] + s * MyBase::mm[ 1];
953 MyBase::mm[ 4] = c * MyBase::mm[ 4] + s * MyBase::mm[ 5];
954 MyBase::mm[ 8] = c * MyBase::mm[ 8] + s * MyBase::mm[ 9];
955 MyBase::mm[12] = c * MyBase::mm[12] + s * MyBase::mm[13];
956
957 MyBase::mm[ 1] = a1;
958 MyBase::mm[ 5] = a5;
959 MyBase::mm[ 9] = a9;
960 MyBase::mm[13] = a13;
961
962 }
963 break;
964
965 default:
966 assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
967 }
968 }
969
970 /// @brief Sets the matrix to a shear along axis0 by a fraction of axis1.
971 /// @param axis0 The fixed axis of the shear.
972 /// @param axis1 The shear axis.
973 /// @param shearby The shear factor.
974 void setToShear(Axis axis0, Axis axis1, T shearby)
975 {
976 *this = shear<Mat4<T> >(axis0, axis1, shearby);
977 }
978
979
980 /// @brief Left multiplies a shearing transformation into the matrix.
981 /// @see setToShear
982 void preShear(Axis axis0, Axis axis1, T shear)
983 {
984 int index0 = static_cast<int>(axis0);
985 int index1 = static_cast<int>(axis1);
986
987 // to row "index1" add a multiple of the index0 row
988 MyBase::mm[index1 * 4 + 0] += shear * MyBase::mm[index0 * 4 + 0];
989 MyBase::mm[index1 * 4 + 1] += shear * MyBase::mm[index0 * 4 + 1];
990 MyBase::mm[index1 * 4 + 2] += shear * MyBase::mm[index0 * 4 + 2];
991 MyBase::mm[index1 * 4 + 3] += shear * MyBase::mm[index0 * 4 + 3];
992 }
993
994
995 /// @brief Right multiplies a shearing transformation into the matrix.
996 /// @see setToShear
997 void postShear(Axis axis0, Axis axis1, T shear)
998 {
999 int index0 = static_cast<int>(axis0);
1000 int index1 = static_cast<int>(axis1);
1001
1002 // to collumn "index0" add a multiple of the index1 row
1003 MyBase::mm[index0 + 0] += shear * MyBase::mm[index1 + 0];
1004 MyBase::mm[index0 + 4] += shear * MyBase::mm[index1 + 4];
1005 MyBase::mm[index0 + 8] += shear * MyBase::mm[index1 + 8];
1006 MyBase::mm[index0 + 12] += shear * MyBase::mm[index1 + 12];
1007
1008 }
1009
1010 /// Transform a Vec4 by post-multiplication.
1011 template<typename T0>
1012 Vec4<T0> transform(const Vec4<T0> &v) const
1013 {
1014 return static_cast< Vec4<T0> >(v * *this);
1015 }
1016
1017 /// Transform a Vec3 by post-multiplication, without homogenous division.
1018 template<typename T0>
1019 Vec3<T0> transform(const Vec3<T0> &v) const
1020 {
1021 return static_cast< Vec3<T0> >(v * *this);
1022 }
1023
1024 /// Transform a Vec4 by pre-multiplication.
1025 template<typename T0>
1026 Vec4<T0> pretransform(const Vec4<T0> &v) const
1027 {
1028 return static_cast< Vec4<T0> >(*this * v);
1029 }
1030
1031 /// Transform a Vec3 by pre-multiplication, without homogenous division.
1032 template<typename T0>
1033 Vec3<T0> pretransform(const Vec3<T0> &v) const
1034 {
1035 return static_cast< Vec3<T0> >(*this * v);
1036 }
1037
1038 /// Transform a Vec3 by post-multiplication, doing homogenous divison.
1039 template<typename T0>
1040 Vec3<T0> transformH(const Vec3<T0> &p) const
1041 {
1042 T0 w;
1043
1044 // w = p * (*this).col(3);
1045 w = static_cast<T0>(p[0] * MyBase::mm[ 3] + p[1] * MyBase::mm[ 7]
1046 + p[2] * MyBase::mm[11] + MyBase::mm[15]);
1047
1048 if ( !isExactlyEqual(w , 0.0) ) {
1049 return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 4] +
1050 p[2] * MyBase::mm[ 8] + MyBase::mm[12]) / w),
1051 static_cast<T0>((p[0] * MyBase::mm[ 1] + p[1] * MyBase::mm[ 5] +
1052 p[2] * MyBase::mm[ 9] + MyBase::mm[13]) / w),
1053 static_cast<T0>((p[0] * MyBase::mm[ 2] + p[1] * MyBase::mm[ 6] +
1054 p[2] * MyBase::mm[10] + MyBase::mm[14]) / w));
1055 }
1056
1057 return Vec3<T0>(0, 0, 0);
1058 }
1059
1060 /// Transform a Vec3 by pre-multiplication, doing homogenous division.
1061 template<typename T0>
1062 Vec3<T0> pretransformH(const Vec3<T0> &p) const
1063 {
1064 T0 w;
1065
1066 // w = p * (*this).col(3);
1067 w = p[0] * MyBase::mm[12] + p[1] * MyBase::mm[13] + p[2] * MyBase::mm[14] + MyBase::mm[15];
1068
1069 if ( !isExactlyEqual(w , 0.0) ) {
1070 return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 1] +
1071 p[2] * MyBase::mm[ 2] + MyBase::mm[ 3]) / w),
1072 static_cast<T0>((p[0] * MyBase::mm[ 4] + p[1] * MyBase::mm[ 5] +
1073 p[2] * MyBase::mm[ 6] + MyBase::mm[ 7]) / w),
1074 static_cast<T0>((p[0] * MyBase::mm[ 8] + p[1] * MyBase::mm[ 9] +
1075 p[2] * MyBase::mm[10] + MyBase::mm[11]) / w));
1076 }
1077
1078 return Vec3<T0>(0, 0, 0);
1079 }
1080
1081 /// Transform a Vec3 by post-multiplication, without translation.
1082 template<typename T0>
1083 Vec3<T0> transform3x3(const Vec3<T0> &v) const
1084 {
1085 return Vec3<T0>(
1086 static_cast<T0>(v[0] * MyBase::mm[ 0] + v[1] * MyBase::mm[ 4] + v[2] * MyBase::mm[ 8]),
1087 static_cast<T0>(v[0] * MyBase::mm[ 1] + v[1] * MyBase::mm[ 5] + v[2] * MyBase::mm[ 9]),
1088 static_cast<T0>(v[0] * MyBase::mm[ 2] + v[1] * MyBase::mm[ 6] + v[2] * MyBase::mm[10]));
1089 }
1090
1091
1092private:
1093 bool invert(Mat4<T> &inverse, T tolerance) const;
1094
1095 T det2(const Mat4<T> &a, int i0, int i1, int j0, int j1) const {
1096 int i0row = i0 * 4;
1097 int i1row = i1 * 4;
1098 return a.mm[i0row+j0]*a.mm[i1row+j1] - a.mm[i0row+j1]*a.mm[i1row+j0];
1099 }
1100
1101 T det3(const Mat4<T> &a, int i0, int i1, int i2,
1102 int j0, int j1, int j2) const {
1103 int i0row = i0 * 4;
1104 return a.mm[i0row+j0]*det2(a, i1,i2, j1,j2) +
1105 a.mm[i0row+j1]*det2(a, i1,i2, j2,j0) +
1106 a.mm[i0row+j2]*det2(a, i1,i2, j0,j1);
1107 }
1108}; // class Mat4
1109
1110
1111/// @relates Mat4
1112/// @brief Equality operator, does exact floating point comparisons
1113template <typename T0, typename T1>
1114 bool operator==(const Mat4<T0> &m0, const Mat4<T1> &m1)
1115{
1116 const T0 *t0 = m0.asPointer();
1117 const T1 *t1 = m1.asPointer();
1118
1119 for (int i=0; i<16; ++i) if (!isExactlyEqual(t0[i], t1[i])) return false;
1120 return true;
1121}
1122
1123/// @relates Mat4
1124/// @brief Inequality operator, does exact floating point comparisons
1125template <typename T0, typename T1>
1126 bool operator!=(const Mat4<T0> &m0, const Mat4<T1> &m1) { return !(m0 == m1); }
1127
1128/// @relates Mat4
1129/// @brief Multiply each element of the given matrix by @a scalar and return the result.
1130template <typename S, typename T>
1131 Mat4<typename promote<S, T>::type> operator*(S scalar, const Mat4<T> &m)
1132{
1133 return m*scalar;
1134}
1135
1136/// @relates Mat4
1137/// @brief Multiply each element of the given matrix by @a scalar and return the result.
1138template <typename S, typename T>
1139 Mat4<typename promote<S, T>::type> operator*(const Mat4<T> &m, S scalar)
1140{
1141 Mat4<typename promote<S, T>::type> result(m);
1142 result *= scalar;
1143 return result;
1144}
1145
1146/// @relates Mat4
1147/// @brief Multiply @a _m by @a _v and return the resulting vector.
1148template<typename T, typename MT>
1149inline Vec4<typename promote<T, MT>::type>
1150operator*(const Mat4<MT> &_m,
1151 const Vec4<T> &_v)
1152{
1153 MT const *m = _m.asPointer();
1154 return Vec4<typename promote<T, MT>::type>(
1155 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + _v[3]*m[3],
1156 _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + _v[3]*m[7],
1157 _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + _v[3]*m[11],
1158 _v[0]*m[12] + _v[1]*m[13] + _v[2]*m[14] + _v[3]*m[15]);
1159}
1160
1161/// @relates Mat4
1162/// @brief Multiply @a _v by @a _m and return the resulting vector.
1163template<typename T, typename MT>
1164inline Vec4<typename promote<T, MT>::type>
1165operator*(const Vec4<T> &_v,
1166 const Mat4<MT> &_m)
1167{
1168 MT const *m = _m.asPointer();
1169 return Vec4<typename promote<T, MT>::type>(
1170 _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + _v[3]*m[12],
1171 _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + _v[3]*m[13],
1172 _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + _v[3]*m[14],
1173 _v[0]*m[3] + _v[1]*m[7] + _v[2]*m[11] + _v[3]*m[15]);
1174}
1175
1176/// @relates Mat4
1177/// @brief Multiply @a _m by @a _v and return the resulting vector.
1178template<typename T, typename MT>
1179inline Vec3<typename promote<T, MT>::type>
1180operator*(const Mat4<MT> &_m, const Vec3<T> &_v)
1181{
1182 MT const *m = _m.asPointer();
1183 return Vec3<typename promote<T, MT>::type>(
1184 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + m[3],
1185 _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + m[7],
1186 _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + m[11]);
1187}
1188
1189/// @relates Mat4
1190/// @brief Multiply @a _v by @a _m and return the resulting vector.
1191template<typename T, typename MT>
1192inline Vec3<typename promote<T, MT>::type>
1193operator*(const Vec3<T> &_v, const Mat4<MT> &_m)
1194{
1195 MT const *m = _m.asPointer();
1196 return Vec3<typename promote<T, MT>::type>(
1197 _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + m[12],
1198 _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + m[13],
1199 _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + m[14]);
1200}
1201
1202/// @relates Mat4
1203/// @brief Add corresponding elements of @a m0 and @a m1 and return the result.
1204template <typename T0, typename T1>
1205 Mat4<typename promote<T0, T1>::type>
1206operator+(const Mat4<T0> &m0, const Mat4<T1> &m1)
1207{
1208 Mat4<typename promote<T0, T1>::type> result(m0);
1209 result += m1;
1210 return result;
1211}
1212
1213/// @relates Mat4
1214/// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result.
1215template <typename T0, typename T1>
1216 Mat4<typename promote<T0, T1>::type>
1217operator-(const Mat4<T0> &m0, const Mat4<T1> &m1)
1218{
1219 Mat4<typename promote<T0, T1>::type> result(m0);
1220 result -= m1;
1221 return result;
1222}
1223
1224/// @relates Mat4
1225/// @brief Multiply @a m0 by @a m1 and return the resulting matrix.
1226template <typename T0, typename T1>
1227 Mat4<typename promote<T0, T1>::type>
1228operator*(const Mat4<T0> &m0, const Mat4<T1> &m1)
1229{
1230 Mat4<typename promote<T0, T1>::type> result(m0);
1231 result *= m1;
1232 return result;
1233}
1234
1235
1236/// Transform a Vec3 by pre-multiplication, without translation.
1237/// Presumes this matrix is inverse of coordinate transform
1238/// Synonymous to "pretransform3x3"
1239template<typename T0, typename T1>
1240 Vec3<T1> transformNormal(const Mat4<T0> &m, const Vec3<T1> &n)
1241{
1242 return Vec3<T1>(
1243 static_cast<T1>(m[0][0]*n[0] + m[0][1]*n[1] + m[0][2]*n[2]),
1244 static_cast<T1>(m[1][0]*n[0] + m[1][1]*n[1] + m[1][2]*n[2]),
1245 static_cast<T1>(m[2][0]*n[0] + m[2][1]*n[1] + m[2][2]*n[2]));
1246}
1247
1248
1249/// Invert via gauss-jordan elimination. Modified from dreamworks internal mx library
1250template<typename T>
1251bool Mat4<T>::invert(Mat4<T> &inverse, T tolerance) const
1252{
1253 Mat4<T> temp(*this);
1254 inverse.setIdentity();
1255
1256 // Forward elimination step
1257 double det = 1.0;
1258 for (int i = 0; i < 4; ++i) {
1259 int row = i;
1260 double max = fabs(temp[i][i]);
1261
1262 for (int k = i+1; k < 4; ++k) {
1263 if (fabs(temp[k][i]) > max) {
1264 row = k;
1265 max = fabs(temp[k][i]);
1266 }
1267 }
1268
1269 if (isExactlyEqual(max, 0.0)) return false;
1270
1271 // must move pivot to row i
1272 if (row != i) {
1273 det = -det;
1274 for (int k = 0; k < 4; ++k) {
1275 std::swap(temp[row][k], temp[i][k]);
1276 std::swap(inverse[row][k], inverse[i][k]);
1277 }
1278 }
1279
1280 double pivot = temp[i][i];
1281 det *= pivot;
1282
1283 // scale row i
1284 for (int k = 0; k < 4; ++k) {
1285 temp[i][k] /= pivot;
1286 inverse[i][k] /= pivot;
1287 }
1288
1289 // eliminate in rows below i
1290 for (int j = i+1; j < 4; ++j) {
1291 double t = temp[j][i];
1292 if (!isExactlyEqual(t, 0.0)) {
1293 // subtract scaled row i from row j
1294 for (int k = 0; k < 4; ++k) {
1295 temp[j][k] -= temp[i][k] * t;
1296 inverse[j][k] -= inverse[i][k] * t;
1297 }
1298 }
1299 }
1300 }
1301
1302 // Backward elimination step
1303 for (int i = 3; i > 0; --i) {
1304 for (int j = 0; j < i; ++j) {
1305 double t = temp[j][i];
1306
1307 if (!isExactlyEqual(t, 0.0)) {
1308 for (int k = 0; k < 4; ++k) {
1309 inverse[j][k] -= inverse[i][k]*t;
1310 }
1311 }
1312 }
1313 }
1314 return det*det >= tolerance*tolerance;
1315}
1316
1317template <typename T>
1318 inline bool isAffine(const Mat4<T>& m) {
1319 return (m.col(3) == Vec4<T>(0, 0, 0, 1));
1320}
1321
1322template <typename T>
1323 inline bool hasTranslation(const Mat4<T>& m) {
1324 return (m.row(3) != Vec4<T>(0, 0, 0, 1));
1325}
1326
1327template<typename T>
1328inline Mat4<T>
1329Abs(const Mat4<T>& m)
1330{
1331 Mat4<T> out;
1332 const T* ip = m.asPointer();
1333 T* op = out.asPointer();
1334 for (unsigned i = 0; i < 16; ++i, ++op, ++ip) *op = math::Abs(*ip);
1335 return out;
1336}
1337
1338template<typename Type1, typename Type2>
1339inline Mat4<Type1>
1340cwiseAdd(const Mat4<Type1>& m, const Type2 s)
1341{
1342 Mat4<Type1> out;
1343 const Type1* ip = m.asPointer();
1344 Type1* op = out.asPointer();
1345 for (unsigned i = 0; i < 16; ++i, ++op, ++ip) {
1346 OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
1347 *op = *ip + s;
1348 OPENVDB_NO_TYPE_CONVERSION_WARNING_END
1349 }
1350 return out;
1351}
1352
1353template<typename T>
1354inline bool
1355cwiseLessThan(const Mat4<T>& m0, const Mat4<T>& m1)
1356{
1357 return cwiseLessThan<4, T>(m0, m1);
1358}
1359
1360template<typename T>
1361inline bool
1362cwiseGreaterThan(const Mat4<T>& m0, const Mat4<T>& m1)
1363{
1364 return cwiseGreaterThan<4, T>(m0, m1);
1365}
1366
1367 using Mat4s = Mat4<float>;
1368 using Mat4d = Mat4<double>;
1369 using Mat4f = Mat4d;
1370
1371#if OPENVDB_ABI_VERSION_NUMBER >= 8
1372 OPENVDB_IS_POD(Mat4s)
1373 OPENVDB_IS_POD(Mat4d)
1374#endif
1375
1376} // namespace math
1377
1378
1379 template<> inline math::Mat4s zeroVal<math::Mat4s>() { return math::Mat4s::zero(); }
1380 template<> inline math::Mat4d zeroVal<math::Mat4d>() { return math::Mat4d::zero(); }
1381
1382} // namespace OPENVDB_VERSION_NAME
1383} // namespace openvdb
1384
1385#endif // OPENVDB_UTIL_MAT4_H_HAS_BEEN_INCLUDED
Mat3.h
Math.h
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
OPENVDB_IS_POD
#define OPENVDB_IS_POD(Type)
Definition: Math.h:55
Platform.h
OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
Bracket code with OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN/_END, to inhibit warnings about type conve...
Definition: Platform.h:206
OPENVDB_NO_TYPE_CONVERSION_WARNING_END
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_END
Definition: Platform.h:207
Vec3.h
Vec4.h
openvdb::v9_0::ArithmeticError
Definition: Exceptions.h:56
openvdb::v9_0::math::Mat3
3x3 matrix class.
Definition: Mat3.h:29
openvdb::v9_0::math::Mat3::det
T det() const
Determinant of matrix.
Definition: Mat3.h:493
openvdb::v9_0::math::Mat4
4x4 -matrix class.
Definition: Mat4.h:24
openvdb::v9_0::math::Mat4::postTranslate
void postTranslate(const Vec3< T0 > &tr)
Right multiplies by the specified translation matrix, i.e. (*this) * Trans.
Definition: Mat4.h:728
openvdb::v9_0::math::Mat4::Mat4
Mat4()=default
Trivial constructor, the matrix is NOT initialized.
openvdb::v9_0::math::Mat4::preScale
void preScale(const Vec3< T0 > &v)
Definition: Mat4.h:750
openvdb::v9_0::math::Mat4::operator+
Mat4< typename promote< T0, T1 >::type > operator+(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Add corresponding elements of m0 and m1 and return the result.
Definition: Mat4.h:1206
openvdb::v9_0::math::Mat4::operator*=
const Mat4< T > & operator*=(const Mat4< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat4.h:453
openvdb::v9_0::math::Mat4::getTranslation
Vec3< T > getTranslation() const
Return the translation component.
Definition: Mat4.h:323
openvdb::v9_0::math::Mat4::Mat4
Mat4(const Mat4< Source > &m)
Conversion constructor.
Definition: Mat4.h:121
openvdb::v9_0::math::Mat4::preTranslate
void preTranslate(const Vec3< T0 > &tr)
Left multiples by the specified translation, i.e. Trans * (*this)
Definition: Mat4.h:717
openvdb::v9_0::math::Mat4::postRotate
void postRotate(Axis axis, T angle)
Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:892
openvdb::v9_0::math::Mat4::setIdentity
void setIdentity()
Set this matrix to identity.
Definition: Mat4.h:279
openvdb::v9_0::math::Mat4::operator*
Mat4< typename promote< S, T >::type > operator*(const Mat4< T > &m, S scalar)
Multiply each element of the given matrix by scalar and return the result.
Definition: Mat4.h:1139
openvdb::v9_0::math::Mat4::operator*
Vec4< typename promote< T, MT >::type > operator*(const Vec4< T > &_v, const Mat4< MT > &_m)
Multiply _v by _m and return the resulting vector.
Definition: Mat4.h:1165
openvdb::v9_0::math::Mat4::setToRotation
void setToRotation(const Vec3< T > &axis, T angle)
Sets the matrix to a rotation about an arbitrary axis.
Definition: Mat4.h:802
openvdb::v9_0::math::Mat4::setRows
void setRows(const Vec4< T > &v1, const Vec4< T > &v2, const Vec4< T > &v3, const Vec4< T > &v4)
Set the rows of this matrix to the vectors v1, v2, v3, v4.
Definition: Mat4.h:208
openvdb::v9_0::math::Mat4::setZero
void setZero()
Definition: Mat4.h:258
openvdb::v9_0::math::Mat4::setToScale
void setToScale(const Vec3< T0 > &v)
Sets the matrix to a matrix that scales by v.
Definition: Mat4.h:740
openvdb::v9_0::math::Mat4::inverse
Mat4 inverse(T tolerance=0) const
Definition: Mat4.h:499
openvdb::v9_0::math::Mat4::translation
static Mat4 translation(const Vec3d &v)
Sets the matrix to a matrix that translates by v.
Definition: Mat4.h:681
openvdb::v9_0::math::Mat4::preRotate
void preRotate(Axis axis, T angle)
Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:812
openvdb::v9_0::math::Mat4::getMat3
Mat3< T > getMat3() const
Definition: Mat4.h:311
openvdb::v9_0::math::Mat4::col
Vec4< T > col(int j) const
Get jth column, e.g. Vec4f v = m.col(0);.
Definition: Mat4.h:181
openvdb::v9_0::math::Mat4::identity
static const Mat4< T > & identity()
Predefined constant for identity matrix.
Definition: Mat4.h:131
openvdb::v9_0::math::Mat4::eq
bool eq(const Mat4 &m, T eps=1.0e-8) const
Return true if this matrix is equivalent to m within a tolerance of eps.
Definition: Mat4.h:347
openvdb::v9_0::math::Mat4::setToShear
void setToShear(Axis axis0, Axis axis1, T shearby)
Sets the matrix to a shear along axis0 by a fraction of axis1.
Definition: Mat4.h:974
openvdb::v9_0::math::Mat4::pretransform
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Transform a Vec3 by pre-multiplication, without homogenous division.
Definition: Mat4.h:1033
openvdb::v9_0::math::Mat4::transpose
Mat4 transpose() const
Definition: Mat4.h:486
openvdb::v9_0::math::Mat4::postShear
void postShear(Axis axis0, Axis axis1, T shear)
Right multiplies a shearing transformation into the matrix.
Definition: Mat4.h:997
openvdb::v9_0::math::Mat4::operator==
bool operator==(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat4.h:1114
openvdb::v9_0::math::Mat4::setToRotation
void setToRotation(Axis axis, T angle)
Sets the matrix to a rotation about the given axis.
Definition: Mat4.h:797
openvdb::v9_0::math::Mat4::setCol
void setCol(int j, const Vec4< T > &v)
Set jth column to vector v.
Definition: Mat4.h:171
openvdb::v9_0::math::Mat4::operator*
Vec3< typename promote< T, MT >::type > operator*(const Mat4< MT > &_m, const Vec3< T > &_v)
Multiply _m by _v and return the resulting vector.
Definition: Mat4.h:1180
openvdb::v9_0::math::Mat4::operator*
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat4< MT > &_m)
Multiply _v by _m and return the resulting vector.
Definition: Mat4.h:1193
openvdb::v9_0::math::Mat4::transform
Vec4< T0 > transform(const Vec4< T0 > &v) const
Transform a Vec4 by post-multiplication.
Definition: Mat4.h:1012
openvdb::v9_0::math::Mat4::preShear
void preShear(Axis axis0, Axis axis1, T shear)
Left multiplies a shearing transformation into the matrix.
Definition: Mat4.h:982
openvdb::v9_0::math::Mat4::setColumns
void setColumns(const Vec4< T > &v1, const Vec4< T > &v2, const Vec4< T > &v3, const Vec4< T > &v4)
Set the columns of this matrix to the vectors v1, v2, v3, v4.
Definition: Mat4.h:233
openvdb::v9_0::math::Mat4::zero
static const Mat4< T > & zero()
Predefined constant for zero matrix.
Definition: Mat4.h:142
openvdb::v9_0::math::Mat4::transform3x3
Vec3< T0 > transform3x3(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without translation.
Definition: Mat4.h:1083
openvdb::v9_0::math::Mat4::Mat4
Mat4(const Vec4< Source > &v1, const Vec4< Source > &v2, const Vec4< Source > &v3, const Vec4< Source > &v4, bool rows=true)
Definition: Mat4.h:109
openvdb::v9_0::math::Mat4::setTranslation
void setTranslation(const Vec3< T > &t)
Definition: Mat4.h:328
openvdb::v9_0::math::Mat4::pretransformH
Vec3< T0 > pretransformH(const Vec3< T0 > &p) const
Transform a Vec3 by pre-multiplication, doing homogenous division.
Definition: Mat4.h:1062
openvdb::v9_0::math::Mat4::setToRotation
void setToRotation(const Vec3< T > &v1, const Vec3< T > &v2)
Sets the matrix to a rotation that maps v1 onto v2 about the cross product of v1 and v2.
Definition: Mat4.h:806
openvdb::v9_0::math::Mat4::setToTranslation
void setToTranslation(const Vec3< T0 > &v)
Sets the matrix to a matrix that translates by v.
Definition: Mat4.h:692
openvdb::v9_0::math::Mat4::det
T det() const
Determinant of matrix.
Definition: Mat4.h:651
openvdb::v9_0::math::Mat4::operator*
Mat4< typename promote< S, T >::type > operator*(S scalar, const Mat4< T > &m)
Multiply each element of the given matrix by scalar and return the result.
Definition: Mat4.h:1131
openvdb::v9_0::math::Mat4::row
Vec4< T > row(int i) const
Get ith row, e.g. Vec4f v = m.row(1);.
Definition: Mat4.h:164
openvdb::v9_0::math::Mat4::operator+=
const Mat4< T > & operator+=(const Mat4< S > &m1)
Add each element of the given matrix to the corresponding element of this matrix.
Definition: Mat4.h:395
openvdb::v9_0::math::Mat4::operator-
Mat4< typename promote< T0, T1 >::type > operator-(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Subtract corresponding elements of m0 and m1 and return the result.
Definition: Mat4.h:1217
openvdb::v9_0::math::Mat4::Mat4
Mat4(Source *a)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat4.h:65
openvdb::v9_0::math::Mat4::operator!=
bool operator!=(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat4.h:1126
openvdb::v9_0::math::Mat4::operator()
T & operator()(int i, int j)
Definition: Mat4.h:190
openvdb::v9_0::math::Mat4::setRow
void setRow(int i, const Vec4< T > &v)
Set ith row to vector v.
Definition: Mat4.h:153
openvdb::v9_0::math::Mat4::Mat4
Mat4(Source a, Source b, Source c, Source d, Source e, Source f, Source g, Source h, Source i, Source j, Source k, Source l, Source m, Source n, Source o, Source p)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat4.h:80
openvdb::v9_0::math::Mat4::postScale
void postScale(const Vec3< T0 > &v)
Definition: Mat4.h:772
openvdb::v9_0::math::Mat4::transform
Vec3< T0 > transform(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without homogenous division.
Definition: Mat4.h:1019
openvdb::v9_0::math::Mat4::operator*
Mat4< typename promote< T0, T1 >::type > operator*(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Multiply m0 by m1 and return the resulting matrix.
Definition: Mat4.h:1228
openvdb::v9_0::math::Mat4::operator()
T operator()(int i, int j) const
Definition: Mat4.h:200
openvdb::v9_0::math::Mat4::operator-=
const Mat4< T > & operator-=(const Mat4< S > &m1)
Subtract each element of the given matrix from the corresponding element of this matrix.
Definition: Mat4.h:424
openvdb::v9_0::math::Mat4::transformH
Vec3< T0 > transformH(const Vec3< T0 > &p) const
Transform a Vec3 by post-multiplication, doing homogenous divison.
Definition: Mat4.h:1040
openvdb::v9_0::math::Mat4::setMat3
void setMat3(const Mat3< T > &m)
Set upper left to a Mat3.
Definition: Mat4.h:304
openvdb::v9_0::math::Mat4::pretransform
Vec4< T0 > pretransform(const Vec4< T0 > &v) const
Transform a Vec4 by pre-multiplication.
Definition: Mat4.h:1026
openvdb::v9_0::math::Mat4::operator=
const Mat4 & operator=(const Mat4< Source > &m)
Assignment operator.
Definition: Mat4.h:337
openvdb::v9_0::math::Mat4::operator*=
const Mat4< T > & operator*=(S scalar)
Multiply each element of this matrix by scalar.
Definition: Mat4.h:369
openvdb::v9_0::math::Mat4::ValueType
T ValueType
Definition: Mat4.h:35
openvdb::v9_0::math::Mat4::operator*
Vec4< typename promote< T, MT >::type > operator*(const Mat4< MT > &_m, const Vec4< T > &_v)
Multiply _m by _v and return the resulting vector.
Definition: Mat4.h:1150
openvdb::v9_0::math::Mat4::value_type
T value_type
Data type held by the matrix.
Definition: Mat4.h:34
openvdb::v9_0::math::Mat4::operator-
Mat4< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat4.h:357
openvdb::v9_0::math::Mat
Definition: Mat.h:27
openvdb::v9_0::math::Mat::mm
T mm[SIZE *SIZE]
Definition: Mat.h:182
openvdb::v9_0::math::Mat::asPointer
T * asPointer()
Direct access to the internal data.
Definition: Mat.h:123
openvdb::v9_0::math::Vec3
Definition: Vec3.h:24
openvdb::v9_0::math::Vec3::x
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:89
openvdb::v9_0::math::Vec3::dot
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:195
openvdb::v9_0::math::Vec3::y
T & y()
Definition: Vec3.h:90
openvdb::v9_0::math::Vec3::z
T & z()
Definition: Vec3.h:91
openvdb::v9_0::math::Vec4
Definition: Vec4.h:25
openvdb::v9_0::math::mat3_internal::pivot
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition: Mat3.h:682
openvdb::v9_0::math::hasTranslation
bool hasTranslation(const Mat4< T > &m)
Definition: Mat4.h:1323
openvdb::v9_0::math::transformNormal
Vec3< T1 > transformNormal(const Mat4< T0 > &m, const Vec3< T1 > &n)
Definition: Mat4.h:1240
openvdb::v9_0::math::isExactlyEqual
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:444
openvdb::v9_0::math::isAffine
bool isAffine(const Mat4< T > &m)
Definition: Mat4.h:1318
openvdb::v9_0::math::Abs
Mat4< T > Abs(const Mat4< T > &m)
Definition: Mat4.h:1329
openvdb::v9_0::math::angle
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:450
openvdb::v9_0::math::cwiseLessThan
bool cwiseLessThan(const Mat4< T > &m0, const Mat4< T > &m1)
Definition: Mat4.h:1355
openvdb::v9_0::math::cwiseAdd
Mat4< Type1 > cwiseAdd(const Mat4< Type1 > &m, const Type2 s)
Definition: Mat4.h:1340
openvdb::v9_0::math::shear
MatType shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
Set the matrix to a shear along axis0 by a fraction of axis1.
Definition: Mat.h:710
openvdb::v9_0::math::cwiseGreaterThan
bool cwiseGreaterThan(const Mat4< T > &m0, const Mat4< T > &m1)
Definition: Mat4.h:1362
openvdb::v9_0::math::isApproxEqual
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition: Math.h:407
openvdb::v9_0::math::Mat4d
Mat4< double > Mat4d
Definition: Mat4.h:1368
openvdb::v9_0::math::Axis
Axis
Definition: Math.h:904
openvdb::v9_0::math::Z_AXIS
@ Z_AXIS
Definition: Math.h:907
openvdb::v9_0::math::X_AXIS
@ X_AXIS
Definition: Math.h:905
openvdb::v9_0::math::Y_AXIS
@ Y_AXIS
Definition: Math.h:906
openvdb::v9_0::tools::composite::max
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:107
openvdb
Definition: Exceptions.h:13
Exceptions.h
OPENVDB_THROW
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:74
OPENVDB_VERSION_NAME
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h.in:116
OPENVDB_USE_VERSION_NAMESPACE
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h.in:202