OpenVDB 12.0.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: Apache-2.0
3
4#ifndef OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
5#define OPENVDB_MATH_MAT4_H_HAS_BEEN_INCLUDED
6
8#include <openvdb/Platform.h>
10#include "Math.h"
11#include "Mat3.h"
12#include "Vec3.h"
13#include "Vec4.h"
14#include <algorithm> // for std::copy(), std::swap()
15#include <iomanip>
16#include <cmath>
17
18
19namespace openvdb {
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;
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 OPENVDB_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 OPENVDB_ASSERT(i>=0 && i<4);
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 OPENVDB_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 OPENVDB_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 OPENVDB_ASSERT(i>=0 && i<4);
179 OPENVDB_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 OPENVDB_ASSERT(i>=0 && i<4);
189 OPENVDB_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
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
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
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;
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
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>
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());
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());
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 /// @brief Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
795 /// @param axis The axis (one of X, Y, Z) of rotation.
796 /// @param angle The clock-wise rotation angle, in radians.
797 void preRotate(Axis axis, T angle)
798 {
799 OPENVDB_ASSERT(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
800
801 T c = static_cast<T>(cos(angle));
802 T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
803
804 switch (axis)
805 {
806 case X_AXIS:
807 {
808 T a4, a5, a6, a7;
809
810 a4 = c * MyBase::mm[ 4] - s * MyBase::mm[ 8];
811 a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 9];
812 a6 = c * MyBase::mm[ 6] - s * MyBase::mm[10];
813 a7 = c * MyBase::mm[ 7] - s * MyBase::mm[11];
814
815
816 MyBase::mm[ 8] = s * MyBase::mm[ 4] + c * MyBase::mm[ 8];
817 MyBase::mm[ 9] = s * MyBase::mm[ 5] + c * MyBase::mm[ 9];
818 MyBase::mm[10] = s * MyBase::mm[ 6] + c * MyBase::mm[10];
819 MyBase::mm[11] = s * MyBase::mm[ 7] + c * MyBase::mm[11];
820
821 MyBase::mm[ 4] = a4;
822 MyBase::mm[ 5] = a5;
823 MyBase::mm[ 6] = a6;
824 MyBase::mm[ 7] = a7;
825 }
826 break;
827
828 case Y_AXIS:
829 {
830 T a0, a1, a2, a3;
831
832 a0 = c * MyBase::mm[ 0] + s * MyBase::mm[ 8];
833 a1 = c * MyBase::mm[ 1] + s * MyBase::mm[ 9];
834 a2 = c * MyBase::mm[ 2] + s * MyBase::mm[10];
835 a3 = c * MyBase::mm[ 3] + s * MyBase::mm[11];
836
837 MyBase::mm[ 8] = -s * MyBase::mm[ 0] + c * MyBase::mm[ 8];
838 MyBase::mm[ 9] = -s * MyBase::mm[ 1] + c * MyBase::mm[ 9];
839 MyBase::mm[10] = -s * MyBase::mm[ 2] + c * MyBase::mm[10];
840 MyBase::mm[11] = -s * MyBase::mm[ 3] + c * MyBase::mm[11];
841
842
843 MyBase::mm[ 0] = a0;
844 MyBase::mm[ 1] = a1;
845 MyBase::mm[ 2] = a2;
846 MyBase::mm[ 3] = a3;
847 }
848 break;
849
850 case Z_AXIS:
851 {
852 T a0, a1, a2, a3;
853
854 a0 = c * MyBase::mm[ 0] - s * MyBase::mm[ 4];
855 a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 5];
856 a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 6];
857 a3 = c * MyBase::mm[ 3] - s * MyBase::mm[ 7];
858
859 MyBase::mm[ 4] = s * MyBase::mm[ 0] + c * MyBase::mm[ 4];
860 MyBase::mm[ 5] = s * MyBase::mm[ 1] + c * MyBase::mm[ 5];
861 MyBase::mm[ 6] = s * MyBase::mm[ 2] + c * MyBase::mm[ 6];
862 MyBase::mm[ 7] = s * MyBase::mm[ 3] + c * MyBase::mm[ 7];
863
864 MyBase::mm[ 0] = a0;
865 MyBase::mm[ 1] = a1;
866 MyBase::mm[ 2] = a2;
867 MyBase::mm[ 3] = a3;
868 }
869 break;
870
871 default:
872 OPENVDB_ASSERT(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
873 }
874 }
875
876
877 /// @brief Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
878 /// @param axis The axis (one of X, Y, Z) of rotation.
879 /// @param angle The clock-wise rotation angle, in radians.
880 void postRotate(Axis axis, T angle)
881 {
882 T c = static_cast<T>(cos(angle));
883 T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
884
885 switch (axis)
886 {
887 case X_AXIS:
888 {
889 T a2, a6, a10, a14;
890
891 a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 1];
892 a6 = c * MyBase::mm[ 6] - s * MyBase::mm[ 5];
893 a10 = c * MyBase::mm[10] - s * MyBase::mm[ 9];
894 a14 = c * MyBase::mm[14] - s * MyBase::mm[13];
895
896
897 MyBase::mm[ 1] = c * MyBase::mm[ 1] + s * MyBase::mm[ 2];
898 MyBase::mm[ 5] = c * MyBase::mm[ 5] + s * MyBase::mm[ 6];
899 MyBase::mm[ 9] = c * MyBase::mm[ 9] + s * MyBase::mm[10];
900 MyBase::mm[13] = c * MyBase::mm[13] + s * MyBase::mm[14];
901
902 MyBase::mm[ 2] = a2;
903 MyBase::mm[ 6] = a6;
904 MyBase::mm[10] = a10;
905 MyBase::mm[14] = a14;
906 }
907 break;
908
909 case Y_AXIS:
910 {
911 T a2, a6, a10, a14;
912
913 a2 = c * MyBase::mm[ 2] + s * MyBase::mm[ 0];
914 a6 = c * MyBase::mm[ 6] + s * MyBase::mm[ 4];
915 a10 = c * MyBase::mm[10] + s * MyBase::mm[ 8];
916 a14 = c * MyBase::mm[14] + s * MyBase::mm[12];
917
918 MyBase::mm[ 0] = c * MyBase::mm[ 0] - s * MyBase::mm[ 2];
919 MyBase::mm[ 4] = c * MyBase::mm[ 4] - s * MyBase::mm[ 6];
920 MyBase::mm[ 8] = c * MyBase::mm[ 8] - s * MyBase::mm[10];
921 MyBase::mm[12] = c * MyBase::mm[12] - s * MyBase::mm[14];
922
923 MyBase::mm[ 2] = a2;
924 MyBase::mm[ 6] = a6;
925 MyBase::mm[10] = a10;
926 MyBase::mm[14] = a14;
927 }
928 break;
929
930 case Z_AXIS:
931 {
932 T a1, a5, a9, a13;
933
934 a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 0];
935 a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 4];
936 a9 = c * MyBase::mm[ 9] - s * MyBase::mm[ 8];
937 a13 = c * MyBase::mm[13] - s * MyBase::mm[12];
938
939 MyBase::mm[ 0] = c * MyBase::mm[ 0] + s * MyBase::mm[ 1];
940 MyBase::mm[ 4] = c * MyBase::mm[ 4] + s * MyBase::mm[ 5];
941 MyBase::mm[ 8] = c * MyBase::mm[ 8] + s * MyBase::mm[ 9];
942 MyBase::mm[12] = c * MyBase::mm[12] + s * MyBase::mm[13];
943
944 MyBase::mm[ 1] = a1;
945 MyBase::mm[ 5] = a5;
946 MyBase::mm[ 9] = a9;
947 MyBase::mm[13] = a13;
948
949 }
950 break;
951
952 default:
953 OPENVDB_ASSERT(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
954 }
955 }
956
957 /// @brief Sets the matrix to a shear along axis0 by a fraction of axis1.
958 /// @param axis0 The fixed axis of the shear.
959 /// @param axis1 The shear axis.
960 /// @param shearby The shear factor.
961 void setToShear(Axis axis0, Axis axis1, T shearby)
962 {
963 *this = shear<Mat4<T> >(axis0, axis1, shearby);
964 }
965
966
967 /// @brief Left multiplies a shearing transformation into the matrix.
968 /// @see setToShear
969 void preShear(Axis axis0, Axis axis1, T shear)
970 {
971 int index0 = static_cast<int>(axis0);
972 int index1 = static_cast<int>(axis1);
973
974 // to row "index1" add a multiple of the index0 row
975 MyBase::mm[index1 * 4 + 0] += shear * MyBase::mm[index0 * 4 + 0];
976 MyBase::mm[index1 * 4 + 1] += shear * MyBase::mm[index0 * 4 + 1];
977 MyBase::mm[index1 * 4 + 2] += shear * MyBase::mm[index0 * 4 + 2];
978 MyBase::mm[index1 * 4 + 3] += shear * MyBase::mm[index0 * 4 + 3];
979 }
980
981
982 /// @brief Right multiplies a shearing transformation into the matrix.
983 /// @see setToShear
984 void postShear(Axis axis0, Axis axis1, T shear)
985 {
986 int index0 = static_cast<int>(axis0);
987 int index1 = static_cast<int>(axis1);
988
989 // to collumn "index0" add a multiple of the index1 row
990 MyBase::mm[index0 + 0] += shear * MyBase::mm[index1 + 0];
991 MyBase::mm[index0 + 4] += shear * MyBase::mm[index1 + 4];
992 MyBase::mm[index0 + 8] += shear * MyBase::mm[index1 + 8];
993 MyBase::mm[index0 + 12] += shear * MyBase::mm[index1 + 12];
994
995 }
996
997 /// Transform a Vec4 by post-multiplication.
998 template<typename T0>
1000 {
1001 return static_cast< Vec4<T0> >(v * *this);
1002 }
1003
1004 /// Transform a Vec3 by post-multiplication, without homogenous division.
1005 template<typename T0>
1007 {
1008 return static_cast< Vec3<T0> >(v * *this);
1009 }
1010
1011 /// Transform a Vec4 by pre-multiplication.
1012 template<typename T0>
1014 {
1015 return static_cast< Vec4<T0> >(*this * v);
1016 }
1017
1018 /// Transform a Vec3 by pre-multiplication, without homogenous division.
1019 template<typename T0>
1021 {
1022 return static_cast< Vec3<T0> >(*this * v);
1023 }
1024
1025 /// Transform a Vec3 by post-multiplication, doing homogenous divison.
1026 template<typename T0>
1028 {
1029 T0 w;
1030
1031 // w = p * (*this).col(3);
1032 w = static_cast<T0>(p[0] * MyBase::mm[ 3] + p[1] * MyBase::mm[ 7]
1033 + p[2] * MyBase::mm[11] + MyBase::mm[15]);
1034
1035 if ( !isExactlyEqual(w , 0.0) ) {
1036 return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 4] +
1037 p[2] * MyBase::mm[ 8] + MyBase::mm[12]) / w),
1038 static_cast<T0>((p[0] * MyBase::mm[ 1] + p[1] * MyBase::mm[ 5] +
1039 p[2] * MyBase::mm[ 9] + MyBase::mm[13]) / w),
1040 static_cast<T0>((p[0] * MyBase::mm[ 2] + p[1] * MyBase::mm[ 6] +
1041 p[2] * MyBase::mm[10] + MyBase::mm[14]) / w));
1042 }
1043
1044 return Vec3<T0>(0, 0, 0);
1045 }
1046
1047 /// Transform a Vec3 by pre-multiplication, doing homogenous division.
1048 template<typename T0>
1050 {
1051 T0 w;
1052
1053 // w = p * (*this).col(3);
1054 w = p[0] * MyBase::mm[12] + p[1] * MyBase::mm[13] + p[2] * MyBase::mm[14] + MyBase::mm[15];
1055
1056 if ( !isExactlyEqual(w , 0.0) ) {
1057 return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 1] +
1058 p[2] * MyBase::mm[ 2] + MyBase::mm[ 3]) / w),
1059 static_cast<T0>((p[0] * MyBase::mm[ 4] + p[1] * MyBase::mm[ 5] +
1060 p[2] * MyBase::mm[ 6] + MyBase::mm[ 7]) / w),
1061 static_cast<T0>((p[0] * MyBase::mm[ 8] + p[1] * MyBase::mm[ 9] +
1062 p[2] * MyBase::mm[10] + MyBase::mm[11]) / w));
1063 }
1064
1065 return Vec3<T0>(0, 0, 0);
1066 }
1067
1068 /// Transform a Vec3 by post-multiplication, without translation.
1069 template<typename T0>
1071 {
1072 return Vec3<T0>(
1073 static_cast<T0>(v[0] * MyBase::mm[ 0] + v[1] * MyBase::mm[ 4] + v[2] * MyBase::mm[ 8]),
1074 static_cast<T0>(v[0] * MyBase::mm[ 1] + v[1] * MyBase::mm[ 5] + v[2] * MyBase::mm[ 9]),
1075 static_cast<T0>(v[0] * MyBase::mm[ 2] + v[1] * MyBase::mm[ 6] + v[2] * MyBase::mm[10]));
1076 }
1077
1078
1079private:
1080 bool invert(Mat4<T> &inverse, T tolerance) const;
1081
1082 T det2(const Mat4<T> &a, int i0, int i1, int j0, int j1) const {
1083 int i0row = i0 * 4;
1084 int i1row = i1 * 4;
1085 return a.mm[i0row+j0]*a.mm[i1row+j1] - a.mm[i0row+j1]*a.mm[i1row+j0];
1086 }
1087
1088 T det3(const Mat4<T> &a, int i0, int i1, int i2,
1089 int j0, int j1, int j2) const {
1090 int i0row = i0 * 4;
1091 return a.mm[i0row+j0]*det2(a, i1,i2, j1,j2) +
1092 a.mm[i0row+j1]*det2(a, i1,i2, j2,j0) +
1093 a.mm[i0row+j2]*det2(a, i1,i2, j0,j1);
1094 }
1095}; // class Mat4
1096
1097
1098/// @relates Mat4
1099/// @brief Equality operator, does exact floating point comparisons
1100template <typename T0, typename T1>
1101bool operator==(const Mat4<T0> &m0, const Mat4<T1> &m1)
1102{
1103 const T0 *t0 = m0.asPointer();
1104 const T1 *t1 = m1.asPointer();
1105
1106 for (int i=0; i<16; ++i) if (!isExactlyEqual(t0[i], t1[i])) return false;
1107 return true;
1108}
1109
1110/// @relates Mat4
1111/// @brief Inequality operator, does exact floating point comparisons
1112template <typename T0, typename T1>
1113bool operator!=(const Mat4<T0> &m0, const Mat4<T1> &m1) { return !(m0 == m1); }
1114
1115/// @relates Mat4
1116/// @brief Multiply each element of the given matrix by @a scalar and return the result.
1117template <typename S, typename T>
1119{
1120 return m*scalar;
1121}
1122
1123/// @relates Mat4
1124/// @brief Multiply each element of the given matrix by @a scalar and return the result.
1125template <typename S, typename T>
1127{
1129 result *= scalar;
1130 return result;
1131}
1132
1133/// @relates Mat4
1134/// @brief Multiply @a _m by @a _v and return the resulting vector.
1135template<typename T, typename MT>
1138 const Vec4<T> &_v)
1139{
1140 MT const *m = _m.asPointer();
1142 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + _v[3]*m[3],
1143 _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + _v[3]*m[7],
1144 _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + _v[3]*m[11],
1145 _v[0]*m[12] + _v[1]*m[13] + _v[2]*m[14] + _v[3]*m[15]);
1146}
1147
1148/// @relates Mat4
1149/// @brief Multiply @a _v by @a _m and return the resulting vector.
1150template<typename T, typename MT>
1153 const Mat4<MT> &_m)
1154{
1155 MT const *m = _m.asPointer();
1157 _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + _v[3]*m[12],
1158 _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + _v[3]*m[13],
1159 _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + _v[3]*m[14],
1160 _v[0]*m[3] + _v[1]*m[7] + _v[2]*m[11] + _v[3]*m[15]);
1161}
1162
1163/// @relates Mat4
1164/// @brief Multiply @a _m by @a _v and return the resulting vector.
1165template<typename T, typename MT>
1167operator*(const Mat4<MT> &_m, const Vec3<T> &_v)
1168{
1169 MT const *m = _m.asPointer();
1171 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + m[3],
1172 _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + m[7],
1173 _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + m[11]);
1174}
1175
1176/// @relates Mat4
1177/// @brief Multiply @a _v by @a _m and return the resulting vector.
1178template<typename T, typename MT>
1180operator*(const Vec3<T> &_v, const Mat4<MT> &_m)
1181{
1182 MT const *m = _m.asPointer();
1184 _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + m[12],
1185 _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + m[13],
1186 _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + m[14]);
1187}
1188
1189/// @relates Mat4
1190/// @brief Add corresponding elements of @a m0 and @a m1 and return the result.
1191template <typename T0, typename T1>
1193operator+(const Mat4<T0> &m0, const Mat4<T1> &m1)
1194{
1196 result += m1;
1197 return result;
1198}
1199
1200/// @relates Mat4
1201/// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result.
1202template <typename T0, typename T1>
1204operator-(const Mat4<T0> &m0, const Mat4<T1> &m1)
1205{
1207 result -= m1;
1208 return result;
1209}
1210
1211/// @relates Mat4
1212/// @brief Multiply @a m0 by @a m1 and return the resulting matrix.
1213template <typename T0, typename T1>
1215operator*(const Mat4<T0> &m0, const Mat4<T1> &m1)
1216{
1218 result *= m1;
1219 return result;
1220}
1221
1222
1223/// Transform a Vec3 by pre-multiplication, without translation.
1224/// Presumes this matrix is inverse of coordinate transform
1225/// Synonymous to "pretransform3x3"
1226template<typename T0, typename T1>
1228{
1229 return Vec3<T1>(
1230 static_cast<T1>(m[0][0]*n[0] + m[0][1]*n[1] + m[0][2]*n[2]),
1231 static_cast<T1>(m[1][0]*n[0] + m[1][1]*n[1] + m[1][2]*n[2]),
1232 static_cast<T1>(m[2][0]*n[0] + m[2][1]*n[1] + m[2][2]*n[2]));
1233}
1234
1235
1236/// Invert via gauss-jordan elimination. Modified from dreamworks internal mx library
1237template<typename T>
1238bool Mat4<T>::invert(Mat4<T> &inverse, T tolerance) const
1239{
1240 Mat4<T> temp(*this);
1241 inverse.setIdentity();
1242
1243 // Forward elimination step
1244 double det = 1.0;
1245 for (int i = 0; i < 4; ++i) {
1246 int row = i;
1247 double max = fabs(temp[i][i]);
1248
1249 for (int k = i+1; k < 4; ++k) {
1250 if (fabs(temp[k][i]) > max) {
1251 row = k;
1252 max = fabs(temp[k][i]);
1253 }
1254 }
1255
1256 if (isExactlyEqual(max, 0.0)) return false;
1257
1258 // must move pivot to row i
1259 if (row != i) {
1260 det = -det;
1261 for (int k = 0; k < 4; ++k) {
1262 std::swap(temp[row][k], temp[i][k]);
1263 std::swap(inverse[row][k], inverse[i][k]);
1264 }
1265 }
1266
1267 double pivot = temp[i][i];
1268 det *= pivot;
1269
1270 // scale row i
1271 for (int k = 0; k < 4; ++k) {
1272 temp[i][k] /= pivot;
1273 inverse[i][k] /= pivot;
1274 }
1275
1276 // eliminate in rows below i
1277 for (int j = i+1; j < 4; ++j) {
1278 double t = temp[j][i];
1279 if (!isExactlyEqual(t, 0.0)) {
1280 // subtract scaled row i from row j
1281 for (int k = 0; k < 4; ++k) {
1282 temp[j][k] -= temp[i][k] * t;
1283 inverse[j][k] -= inverse[i][k] * t;
1284 }
1285 }
1286 }
1287 }
1288
1289 // Backward elimination step
1290 for (int i = 3; i > 0; --i) {
1291 for (int j = 0; j < i; ++j) {
1292 double t = temp[j][i];
1293
1294 if (!isExactlyEqual(t, 0.0)) {
1295 for (int k = 0; k < 4; ++k) {
1296 inverse[j][k] -= inverse[i][k]*t;
1297 }
1298 }
1299 }
1300 }
1301 return det*det >= tolerance*tolerance;
1302}
1303
1304template <typename T>
1305inline bool isAffine(const Mat4<T>& m) {
1306 return (m.col(3) == Vec4<T>(0, 0, 0, 1));
1307}
1308
1309template <typename T>
1310inline bool hasTranslation(const Mat4<T>& m) {
1311 return (m.row(3) != Vec4<T>(0, 0, 0, 1));
1312}
1313
1314template<typename T>
1315inline Mat4<T>
1316Abs(const Mat4<T>& m)
1317{
1318 Mat4<T> out;
1319 const T* ip = m.asPointer();
1320 T* op = out.asPointer();
1321 for (unsigned i = 0; i < 16; ++i, ++op, ++ip) *op = math::Abs(*ip);
1322 return out;
1323}
1324
1325template<typename Type1, typename Type2>
1326inline Mat4<Type1>
1327cwiseAdd(const Mat4<Type1>& m, const Type2 s)
1328{
1329 Mat4<Type1> out;
1330 const Type1* ip = m.asPointer();
1331 Type1* op = out.asPointer();
1332 for (unsigned i = 0; i < 16; ++i, ++op, ++ip) {
1334 *op = *ip + s;
1336 }
1337 return out;
1338}
1339
1340template<typename T>
1341inline bool
1342cwiseLessThan(const Mat4<T>& m0, const Mat4<T>& m1)
1343{
1344 return cwiseLessThan<4, T>(m0, m1);
1345}
1346
1347template<typename T>
1348inline bool
1349cwiseGreaterThan(const Mat4<T>& m0, const Mat4<T>& m1)
1350{
1351 return cwiseGreaterThan<4, T>(m0, m1);
1352}
1353
1356using Mat4f = Mat4d;
1357
1360
1361} // namespace math
1362
1363
1364template<> inline math::Mat4s zeroVal<math::Mat4s>() { return math::Mat4s::zero(); }
1365template<> inline math::Mat4d zeroVal<math::Mat4d>() { return math::Mat4d::zero(); }
1366
1367} // namespace OPENVDB_VERSION_NAME
1368} // namespace openvdb
1369
1370#endif // OPENVDB_UTIL_MAT4_H_HAS_BEEN_INCLUDED
#define OPENVDB_ASSERT(X)
Definition Assert.h:41
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
#define OPENVDB_IS_POD(Type)
Definition Math.h:56
#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:221
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_END
Definition Platform.h:222
Definition Exceptions.h:56
3x3 matrix class.
Definition Mat3.h:29
T det() const
Determinant of matrix.
Definition Mat3.h:479
4x4 -matrix class.
Definition Mat4.h:31
void postTranslate(const Vec3< T0 > &tr)
Right multiplies by the specified translation matrix, i.e. (*this) * Trans.
Definition Mat4.h:714
void preScale(const Vec3< T0 > &v)
Definition Mat4.h:736
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:1193
const Mat4< T > & operator*=(const Mat4< S > &m1)
Multiply this matrix by the given matrix.
Definition Mat4.h:439
Vec3< T > getTranslation() const
Return the translation component.
Definition Mat4.h:309
Mat4(const Mat4< Source > &m)
Conversion constructor.
Definition Mat4.h:107
void preTranslate(const Vec3< T0 > &tr)
Left multiples by the specified translation, i.e. Trans * (*this)
Definition Mat4.h:703
void postRotate(Axis axis, T angle)
Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition Mat4.h:880
void setIdentity()
Set this matrix to identity.
Definition Mat4.h:265
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:1126
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:1152
void setToRotation(const Vec3< T > &axis, T angle)
Sets the matrix to a rotation about an arbitrary axis.
Definition Mat4.h:788
void setRows(const Vec4< Real > &v1, const Vec4< Real > &v2, const Vec4< Real > &v3, const Vec4< Real > &v4)
Definition Mat4.h:194
void setZero()
Definition Mat4.h:244
void setToScale(const Vec3< T0 > &v)
Sets the matrix to a matrix that scales by v.
Definition Mat4.h:726
Mat4 inverse(T tolerance=0) const
Definition Mat4.h:485
static Mat4 translation(const Vec3d &v)
Sets the matrix to a matrix that translates by v.
Definition Mat4.h:667
void preRotate(Axis axis, T angle)
Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition Mat4.h:797
Mat3< T > getMat3() const
Definition Mat4.h:297
Vec4< T > col(int j) const
Get jth column, e.g. Vec4f v = m.col(0);.
Definition Mat4.h:167
static const Mat4< T > & identity()
Predefined constant for identity matrix.
Definition Mat4.h:117
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
void setToShear(Axis axis0, Axis axis1, T shearby)
Sets the matrix to a shear along axis0 by a fraction of axis1.
Definition Mat4.h:961
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Transform a Vec3 by pre-multiplication, without homogenous division.
Definition Mat4.h:1020
Mat4 transpose() const
Definition Mat4.h:472
void postShear(Axis axis0, Axis axis1, T shear)
Right multiplies a shearing transformation into the matrix.
Definition Mat4.h:984
bool operator==(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition Mat4.h:1101
void setToRotation(Axis axis, T angle)
Sets the matrix to a rotation about the given axis.
Definition Mat4.h:783
void setCol(int j, const Vec4< T > &v)
Set jth column to vector v.
Definition Mat4.h:157
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:1167
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:1180
Vec4< T0 > transform(const Vec4< T0 > &v) const
Transform a Vec4 by post-multiplication.
Definition Mat4.h:999
void preShear(Axis axis0, Axis axis1, T shear)
Left multiplies a shearing transformation into the matrix.
Definition Mat4.h:969
void setColumns(const Vec4< Real > &v1, const Vec4< Real > &v2, const Vec4< Real > &v3, const Vec4< Real > &v4)
Definition Mat4.h:219
Mat< 4, Real > MyBase
Definition Mat4.h:36
static const Mat4< T > & zero()
Predefined constant for zero matrix.
Definition Mat4.h:128
Vec3< T0 > transform3x3(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without translation.
Definition Mat4.h:1070
Mat4(const Vec4< Source > &v1, const Vec4< Source > &v2, const Vec4< Source > &v3, const Vec4< Source > &v4, bool rows=true)
Definition Mat4.h:95
void setTranslation(const Vec3< T > &t)
Definition Mat4.h:314
Vec3< T0 > pretransformH(const Vec3< T0 > &p) const
Transform a Vec3 by pre-multiplication, doing homogenous division.
Definition Mat4.h:1049
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
void setToTranslation(const Vec3< T0 > &v)
Sets the matrix to a matrix that translates by v.
Definition Mat4.h:678
Real det() const
Definition Mat4.h:637
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:1118
Vec4< T > row(int i) const
Get ith row, e.g. Vec4f v = m.row(1);.
Definition Mat4.h:150
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
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:1204
Mat4(Source *a)
Constructor given array of elements, the ordering is in row major form:
Definition Mat4.h:51
bool operator!=(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition Mat4.h:1113
T & operator()(int i, int j)
Definition Mat4.h:176
void setRow(int i, const Vec4< T > &v)
Set ith row to vector v.
Definition Mat4.h:139
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
void postScale(const Vec3< T0 > &v)
Definition Mat4.h:758
Vec3< T0 > transform(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without homogenous division.
Definition Mat4.h:1006
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:1215
T operator()(int i, int j) const
Definition Mat4.h:186
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
Vec3< T0 > transformH(const Vec3< T0 > &p) const
Transform a Vec3 by post-multiplication, doing homogenous divison.
Definition Mat4.h:1027
void setMat3(const Mat3< T > &m)
Set upper left to a Mat3.
Definition Mat4.h:290
Vec4< T0 > pretransform(const Vec4< T0 > &v) const
Transform a Vec4 by pre-multiplication.
Definition Mat4.h:1013
const Mat4 & operator=(const Mat4< Source > &m)
Assignment operator.
Definition Mat4.h:323
const Mat4< T > & operator*=(S scalar)
Multiply each element of this matrix by scalar.
Definition Mat4.h:355
Real ValueType
Definition Mat4.h:35
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:1137
Real value_type
Definition Mat4.h:34
Mat4< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition Mat4.h:343
T mm[SIZE *SIZE]
Definition Mat.h:160
T * asPointer()
Direct access to the internal data.
Definition Mat.h:101
static unsigned numElements()
Definition Mat.h:41
Definition Vec3.h:25
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition Vec3.h:86
T dot(const Vec3< T > &v) const
Dot product.
Definition Vec3.h:192
T & y()
Definition Vec3.h:87
T & z()
Definition Vec3.h:88
Definition Vec4.h:25
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition Mat3.h:668
Vec3< T1 > transformNormal(const Mat4< T0 > &m, const Vec3< T1 > &n)
Definition Mat4.h:1227
bool hasTranslation(const Mat4< T > &m)
Definition Mat4.h:1310
Mat4< float > Mat4s
Definition Mat4.h:1354
bool cwiseLessThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1015
Mat4d Mat4f
Definition Mat4.h:1356
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
Vec3< double > Vec3d
Definition Vec3.h:665
bool isAffine(const Mat4< T > &m)
Definition Mat4.h:1305
MatType rotation(const Quat< typename MatType::value_type > &q, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the rotation matrix specified by the given quaternion.
Definition Mat.h:172
auto cwiseAdd(const math::Vec3< math::half > &v, const float s)
Definition Types.h:694
Coord Abs(const Coord &xyz)
Definition Coord.h:518
Mat4< double > Mat4d
Definition Mat4.h:1355
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition Vec2.h:446
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition Math.h:443
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
Axis
Definition Math.h:901
@ Z_AXIS
Definition Math.h:904
@ X_AXIS
Definition Math.h:902
@ Y_AXIS
Definition Math.h:903
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1029
constexpr T zeroVal()
Return the value of type T that corresponds to zero.
Definition Math.h:70
Definition Exceptions.h:13
#define OPENVDB_THROW(exception, message)
Definition Exceptions.h:74
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition version.h.in:121
#define OPENVDB_USE_VERSION_NAMESPACE
Definition version.h.in:218