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