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