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