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
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:159
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
Definition: Mat.h:169
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:118
T trace() const
Trace of matrix.
Definition: Mat3.h:509
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:534
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
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:175
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
const T * asPointer() const
Definition: Mat3.h:193
T mm[SIZE *SIZE]
Definition: Mat.h:165
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:97
Axis
Definition: Math.h:849
void setIdentity()
Set this matrix to identity.
Definition: Mat3.h:297
Mat3(Source *a)
Definition: Mat3.h:83
T det() const
Determinant of matrix.
Definition: Mat3.h:500
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
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:676
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:36
Mat3 cofactor() const
Return the cofactor matrix of this matrix.
Definition: Mat3.h:444
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
T operator()(int i, int j) const
Definition: Mat3.h:208
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
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
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< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:336
const T * operator[](int i) const
Definition: Mat3.h:189
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:526
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
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:486
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
Mat3 adjoint() const
Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Definition: Mat3.h:459
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
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 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
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
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:475
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:577