OpenVDB  5.0.0
Mat3.h
Go to the documentation of this file.
1 //
3 // Copyright (c) 2012-2017 DreamWorks Animation LLC
4 //
5 // All rights reserved. This software is distributed under the
6 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
7 //
8 // Redistributions of source code must retain the above copyright
9 // and license notice and the following restrictions and disclaimer.
10 //
11 // * Neither the name of DreamWorks Animation nor the names of
12 // its contributors may be used to endorse or promote products derived
13 // from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
21 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 // IN NO EVENT SHALL THE COPYRIGHT HOLDERS' AND CONTRIBUTORS' AGGREGATE
27 // LIABILITY FOR ALL CLAIMS REGARDLESS OF THEIR BASIS EXCEED US$250.00.
28 //
30 
31 #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
32 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
33 
34 #include <openvdb/Exceptions.h>
35 #include "Vec3.h"
36 #include "Mat.h"
37 #include <algorithm> // for std::copy()
38 #include <cassert>
39 #include <cmath>
40 #include <iomanip>
41 
42 
43 namespace openvdb {
45 namespace OPENVDB_VERSION_NAME {
46 namespace math {
47 
48 template<typename T> class Vec3;
49 template<typename T> class Mat4;
50 template<typename T> class Quat;
51 
54 template<typename T>
55 class Mat3: public Mat<3, T>
56 {
57 public:
59  using value_type = T;
60  using ValueType = T;
61  using MyBase = Mat<3, T>;
63  Mat3() {}
64 
67  Mat3(const Quat<T> &q)
68  { setToRotation(q); }
69 
70 
72 
77  template<typename Source>
78  Mat3(Source a, Source b, Source c,
79  Source d, Source e, Source f,
80  Source g, Source h, Source i)
81  {
82  MyBase::mm[0] = static_cast<ValueType>(a);
83  MyBase::mm[1] = static_cast<ValueType>(b);
84  MyBase::mm[2] = static_cast<ValueType>(c);
85  MyBase::mm[3] = static_cast<ValueType>(d);
86  MyBase::mm[4] = static_cast<ValueType>(e);
87  MyBase::mm[5] = static_cast<ValueType>(f);
88  MyBase::mm[6] = static_cast<ValueType>(g);
89  MyBase::mm[7] = static_cast<ValueType>(h);
90  MyBase::mm[8] = static_cast<ValueType>(i);
91  } // constructor1Test
92 
95  template<typename Source>
96  Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true)
97  {
98  if (rows) {
99  this->setRows(v1, v2, v3);
100  } else {
101  this->setColumns(v1, v2, v3);
102  }
103  }
104 
109  template<typename Source>
110  Mat3(Source *a)
111  {
112  MyBase::mm[0] = a[0];
113  MyBase::mm[1] = a[1];
114  MyBase::mm[2] = a[2];
115  MyBase::mm[3] = a[3];
116  MyBase::mm[4] = a[4];
117  MyBase::mm[5] = a[5];
118  MyBase::mm[6] = a[6];
119  MyBase::mm[7] = a[7];
120  MyBase::mm[8] = a[8];
121  } // constructor1Test
122 
124  Mat3(const Mat<3, T> &m)
125  {
126  for (int i=0; i<3; ++i) {
127  for (int j=0; j<3; ++j) {
128  MyBase::mm[i*3 + j] = m[i][j];
129  }
130  }
131  }
132 
134  template<typename Source>
135  explicit Mat3(const Mat3<Source> &m)
136  {
137  for (int i=0; i<3; ++i) {
138  for (int j=0; j<3; ++j) {
139  MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
140  }
141  }
142  }
143 
145  explicit Mat3(const Mat4<T> &m)
146  {
147  for (int i=0; i<3; ++i) {
148  for (int j=0; j<3; ++j) {
149  MyBase::mm[i*3 + j] = m[i][j];
150  }
151  }
152  }
153 
155  static const Mat3<T>& identity() {
156  static const Mat3<T> sIdentity = Mat3<T>(
157  1, 0, 0,
158  0, 1, 0,
159  0, 0, 1
160  );
161  return sIdentity;
162  }
163 
165  static const Mat3<T>& zero() {
166  static const Mat3<T> sZero = Mat3<T>(
167  0, 0, 0,
168  0, 0, 0,
169  0, 0, 0
170  );
171  return sZero;
172  }
173 
175  void setRow(int i, const Vec3<T> &v)
176  {
177  // assert(i>=0 && i<3);
178  int i3 = i * 3;
179 
180  MyBase::mm[i3+0] = v[0];
181  MyBase::mm[i3+1] = v[1];
182  MyBase::mm[i3+2] = v[2];
183  } // rowColumnTest
184 
186  Vec3<T> row(int i) const
187  {
188  // assert(i>=0 && i<3);
189  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
190  } // rowColumnTest
191 
193  void setCol(int j, const Vec3<T>& v)
194  {
195  // assert(j>=0 && j<3);
196  MyBase::mm[0+j] = v[0];
197  MyBase::mm[3+j] = v[1];
198  MyBase::mm[6+j] = v[2];
199  } // rowColumnTest
200 
202  Vec3<T> col(int j) const
203  {
204  // assert(j>=0 && j<3);
205  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
206  } // rowColumnTest
207 
208  // NB: The following two methods were changed to
209  // work around a gccWS5 compiler issue related to strict
210  // aliasing (see FX-475).
211 
213  T* operator[](int i) { return &(MyBase::mm[i*3]); }
216  const T* operator[](int i) const { return &(MyBase::mm[i*3]); }
218 
219  T* asPointer() {return MyBase::mm;}
220  const T* asPointer() const {return MyBase::mm;}
221 
225  T& operator()(int i, int j)
226  {
227  // assert(i>=0 && i<3);
228  // assert(j>=0 && j<3);
229  return MyBase::mm[3*i+j];
230  } // trivial
231 
235  T operator()(int i, int j) const
236  {
237  // assert(i>=0 && i<3);
238  // assert(j>=0 && j<3);
239  return MyBase::mm[3*i+j];
240  } // trivial
241 
243  void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
244  {
245  MyBase::mm[0] = v1[0];
246  MyBase::mm[1] = v1[1];
247  MyBase::mm[2] = v1[2];
248  MyBase::mm[3] = v2[0];
249  MyBase::mm[4] = v2[1];
250  MyBase::mm[5] = v2[2];
251  MyBase::mm[6] = v3[0];
252  MyBase::mm[7] = v3[1];
253  MyBase::mm[8] = v3[2];
254  } // setRows
255 
257  void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
258  {
259  MyBase::mm[0] = v1[0];
260  MyBase::mm[1] = v2[0];
261  MyBase::mm[2] = v3[0];
262  MyBase::mm[3] = v1[1];
263  MyBase::mm[4] = v2[1];
264  MyBase::mm[5] = v3[1];
265  MyBase::mm[6] = v1[2];
266  MyBase::mm[7] = v2[2];
267  MyBase::mm[8] = v3[2];
268  } // setColumns
269 
271  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
272  {
273  MyBase::mm[0] = vdiag[0];
274  MyBase::mm[1] = vtri[0];
275  MyBase::mm[2] = vtri[1];
276  MyBase::mm[3] = vtri[0];
277  MyBase::mm[4] = vdiag[1];
278  MyBase::mm[5] = vtri[2];
279  MyBase::mm[6] = vtri[1];
280  MyBase::mm[7] = vtri[2];
281  MyBase::mm[8] = vdiag[2];
282  } // setSymmetricTest
283 
285  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
286  {
287  return Mat3(
288  vdiag[0], vtri[0], vtri[1],
289  vtri[0], vdiag[1], vtri[2],
290  vtri[1], vtri[2], vdiag[2]
291  );
292  }
293 
295  void setSkew(const Vec3<T> &v)
296  {*this = skew(v);}
297 
301  void setToRotation(const Quat<T> &q)
302  {*this = rotation<Mat3<T> >(q);}
303 
306  void setToRotation(const Vec3<T> &axis, T angle)
307  {*this = rotation<Mat3<T> >(axis, angle);}
308 
310  void setZero()
311  {
312  MyBase::mm[0] = 0;
313  MyBase::mm[1] = 0;
314  MyBase::mm[2] = 0;
315  MyBase::mm[3] = 0;
316  MyBase::mm[4] = 0;
317  MyBase::mm[5] = 0;
318  MyBase::mm[6] = 0;
319  MyBase::mm[7] = 0;
320  MyBase::mm[8] = 0;
321  } // trivial
322 
324  void setIdentity()
325  {
326  MyBase::mm[0] = 1;
327  MyBase::mm[1] = 0;
328  MyBase::mm[2] = 0;
329  MyBase::mm[3] = 0;
330  MyBase::mm[4] = 1;
331  MyBase::mm[5] = 0;
332  MyBase::mm[6] = 0;
333  MyBase::mm[7] = 0;
334  MyBase::mm[8] = 1;
335  } // trivial
336 
338  template<typename Source>
339  const Mat3& operator=(const Mat3<Source> &m)
340  {
341  const Source *src = m.asPointer();
342 
343  // don't suppress type conversion warnings
344  std::copy(src, (src + this->numElements()), MyBase::mm);
345  return *this;
346  } // opEqualToTest
347 
349  bool eq(const Mat3 &m, T eps=1.0e-8) const
350  {
351  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
352  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
353  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
354  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
355  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
356  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
357  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
358  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
359  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
360  } // trivial
361 
364  {
365  return Mat3<T>(
366  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
367  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
368  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
369  );
370  } // trivial
371 
373  // friend Mat3 operator*(T scalar, const Mat3& m) {
374  // return m*scalar;
375  // }
376 
378  template <typename S>
379  const Mat3<T>& operator*=(S scalar)
380  {
381  MyBase::mm[0] *= scalar;
382  MyBase::mm[1] *= scalar;
383  MyBase::mm[2] *= scalar;
384  MyBase::mm[3] *= scalar;
385  MyBase::mm[4] *= scalar;
386  MyBase::mm[5] *= scalar;
387  MyBase::mm[6] *= scalar;
388  MyBase::mm[7] *= scalar;
389  MyBase::mm[8] *= scalar;
390  return *this;
391  }
392 
394  template <typename S>
395  const Mat3<T> &operator+=(const Mat3<S> &m1)
396  {
397  const S *s = m1.asPointer();
398 
399  MyBase::mm[0] += s[0];
400  MyBase::mm[1] += s[1];
401  MyBase::mm[2] += s[2];
402  MyBase::mm[3] += s[3];
403  MyBase::mm[4] += s[4];
404  MyBase::mm[5] += s[5];
405  MyBase::mm[6] += s[6];
406  MyBase::mm[7] += s[7];
407  MyBase::mm[8] += s[8];
408  return *this;
409  }
410 
412  template <typename S>
413  const Mat3<T> &operator-=(const Mat3<S> &m1)
414  {
415  const S *s = m1.asPointer();
416 
417  MyBase::mm[0] -= s[0];
418  MyBase::mm[1] -= s[1];
419  MyBase::mm[2] -= s[2];
420  MyBase::mm[3] -= s[3];
421  MyBase::mm[4] -= s[4];
422  MyBase::mm[5] -= s[5];
423  MyBase::mm[6] -= s[6];
424  MyBase::mm[7] -= s[7];
425  MyBase::mm[8] -= s[8];
426  return *this;
427  }
428 
430  template <typename S>
431  const Mat3<T> &operator*=(const Mat3<S> &m1)
432  {
433  Mat3<T> m0(*this);
434  const T* s0 = m0.asPointer();
435  const S* s1 = m1.asPointer();
436 
437  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
438  s0[1] * s1[3] +
439  s0[2] * s1[6]);
440  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
441  s0[1] * s1[4] +
442  s0[2] * s1[7]);
443  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
444  s0[1] * s1[5] +
445  s0[2] * s1[8]);
446 
447  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
448  s0[4] * s1[3] +
449  s0[5] * s1[6]);
450  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
451  s0[4] * s1[4] +
452  s0[5] * s1[7]);
453  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
454  s0[4] * s1[5] +
455  s0[5] * s1[8]);
456 
457  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
458  s0[7] * s1[3] +
459  s0[8] * s1[6]);
460  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
461  s0[7] * s1[4] +
462  s0[8] * s1[7]);
463  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
464  s0[7] * s1[5] +
465  s0[8] * s1[8]);
466 
467  return *this;
468  }
469 
471  Mat3 cofactor() const
472  {
473  return Mat3<T>(
474  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
475  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
476  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
477  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
478  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
479  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
480  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
481  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
482  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
483  }
484 
486  Mat3 adjoint() const
487  {
488  return Mat3<T>(
489  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
490  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
491  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
492  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
493  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
494  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
495  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
496  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
497  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
498 
499  } // adjointTest
500 
502  Mat3 transpose() const
503  {
504  return Mat3<T>(
505  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
506  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
507  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
508 
509  } // transposeTest
510 
513  Mat3 inverse(T tolerance = 0) const
514  {
515  Mat3<T> inv(this->adjoint());
516 
517  const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
518 
519  // If the determinant is 0, m was singular and the result will be invalid.
520  if (isApproxEqual(det,T(0.0),tolerance)) {
521  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
522  }
523  return inv * (T(1)/det);
524  } // invertTest
525 
527  T det() const
528  {
529  const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
530  const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
531  const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
532  return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
533  } // determinantTest
534 
536  T trace() const
537  {
538  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
539  }
540 
545  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
546  {
547  return snapMatBasis(*this, axis, direction);
548  }
549 
552  template<typename T0>
553  Vec3<T0> transform(const Vec3<T0> &v) const
554  {
555  return static_cast< Vec3<T0> >(v * *this);
556  } // xformVectorTest
557 
560  template<typename T0>
562  {
563  return static_cast< Vec3<T0> >(*this * v);
564  } // xformTVectorTest
565 
566 
569  Mat3 timesDiagonal(const Vec3<T>& diag) const
570  {
571  Mat3 ret(*this);
572 
573  ret.mm[0] *= diag(0);
574  ret.mm[1] *= diag(1);
575  ret.mm[2] *= diag(2);
576  ret.mm[3] *= diag(0);
577  ret.mm[4] *= diag(1);
578  ret.mm[5] *= diag(2);
579  ret.mm[6] *= diag(0);
580  ret.mm[7] *= diag(1);
581  ret.mm[8] *= diag(2);
582  return ret;
583  }
584 }; // class Mat3
585 
586 
589 template <typename T0, typename T1>
590 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
591 {
592  const T0 *t0 = m0.asPointer();
593  const T1 *t1 = m1.asPointer();
594 
595  for (int i=0; i<9; ++i) {
596  if (!isExactlyEqual(t0[i], t1[i])) return false;
597  }
598  return true;
599 }
600 
603 template <typename T0, typename T1>
604 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
605 
608 template <typename S, typename T>
610 { return m*scalar; }
611 
614 template <typename S, typename T>
616 {
618  result *= scalar;
619  return result;
620 }
621 
624 template <typename T0, typename T1>
626 {
628  result += m1;
629  return result;
630 }
631 
634 template <typename T0, typename T1>
636 {
638  result -= m1;
639  return result;
640 }
641 
642 
644 template <typename T0, typename T1>
646 {
648  result *= m1;
649  return result;
650 }
651 
654 template<typename T, typename MT>
656 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
657 {
658  MT const *m = _m.asPointer();
660  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
661  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
662  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
663 }
664 
667 template<typename T, typename MT>
669 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
670 {
671  MT const *m = _m.asPointer();
673  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
674  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
675  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
676 }
677 
680 template<typename T, typename MT>
681 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
682 {
683  Vec3<T> mult = _v * _m;
684  _v = mult;
685  return _v;
686 }
687 
690 template <typename T>
691 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
692 {
693  return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
694  v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
695  v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
696 }// outerProduct
697 
700 using Mat3f = Mat3d;
701 
702 
706 template<typename T, typename T0>
707 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
708 {
709  Mat3<T> x = m1.inverse() * m2;
710  powSolve(x, x, t);
711  Mat3<T> m = m1 * x;
712  return m;
713 }
714 
715 
716 namespace mat3_internal {
717 
718 template<typename T>
719 inline void
720 pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
721 {
722  const int& n = Mat3<T>::size; // should be 3
723  T temp;
725  double cotan_of_2_theta;
726  double tan_of_theta;
727  double cosin_of_theta;
728  double sin_of_theta;
729  double z;
730 
731  double Sij = S(i,j);
732 
733  double Sjj_minus_Sii = D[j] - D[i];
734 
735  if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
736  tan_of_theta = Sij / Sjj_minus_Sii;
737  } else {
739  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
740 
741  if (cotan_of_2_theta < 0.) {
742  tan_of_theta =
743  -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
744  } else {
745  tan_of_theta =
746  1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
747  }
748  }
749 
750  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
751  sin_of_theta = cosin_of_theta * tan_of_theta;
752  z = tan_of_theta * Sij;
753  S(i,j) = 0;
754  D[i] -= z;
755  D[j] += z;
756  for (int k = 0; k < i; ++k) {
757  temp = S(k,i);
758  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
759  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
760  }
761  for (int k = i+1; k < j; ++k) {
762  temp = S(i,k);
763  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
764  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
765  }
766  for (int k = j+1; k < n; ++k) {
767  temp = S(i,k);
768  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
769  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
770  }
771  for (int k = 0; k < n; ++k)
772  {
773  temp = Q(k,i);
774  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
775  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
776  }
777 }
778 
779 } // namespace mat3_internal
780 
781 
787 template<typename T>
788 inline bool
790  unsigned int MAX_ITERATIONS=250)
791 {
794  Q = Mat3<T>::identity();
795  int n = Mat3<T>::size; // should be 3
796 
798  Mat3<T> S(input);
799 
800  for (int i = 0; i < n; ++i) {
801  D[i] = S(i,i);
802  }
803 
804  unsigned int iterations(0);
807  do {
810  double er = 0;
811  for (int i = 0; i < n; ++i) {
812  for (int j = i+1; j < n; ++j) {
813  er += fabs(S(i,j));
814  }
815  }
816  if (std::abs(er) < math::Tolerance<T>::value()) {
817  return true;
818  }
819  iterations++;
820 
821  T max_element = 0;
822  int ip = 0;
823  int jp = 0;
825  for (int i = 0; i < n; ++i) {
826  for (int j = i+1; j < n; ++j){
827 
828  if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
830  S(i,j) = 0;
831  }
832  if (fabs(S(i,j)) > max_element) {
833  max_element = fabs(S(i,j));
834  ip = i;
835  jp = j;
836  }
837  }
838  }
839  mat3_internal::pivot(ip, jp, S, D, Q);
840  } while (iterations < MAX_ITERATIONS);
841 
842  return false;
843 }
844 
845 } // namespace math
846 } // namespace OPENVDB_VERSION_NAME
847 } // namespace openvdb
848 
849 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
850 
851 // Copyright (c) 2012-2017 DreamWorks Animation LLC
852 // All rights reserved. This software is distributed under the
853 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3, bool rows=true)
Definition: Mat3.h:96
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:175
Definition: Mat.h:196
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:391
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:502
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:561
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:604
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:569
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:108
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:257
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:145
T trace() const
Trace of matrix.
Definition: Mat3.h:536
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:243
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:615
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:354
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:202
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:295
T value_type
Definition: Mat.h:56
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:271
const T * asPointer() const
Definition: Mat3.h:220
Mat3< double > Mat3d
Definition: Mat3.h:699
T * asPointer()
Definition: Mat3.h:219
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:379
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:635
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:472
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:124
const Mat3< T > & operator*=(const Mat3< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat3.h:431
Tolerance for floating-point comparison.
Definition: Math.h:117
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:78
void setIdentity()
Set this matrix to identity.
Definition: Mat3.h:324
Mat3(Source *a)
Definition: Mat3.h:110
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:609
T mm[SIZE *SIZE]
Definition: Mat.h:192
T det() const
Determinant of matrix.
Definition: Mat3.h:527
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h:136
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:691
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:306
Axis
Definition: Math.h:852
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:63
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:285
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:590
Mat3 cofactor() const
Return the cofactor matrix of this matrix.
Definition: Mat3.h:471
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:301
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:193
Definition: Exceptions.h:39
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:656
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:165
Mat3(const Quat< T > &q)
Definition: Mat3.h:67
T operator()(int i, int j) const
Definition: Mat3.h:235
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:395
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:625
4x4 -matrix class.
Definition: Mat3.h:49
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:513
Definition: Mat.h:53
Definition: Exceptions.h:82
void setZero()
Set this matrix to zero.
Definition: Mat3.h:310
Mat3 adjoint() const
Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Definition: Mat3.h:486
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:155
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:707
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition: Mat3.h:545
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:363
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:349
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:852
const T * operator[](int i) const
Definition: Mat3.h:216
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:553
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:413
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:738
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:645
3x3 matrix class.
Definition: Mat3.h:55
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:135
Definition: Mat.h:197
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:188
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:339
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:669
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:789
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition: Mat3.h:720
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:781
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:186
T ValueType
Definition: Mat.h:57
T & operator()(int i, int j)
Definition: Mat3.h:225