OpenVDB  7.0.0
Mat3.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_MAT3_H_HAS_BEEN_INCLUDED
5 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
6 
7 #include <openvdb/Exceptions.h>
8 #include "Vec3.h"
9 #include "Mat.h"
10 #include <algorithm> // for std::copy()
11 #include <cassert>
12 #include <cmath>
13 #include <iomanip>
14 
15 
16 namespace openvdb {
18 namespace OPENVDB_VERSION_NAME {
19 namespace math {
20 
21 template<typename T> class Vec3;
22 template<typename T> class Mat4;
23 template<typename T> class Quat;
24 
27 template<typename T>
28 class Mat3: public Mat<3, T>
29 {
30 public:
32  using value_type = T;
33  using ValueType = T;
34  using MyBase = Mat<3, T>;
36  Mat3() {}
37 
40  Mat3(const Quat<T> &q)
41  { setToRotation(q); }
42 
43 
45 
50  template<typename Source>
51  Mat3(Source a, Source b, Source c,
52  Source d, Source e, Source f,
53  Source g, Source h, Source i)
54  {
55  MyBase::mm[0] = static_cast<T>(a);
56  MyBase::mm[1] = static_cast<T>(b);
57  MyBase::mm[2] = static_cast<T>(c);
58  MyBase::mm[3] = static_cast<T>(d);
59  MyBase::mm[4] = static_cast<T>(e);
60  MyBase::mm[5] = static_cast<T>(f);
61  MyBase::mm[6] = static_cast<T>(g);
62  MyBase::mm[7] = static_cast<T>(h);
63  MyBase::mm[8] = static_cast<T>(i);
64  } // constructor1Test
65 
68  template<typename Source>
69  Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true)
70  {
71  if (rows) {
72  this->setRows(v1, v2, v3);
73  } else {
74  this->setColumns(v1, v2, v3);
75  }
76  }
77 
82  template<typename Source>
83  Mat3(Source *a)
84  {
85  MyBase::mm[0] = static_cast<T>(a[0]);
86  MyBase::mm[1] = static_cast<T>(a[1]);
87  MyBase::mm[2] = static_cast<T>(a[2]);
88  MyBase::mm[3] = static_cast<T>(a[3]);
89  MyBase::mm[4] = static_cast<T>(a[4]);
90  MyBase::mm[5] = static_cast<T>(a[5]);
91  MyBase::mm[6] = static_cast<T>(a[6]);
92  MyBase::mm[7] = static_cast<T>(a[7]);
93  MyBase::mm[8] = static_cast<T>(a[8]);
94  } // constructor1Test
95 
97  Mat3(const Mat<3, T> &m)
98  {
99  for (int i=0; i<3; ++i) {
100  for (int j=0; j<3; ++j) {
101  MyBase::mm[i*3 + j] = m[i][j];
102  }
103  }
104  }
105 
107  template<typename Source>
108  explicit Mat3(const Mat3<Source> &m)
109  {
110  for (int i=0; i<3; ++i) {
111  for (int j=0; j<3; ++j) {
112  MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
113  }
114  }
115  }
116 
118  explicit Mat3(const Mat4<T> &m)
119  {
120  for (int i=0; i<3; ++i) {
121  for (int j=0; j<3; ++j) {
122  MyBase::mm[i*3 + j] = m[i][j];
123  }
124  }
125  }
126 
128  static const Mat3<T>& identity() {
129  static const Mat3<T> sIdentity = Mat3<T>(
130  1, 0, 0,
131  0, 1, 0,
132  0, 0, 1
133  );
134  return sIdentity;
135  }
136 
138  static const Mat3<T>& zero() {
139  static const Mat3<T> sZero = Mat3<T>(
140  0, 0, 0,
141  0, 0, 0,
142  0, 0, 0
143  );
144  return sZero;
145  }
146 
148  void setRow(int i, const Vec3<T> &v)
149  {
150  // assert(i>=0 && i<3);
151  int i3 = i * 3;
152 
153  MyBase::mm[i3+0] = v[0];
154  MyBase::mm[i3+1] = v[1];
155  MyBase::mm[i3+2] = v[2];
156  } // rowColumnTest
157 
159  Vec3<T> row(int i) const
160  {
161  // assert(i>=0 && i<3);
162  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
163  } // rowColumnTest
164 
166  void setCol(int j, const Vec3<T>& v)
167  {
168  // assert(j>=0 && j<3);
169  MyBase::mm[0+j] = v[0];
170  MyBase::mm[3+j] = v[1];
171  MyBase::mm[6+j] = v[2];
172  } // rowColumnTest
173 
175  Vec3<T> col(int j) const
176  {
177  // assert(j>=0 && j<3);
178  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
179  } // rowColumnTest
180 
181  // NB: The following two methods were changed to
182  // work around a gccWS5 compiler issue related to strict
183  // aliasing (see FX-475).
184 
186  T* operator[](int i) { return &(MyBase::mm[i*3]); }
189  const T* operator[](int i) const { return &(MyBase::mm[i*3]); }
191 
192  T* asPointer() {return MyBase::mm;}
193  const T* asPointer() const {return MyBase::mm;}
194 
198  T& operator()(int i, int j)
199  {
200  // assert(i>=0 && i<3);
201  // assert(j>=0 && j<3);
202  return MyBase::mm[3*i+j];
203  } // trivial
204 
208  T operator()(int i, int j) const
209  {
210  // assert(i>=0 && i<3);
211  // assert(j>=0 && j<3);
212  return MyBase::mm[3*i+j];
213  } // trivial
214 
216  void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
217  {
218  MyBase::mm[0] = v1[0];
219  MyBase::mm[1] = v1[1];
220  MyBase::mm[2] = v1[2];
221  MyBase::mm[3] = v2[0];
222  MyBase::mm[4] = v2[1];
223  MyBase::mm[5] = v2[2];
224  MyBase::mm[6] = v3[0];
225  MyBase::mm[7] = v3[1];
226  MyBase::mm[8] = v3[2];
227  } // setRows
228 
230  void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
231  {
232  MyBase::mm[0] = v1[0];
233  MyBase::mm[1] = v2[0];
234  MyBase::mm[2] = v3[0];
235  MyBase::mm[3] = v1[1];
236  MyBase::mm[4] = v2[1];
237  MyBase::mm[5] = v3[1];
238  MyBase::mm[6] = v1[2];
239  MyBase::mm[7] = v2[2];
240  MyBase::mm[8] = v3[2];
241  } // setColumns
242 
244  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
245  {
246  MyBase::mm[0] = vdiag[0];
247  MyBase::mm[1] = vtri[0];
248  MyBase::mm[2] = vtri[1];
249  MyBase::mm[3] = vtri[0];
250  MyBase::mm[4] = vdiag[1];
251  MyBase::mm[5] = vtri[2];
252  MyBase::mm[6] = vtri[1];
253  MyBase::mm[7] = vtri[2];
254  MyBase::mm[8] = vdiag[2];
255  } // setSymmetricTest
256 
258  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
259  {
260  return Mat3(
261  vdiag[0], vtri[0], vtri[1],
262  vtri[0], vdiag[1], vtri[2],
263  vtri[1], vtri[2], vdiag[2]
264  );
265  }
266 
268  void setSkew(const Vec3<T> &v)
269  {*this = skew(v);}
270 
274  void setToRotation(const Quat<T> &q)
275  {*this = rotation<Mat3<T> >(q);}
276 
279  void setToRotation(const Vec3<T> &axis, T angle)
280  {*this = rotation<Mat3<T> >(axis, angle);}
281 
283  void setZero()
284  {
285  MyBase::mm[0] = 0;
286  MyBase::mm[1] = 0;
287  MyBase::mm[2] = 0;
288  MyBase::mm[3] = 0;
289  MyBase::mm[4] = 0;
290  MyBase::mm[5] = 0;
291  MyBase::mm[6] = 0;
292  MyBase::mm[7] = 0;
293  MyBase::mm[8] = 0;
294  } // trivial
295 
297  void setIdentity()
298  {
299  MyBase::mm[0] = 1;
300  MyBase::mm[1] = 0;
301  MyBase::mm[2] = 0;
302  MyBase::mm[3] = 0;
303  MyBase::mm[4] = 1;
304  MyBase::mm[5] = 0;
305  MyBase::mm[6] = 0;
306  MyBase::mm[7] = 0;
307  MyBase::mm[8] = 1;
308  } // trivial
309 
311  template<typename Source>
312  const Mat3& operator=(const Mat3<Source> &m)
313  {
314  const Source *src = m.asPointer();
315 
316  // don't suppress type conversion warnings
317  std::copy(src, (src + this->numElements()), MyBase::mm);
318  return *this;
319  } // opEqualToTest
320 
322  bool eq(const Mat3 &m, T eps=1.0e-8) const
323  {
324  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
325  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
326  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
327  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
328  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
329  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
330  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
331  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
332  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
333  } // trivial
334 
337  {
338  return Mat3<T>(
339  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
340  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
341  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
342  );
343  } // trivial
344 
346  // friend Mat3 operator*(T scalar, const Mat3& m) {
347  // return m*scalar;
348  // }
349 
351  template <typename S>
352  const Mat3<T>& operator*=(S scalar)
353  {
354  MyBase::mm[0] *= scalar;
355  MyBase::mm[1] *= scalar;
356  MyBase::mm[2] *= scalar;
357  MyBase::mm[3] *= scalar;
358  MyBase::mm[4] *= scalar;
359  MyBase::mm[5] *= scalar;
360  MyBase::mm[6] *= scalar;
361  MyBase::mm[7] *= scalar;
362  MyBase::mm[8] *= scalar;
363  return *this;
364  }
365 
367  template <typename S>
368  const Mat3<T> &operator+=(const Mat3<S> &m1)
369  {
370  const S *s = m1.asPointer();
371 
372  MyBase::mm[0] += s[0];
373  MyBase::mm[1] += s[1];
374  MyBase::mm[2] += s[2];
375  MyBase::mm[3] += s[3];
376  MyBase::mm[4] += s[4];
377  MyBase::mm[5] += s[5];
378  MyBase::mm[6] += s[6];
379  MyBase::mm[7] += s[7];
380  MyBase::mm[8] += s[8];
381  return *this;
382  }
383 
385  template <typename S>
386  const Mat3<T> &operator-=(const Mat3<S> &m1)
387  {
388  const S *s = m1.asPointer();
389 
390  MyBase::mm[0] -= s[0];
391  MyBase::mm[1] -= s[1];
392  MyBase::mm[2] -= s[2];
393  MyBase::mm[3] -= s[3];
394  MyBase::mm[4] -= s[4];
395  MyBase::mm[5] -= s[5];
396  MyBase::mm[6] -= s[6];
397  MyBase::mm[7] -= s[7];
398  MyBase::mm[8] -= s[8];
399  return *this;
400  }
401 
403  template <typename S>
404  const Mat3<T> &operator*=(const Mat3<S> &m1)
405  {
406  Mat3<T> m0(*this);
407  const T* s0 = m0.asPointer();
408  const S* s1 = m1.asPointer();
409 
410  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
411  s0[1] * s1[3] +
412  s0[2] * s1[6]);
413  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
414  s0[1] * s1[4] +
415  s0[2] * s1[7]);
416  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
417  s0[1] * s1[5] +
418  s0[2] * s1[8]);
419 
420  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
421  s0[4] * s1[3] +
422  s0[5] * s1[6]);
423  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
424  s0[4] * s1[4] +
425  s0[5] * s1[7]);
426  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
427  s0[4] * s1[5] +
428  s0[5] * s1[8]);
429 
430  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
431  s0[7] * s1[3] +
432  s0[8] * s1[6]);
433  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
434  s0[7] * s1[4] +
435  s0[8] * s1[7]);
436  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
437  s0[7] * s1[5] +
438  s0[8] * s1[8]);
439 
440  return *this;
441  }
442 
444  Mat3 cofactor() const
445  {
446  return Mat3<T>(
447  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
448  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
449  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
450  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
451  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
452  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
453  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
454  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
455  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
456  }
457 
459  Mat3 adjoint() const
460  {
461  return Mat3<T>(
462  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
463  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
464  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
465  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
466  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
467  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
468  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
469  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
470  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
471 
472  } // adjointTest
473 
475  Mat3 transpose() const
476  {
477  return Mat3<T>(
478  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
479  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
480  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
481 
482  } // transposeTest
483 
486  Mat3 inverse(T tolerance = 0) const
487  {
488  Mat3<T> inv(this->adjoint());
489 
490  const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
491 
492  // If the determinant is 0, m was singular and the result will be invalid.
493  if (isApproxEqual(det,T(0.0),tolerance)) {
494  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
495  }
496  return inv * (T(1)/det);
497  } // invertTest
498 
500  T det() const
501  {
502  const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
503  const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
504  const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
505  return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
506  } // determinantTest
507 
509  T trace() const
510  {
511  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
512  }
513 
518  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
519  {
520  return snapMatBasis(*this, axis, direction);
521  }
522 
525  template<typename T0>
526  Vec3<T0> transform(const Vec3<T0> &v) const
527  {
528  return static_cast< Vec3<T0> >(v * *this);
529  } // xformVectorTest
530 
533  template<typename T0>
535  {
536  return static_cast< Vec3<T0> >(*this * v);
537  } // xformTVectorTest
538 
539 
542  Mat3 timesDiagonal(const Vec3<T>& diag) const
543  {
544  Mat3 ret(*this);
545 
546  ret.mm[0] *= diag(0);
547  ret.mm[1] *= diag(1);
548  ret.mm[2] *= diag(2);
549  ret.mm[3] *= diag(0);
550  ret.mm[4] *= diag(1);
551  ret.mm[5] *= diag(2);
552  ret.mm[6] *= diag(0);
553  ret.mm[7] *= diag(1);
554  ret.mm[8] *= diag(2);
555  return ret;
556  }
557 }; // class Mat3
558 
559 
562 template <typename T0, typename T1>
563 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
564 {
565  const T0 *t0 = m0.asPointer();
566  const T1 *t1 = m1.asPointer();
567 
568  for (int i=0; i<9; ++i) {
569  if (!isExactlyEqual(t0[i], t1[i])) return false;
570  }
571  return true;
572 }
573 
576 template <typename T0, typename T1>
577 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
578 
581 template <typename S, typename T>
583 { return m*scalar; }
584 
587 template <typename S, typename T>
589 {
591  result *= scalar;
592  return result;
593 }
594 
597 template <typename T0, typename T1>
599 {
601  result += m1;
602  return result;
603 }
604 
607 template <typename T0, typename T1>
609 {
611  result -= m1;
612  return result;
613 }
614 
615 
617 template <typename T0, typename T1>
619 {
621  result *= m1;
622  return result;
623 }
624 
627 template<typename T, typename MT>
629 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
630 {
631  MT const *m = _m.asPointer();
633  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
634  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
635  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
636 }
637 
640 template<typename T, typename MT>
642 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
643 {
644  MT const *m = _m.asPointer();
646  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
647  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
648  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
649 }
650 
653 template<typename T, typename MT>
654 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
655 {
656  Vec3<T> mult = _v * _m;
657  _v = mult;
658  return _v;
659 }
660 
663 template <typename T>
664 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
665 {
666  return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
667  v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
668  v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
669 }// outerProduct
670 
671 
675 template<typename T, typename T0>
676 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
677 {
678  Mat3<T> x = m1.inverse() * m2;
679  powSolve(x, x, t);
680  Mat3<T> m = m1 * x;
681  return m;
682 }
683 
684 
685 namespace mat3_internal {
686 
687 template<typename T>
688 inline void
689 pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
690 {
691  const int& n = Mat3<T>::size; // should be 3
692  T temp;
694  double cotan_of_2_theta;
695  double tan_of_theta;
696  double cosin_of_theta;
697  double sin_of_theta;
698  double z;
699 
700  double Sij = S(i,j);
701 
702  double Sjj_minus_Sii = D[j] - D[i];
703 
704  if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
705  tan_of_theta = Sij / Sjj_minus_Sii;
706  } else {
708  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
709 
710  if (cotan_of_2_theta < 0.) {
711  tan_of_theta =
712  -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
713  } else {
714  tan_of_theta =
715  1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
716  }
717  }
718 
719  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
720  sin_of_theta = cosin_of_theta * tan_of_theta;
721  z = tan_of_theta * Sij;
722  S(i,j) = 0;
723  D[i] -= z;
724  D[j] += z;
725  for (int k = 0; k < i; ++k) {
726  temp = S(k,i);
727  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
728  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
729  }
730  for (int k = i+1; k < j; ++k) {
731  temp = S(i,k);
732  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
733  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
734  }
735  for (int k = j+1; k < n; ++k) {
736  temp = S(i,k);
737  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
738  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
739  }
740  for (int k = 0; k < n; ++k)
741  {
742  temp = Q(k,i);
743  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
744  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
745  }
746 }
747 
748 } // namespace mat3_internal
749 
750 
756 template<typename T>
757 inline bool
759  unsigned int MAX_ITERATIONS=250)
760 {
763  Q = Mat3<T>::identity();
764  int n = Mat3<T>::size; // should be 3
765 
767  Mat3<T> S(input);
768 
769  for (int i = 0; i < n; ++i) {
770  D[i] = S(i,i);
771  }
772 
773  unsigned int iterations(0);
776  do {
779  double er = 0;
780  for (int i = 0; i < n; ++i) {
781  for (int j = i+1; j < n; ++j) {
782  er += fabs(S(i,j));
783  }
784  }
785  if (std::abs(er) < math::Tolerance<T>::value()) {
786  return true;
787  }
788  iterations++;
789 
790  T max_element = 0;
791  int ip = 0;
792  int jp = 0;
794  for (int i = 0; i < n; ++i) {
795  for (int j = i+1; j < n; ++j){
796 
797  if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
799  S(i,j) = 0;
800  }
801  if (fabs(S(i,j)) > max_element) {
802  max_element = fabs(S(i,j));
803  ip = i;
804  jp = j;
805  }
806  }
807  }
808  mat3_internal::pivot(ip, jp, S, D, Q);
809  } while (iterations < MAX_ITERATIONS);
810 
811  return false;
812 }
813 
814 
817 using Mat3f = Mat3d;
818 
819 } // namespace math
820 
821 
822 template<> inline math::Mat3s zeroVal<math::Mat3s>() { return math::Mat3s::zero(); }
823 template<> inline math::Mat3d zeroVal<math::Mat3d>() { return math::Mat3d::zero(); }
824 
825 } // namespace OPENVDB_VERSION_NAME
826 } // namespace openvdb
827 
828 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat3< MT > &_m)
Multiply _v by _m and return the resulting vector.
Definition: Mat3.h:642
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition: Mat3.h:689
bool isApproxEqual(const Type &a, const Type &b)
Return true if a is equal to b to within the default floating-point comparison tolerance.
Definition: Math.h:351
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3, bool rows=true)
Definition: Mat3.h:69
void setColumns(const Vec3< T > &v1, const Vec3< T > &v2, const Vec3< T > &v3)
Set the columns of this matrix to the vectors v1, v2, v3.
Definition: Mat3.h:230
Mat3 adjoint() const
Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Definition: Mat3.h:459
Definition: Mat.h:169
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:118
T ValueType
Definition: Mat.h:30
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:713
Mat3< typename promote< S, T >::type > operator*(const Mat3< T > &m, S scalar)
Multiply each element of the given matrix by scalar and return the result.
Definition: Mat3.h:588
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:82
Mat3< double > Mat3d
Definition: Mat3.h:816
Definition: Mat.h:26
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:475
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:526
T mm[SIZE *SIZE]
Definition: Mat.h:165
T trace() const
Trace of matrix.
Definition: Mat3.h:509
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:97
Axis
Definition: Math.h:849
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:336
bool eq(const Mat3 &m, T eps=1.0e-8) const
Return true if this matrix is equivalent to m within a tolerance of eps.
Definition: Mat3.h:322
void setIdentity()
Set this matrix to identity.
Definition: Mat3.h:297
Mat3(Source *a)
Definition: Mat3.h:83
const T * asPointer() const
Definition: Mat3.h:193
void setRows(const Vec3< T > &v1, const Vec3< T > &v2, const Vec3< T > &v3)
Set the rows of this matrix to the vectors v1, v2, v3.
Definition: Mat3.h:216
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:279
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:534
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:676
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:175
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:36
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:268
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:244
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h:102
T * asPointer()
Definition: Mat3.h:192
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:352
Mat3(const Quat< T > &q)
Definition: Mat3.h:40
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Subtract corresponding elements of m0 and m1 and return the result.
Definition: Mat3.h:608
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Add corresponding elements of m0 and m1 and return the result.
Definition: Mat3.h:598
const Mat3< T > & operator*=(const Mat3< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat3.h:404
4x4 -matrix class.
Definition: Mat3.h:22
T operator()(int i, int j) const
Definition: Mat3.h:208
Mat3(Source a, Source b, Source c, Source d, Source e, Source f, Source g, Source h, Source i)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat3.h:51
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Multiply each element of the given matrix by scalar and return the result.
Definition: Mat3.h:582
Definition: Exceptions.h:13
Definition: Exceptions.h:56
bool diagonalizeSymmetricMatrix(const Mat3< T > &input, Mat3< T > &Q, Vec3< T > &D, unsigned int MAX_ITERATIONS=250)
Use Jacobi iterations to decompose a symmetric 3x3 matrix (diagonalize and compute eigenvectors) ...
Definition: Mat3.h:758
static Mat3 symmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Return a matrix with the prescribed diagonal and symmetric triangular components. ...
Definition: Mat3.h:258
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:563
T det() const
Determinant of matrix.
Definition: Mat3.h:500
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:274
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:166
Mat3 timesDiagonal(const Vec3< T > &diag) const
Treat diag as a diagonal matrix and return the product of this matrix with diag (from the right)...
Definition: Mat3.h:542
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:486
const Mat3< T > & operator-=(const Mat3< S > &m1)
Subtract each element of the given matrix from the corresponding element of this matrix.
Definition: Mat3.h:386
Vec3< typename promote< T, MT >::type > operator*(const Mat3< MT > &_m, const Vec3< T > &_v)
Multiply _m by _v and return the resulting vector.
Definition: Mat3.h:629
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:138
const Mat3< T > & operator+=(const Mat3< S > &m1)
Add each element of the given matrix to the corresponding element of this matrix. ...
Definition: Mat3.h:368
Tolerance for floating-point comparison.
Definition: Math.h:90
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:827
3x3 matrix class.
Definition: Mat3.h:28
const T * operator[](int i) const
Definition: Mat3.h:189
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:108
void setZero()
Set this matrix to zero.
Definition: Mat3.h:283
Definition: Mat.h:170
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:128
T value_type
Definition: Mat.h:29
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Multiply m0 by m1 and return the resulting matrix.
Definition: Mat3.h:618
MatType snapMatBasis(const MatType &source, Axis axis, const Vec3< typename MatType::value_type > &direction)
This function snaps a specific axis to a specific direction, preserving scaling.
Definition: Mat.h:756
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:664
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:445
Mat3 cofactor() const
Return the cofactor matrix of this matrix.
Definition: Mat3.h:444
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:312
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition: Mat3.h:518
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:388
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:154
T & operator()(int i, int j)
Definition: Mat3.h:198
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:148
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:159
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:577