OpenVDB: openvdb/math/Mat4.h Source File

OpenVDB 10.1.0

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