OpenVDB  3.2.0
Mat3.h
Go to the documentation of this file.
1 //
3 // Copyright (c) 2012-2016 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 <iomanip>
35 #include <assert.h>
36 #include <math.h>
37 #include <openvdb/Exceptions.h>
38 #include "Vec3.h"
39 #include "Mat.h"
40 
41 
42 namespace openvdb {
44 namespace OPENVDB_VERSION_NAME {
45 namespace math {
46 
47 template<typename T> class Vec3;
48 template<typename T> class Mat4;
49 template<typename T> class Quat;
50 
53 template<typename T>
54 class Mat3: public Mat<3, T>
55 {
56 public:
58  typedef T value_type;
59  typedef T ValueType;
60  typedef Mat<3, T> MyBase;
62  Mat3() {}
63 
66  Mat3(const Quat<T> &q)
67  { setToRotation(q); }
68 
69 
71 
76  template<typename Source>
77  Mat3(Source a, Source b, Source c,
78  Source d, Source e, Source f,
79  Source g, Source h, Source i)
80  {
81  MyBase::mm[0] = static_cast<ValueType>(a);
82  MyBase::mm[1] = static_cast<ValueType>(b);
83  MyBase::mm[2] = static_cast<ValueType>(c);
84  MyBase::mm[3] = static_cast<ValueType>(d);
85  MyBase::mm[4] = static_cast<ValueType>(e);
86  MyBase::mm[5] = static_cast<ValueType>(f);
87  MyBase::mm[6] = static_cast<ValueType>(g);
88  MyBase::mm[7] = static_cast<ValueType>(h);
89  MyBase::mm[8] = static_cast<ValueType>(i);
90  } // constructor1Test
91 
94  template<typename Source>
95  Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true)
96  {
97  if (rows) {
98  this->setRows(v1, v2, v3);
99  } else {
100  this->setColumns(v1, v2, v3);
101  }
102  }
103 
108  template<typename Source>
109  Mat3(Source *a)
110  {
111  MyBase::mm[0] = a[0];
112  MyBase::mm[1] = a[1];
113  MyBase::mm[2] = a[2];
114  MyBase::mm[3] = a[3];
115  MyBase::mm[4] = a[4];
116  MyBase::mm[5] = a[5];
117  MyBase::mm[6] = a[6];
118  MyBase::mm[7] = a[7];
119  MyBase::mm[8] = a[8];
120  } // constructor1Test
121 
123  Mat3(const Mat<3, T> &m)
124  {
125  for (int i=0; i<3; ++i) {
126  for (int j=0; j<3; ++j) {
127  MyBase::mm[i*3 + j] = m[i][j];
128  }
129  }
130  }
131 
133  template<typename Source>
134  explicit Mat3(const Mat3<Source> &m)
135  {
136  for (int i=0; i<3; ++i) {
137  for (int j=0; j<3; ++j) {
138  MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
139  }
140  }
141  }
142 
144  explicit Mat3(const Mat4<T> &m)
145  {
146  for (int i=0; i<3; ++i) {
147  for (int j=0; j<3; ++j) {
148  MyBase::mm[i*3 + j] = m[i][j];
149  }
150  }
151  }
152 
154  static const Mat3<T>& identity() {
155  return sIdentity;
156  }
157 
159  static const Mat3<T>& zero() {
160  return sZero;
161  }
162 
164  void setRow(int i, const Vec3<T> &v)
165  {
166  // assert(i>=0 && i<3);
167  int i3 = i * 3;
168 
169  MyBase::mm[i3+0] = v[0];
170  MyBase::mm[i3+1] = v[1];
171  MyBase::mm[i3+2] = v[2];
172  } // rowColumnTest
173 
175  Vec3<T> row(int i) const
176  {
177  // assert(i>=0 && i<3);
178  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
179  } // rowColumnTest
180 
182  void setCol(int j, const Vec3<T>& v)
183  {
184  // assert(j>=0 && j<3);
185  MyBase::mm[0+j] = v[0];
186  MyBase::mm[3+j] = v[1];
187  MyBase::mm[6+j] = v[2];
188  } // rowColumnTest
189 
191  Vec3<T> col(int j) const
192  {
193  // assert(j>=0 && j<3);
194  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
195  } // rowColumnTest
196 
197  // NB: The following two methods were changed to
198  // work around a gccWS5 compiler issue related to strict
199  // aliasing (see FX-475).
200 
202  T* operator[](int i) { return &(MyBase::mm[i*3]); }
205  const T* operator[](int i) const { return &(MyBase::mm[i*3]); }
207 
208  T* asPointer() {return MyBase::mm;}
209  const T* asPointer() const {return MyBase::mm;}
210 
214  T& operator()(int i, int j)
215  {
216  // assert(i>=0 && i<3);
217  // assert(j>=0 && j<3);
218  return MyBase::mm[3*i+j];
219  } // trivial
220 
224  T operator()(int i, int j) const
225  {
226  // assert(i>=0 && i<3);
227  // assert(j>=0 && j<3);
228  return MyBase::mm[3*i+j];
229  } // trivial
230 
232  void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
233  {
234  MyBase::mm[0] = v1[0];
235  MyBase::mm[1] = v1[1];
236  MyBase::mm[2] = v1[2];
237  MyBase::mm[3] = v2[0];
238  MyBase::mm[4] = v2[1];
239  MyBase::mm[5] = v2[2];
240  MyBase::mm[6] = v3[0];
241  MyBase::mm[7] = v3[1];
242  MyBase::mm[8] = v3[2];
243  } // setRows
244 
246  void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
247  {
248  MyBase::mm[0] = v1[0];
249  MyBase::mm[1] = v2[0];
250  MyBase::mm[2] = v3[0];
251  MyBase::mm[3] = v1[1];
252  MyBase::mm[4] = v2[1];
253  MyBase::mm[5] = v3[1];
254  MyBase::mm[6] = v1[2];
255  MyBase::mm[7] = v2[2];
256  MyBase::mm[8] = v3[2];
257  } // setColumns
258 
260  OPENVDB_DEPRECATED void setBasis(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
261  {
262  this->setRows(v1, v2, v3);
263  }
264 
266  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
267  {
268  MyBase::mm[0] = vdiag[0];
269  MyBase::mm[1] = vtri[0];
270  MyBase::mm[2] = vtri[1];
271  MyBase::mm[3] = vtri[0];
272  MyBase::mm[4] = vdiag[1];
273  MyBase::mm[5] = vtri[2];
274  MyBase::mm[6] = vtri[1];
275  MyBase::mm[7] = vtri[2];
276  MyBase::mm[8] = vdiag[2];
277  } // setSymmetricTest
278 
281  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
282  {
283  return Mat3(
284  vdiag[0], vtri[0], vtri[1],
285  vtri[0], vdiag[1], vtri[2],
286  vtri[1], vtri[2], vdiag[2]
287  );
288  }
289 
291  void setSkew(const Vec3<T> &v)
292  {*this = skew(v);}
293 
297  void setToRotation(const Quat<T> &q)
298  {*this = rotation<Mat3<T> >(q);}
299 
302  void setToRotation(const Vec3<T> &axis, T angle)
303  {*this = rotation<Mat3<T> >(axis, angle);}
304 
306  void setZero()
307  {
308  MyBase::mm[0] = 0;
309  MyBase::mm[1] = 0;
310  MyBase::mm[2] = 0;
311  MyBase::mm[3] = 0;
312  MyBase::mm[4] = 0;
313  MyBase::mm[5] = 0;
314  MyBase::mm[6] = 0;
315  MyBase::mm[7] = 0;
316  MyBase::mm[8] = 0;
317  } // trivial
318 
320  void setIdentity()
321  {
322  MyBase::mm[0] = 1;
323  MyBase::mm[1] = 0;
324  MyBase::mm[2] = 0;
325  MyBase::mm[3] = 0;
326  MyBase::mm[4] = 1;
327  MyBase::mm[5] = 0;
328  MyBase::mm[6] = 0;
329  MyBase::mm[7] = 0;
330  MyBase::mm[8] = 1;
331  } // trivial
332 
334  template<typename Source>
335  const Mat3& operator=(const Mat3<Source> &m)
336  {
337  const Source *src = m.asPointer();
338 
339  // don't suppress type conversion warnings
340  std::copy(src, (src + this->numElements()), MyBase::mm);
341  return *this;
342  } // opEqualToTest
343 
345  bool eq(const Mat3 &m, T eps=1.0e-8) const
346  {
347  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
348  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
349  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
350  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
351  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
352  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
353  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
354  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
355  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
356  } // trivial
357 
360  {
361  return Mat3<T>(
362  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
363  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
364  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
365  );
366  } // trivial
367 
369  // friend Mat3 operator*(T scalar, const Mat3& m) {
370  // return m*scalar;
371  // }
372 
374  template <typename S>
375  const Mat3<T>& operator*=(S scalar)
376  {
377  MyBase::mm[0] *= scalar;
378  MyBase::mm[1] *= scalar;
379  MyBase::mm[2] *= scalar;
380  MyBase::mm[3] *= scalar;
381  MyBase::mm[4] *= scalar;
382  MyBase::mm[5] *= scalar;
383  MyBase::mm[6] *= scalar;
384  MyBase::mm[7] *= scalar;
385  MyBase::mm[8] *= scalar;
386  return *this;
387  }
388 
390  template <typename S>
391  const Mat3<T> &operator+=(const Mat3<S> &m1)
392  {
393  const S *s = m1.asPointer();
394 
395  MyBase::mm[0] += s[0];
396  MyBase::mm[1] += s[1];
397  MyBase::mm[2] += s[2];
398  MyBase::mm[3] += s[3];
399  MyBase::mm[4] += s[4];
400  MyBase::mm[5] += s[5];
401  MyBase::mm[6] += s[6];
402  MyBase::mm[7] += s[7];
403  MyBase::mm[8] += s[8];
404  return *this;
405  }
406 
408  template <typename S>
409  const Mat3<T> &operator-=(const Mat3<S> &m1)
410  {
411  const S *s = m1.asPointer();
412 
413  MyBase::mm[0] -= s[0];
414  MyBase::mm[1] -= s[1];
415  MyBase::mm[2] -= s[2];
416  MyBase::mm[3] -= s[3];
417  MyBase::mm[4] -= s[4];
418  MyBase::mm[5] -= s[5];
419  MyBase::mm[6] -= s[6];
420  MyBase::mm[7] -= s[7];
421  MyBase::mm[8] -= s[8];
422  return *this;
423  }
424 
426  template <typename S>
427  const Mat3<T> &operator*=(const Mat3<S> &m1)
428  {
429  Mat3<T> m0(*this);
430  const T* s0 = m0.asPointer();
431  const S* s1 = m1.asPointer();
432 
433  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
434  s0[1] * s1[3] +
435  s0[2] * s1[6]);
436  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
437  s0[1] * s1[4] +
438  s0[2] * s1[7]);
439  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
440  s0[1] * s1[5] +
441  s0[2] * s1[8]);
442 
443  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
444  s0[4] * s1[3] +
445  s0[5] * s1[6]);
446  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
447  s0[4] * s1[4] +
448  s0[5] * s1[7]);
449  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
450  s0[4] * s1[5] +
451  s0[5] * s1[8]);
452 
453  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
454  s0[7] * s1[3] +
455  s0[8] * s1[6]);
456  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
457  s0[7] * s1[4] +
458  s0[8] * s1[7]);
459  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
460  s0[7] * s1[5] +
461  s0[8] * s1[8]);
462 
463  return *this;
464  }
465 
467  Mat3 cofactor() const
468  {
469  return Mat3<T>(
470  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
471  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
472  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
473  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
474  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
475  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
476  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
477  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
478  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
479  }
480 
482  Mat3 adjoint() const
483  {
484  return Mat3<T>(
485  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
486  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
487  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
488  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
489  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
490  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
491  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
492  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
493  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
494 
495  } // adjointTest
496 
498  Mat3 transpose() const
499  {
500  return Mat3<T>(
501  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
502  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
503  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
504 
505  } // transposeTest
506 
509  Mat3 inverse(T tolerance = 0) const
510  {
511  Mat3<T> inv(this->adjoint());
512 
513  const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
514 
515  // If the determinant is 0, m was singular and "this" will contain junk.
516  if (isApproxEqual(det,T(0.0),tolerance)) {
517  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
518  }
519  return inv * (T(1)/det);
520  } // invertTest
521 
523  T det() const
524  {
525  const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
526  const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
527  const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
528  return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
529  } // determinantTest
530 
532  T trace() const
533  {
534  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
535  }
536 
541  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
542  {
543  return snapMatBasis(*this, axis, direction);
544  }
545 
548  template<typename T0>
549  Vec3<T0> transform(const Vec3<T0> &v) const
550  {
551  return static_cast< Vec3<T0> >(v * *this);
552  } // xformVectorTest
553 
556  template<typename T0>
558  {
559  return static_cast< Vec3<T0> >(*this * v);
560  } // xformTVectorTest
561 
562 
565  Mat3 timesDiagonal(const Vec3<T>& diag) const
566  {
567  Mat3 ret(*this);
568 
569  ret.mm[0] *= diag(0);
570  ret.mm[1] *= diag(1);
571  ret.mm[2] *= diag(2);
572  ret.mm[3] *= diag(0);
573  ret.mm[4] *= diag(1);
574  ret.mm[5] *= diag(2);
575  ret.mm[6] *= diag(0);
576  ret.mm[7] *= diag(1);
577  ret.mm[8] *= diag(2);
578  return ret;
579  }
580 
581 private:
582  static const Mat3<T> sIdentity;
583  static const Mat3<T> sZero;
584 }; // class Mat3
585 
586 
587 template <typename T>
588 const Mat3<T> Mat3<T>::sIdentity = Mat3<T>(1, 0, 0,
589  0, 1, 0,
590  0, 0, 1);
591 
592 template <typename T>
593 const Mat3<T> Mat3<T>::sZero = Mat3<T>(0, 0, 0,
594  0, 0, 0,
595  0, 0, 0);
596 
599 template <typename T0, typename T1>
600 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
601 {
602  const T0 *t0 = m0.asPointer();
603  const T1 *t1 = m1.asPointer();
604 
605  for (int i=0; i<9; ++i) {
606  if (!isExactlyEqual(t0[i], t1[i])) return false;
607  }
608  return true;
609 }
610 
613 template <typename T0, typename T1>
614 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
615 
618 template <typename S, typename T>
620 { return m*scalar; }
621 
624 template <typename S, typename T>
626 {
628  result *= scalar;
629  return result;
630 }
631 
634 template <typename T0, typename T1>
636 {
638  result += m1;
639  return result;
640 }
641 
644 template <typename T0, typename T1>
646 {
648  result -= m1;
649  return result;
650 }
651 
652 
657 template <typename T0, typename T1>
659 {
661  result *= m1;
662  return result;
663 }
664 
667 template<typename T, typename MT>
669 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
670 {
671  MT const *m = _m.asPointer();
673  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
674  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
675  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
676 }
677 
680 template<typename T, typename MT>
682 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
683 {
684  MT const *m = _m.asPointer();
686  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
687  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
688  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
689 }
690 
693 template<typename T, typename MT>
694 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
695 {
696  Vec3<T> mult = _v * _m;
697  _v = mult;
698  return _v;
699 }
700 
703 template <typename T>
704 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
705 {
706  return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
707  v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
708  v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
709 }// outerProduct
710 
713 
714 #if DWREAL_IS_DOUBLE == 1
715 typedef Mat3d Mat3f;
716 #else
717 typedef Mat3s Mat3f;
718 #endif // DWREAL_IS_DOUBLE
719 
720 
724 template<typename T, typename T0>
725 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
726 {
727  Mat3<T> x = m1.inverse() * m2;
728  powSolve(x, x, t);
729  Mat3<T> m = m1 * x;
730  return m;
731 }
732 
733 
734 namespace {
735  template<typename T>
736  void pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
737  {
738  const int& n = Mat3<T>::size; // should be 3
739  T temp;
741  double cotan_of_2_theta;
742  double tan_of_theta;
743  double cosin_of_theta;
744  double sin_of_theta;
745  double z;
746 
747  double Sij = S(i,j);
748 
749  double Sjj_minus_Sii = D[j] - D[i];
750 
751  if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
752  tan_of_theta = Sij / Sjj_minus_Sii;
753  } else {
755  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
756 
757  if (cotan_of_2_theta < 0.) {
758  tan_of_theta =
759  -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
760  } else {
761  tan_of_theta =
762  1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
763  }
764  }
765 
766  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
767  sin_of_theta = cosin_of_theta * tan_of_theta;
768  z = tan_of_theta * Sij;
769  S(i,j) = 0;
770  D[i] -= z;
771  D[j] += z;
772  for (int k = 0; k < i; ++k) {
773  temp = S(k,i);
774  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
775  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
776  }
777  for (int k = i+1; k < j; ++k) {
778  temp = S(i,k);
779  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
780  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
781  }
782  for (int k = j+1; k < n; ++k) {
783  temp = S(i,k);
784  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
785  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
786  }
787  for (int k = 0; k < n; ++k)
788  {
789  temp = Q(k,i);
790  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
791  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
792  }
793  }
794 }
795 
796 
802 template<typename T>
804  unsigned int MAX_ITERATIONS=250)
805 {
808  Q = Mat3<T>::identity();
809  int n = Mat3<T>::size; // should be 3
810 
812  Mat3<T> S(input);
813 
814  for (int i = 0; i < n; ++i) {
815  D[i] = S(i,i);
816  }
817 
818  unsigned int iterations(0);
821  do {
824  double er = 0;
825  for (int i = 0; i < n; ++i) {
826  for (int j = i+1; j < n; ++j) {
827  er += fabs(S(i,j));
828  }
829  }
830  if (std::abs(er) < math::Tolerance<T>::value()) {
831  return true;
832  }
833  iterations++;
834 
835  T max_element = 0;
836  int ip = 0;
837  int jp = 0;
839  for (int i = 0; i < n; ++i) {
840  for (int j = i+1; j < n; ++j){
841 
842  if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
844  S(i,j) = 0;
845  }
846  if (fabs(S(i,j)) > max_element) {
847  max_element = fabs(S(i,j));
848  ip = i;
849  jp = j;
850  }
851  }
852  }
853  pivot(ip, jp, S, D, Q);
854  } while (iterations < MAX_ITERATIONS);
855 
856  return false;
857 }
858 
859 } // namespace math
860 } // namespace OPENVDB_VERSION_NAME
861 } // namespace openvdb
862 
863 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
864 
865 // Copyright (c) 2012-2016 DreamWorks Animation LLC
866 // All rights reserved. This software is distributed under the
867 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
Mat3(const Quat< T > &q)
Definition: Mat3.h:66
T operator()(int i, int j) const
Definition: Mat3.h:224
const T * operator[](int i) const
Definition: Mat3.h:205
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:335
static Mat3 symmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Definition: Mat3.h:281
#define OPENVDB_DEPRECATED
Definition: Platform.h:49
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:191
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:614
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:407
Mat3< float > Mat3s
Definition: Mat3.h:711
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:159
Definition: Mat.h:146
Definition: Mat.h:145
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:97
T mm[SIZE *SIZE]
Definition: Mat.h:141
T trace() const
Trace of matrix.
Definition: Mat3.h:532
Definition: Exceptions.h:78
T & operator()(int i, int j)
Definition: Mat3.h:214
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:77
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:359
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:232
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:557
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Matrix multiplication.
Definition: Mat3.h:658
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:600
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:144
Mat3< typename promote< S, T >::type > operator*(const Mat3< T > &m, S scalar)
Returns M, where for .
Definition: Mat3.h:625
4x4 -matrix class.
Definition: Mat3.h:48
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:645
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:725
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:370
T det() const
Determinant of matrix.
Definition: Mat3.h:523
void setZero()
Set this matrix to zero.
Definition: Mat3.h:306
Mat3(Source *a)
Definition: Mat3.h:109
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:549
#define OPENVDB_VERSION_NAME
Definition: version.h:43
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Returns M, where for .
Definition: Mat3.h:619
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:246
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:302
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat3< MT > &_m)
Returns v, where for .
Definition: Mat3.h:682
Mat3< double > Mat3d
Definition: Mat3.h:712
T ValueType
Definition: Mat3.h:59
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:266
const Mat3< T > & operator-=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:409
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:291
Definition: Exceptions.h:39
Tolerance for floating-point comparison.
Definition: Math.h:125
3x3 matrix class.
Definition: Mat3.h:54
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:375
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:803
OPENVDB_DEPRECATED void setBasis(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:260
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:134
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:704
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:123
void setIdentity()
Set "this" matrix to identity.
Definition: Mat3.h:320
Mat< 3, T > MyBase
Definition: Mat3.h:60
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:164
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:154
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:509
Vec3< typename promote< T, MT >::type > operator*(const Mat3< MT > &_m, const Vec3< T > &_v)
Returns v, where for .
Definition: Mat3.h:669
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:182
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:62
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:730
T value_type
Data type held by the matrix.
Definition: Mat3.h:58
Mat3s Mat3f
Definition: Mat3.h:717
Axis
Definition: Math.h:856
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:687
const T * asPointer() const
Definition: Mat3.h:209
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:810
const Mat3< T > & operator*=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:427
bool eq(const Mat3 &m, T eps=1.0e-8) const
Test if "this" is equivalent to m with tolerance of eps value.
Definition: Mat3.h:345
Mat3 adjoint() const
returns adjoint of "this", i.e. the transpose of the cofactor of "this"
Definition: Mat3.h:482
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:175
Definition: Mat.h:52
Mat3 cofactor() const
Return the cofactor matrix of "this".
Definition: Mat3.h:467
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:498
Mat3 timesDiagonal(const Vec3< T > &diag) const
Definition: Mat3.h:565
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:297
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3, bool rows=true)
Definition: Mat3.h:95
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition: Mat3.h:541
const Mat3< T > & operator+=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:391
T * asPointer()
Definition: Mat3.h:208
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:635