OpenVDB  12.0.0
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 
7 #include <openvdb/Exceptions.h>
8 #include <openvdb/Platform.h>
9 #include <openvdb/util/Assert.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 
19 namespace openvdb {
21 namespace OPENVDB_VERSION_NAME {
22 namespace math {
23 
24 template<typename T> class Vec4;
25 
26 
27 /// @class Mat4 Mat4.h
28 /// @brief 4x4 -matrix class.
29 template<typename T>
30 class Mat4: public Mat<4, T>
31 {
32 public:
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  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
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
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
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  /// @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>
999  Vec4<T0> transform(const Vec4<T0> &v) const
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>
1006  Vec3<T0> transform(const Vec3<T0> &v) const
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>
1027  Vec3<T0> transformH(const Vec3<T0> &p) const
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 
1079 private:
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
1100 template <typename T0, typename T1>
1101 bool 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
1112 template <typename T0, typename T1>
1113 bool 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.
1117 template <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.
1125 template <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.
1135 template<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.
1150 template<typename T, typename MT>
1152 operator*(const Vec4<T> &_v,
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.
1165 template<typename T, typename MT>
1167 operator*(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.
1178 template<typename T, typename MT>
1180 operator*(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.
1191 template <typename T0, typename T1>
1193 operator+(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.
1202 template <typename T0, typename T1>
1204 operator-(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.
1213 template <typename T0, typename T1>
1215 operator*(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"
1226 template<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
1237 template<typename T>
1238 bool 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 
1304 template <typename T>
1305 inline bool isAffine(const Mat4<T>& m) {
1306  return (m.col(3) == Vec4<T>(0, 0, 0, 1));
1307 }
1308 
1309 template <typename T>
1310 inline bool hasTranslation(const Mat4<T>& m) {
1311  return (m.row(3) != Vec4<T>(0, 0, 0, 1));
1312 }
1313 
1314 template<typename T>
1315 inline Mat4<T>
1316 Abs(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 
1325 template<typename Type1, typename Type2>
1326 inline Mat4<Type1>
1327 cwiseAdd(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 
1340 template<typename T>
1341 inline bool
1342 cwiseLessThan(const Mat4<T>& m0, const Mat4<T>& m1)
1343 {
1344  return cwiseLessThan<4, T>(m0, m1);
1345 }
1346 
1347 template<typename T>
1348 inline bool
1349 cwiseGreaterThan(const Mat4<T>& m0, const Mat4<T>& m1)
1350 {
1351  return cwiseGreaterThan<4, T>(m0, m1);
1352 }
1353 
1356 using Mat4f = Mat4d;
1357 
1360 
1361 } // namespace math
1362 
1363 
1364 template<> inline math::Mat4s zeroVal<math::Mat4s>() { return math::Mat4s::zero(); }
1365 template<> 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_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
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:443
T & y()
Definition: Vec3.h:87
Mat4 transpose() const
Definition: Mat4.h:472
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
T & operator()(int i, int j)
Definition: Mat4.h:176
void setToScale(const Vec3< T0 > &v)
Sets the matrix to a matrix that scales by v.
Definition: Mat4.h:726
bool cwiseGreaterThan(const Mat4< T > &m0, const Mat4< T > &m1)
Definition: Mat4.h:1349
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
T det() const
Determinant of matrix.
Definition: Mat4.h:637
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:74
T operator()(int i, int j) const
Definition: Mat4.h:186
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
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
Mat4< double > Mat4d
Definition: Mat4.h:1355
void setToRotation(Axis axis, T angle)
Sets the matrix to a rotation about the given axis.
Definition: Mat4.h:783
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
void setIdentity()
Set this matrix to identity.
Definition: Mat4.h:265
Definition: Math.h:903
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Transform a Vec3 by pre-multiplication, without homogenous division.
Definition: Mat4.h:1020
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
Vec4< T0 > transform(const Vec4< T0 > &v) const
Transform a Vec4 by post-multiplication.
Definition: Mat4.h:999
3x3 matrix class.
Definition: Mat3.h:28
Vec3< T0 > transformH(const Vec3< T0 > &p) const
Transform a Vec3 by post-multiplication, doing homogenous divison.
Definition: Mat4.h:1027
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
void setCol(int j, const Vec4< T > &v)
Set jth column to vector v.
Definition: Mat4.h:157
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
Definition: Math.h:904
T & z()
Definition: Vec3.h:88
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
Vec3< T0 > pretransformH(const Vec3< T0 > &p) const
Transform a Vec3 by pre-multiplication, doing homogenous division.
Definition: Mat4.h:1049
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:110
Mat4(const Vec4< Source > &v1, const Vec4< Source > &v2, const Vec4< Source > &v3, const Vec4< Source > &v4, bool rows=true)
Definition: Mat4.h:95
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
OutGridT XformOp & op
Definition: ValueTransformer.h:139
bool cwiseLessThan(const Mat4< T > &m0, const Mat4< T > &m1)
Definition: Mat4.h:1342
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_END
Definition: Platform.h:222
Mat4< T > Abs(const Mat4< T > &m)
Definition: Mat4.h:1316
Mat4 inverse(T tolerance=0) const
Definition: Mat4.h:485
Definition: Mat.h:26
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:86
void preScale(const Vec3< T0 > &v)
Definition: Mat4.h:736
static const Mat4< T > & identity()
Predefined constant for identity matrix.
Definition: Mat4.h:117
4x4 -matrix class.
Definition: Mat3.h:22
Mat4(Source *a)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat4.h:51
Real value_type
Definition: Mat.h:29
static const Mat4< T > & zero()
Predefined constant for zero matrix.
Definition: Mat4.h:128
Mat3< T > getMat3() const
Definition: Mat4.h:297
#define OPENVDB_ASSERT(X)
Definition: Assert.h:41
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
void setRow(int i, const Vec4< T > &v)
Set ith row to vector v.
Definition: Mat4.h:139
bool hasTranslation(const Mat4< T > &m)
Definition: Mat4.h:1310
Axis
Definition: Math.h:901
Vec3< T1 > transformNormal(const Mat4< T0 > &m, const Vec3< T1 > &n)
Definition: Mat4.h:1227
void postRotate(Axis axis, T angle)
Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:880
T det() const
Determinant of matrix.
Definition: Mat3.h:479
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
bool operator!=(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat4.h:1113
static Mat4 translation(const Vec3d &v)
Sets the matrix to a matrix that translates by v.
Definition: Mat4.h:667
T mm[SIZE *SIZE]
Definition: Mat.h:160
void setToTranslation(const Vec3< T0 > &v)
Sets the matrix to a matrix that translates by v.
Definition: Mat4.h:678
bool operator==(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat4.h:1101
const Mat4< T > & operator*=(S scalar)
Multiply each element of this matrix by scalar.
Definition: Mat4.h:355
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
Definition: Exceptions.h:13
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
constexpr T zeroVal()
Return the value of type T that corresponds to zero.
Definition: Math.h:70
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
Mat4< Type1 > cwiseAdd(const Mat4< Type1 > &m, const Type2 s)
Definition: Mat4.h:1327
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition: Mat3.h:668
void setToRotation(const Vec3< T > &axis, T angle)
Sets the matrix to a rotation about an arbitrary axis.
Definition: Mat4.h:788
Definition: Mat.h:165
Real ValueType
Definition: Mat.h:30
void postScale(const Vec3< T0 > &v)
Definition: Mat4.h:758
Definition: Math.h:902
Mat4< float > Mat4s
Definition: Mat4.h:1354
void setTranslation(const Vec3< T > &t)
Definition: Mat4.h:314
void postShear(Axis axis0, Axis axis1, T shear)
Right multiplies a shearing transformation into the matrix.
Definition: Mat4.h:984
Vec4< T > row(int i) const
Get ith row, e.g. Vec4f v = m.row(1);.
Definition: Mat4.h:150
void preShear(Axis axis0, Axis axis1, T shear)
Left multiplies a shearing transformation into the matrix.
Definition: Mat4.h:969
Definition: Exceptions.h:56
void preTranslate(const Vec3< T0 > &tr)
Left multiples by the specified translation, i.e. Trans * (*this)
Definition: Mat4.h:703
Vec4< T > col(int j) const
Get jth column, e.g. Vec4f v = m.col(0);.
Definition: Mat4.h:167
const Mat4< T > & operator*=(const Mat4< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat4.h:439
void setZero()
Definition: Mat4.h:244
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
Vec4< T0 > pretransform(const Vec4< T0 > &v) const
Transform a Vec4 by pre-multiplication.
Definition: Mat4.h:1013
Vec3< T > getTranslation() const
Return the translation component.
Definition: Mat4.h:309
Vec3< T0 > transform(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without homogenous division.
Definition: Mat4.h:1006
Mat4(const Mat4< Source > &m)
Conversion constructor.
Definition: Mat4.h:107
void postTranslate(const Vec3< T0 > &tr)
Right multiplies by the specified translation matrix, i.e. (*this) * Trans.
Definition: Mat4.h:714
Definition: Mat4.h:24
void setMat3(const Mat3< T > &m)
Set upper left to a Mat3.
Definition: Mat4.h:290
const Mat4 & operator=(const Mat4< Source > &m)
Assignment operator.
Definition: Mat4.h:323
Mat4< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat4.h:343
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h.in:121
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
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
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
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
T * asPointer()
Direct access to the internal data.
Definition: Mat.h:101
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:192
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
Vec3< T0 > transform3x3(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without translation.
Definition: Mat4.h:1070
void preRotate(Axis axis, T angle)
Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:797
bool isAffine(const Mat4< T > &m)
Definition: Mat4.h:1305
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h.in:218
#define OPENVDB_IS_POD(Type)
Definition: Math.h:56