HDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Mat4.h
Go to the documentation of this file.
1 ///////////////////////////////////////////////////////////////////////////
2 //
3 // Copyright (c) 2012-2018 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 "Math.h"
37 #include "Mat3.h"
38 #include "Vec3.h"
39 #include "Vec4.h"
40 #include <algorithm> // for std::copy(), std::swap()
41 #include <cassert>
42 #include <iomanip>
43 #include <cmath>
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  using value_type = T;
62  using ValueType = T;
63  using MyBase = Mat<4, T>;
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 this matrix to zero
290  void setZero()
291  {
292  MyBase::mm[ 0] = 0;
293  MyBase::mm[ 1] = 0;
294  MyBase::mm[ 2] = 0;
295  MyBase::mm[ 3] = 0;
296  MyBase::mm[ 4] = 0;
297  MyBase::mm[ 5] = 0;
298  MyBase::mm[ 6] = 0;
299  MyBase::mm[ 7] = 0;
300  MyBase::mm[ 8] = 0;
301  MyBase::mm[ 9] = 0;
302  MyBase::mm[10] = 0;
303  MyBase::mm[11] = 0;
304  MyBase::mm[12] = 0;
305  MyBase::mm[13] = 0;
306  MyBase::mm[14] = 0;
307  MyBase::mm[15] = 0;
308  }
309 
310  /// Set this matrix to identity
311  void setIdentity()
312  {
313  MyBase::mm[ 0] = 1;
314  MyBase::mm[ 1] = 0;
315  MyBase::mm[ 2] = 0;
316  MyBase::mm[ 3] = 0;
317 
318  MyBase::mm[ 4] = 0;
319  MyBase::mm[ 5] = 1;
320  MyBase::mm[ 6] = 0;
321  MyBase::mm[ 7] = 0;
322 
323  MyBase::mm[ 8] = 0;
324  MyBase::mm[ 9] = 0;
325  MyBase::mm[10] = 1;
326  MyBase::mm[11] = 0;
327 
328  MyBase::mm[12] = 0;
329  MyBase::mm[13] = 0;
330  MyBase::mm[14] = 0;
331  MyBase::mm[15] = 1;
332  }
333 
334 
335  /// Set upper left to a Mat3
336  void setMat3(const Mat3<T> &m)
337  {
338  for (int i = 0; i < 3; i++)
339  for (int j=0; j < 3; j++)
340  MyBase::mm[i*4+j] = m[i][j];
341  }
342 
343  Mat3<T> getMat3() const
344  {
345  Mat3<T> m;
346 
347  for (int i = 0; i < 3; i++)
348  for (int j = 0; j < 3; j++)
349  m[i][j] = MyBase::mm[i*4+j];
350 
351  return m;
352  }
353 
354  /// Return the translation component
356  {
357  return Vec3<T>(MyBase::mm[12], MyBase::mm[13], MyBase::mm[14]);
358  }
359 
360  void setTranslation(const Vec3<T> &t)
361  {
362  MyBase::mm[12] = t[0];
363  MyBase::mm[13] = t[1];
364  MyBase::mm[14] = t[2];
365  }
366 
367  /// Assignment operator
368  template<typename Source>
369  const Mat4& operator=(const Mat4<Source> &m)
370  {
371  const Source *src = m.asPointer();
372 
373  // don't suppress warnings when assigning from different numerical types
374  std::copy(src, (src + this->numElements()), MyBase::mm);
375  return *this;
376  }
377 
378  /// Return @c true if this matrix is equivalent to @a m within a tolerance of @a eps.
379  bool eq(const Mat4 &m, T eps=1.0e-8) const
380  {
381  for (int i = 0; i < 16; i++) {
382  if (!isApproxEqual(MyBase::mm[i], m.mm[i], eps))
383  return false;
384  }
385  return true;
386  }
387 
388  /// Negation operator, for e.g. m1 = -m2;
390  {
391  return Mat4<T>(
392  -MyBase::mm[ 0], -MyBase::mm[ 1], -MyBase::mm[ 2], -MyBase::mm[ 3],
393  -MyBase::mm[ 4], -MyBase::mm[ 5], -MyBase::mm[ 6], -MyBase::mm[ 7],
394  -MyBase::mm[ 8], -MyBase::mm[ 9], -MyBase::mm[10], -MyBase::mm[11],
395  -MyBase::mm[12], -MyBase::mm[13], -MyBase::mm[14], -MyBase::mm[15]
396  );
397  } // trivial
398 
399  /// Multiply each element of this matrix by @a scalar.
400  template <typename S>
401  const Mat4<T>& operator*=(S scalar)
402  {
403  MyBase::mm[ 0] *= scalar;
404  MyBase::mm[ 1] *= scalar;
405  MyBase::mm[ 2] *= scalar;
406  MyBase::mm[ 3] *= scalar;
407 
408  MyBase::mm[ 4] *= scalar;
409  MyBase::mm[ 5] *= scalar;
410  MyBase::mm[ 6] *= scalar;
411  MyBase::mm[ 7] *= scalar;
412 
413  MyBase::mm[ 8] *= scalar;
414  MyBase::mm[ 9] *= scalar;
415  MyBase::mm[10] *= scalar;
416  MyBase::mm[11] *= scalar;
417 
418  MyBase::mm[12] *= scalar;
419  MyBase::mm[13] *= scalar;
420  MyBase::mm[14] *= scalar;
421  MyBase::mm[15] *= scalar;
422  return *this;
423  }
424 
425  /// Add each element of the given matrix to the corresponding element of this matrix.
426  template <typename S>
427  const Mat4<T> &operator+=(const Mat4<S> &m1)
428  {
429  const S* s = m1.asPointer();
430 
431  MyBase::mm[ 0] += s[ 0];
432  MyBase::mm[ 1] += s[ 1];
433  MyBase::mm[ 2] += s[ 2];
434  MyBase::mm[ 3] += s[ 3];
435 
436  MyBase::mm[ 4] += s[ 4];
437  MyBase::mm[ 5] += s[ 5];
438  MyBase::mm[ 6] += s[ 6];
439  MyBase::mm[ 7] += s[ 7];
440 
441  MyBase::mm[ 8] += s[ 8];
442  MyBase::mm[ 9] += s[ 9];
443  MyBase::mm[10] += s[10];
444  MyBase::mm[11] += s[11];
445 
446  MyBase::mm[12] += s[12];
447  MyBase::mm[13] += s[13];
448  MyBase::mm[14] += s[14];
449  MyBase::mm[15] += s[15];
450 
451  return *this;
452  }
453 
454  /// Subtract each element of the given matrix from the corresponding element of this matrix.
455  template <typename S>
456  const Mat4<T> &operator-=(const Mat4<S> &m1)
457  {
458  const S* s = m1.asPointer();
459 
460  MyBase::mm[ 0] -= s[ 0];
461  MyBase::mm[ 1] -= s[ 1];
462  MyBase::mm[ 2] -= s[ 2];
463  MyBase::mm[ 3] -= s[ 3];
464 
465  MyBase::mm[ 4] -= s[ 4];
466  MyBase::mm[ 5] -= s[ 5];
467  MyBase::mm[ 6] -= s[ 6];
468  MyBase::mm[ 7] -= s[ 7];
469 
470  MyBase::mm[ 8] -= s[ 8];
471  MyBase::mm[ 9] -= s[ 9];
472  MyBase::mm[10] -= s[10];
473  MyBase::mm[11] -= s[11];
474 
475  MyBase::mm[12] -= s[12];
476  MyBase::mm[13] -= s[13];
477  MyBase::mm[14] -= s[14];
478  MyBase::mm[15] -= s[15];
479 
480  return *this;
481  }
482 
483  /// Multiply this matrix by the given matrix.
484  template <typename S>
485  const Mat4<T> &operator*=(const Mat4<S> &m1)
486  {
487  Mat4<T> m0(*this);
488 
489  const T* s0 = m0.asPointer();
490  const S* s1 = m1.asPointer();
491 
492  for (int i = 0; i < 4; i++) {
493  int i4 = 4 * i;
494  MyBase::mm[i4+0] = static_cast<T>(s0[i4+0] * s1[ 0] +
495  s0[i4+1] * s1[ 4] +
496  s0[i4+2] * s1[ 8] +
497  s0[i4+3] * s1[12]);
498 
499  MyBase::mm[i4+1] = static_cast<T>(s0[i4+0] * s1[ 1] +
500  s0[i4+1] * s1[ 5] +
501  s0[i4+2] * s1[ 9] +
502  s0[i4+3] * s1[13]);
503 
504  MyBase::mm[i4+2] = static_cast<T>(s0[i4+0] * s1[ 2] +
505  s0[i4+1] * s1[ 6] +
506  s0[i4+2] * s1[10] +
507  s0[i4+3] * s1[14]);
508 
509  MyBase::mm[i4+3] = static_cast<T>(s0[i4+0] * s1[ 3] +
510  s0[i4+1] * s1[ 7] +
511  s0[i4+2] * s1[11] +
512  s0[i4+3] * s1[15]);
513  }
514  return *this;
515  }
516 
517  /// @return transpose of this
518  Mat4 transpose() const
519  {
520  return Mat4<T>(
521  MyBase::mm[ 0], MyBase::mm[ 4], MyBase::mm[ 8], MyBase::mm[12],
522  MyBase::mm[ 1], MyBase::mm[ 5], MyBase::mm[ 9], MyBase::mm[13],
523  MyBase::mm[ 2], MyBase::mm[ 6], MyBase::mm[10], MyBase::mm[14],
524  MyBase::mm[ 3], MyBase::mm[ 7], MyBase::mm[11], MyBase::mm[15]
525  );
526  }
527 
528 
529  /// @return inverse of this
530  /// @throw ArithmeticError if singular
531  Mat4 inverse(T tolerance = 0) const
532  {
533  //
534  // inv [ A | b ] = [ E | f ] A: 3x3, b: 3x1, c': 1x3 d: 1x1
535  // [ c' | d ] [ g' | h ]
536  //
537  // If A is invertible use
538  //
539  // E = A^-1 + p*h*r
540  // p = A^-1 * b
541  // f = -p * h
542  // g' = -h * c'
543  // h = 1 / (d - c'*p)
544  // r' = c'*A^-1
545  //
546  // Otherwise use gauss-jordan elimination
547  //
548 
549  //
550  // We create this alias to ourself so we can easily use own subscript
551  // operator.
552  const Mat4<T>& m(*this);
553 
554  T m0011 = m[0][0] * m[1][1];
555  T m0012 = m[0][0] * m[1][2];
556  T m0110 = m[0][1] * m[1][0];
557  T m0210 = m[0][2] * m[1][0];
558  T m0120 = m[0][1] * m[2][0];
559  T m0220 = m[0][2] * m[2][0];
560 
561  T detA = m0011 * m[2][2] - m0012 * m[2][1] - m0110 * m[2][2]
562  + m0210 * m[2][1] + m0120 * m[1][2] - m0220 * m[1][1];
563 
564  bool hasPerspective =
565  (!isExactlyEqual(m[0][3], T(0.0)) ||
566  !isExactlyEqual(m[1][3], T(0.0)) ||
567  !isExactlyEqual(m[2][3], T(0.0)) ||
568  !isExactlyEqual(m[3][3], T(1.0)));
569 
570  T det;
571  if (hasPerspective) {
572  det = m[0][3] * det3(m, 1,2,3, 0,2,1)
573  + m[1][3] * det3(m, 2,0,3, 0,2,1)
574  + m[2][3] * det3(m, 3,0,1, 0,2,1)
575  + m[3][3] * detA;
576  } else {
577  det = detA * m[3][3];
578  }
579 
580  Mat4<T> inv;
581  bool invertible;
582 
583  if (isApproxEqual(det,T(0.0),tolerance)) {
584  invertible = false;
585 
586  } else if (isApproxEqual(detA,T(0.0),T(1e-8))) {
587  // det is too small to rely on inversion by subblocks
588  invertible = m.invert(inv, tolerance);
589 
590  } else {
591  invertible = true;
592  detA = 1.0 / detA;
593 
594  //
595  // Calculate A^-1
596  //
597  inv[0][0] = detA * ( m[1][1] * m[2][2] - m[1][2] * m[2][1]);
598  inv[0][1] = detA * (-m[0][1] * m[2][2] + m[0][2] * m[2][1]);
599  inv[0][2] = detA * ( m[0][1] * m[1][2] - m[0][2] * m[1][1]);
600 
601  inv[1][0] = detA * (-m[1][0] * m[2][2] + m[1][2] * m[2][0]);
602  inv[1][1] = detA * ( m[0][0] * m[2][2] - m0220);
603  inv[1][2] = detA * ( m0210 - m0012);
604 
605  inv[2][0] = detA * ( m[1][0] * m[2][1] - m[1][1] * m[2][0]);
606  inv[2][1] = detA * ( m0120 - m[0][0] * m[2][1]);
607  inv[2][2] = detA * ( m0011 - m0110);
608 
609  if (hasPerspective) {
610  //
611  // Calculate r, p, and h
612  //
613  Vec3<T> r;
614  r[0] = m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
615  + m[3][2] * inv[2][0];
616  r[1] = m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
617  + m[3][2] * inv[2][1];
618  r[2] = m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
619  + m[3][2] * inv[2][2];
620 
621  Vec3<T> p;
622  p[0] = inv[0][0] * m[0][3] + inv[0][1] * m[1][3]
623  + inv[0][2] * m[2][3];
624  p[1] = inv[1][0] * m[0][3] + inv[1][1] * m[1][3]
625  + inv[1][2] * m[2][3];
626  p[2] = inv[2][0] * m[0][3] + inv[2][1] * m[1][3]
627  + inv[2][2] * m[2][3];
628 
629  T h = m[3][3] - p.dot(Vec3<T>(m[3][0],m[3][1],m[3][2]));
630  if (isApproxEqual(h,T(0.0),tolerance)) {
631  invertible = false;
632 
633  } else {
634  h = 1.0 / h;
635 
636  //
637  // Calculate h, g, and f
638  //
639  inv[3][3] = h;
640  inv[3][0] = -h * r[0];
641  inv[3][1] = -h * r[1];
642  inv[3][2] = -h * r[2];
643 
644  inv[0][3] = -h * p[0];
645  inv[1][3] = -h * p[1];
646  inv[2][3] = -h * p[2];
647 
648  //
649  // Calculate E
650  //
651  p *= h;
652  inv[0][0] += p[0] * r[0];
653  inv[0][1] += p[0] * r[1];
654  inv[0][2] += p[0] * r[2];
655  inv[1][0] += p[1] * r[0];
656  inv[1][1] += p[1] * r[1];
657  inv[1][2] += p[1] * r[2];
658  inv[2][0] += p[2] * r[0];
659  inv[2][1] += p[2] * r[1];
660  inv[2][2] += p[2] * r[2];
661  }
662  } else {
663  // Equations are much simpler in the non-perspective case
664  inv[3][0] = - (m[3][0] * inv[0][0] + m[3][1] * inv[1][0]
665  + m[3][2] * inv[2][0]);
666  inv[3][1] = - (m[3][0] * inv[0][1] + m[3][1] * inv[1][1]
667  + m[3][2] * inv[2][1]);
668  inv[3][2] = - (m[3][0] * inv[0][2] + m[3][1] * inv[1][2]
669  + m[3][2] * inv[2][2]);
670  inv[0][3] = 0.0;
671  inv[1][3] = 0.0;
672  inv[2][3] = 0.0;
673  inv[3][3] = 1.0;
674  }
675  }
676 
677  if (!invertible) OPENVDB_THROW(ArithmeticError, "Inversion of singular 4x4 matrix");
678  return inv;
679  }
680 
681 
682  /// Determinant of matrix
683  T det() const
684  {
685  const T *ap;
686  Mat3<T> submat;
687  T det;
688  T *sp;
689  int i, j, k, sign;
690 
691  det = 0;
692  sign = 1;
693  for (i = 0; i < 4; i++) {
694  ap = &MyBase::mm[ 0];
695  sp = submat.asPointer();
696  for (j = 0; j < 4; j++) {
697  for (k = 0; k < 4; k++) {
698  if ((k != i) && (j != 0)) {
699  *sp++ = *ap;
700  }
701  ap++;
702  }
703  }
704 
705  det += sign * MyBase::mm[i] * submat.det();
706  sign = -sign;
707  }
708 
709  return det;
710  }
711 
712  /// Sets the matrix to a matrix that translates by v
713  static Mat4 translation(const Vec3d& v)
714  {
715  return Mat4(
716  T(1), T(0), T(0), T(0),
717  T(0), T(1), T(0), T(0),
718  T(0), T(0), T(1), T(0),
719  T(v.x()), T(v.y()),T(v.z()), T(1));
720  }
721 
722  /// Sets the matrix to a matrix that translates by v
723  template <typename T0>
725  {
726  MyBase::mm[ 0] = 1;
727  MyBase::mm[ 1] = 0;
728  MyBase::mm[ 2] = 0;
729  MyBase::mm[ 3] = 0;
730 
731  MyBase::mm[ 4] = 0;
732  MyBase::mm[ 5] = 1;
733  MyBase::mm[ 6] = 0;
734  MyBase::mm[ 7] = 0;
735 
736  MyBase::mm[ 8] = 0;
737  MyBase::mm[ 9] = 0;
738  MyBase::mm[10] = 1;
739  MyBase::mm[11] = 0;
740 
741  MyBase::mm[12] = v.x();
742  MyBase::mm[13] = v.y();
743  MyBase::mm[14] = v.z();
744  MyBase::mm[15] = 1;
745  }
746 
747  /// Left multiples by the specified translation, i.e. Trans * (*this)
748  template <typename T0>
749  void preTranslate(const Vec3<T0>& tr)
750  {
751  Vec3<T> tmp(tr.x(), tr.y(), tr.z());
752  Mat4<T> Tr = Mat4<T>::translation(tmp);
753 
754  *this = Tr * (*this);
755 
756  }
757 
758  /// Right multiplies by the specified translation matrix, i.e. (*this) * Trans
759  template <typename T0>
760  void postTranslate(const Vec3<T0>& tr)
761  {
762  Vec3<T> tmp(tr.x(), tr.y(), tr.z());
763  Mat4<T> Tr = Mat4<T>::translation(tmp);
764 
765  *this = (*this) * Tr;
766 
767  }
768 
769 
770  /// Sets the matrix to a matrix that scales by v
771  template <typename T0>
772  void setToScale(const Vec3<T0>& v)
773  {
774  this->setIdentity();
775  MyBase::mm[ 0] = v.x();
776  MyBase::mm[ 5] = v.y();
777  MyBase::mm[10] = v.z();
778  }
779 
780  // Left multiples by the specified scale matrix, i.e. Sc * (*this)
781  template <typename T0>
782  void preScale(const Vec3<T0>& v)
783  {
784  MyBase::mm[ 0] *= v.x();
785  MyBase::mm[ 1] *= v.x();
786  MyBase::mm[ 2] *= v.x();
787  MyBase::mm[ 3] *= v.x();
788 
789  MyBase::mm[ 4] *= v.y();
790  MyBase::mm[ 5] *= v.y();
791  MyBase::mm[ 6] *= v.y();
792  MyBase::mm[ 7] *= v.y();
793 
794  MyBase::mm[ 8] *= v.z();
795  MyBase::mm[ 9] *= v.z();
796  MyBase::mm[10] *= v.z();
797  MyBase::mm[11] *= v.z();
798  }
799 
800 
801 
802  // Right multiples by the specified scale matrix, i.e. (*this) * Sc
803  template <typename T0>
804  void postScale(const Vec3<T0>& v)
805  {
806 
807  MyBase::mm[ 0] *= v.x();
808  MyBase::mm[ 1] *= v.y();
809  MyBase::mm[ 2] *= v.z();
810 
811  MyBase::mm[ 4] *= v.x();
812  MyBase::mm[ 5] *= v.y();
813  MyBase::mm[ 6] *= v.z();
814 
815  MyBase::mm[ 8] *= v.x();
816  MyBase::mm[ 9] *= v.y();
817  MyBase::mm[10] *= v.z();
818 
819  MyBase::mm[12] *= v.x();
820  MyBase::mm[13] *= v.y();
821  MyBase::mm[14] *= v.z();
822 
823  }
824 
825 
826  /// @brief Sets the matrix to a rotation about the given axis.
827  /// @param axis The axis (one of X, Y, Z) to rotate about.
828  /// @param angle The rotation angle, in radians.
829  void setToRotation(Axis axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
830 
831  /// @brief Sets the matrix to a rotation about an arbitrary axis
832  /// @param axis The axis of rotation (cannot be zero-length)
833  /// @param angle The rotation angle, in radians.
834  void setToRotation(const Vec3<T>& axis, T angle) {*this = rotation<Mat4<T> >(axis, angle);}
835 
836  /// @brief Sets the matrix to a rotation that maps v1 onto v2 about the cross
837  /// product of v1 and v2.
838  void setToRotation(const Vec3<T>& v1, const Vec3<T>& v2) {*this = rotation<Mat4<T> >(v1, v2);}
839 
840 
841  /// @brief Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
842  /// @param axis The axis (one of X, Y, Z) of rotation.
843  /// @param angle The clock-wise rotation angle, in radians.
844  void preRotate(Axis axis, T angle)
845  {
846  T c = static_cast<T>(cos(angle));
847  T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
848 
849  switch (axis) {
850  case X_AXIS:
851  {
852  T a4, a5, a6, a7;
853 
854  a4 = c * MyBase::mm[ 4] - s * MyBase::mm[ 8];
855  a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 9];
856  a6 = c * MyBase::mm[ 6] - s * MyBase::mm[10];
857  a7 = c * MyBase::mm[ 7] - s * MyBase::mm[11];
858 
859 
860  MyBase::mm[ 8] = s * MyBase::mm[ 4] + c * MyBase::mm[ 8];
861  MyBase::mm[ 9] = s * MyBase::mm[ 5] + c * MyBase::mm[ 9];
862  MyBase::mm[10] = s * MyBase::mm[ 6] + c * MyBase::mm[10];
863  MyBase::mm[11] = s * MyBase::mm[ 7] + c * MyBase::mm[11];
864 
865  MyBase::mm[ 4] = a4;
866  MyBase::mm[ 5] = a5;
867  MyBase::mm[ 6] = a6;
868  MyBase::mm[ 7] = a7;
869  }
870  break;
871 
872  case Y_AXIS:
873  {
874  T a0, a1, a2, a3;
875 
876  a0 = c * MyBase::mm[ 0] + s * MyBase::mm[ 8];
877  a1 = c * MyBase::mm[ 1] + s * MyBase::mm[ 9];
878  a2 = c * MyBase::mm[ 2] + s * MyBase::mm[10];
879  a3 = c * MyBase::mm[ 3] + s * MyBase::mm[11];
880 
881  MyBase::mm[ 8] = -s * MyBase::mm[ 0] + c * MyBase::mm[ 8];
882  MyBase::mm[ 9] = -s * MyBase::mm[ 1] + c * MyBase::mm[ 9];
883  MyBase::mm[10] = -s * MyBase::mm[ 2] + c * MyBase::mm[10];
884  MyBase::mm[11] = -s * MyBase::mm[ 3] + c * MyBase::mm[11];
885 
886 
887  MyBase::mm[ 0] = a0;
888  MyBase::mm[ 1] = a1;
889  MyBase::mm[ 2] = a2;
890  MyBase::mm[ 3] = a3;
891  }
892  break;
893 
894  case Z_AXIS:
895  {
896  T a0, a1, a2, a3;
897 
898  a0 = c * MyBase::mm[ 0] - s * MyBase::mm[ 4];
899  a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 5];
900  a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 6];
901  a3 = c * MyBase::mm[ 3] - s * MyBase::mm[ 7];
902 
903  MyBase::mm[ 4] = s * MyBase::mm[ 0] + c * MyBase::mm[ 4];
904  MyBase::mm[ 5] = s * MyBase::mm[ 1] + c * MyBase::mm[ 5];
905  MyBase::mm[ 6] = s * MyBase::mm[ 2] + c * MyBase::mm[ 6];
906  MyBase::mm[ 7] = s * MyBase::mm[ 3] + c * MyBase::mm[ 7];
907 
908  MyBase::mm[ 0] = a0;
909  MyBase::mm[ 1] = a1;
910  MyBase::mm[ 2] = a2;
911  MyBase::mm[ 3] = a3;
912  }
913  break;
914 
915  default:
916  assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
917  }
918  }
919 
920 
921  /// @brief Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
922  /// @param axis The axis (one of X, Y, Z) of rotation.
923  /// @param angle The clock-wise rotation angle, in radians.
924  void postRotate(Axis axis, T angle)
925  {
926  T c = static_cast<T>(cos(angle));
927  T s = -static_cast<T>(sin(angle)); // the "-" makes it clockwise
928 
929 
930 
931  switch (axis) {
932  case X_AXIS:
933  {
934  T a2, a6, a10, a14;
935 
936  a2 = c * MyBase::mm[ 2] - s * MyBase::mm[ 1];
937  a6 = c * MyBase::mm[ 6] - s * MyBase::mm[ 5];
938  a10 = c * MyBase::mm[10] - s * MyBase::mm[ 9];
939  a14 = c * MyBase::mm[14] - s * MyBase::mm[13];
940 
941 
942  MyBase::mm[ 1] = c * MyBase::mm[ 1] + s * MyBase::mm[ 2];
943  MyBase::mm[ 5] = c * MyBase::mm[ 5] + s * MyBase::mm[ 6];
944  MyBase::mm[ 9] = c * MyBase::mm[ 9] + s * MyBase::mm[10];
945  MyBase::mm[13] = c * MyBase::mm[13] + s * MyBase::mm[14];
946 
947  MyBase::mm[ 2] = a2;
948  MyBase::mm[ 6] = a6;
949  MyBase::mm[10] = a10;
950  MyBase::mm[14] = a14;
951  }
952  break;
953 
954  case Y_AXIS:
955  {
956  T a2, a6, a10, a14;
957 
958  a2 = c * MyBase::mm[ 2] + s * MyBase::mm[ 0];
959  a6 = c * MyBase::mm[ 6] + s * MyBase::mm[ 4];
960  a10 = c * MyBase::mm[10] + s * MyBase::mm[ 8];
961  a14 = c * MyBase::mm[14] + s * MyBase::mm[12];
962 
963  MyBase::mm[ 0] = c * MyBase::mm[ 0] - s * MyBase::mm[ 2];
964  MyBase::mm[ 4] = c * MyBase::mm[ 4] - s * MyBase::mm[ 6];
965  MyBase::mm[ 8] = c * MyBase::mm[ 8] - s * MyBase::mm[10];
966  MyBase::mm[12] = c * MyBase::mm[12] - s * MyBase::mm[14];
967 
968  MyBase::mm[ 2] = a2;
969  MyBase::mm[ 6] = a6;
970  MyBase::mm[10] = a10;
971  MyBase::mm[14] = a14;
972  }
973  break;
974 
975  case Z_AXIS:
976  {
977  T a1, a5, a9, a13;
978 
979  a1 = c * MyBase::mm[ 1] - s * MyBase::mm[ 0];
980  a5 = c * MyBase::mm[ 5] - s * MyBase::mm[ 4];
981  a9 = c * MyBase::mm[ 9] - s * MyBase::mm[ 8];
982  a13 = c * MyBase::mm[13] - s * MyBase::mm[12];
983 
984  MyBase::mm[ 0] = c * MyBase::mm[ 0] + s * MyBase::mm[ 1];
985  MyBase::mm[ 4] = c * MyBase::mm[ 4] + s * MyBase::mm[ 5];
986  MyBase::mm[ 8] = c * MyBase::mm[ 8] + s * MyBase::mm[ 9];
987  MyBase::mm[12] = c * MyBase::mm[12] + s * MyBase::mm[13];
988 
989  MyBase::mm[ 1] = a1;
990  MyBase::mm[ 5] = a5;
991  MyBase::mm[ 9] = a9;
992  MyBase::mm[13] = a13;
993 
994  }
995  break;
996 
997  default:
998  assert(axis==X_AXIS || axis==Y_AXIS || axis==Z_AXIS);
999  }
1000  }
1001 
1002  /// @brief Sets the matrix to a shear along axis0 by a fraction of axis1.
1003  /// @param axis0 The fixed axis of the shear.
1004  /// @param axis1 The shear axis.
1005  /// @param shearby The shear factor.
1006  void setToShear(Axis axis0, Axis axis1, T shearby)
1007  {
1008  *this = shear<Mat4<T> >(axis0, axis1, shearby);
1009  }
1010 
1011 
1012  /// @brief Left multiplies a shearing transformation into the matrix.
1013  /// @see setToShear
1014  void preShear(Axis axis0, Axis axis1, T shear)
1015  {
1016  int index0 = static_cast<int>(axis0);
1017  int index1 = static_cast<int>(axis1);
1018 
1019  // to row "index1" add a multiple of the index0 row
1020  MyBase::mm[index1 * 4 + 0] += shear * MyBase::mm[index0 * 4 + 0];
1021  MyBase::mm[index1 * 4 + 1] += shear * MyBase::mm[index0 * 4 + 1];
1022  MyBase::mm[index1 * 4 + 2] += shear * MyBase::mm[index0 * 4 + 2];
1023  MyBase::mm[index1 * 4 + 3] += shear * MyBase::mm[index0 * 4 + 3];
1024  }
1025 
1026 
1027  /// @brief Right multiplies a shearing transformation into the matrix.
1028  /// @see setToShear
1029  void postShear(Axis axis0, Axis axis1, T shear)
1030  {
1031  int index0 = static_cast<int>(axis0);
1032  int index1 = static_cast<int>(axis1);
1033 
1034  // to collumn "index0" add a multiple of the index1 row
1035  MyBase::mm[index0 + 0] += shear * MyBase::mm[index1 + 0];
1036  MyBase::mm[index0 + 4] += shear * MyBase::mm[index1 + 4];
1037  MyBase::mm[index0 + 8] += shear * MyBase::mm[index1 + 8];
1038  MyBase::mm[index0 + 12] += shear * MyBase::mm[index1 + 12];
1039 
1040  }
1041 
1042  /// Transform a Vec4 by post-multiplication.
1043  template<typename T0>
1045  {
1046  return static_cast< Vec4<T0> >(v * *this);
1047  }
1048 
1049  /// Transform a Vec3 by post-multiplication, without homogenous division.
1050  template<typename T0>
1052  {
1053  return static_cast< Vec3<T0> >(v * *this);
1054  }
1055 
1056  /// Transform a Vec4 by pre-multiplication.
1057  template<typename T0>
1059  {
1060  return static_cast< Vec4<T0> >(*this * v);
1061  }
1062 
1063  /// Transform a Vec3 by pre-multiplication, without homogenous division.
1064  template<typename T0>
1066  {
1067  return static_cast< Vec3<T0> >(*this * v);
1068  }
1069 
1070  /// Transform a Vec3 by post-multiplication, doing homogenous divison.
1071  template<typename T0>
1072  Vec3<T0> transformH(const Vec3<T0> &p) const
1073  {
1074  T0 w;
1075 
1076  // w = p * (*this).col(3);
1077  w = static_cast<T0>(p[0] * MyBase::mm[ 3] + p[1] * MyBase::mm[ 7]
1078  + p[2] * MyBase::mm[11] + MyBase::mm[15]);
1079 
1080  if ( !isExactlyEqual(w , 0.0) ) {
1081  return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 4] +
1082  p[2] * MyBase::mm[ 8] + MyBase::mm[12]) / w),
1083  static_cast<T0>((p[0] * MyBase::mm[ 1] + p[1] * MyBase::mm[ 5] +
1084  p[2] * MyBase::mm[ 9] + MyBase::mm[13]) / w),
1085  static_cast<T0>((p[0] * MyBase::mm[ 2] + p[1] * MyBase::mm[ 6] +
1086  p[2] * MyBase::mm[10] + MyBase::mm[14]) / w));
1087  }
1088 
1089  return Vec3<T0>(0, 0, 0);
1090  }
1091 
1092  /// Transform a Vec3 by pre-multiplication, doing homogenous division.
1093  template<typename T0>
1095  {
1096  T0 w;
1097 
1098  // w = p * (*this).col(3);
1099  w = p[0] * MyBase::mm[12] + p[1] * MyBase::mm[13] + p[2] * MyBase::mm[14] + MyBase::mm[15];
1100 
1101  if ( !isExactlyEqual(w , 0.0) ) {
1102  return Vec3<T0>(static_cast<T0>((p[0] * MyBase::mm[ 0] + p[1] * MyBase::mm[ 1] +
1103  p[2] * MyBase::mm[ 2] + MyBase::mm[ 3]) / w),
1104  static_cast<T0>((p[0] * MyBase::mm[ 4] + p[1] * MyBase::mm[ 5] +
1105  p[2] * MyBase::mm[ 6] + MyBase::mm[ 7]) / w),
1106  static_cast<T0>((p[0] * MyBase::mm[ 8] + p[1] * MyBase::mm[ 9] +
1107  p[2] * MyBase::mm[10] + MyBase::mm[11]) / w));
1108  }
1109 
1110  return Vec3<T0>(0, 0, 0);
1111  }
1112 
1113  /// Transform a Vec3 by post-multiplication, without translation.
1114  template<typename T0>
1116  {
1117  return Vec3<T0>(
1118  static_cast<T0>(v[0] * MyBase::mm[ 0] + v[1] * MyBase::mm[ 4] + v[2] * MyBase::mm[ 8]),
1119  static_cast<T0>(v[0] * MyBase::mm[ 1] + v[1] * MyBase::mm[ 5] + v[2] * MyBase::mm[ 9]),
1120  static_cast<T0>(v[0] * MyBase::mm[ 2] + v[1] * MyBase::mm[ 6] + v[2] * MyBase::mm[10]));
1121  }
1122 
1123 
1124 private:
1125  bool invert(Mat4<T> &inverse, T tolerance) const;
1126 
1127  T det2(const Mat4<T> &a, int i0, int i1, int j0, int j1) const {
1128  int i0row = i0 * 4;
1129  int i1row = i1 * 4;
1130  return a.mm[i0row+j0]*a.mm[i1row+j1] - a.mm[i0row+j1]*a.mm[i1row+j0];
1131  }
1132 
1133  T det3(const Mat4<T> &a, int i0, int i1, int i2,
1134  int j0, int j1, int j2) const {
1135  int i0row = i0 * 4;
1136  return a.mm[i0row+j0]*det2(a, i1,i2, j1,j2) +
1137  a.mm[i0row+j1]*det2(a, i1,i2, j2,j0) +
1138  a.mm[i0row+j2]*det2(a, i1,i2, j0,j1);
1139  }
1140 }; // class Mat4
1141 
1142 
1143 /// @relates Mat4
1144 /// @brief Equality operator, does exact floating point comparisons
1145 template <typename T0, typename T1>
1146 bool operator==(const Mat4<T0> &m0, const Mat4<T1> &m1)
1147 {
1148  const T0 *t0 = m0.asPointer();
1149  const T1 *t1 = m1.asPointer();
1150 
1151  for (int i=0; i<16; ++i) if (!isExactlyEqual(t0[i], t1[i])) return false;
1152  return true;
1153 }
1154 
1155 /// @relates Mat4
1156 /// @brief Inequality operator, does exact floating point comparisons
1157 template <typename T0, typename T1>
1158 bool operator!=(const Mat4<T0> &m0, const Mat4<T1> &m1) { return !(m0 == m1); }
1159 
1160 /// @relates Mat4
1161 /// @brief Multiply each element of the given matrix by @a scalar and return the result.
1162 template <typename S, typename T>
1164 {
1165  return m*scalar;
1166 }
1167 
1168 /// @relates Mat4
1169 /// @brief Multiply each element of the given matrix by @a scalar and return the result.
1170 template <typename S, typename T>
1172 {
1174  result *= scalar;
1175  return result;
1176 }
1177 
1178 /// @relates Mat4
1179 /// @brief Multiply @a _m by @a _v and return the resulting vector.
1180 template<typename T, typename MT>
1183  const Vec4<T> &_v)
1184 {
1185  MT const *m = _m.asPointer();
1187  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + _v[3]*m[3],
1188  _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + _v[3]*m[7],
1189  _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + _v[3]*m[11],
1190  _v[0]*m[12] + _v[1]*m[13] + _v[2]*m[14] + _v[3]*m[15]);
1191 }
1192 
1193 /// @relates Mat4
1194 /// @brief Multiply @a _v by @a _m and return the resulting vector.
1195 template<typename T, typename MT>
1197 operator*(const Vec4<T> &_v,
1198  const Mat4<MT> &_m)
1199 {
1200  MT const *m = _m.asPointer();
1202  _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + _v[3]*m[12],
1203  _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + _v[3]*m[13],
1204  _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + _v[3]*m[14],
1205  _v[0]*m[3] + _v[1]*m[7] + _v[2]*m[11] + _v[3]*m[15]);
1206 }
1207 
1208 /// @relates Mat4
1209 /// @brief Multiply @a _m by @a _v and return the resulting vector.
1210 template<typename T, typename MT>
1212 operator*(const Mat4<MT> &_m, const Vec3<T> &_v)
1213 {
1214  MT const *m = _m.asPointer();
1216  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2] + m[3],
1217  _v[0]*m[4] + _v[1]*m[5] + _v[2]*m[6] + m[7],
1218  _v[0]*m[8] + _v[1]*m[9] + _v[2]*m[10] + m[11]);
1219 }
1220 
1221 /// @relates Mat4
1222 /// @brief Multiply @a _v by @a _m and return the resulting vector.
1223 template<typename T, typename MT>
1225 operator*(const Vec3<T> &_v, const Mat4<MT> &_m)
1226 {
1227  MT const *m = _m.asPointer();
1229  _v[0]*m[0] + _v[1]*m[4] + _v[2]*m[8] + m[12],
1230  _v[0]*m[1] + _v[1]*m[5] + _v[2]*m[9] + m[13],
1231  _v[0]*m[2] + _v[1]*m[6] + _v[2]*m[10] + m[14]);
1232 }
1233 
1234 /// @relates Mat4
1235 /// @brief Add corresponding elements of @a m0 and @a m1 and return the result.
1236 template <typename T0, typename T1>
1238 operator+(const Mat4<T0> &m0, const Mat4<T1> &m1)
1239 {
1241  result += m1;
1242  return result;
1243 }
1244 
1245 /// @relates Mat4
1246 /// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result.
1247 template <typename T0, typename T1>
1249 operator-(const Mat4<T0> &m0, const Mat4<T1> &m1)
1250 {
1252  result -= m1;
1253  return result;
1254 }
1255 
1256 /// @relates Mat4
1257 /// @brief Multiply @a m0 by @a m1 and return the resulting matrix.
1258 template <typename T0, typename T1>
1260 operator*(const Mat4<T0> &m0, const Mat4<T1> &m1)
1261 {
1263  result *= m1;
1264  return result;
1265 }
1266 
1267 
1268 /// Transform a Vec3 by pre-multiplication, without translation.
1269 /// Presumes this matrix is inverse of coordinate transform
1270 /// Synonymous to "pretransform3x3"
1271 template<typename T0, typename T1>
1273 {
1274  return Vec3<T1>(
1275  static_cast<T1>(m[0][0]*n[0] + m[0][1]*n[1] + m[0][2]*n[2]),
1276  static_cast<T1>(m[1][0]*n[0] + m[1][1]*n[1] + m[1][2]*n[2]),
1277  static_cast<T1>(m[2][0]*n[0] + m[2][1]*n[1] + m[2][2]*n[2]));
1278 }
1279 
1280 
1281 /// Invert via gauss-jordan elimination. Modified from dreamworks internal mx library
1282 template<typename T>
1283 bool Mat4<T>::invert(Mat4<T> &inverse, T tolerance) const
1284 {
1285  Mat4<T> temp(*this);
1286  inverse.setIdentity();
1287 
1288  // Forward elimination step
1289  double det = 1.0;
1290  for (int i = 0; i < 4; ++i) {
1291  int row = i;
1292  double max = fabs(temp[i][i]);
1293 
1294  for (int k = i+1; k < 4; ++k) {
1295  if (fabs(temp[k][i]) > max) {
1296  row = k;
1297  max = fabs(temp[k][i]);
1298  }
1299  }
1300 
1301  if (isExactlyEqual(max, 0.0)) return false;
1302 
1303  // must move pivot to row i
1304  if (row != i) {
1305  det = -det;
1306  for (int k = 0; k < 4; ++k) {
1307  std::swap(temp[row][k], temp[i][k]);
1308  std::swap(inverse[row][k], inverse[i][k]);
1309  }
1310  }
1311 
1312  double pivot = temp[i][i];
1313  det *= pivot;
1314 
1315  // scale row i
1316  for (int k = 0; k < 4; ++k) {
1317  temp[i][k] /= pivot;
1318  inverse[i][k] /= pivot;
1319  }
1320 
1321  // eliminate in rows below i
1322  for (int j = i+1; j < 4; ++j) {
1323  double t = temp[j][i];
1324  if (!isExactlyEqual(t, 0.0)) {
1325  // subtract scaled row i from row j
1326  for (int k = 0; k < 4; ++k) {
1327  temp[j][k] -= temp[i][k] * t;
1328  inverse[j][k] -= inverse[i][k] * t;
1329  }
1330  }
1331  }
1332  }
1333 
1334  // Backward elimination step
1335  for (int i = 3; i > 0; --i) {
1336  for (int j = 0; j < i; ++j) {
1337  double t = temp[j][i];
1338 
1339  if (!isExactlyEqual(t, 0.0)) {
1340  for (int k = 0; k < 4; ++k) {
1341  inverse[j][k] -= inverse[i][k]*t;
1342  }
1343  }
1344  }
1345  }
1346  return det*det >= tolerance*tolerance;
1347 }
1348 
1349 template <typename T>
1350 inline bool isAffine(const Mat4<T>& m) {
1351  return (m.col(3) == Vec4<T>(0, 0, 0, 1));
1352 }
1353 
1354 template <typename T>
1355 inline bool hasTranslation(const Mat4<T>& m) {
1356  return (m.row(3) != Vec4<T>(0, 0, 0, 1));
1357 }
1358 
1359 
1362 using Mat4f = Mat4d;
1363 
1364 } // namespace math
1365 
1366 
1367 template<> inline math::Mat4s zeroVal<math::Mat4s>() { return math::Mat4s::zero(); }
1368 template<> inline math::Mat4d zeroVal<math::Mat4d>() { return math::Mat4d::zero(); }
1369 
1370 } // namespace OPENVDB_VERSION_NAME
1371 } // namespace openvdb
1372 
1373 #endif // OPENVDB_UTIL_MAT4_H_HAS_BEEN_INCLUDED
1374 
1375 // Copyright (c) 2012-2018 DreamWorks Animation LLC
1376 // All rights reserved. This software is distributed under the
1377 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
SYS_API double cos(double x)
Definition: SYS_FPUMath.h:69
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:395
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:720
Vec3< T0 > transformH(const Vec3< T0 > &p) const
Transform a Vec3 by post-multiplication, doing homogenous divison.
Definition: Mat4.h:1072
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:1519
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:838
void setToScale(const Vec3< T0 > &v)
Sets the matrix to a matrix that scales by v.
Definition: Mat4.h:772
GLboolean GLboolean GLboolean GLboolean a
Definition: glcorearb.h:1221
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:189
Vec3< T0 > transform(const Vec3< T0 > &v) const
Transform a Vec3 by post-multiplication, without homogenous division.
Definition: Mat4.h:1051
void setToRotation(const Vec3< T > &axis, T angle)
Sets the matrix to a rotation about an arbitrary axis.
Definition: Mat4.h:834
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:1260
void postRotate(Axis axis, T angle)
Right multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:924
void setToTranslation(const Vec3< T0 > &v)
Sets the matrix to a matrix that translates by v.
Definition: Mat4.h:724
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:311
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:216
void setToRotation(Axis axis, T angle)
Sets the matrix to a rotation about the given axis.
Definition: Mat4.h:829
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:360
void preRotate(Axis axis, T angle)
Left multiplies by a rotation clock-wiseabout the given axis into this matrix.
Definition: Mat4.h:844
Mat4(Source *a)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat4.h:76
bool operator!=(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat4.h:1158
GLdouble n
Definition: glcorearb.h:2007
GLfloat f
Definition: glcorearb.h:1925
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:456
void preScale(const Vec3< T0 > &v)
Definition: Mat4.h:782
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:355
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:133
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:760
Vec3< T0 > pretransformH(const Vec3< T0 > &p) const
Transform a Vec3 by pre-multiplication, doing homogenous division.
Definition: Mat4.h:1094
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:1171
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:1115
void setToShear(Axis axis0, Axis axis1, T shearby)
Sets the matrix to a shear along axis0 by a fraction of axis1.
Definition: Mat4.h:1006
void setMat3(const Mat3< T > &m)
Set upper left to a Mat3.
Definition: Mat4.h:336
const Mat4< T > & operator*=(S scalar)
Multiply each element of this matrix by scalar.
Definition: Mat4.h:401
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:472
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:749
Mat4< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat4.h:389
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:1238
T det() const
Determinant of matrix.
Definition: Mat3.h:527
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:1065
GLboolean GLboolean GLboolean b
Definition: glcorearb.h:1221
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:1163
const Mat4 & operator=(const Mat4< Source > &m)
Assignment operator.
Definition: Mat4.h:369
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:358
void postScale(const Vec3< T0 > &v)
Definition: Mat4.h:804
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:1182
bool hasTranslation(const Mat4< T > &m)
Definition: Mat4.h:1355
GLfloat GLfloat GLfloat GLfloat h
Definition: glcorearb.h:2001
T det() const
Determinant of matrix.
Definition: Mat4.h:683
bool operator==(const Mat4< T0 > &m0, const Mat4< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat4.h:1146
const Mat4< T > & operator*=(const Mat4< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat4.h:485
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
Return true if this matrix is equivalent to m within a tolerance of eps.
Definition: Mat4.h:379
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:110
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:427
void postShear(Axis axis0, Axis axis1, T shear)
Right multiplies a shearing transformation into the matrix.
Definition: Mat4.h:1029
Vec3< T1 > transformNormal(const Mat4< T0 > &m, const Vec3< T1 > &n)
Definition: Mat4.h:1272
Vec4< T0 > transform(const Vec4< T0 > &v) const
Transform a Vec4 by post-multiplication.
Definition: Mat4.h:1044
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)
Multiply _v by _m and return the resulting vector.
Definition: Mat4.h:1225
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:1058
Mat4(const Mat< 4, T > &m)
Copy constructor.
Definition: Mat4.h:131
GLboolean r
Definition: glcorearb.h:1221
void preShear(Axis axis0, Axis axis1, T shear)
Left multiplies a shearing transformation into the matrix.
Definition: Mat4.h:1014
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)
Subtract corresponding elements of m0 and m1 and return the result.
Definition: Mat4.h:1249
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)
Multiply _m by _v and return the resulting vector.
Definition: Mat4.h:1212
bool isAffine(const Mat4< T > &m)
Definition: Mat4.h:1350
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h:135
static const Mat4< T > & zero()
Predefined constant for zero matrix.
Definition: Mat4.h:163
SYS_API double sin(double x)
Definition: SYS_FPUMath.h:71
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:531
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:109
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:1197
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:713