OpenVDB  4.0.1
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 <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  static const Mat3<T> sIdentity = Mat3<T>(
156  1, 0, 0,
157  0, 1, 0,
158  0, 0, 1
159  );
160  return sIdentity;
161  }
162 
164  static const Mat3<T>& zero() {
165  static const Mat3<T> sZero = Mat3<T>(
166  0, 0, 0,
167  0, 0, 0,
168  0, 0, 0
169  );
170  return sZero;
171  }
172 
174  void setRow(int i, const Vec3<T> &v)
175  {
176  // assert(i>=0 && i<3);
177  int i3 = i * 3;
178 
179  MyBase::mm[i3+0] = v[0];
180  MyBase::mm[i3+1] = v[1];
181  MyBase::mm[i3+2] = v[2];
182  } // rowColumnTest
183 
185  Vec3<T> row(int i) const
186  {
187  // assert(i>=0 && i<3);
188  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
189  } // rowColumnTest
190 
192  void setCol(int j, const Vec3<T>& v)
193  {
194  // assert(j>=0 && j<3);
195  MyBase::mm[0+j] = v[0];
196  MyBase::mm[3+j] = v[1];
197  MyBase::mm[6+j] = v[2];
198  } // rowColumnTest
199 
201  Vec3<T> col(int j) const
202  {
203  // assert(j>=0 && j<3);
204  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
205  } // rowColumnTest
206 
207  // NB: The following two methods were changed to
208  // work around a gccWS5 compiler issue related to strict
209  // aliasing (see FX-475).
210 
212  T* operator[](int i) { return &(MyBase::mm[i*3]); }
215  const T* operator[](int i) const { return &(MyBase::mm[i*3]); }
217 
218  T* asPointer() {return MyBase::mm;}
219  const T* asPointer() const {return MyBase::mm;}
220 
224  T& operator()(int i, int j)
225  {
226  // assert(i>=0 && i<3);
227  // assert(j>=0 && j<3);
228  return MyBase::mm[3*i+j];
229  } // trivial
230 
234  T operator()(int i, int j) const
235  {
236  // assert(i>=0 && i<3);
237  // assert(j>=0 && j<3);
238  return MyBase::mm[3*i+j];
239  } // trivial
240 
242  void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
243  {
244  MyBase::mm[0] = v1[0];
245  MyBase::mm[1] = v1[1];
246  MyBase::mm[2] = v1[2];
247  MyBase::mm[3] = v2[0];
248  MyBase::mm[4] = v2[1];
249  MyBase::mm[5] = v2[2];
250  MyBase::mm[6] = v3[0];
251  MyBase::mm[7] = v3[1];
252  MyBase::mm[8] = v3[2];
253  } // setRows
254 
256  void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
257  {
258  MyBase::mm[0] = v1[0];
259  MyBase::mm[1] = v2[0];
260  MyBase::mm[2] = v3[0];
261  MyBase::mm[3] = v1[1];
262  MyBase::mm[4] = v2[1];
263  MyBase::mm[5] = v3[1];
264  MyBase::mm[6] = v1[2];
265  MyBase::mm[7] = v2[2];
266  MyBase::mm[8] = v3[2];
267  } // setColumns
268 
270  OPENVDB_DEPRECATED void setBasis(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
271  {
272  this->setRows(v1, v2, v3);
273  }
274 
276  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
277  {
278  MyBase::mm[0] = vdiag[0];
279  MyBase::mm[1] = vtri[0];
280  MyBase::mm[2] = vtri[1];
281  MyBase::mm[3] = vtri[0];
282  MyBase::mm[4] = vdiag[1];
283  MyBase::mm[5] = vtri[2];
284  MyBase::mm[6] = vtri[1];
285  MyBase::mm[7] = vtri[2];
286  MyBase::mm[8] = vdiag[2];
287  } // setSymmetricTest
288 
291  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
292  {
293  return Mat3(
294  vdiag[0], vtri[0], vtri[1],
295  vtri[0], vdiag[1], vtri[2],
296  vtri[1], vtri[2], vdiag[2]
297  );
298  }
299 
301  void setSkew(const Vec3<T> &v)
302  {*this = skew(v);}
303 
307  void setToRotation(const Quat<T> &q)
308  {*this = rotation<Mat3<T> >(q);}
309 
312  void setToRotation(const Vec3<T> &axis, T angle)
313  {*this = rotation<Mat3<T> >(axis, angle);}
314 
316  void setZero()
317  {
318  MyBase::mm[0] = 0;
319  MyBase::mm[1] = 0;
320  MyBase::mm[2] = 0;
321  MyBase::mm[3] = 0;
322  MyBase::mm[4] = 0;
323  MyBase::mm[5] = 0;
324  MyBase::mm[6] = 0;
325  MyBase::mm[7] = 0;
326  MyBase::mm[8] = 0;
327  } // trivial
328 
330  void setIdentity()
331  {
332  MyBase::mm[0] = 1;
333  MyBase::mm[1] = 0;
334  MyBase::mm[2] = 0;
335  MyBase::mm[3] = 0;
336  MyBase::mm[4] = 1;
337  MyBase::mm[5] = 0;
338  MyBase::mm[6] = 0;
339  MyBase::mm[7] = 0;
340  MyBase::mm[8] = 1;
341  } // trivial
342 
344  template<typename Source>
345  const Mat3& operator=(const Mat3<Source> &m)
346  {
347  const Source *src = m.asPointer();
348 
349  // don't suppress type conversion warnings
350  std::copy(src, (src + this->numElements()), MyBase::mm);
351  return *this;
352  } // opEqualToTest
353 
355  bool eq(const Mat3 &m, T eps=1.0e-8) const
356  {
357  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
358  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
359  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
360  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
361  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
362  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
363  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
364  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
365  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
366  } // trivial
367 
370  {
371  return Mat3<T>(
372  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
373  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
374  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
375  );
376  } // trivial
377 
379  // friend Mat3 operator*(T scalar, const Mat3& m) {
380  // return m*scalar;
381  // }
382 
384  template <typename S>
385  const Mat3<T>& operator*=(S scalar)
386  {
387  MyBase::mm[0] *= scalar;
388  MyBase::mm[1] *= scalar;
389  MyBase::mm[2] *= scalar;
390  MyBase::mm[3] *= scalar;
391  MyBase::mm[4] *= scalar;
392  MyBase::mm[5] *= scalar;
393  MyBase::mm[6] *= scalar;
394  MyBase::mm[7] *= scalar;
395  MyBase::mm[8] *= scalar;
396  return *this;
397  }
398 
400  template <typename S>
401  const Mat3<T> &operator+=(const Mat3<S> &m1)
402  {
403  const S *s = m1.asPointer();
404 
405  MyBase::mm[0] += s[0];
406  MyBase::mm[1] += s[1];
407  MyBase::mm[2] += s[2];
408  MyBase::mm[3] += s[3];
409  MyBase::mm[4] += s[4];
410  MyBase::mm[5] += s[5];
411  MyBase::mm[6] += s[6];
412  MyBase::mm[7] += s[7];
413  MyBase::mm[8] += s[8];
414  return *this;
415  }
416 
418  template <typename S>
419  const Mat3<T> &operator-=(const Mat3<S> &m1)
420  {
421  const S *s = m1.asPointer();
422 
423  MyBase::mm[0] -= s[0];
424  MyBase::mm[1] -= s[1];
425  MyBase::mm[2] -= s[2];
426  MyBase::mm[3] -= s[3];
427  MyBase::mm[4] -= s[4];
428  MyBase::mm[5] -= s[5];
429  MyBase::mm[6] -= s[6];
430  MyBase::mm[7] -= s[7];
431  MyBase::mm[8] -= s[8];
432  return *this;
433  }
434 
436  template <typename S>
437  const Mat3<T> &operator*=(const Mat3<S> &m1)
438  {
439  Mat3<T> m0(*this);
440  const T* s0 = m0.asPointer();
441  const S* s1 = m1.asPointer();
442 
443  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
444  s0[1] * s1[3] +
445  s0[2] * s1[6]);
446  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
447  s0[1] * s1[4] +
448  s0[2] * s1[7]);
449  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
450  s0[1] * s1[5] +
451  s0[2] * s1[8]);
452 
453  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
454  s0[4] * s1[3] +
455  s0[5] * s1[6]);
456  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
457  s0[4] * s1[4] +
458  s0[5] * s1[7]);
459  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
460  s0[4] * s1[5] +
461  s0[5] * s1[8]);
462 
463  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
464  s0[7] * s1[3] +
465  s0[8] * s1[6]);
466  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
467  s0[7] * s1[4] +
468  s0[8] * s1[7]);
469  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
470  s0[7] * s1[5] +
471  s0[8] * s1[8]);
472 
473  return *this;
474  }
475 
477  Mat3 cofactor() const
478  {
479  return Mat3<T>(
480  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
481  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
482  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
483  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
484  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
485  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
486  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
487  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
488  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
489  }
490 
492  Mat3 adjoint() const
493  {
494  return Mat3<T>(
495  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
496  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
497  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
498  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
499  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
500  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
501  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
502  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
503  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
504 
505  } // adjointTest
506 
508  Mat3 transpose() const
509  {
510  return Mat3<T>(
511  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
512  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
513  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
514 
515  } // transposeTest
516 
519  Mat3 inverse(T tolerance = 0) const
520  {
521  Mat3<T> inv(this->adjoint());
522 
523  const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
524 
525  // If the determinant is 0, m was singular and "this" will contain junk.
526  if (isApproxEqual(det,T(0.0),tolerance)) {
527  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
528  }
529  return inv * (T(1)/det);
530  } // invertTest
531 
533  T det() const
534  {
535  const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
536  const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
537  const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
538  return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
539  } // determinantTest
540 
542  T trace() const
543  {
544  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
545  }
546 
551  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
552  {
553  return snapMatBasis(*this, axis, direction);
554  }
555 
558  template<typename T0>
559  Vec3<T0> transform(const Vec3<T0> &v) const
560  {
561  return static_cast< Vec3<T0> >(v * *this);
562  } // xformVectorTest
563 
566  template<typename T0>
568  {
569  return static_cast< Vec3<T0> >(*this * v);
570  } // xformTVectorTest
571 
572 
575  Mat3 timesDiagonal(const Vec3<T>& diag) const
576  {
577  Mat3 ret(*this);
578 
579  ret.mm[0] *= diag(0);
580  ret.mm[1] *= diag(1);
581  ret.mm[2] *= diag(2);
582  ret.mm[3] *= diag(0);
583  ret.mm[4] *= diag(1);
584  ret.mm[5] *= diag(2);
585  ret.mm[6] *= diag(0);
586  ret.mm[7] *= diag(1);
587  ret.mm[8] *= diag(2);
588  return ret;
589  }
590 }; // class Mat3
591 
592 
595 template <typename T0, typename T1>
596 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
597 {
598  const T0 *t0 = m0.asPointer();
599  const T1 *t1 = m1.asPointer();
600 
601  for (int i=0; i<9; ++i) {
602  if (!isExactlyEqual(t0[i], t1[i])) return false;
603  }
604  return true;
605 }
606 
609 template <typename T0, typename T1>
610 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
611 
614 template <typename S, typename T>
616 { return m*scalar; }
617 
620 template <typename S, typename T>
622 {
624  result *= scalar;
625  return result;
626 }
627 
630 template <typename T0, typename T1>
632 {
634  result += m1;
635  return result;
636 }
637 
640 template <typename T0, typename T1>
642 {
644  result -= m1;
645  return result;
646 }
647 
648 
653 template <typename T0, typename T1>
655 {
657  result *= m1;
658  return result;
659 }
660 
663 template<typename T, typename MT>
665 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
666 {
667  MT const *m = _m.asPointer();
669  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
670  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
671  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
672 }
673 
676 template<typename T, typename MT>
678 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
679 {
680  MT const *m = _m.asPointer();
682  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
683  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
684  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
685 }
686 
689 template<typename T, typename MT>
690 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
691 {
692  Vec3<T> mult = _v * _m;
693  _v = mult;
694  return _v;
695 }
696 
699 template <typename T>
700 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
701 {
702  return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
703  v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
704  v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
705 }// outerProduct
706 
709 typedef Mat3d Mat3f;
710 
711 
715 template<typename T, typename T0>
716 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
717 {
718  Mat3<T> x = m1.inverse() * m2;
719  powSolve(x, x, t);
720  Mat3<T> m = m1 * x;
721  return m;
722 }
723 
724 
725 namespace {
726  template<typename T>
727  void pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
728  {
729  const int& n = Mat3<T>::size; // should be 3
730  T temp;
732  double cotan_of_2_theta;
733  double tan_of_theta;
734  double cosin_of_theta;
735  double sin_of_theta;
736  double z;
737 
738  double Sij = S(i,j);
739 
740  double Sjj_minus_Sii = D[j] - D[i];
741 
742  if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
743  tan_of_theta = Sij / Sjj_minus_Sii;
744  } else {
746  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
747 
748  if (cotan_of_2_theta < 0.) {
749  tan_of_theta =
750  -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
751  } else {
752  tan_of_theta =
753  1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
754  }
755  }
756 
757  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
758  sin_of_theta = cosin_of_theta * tan_of_theta;
759  z = tan_of_theta * Sij;
760  S(i,j) = 0;
761  D[i] -= z;
762  D[j] += z;
763  for (int k = 0; k < i; ++k) {
764  temp = S(k,i);
765  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
766  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
767  }
768  for (int k = i+1; k < j; ++k) {
769  temp = S(i,k);
770  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
771  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
772  }
773  for (int k = j+1; k < n; ++k) {
774  temp = S(i,k);
775  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
776  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
777  }
778  for (int k = 0; k < n; ++k)
779  {
780  temp = Q(k,i);
781  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
782  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
783  }
784  }
785 }
786 
787 
793 template<typename T>
795  unsigned int MAX_ITERATIONS=250)
796 {
799  Q = Mat3<T>::identity();
800  int n = Mat3<T>::size; // should be 3
801 
803  Mat3<T> S(input);
804 
805  for (int i = 0; i < n; ++i) {
806  D[i] = S(i,i);
807  }
808 
809  unsigned int iterations(0);
812  do {
815  double er = 0;
816  for (int i = 0; i < n; ++i) {
817  for (int j = i+1; j < n; ++j) {
818  er += fabs(S(i,j));
819  }
820  }
821  if (std::abs(er) < math::Tolerance<T>::value()) {
822  return true;
823  }
824  iterations++;
825 
826  T max_element = 0;
827  int ip = 0;
828  int jp = 0;
830  for (int i = 0; i < n; ++i) {
831  for (int j = i+1; j < n; ++j){
832 
833  if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
835  S(i,j) = 0;
836  }
837  if (fabs(S(i,j)) > max_element) {
838  max_element = fabs(S(i,j));
839  ip = i;
840  jp = j;
841  }
842  }
843  }
844  pivot(ip, jp, S, D, Q);
845  } while (iterations < MAX_ITERATIONS);
846 
847  return false;
848 }
849 
850 } // namespace math
851 } // namespace OPENVDB_VERSION_NAME
852 } // namespace openvdb
853 
854 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
855 
856 // Copyright (c) 2012-2017 DreamWorks Animation LLC
857 // All rights reserved. This software is distributed under the
858 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
const Mat3< T > & operator-=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:419
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat3< MT > &_m)
Returns v, where for .
Definition: Mat3.h:678
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
Mat3< typename promote< S, T >::type > operator*(const Mat3< T > &m, S scalar)
Returns M, where for .
Definition: Mat3.h:621
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:192
#define OPENVDB_DEPRECATED
Definition: Platform.h:49
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:301
Definition: Mat.h:162
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:270
const Mat3< T > & operator*=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:437
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:385
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:174
const T * asPointer() const
Definition: Mat3.h:219
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:242
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:480
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Matrix multiplication.
Definition: Mat3.h:654
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:256
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:101
Mat3d Mat3f
Definition: Mat3.h:709
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:716
Mat3 timesDiagonal(const Vec3< T > &diag) const
Definition: Mat3.h:575
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:276
Mat3< double > Mat3d
Definition: Mat3.h:708
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:62
4x4 -matrix class.
Definition: Mat3.h:48
Definition: Mat.h:161
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:641
Mat3< float > Mat3s
Definition: Mat3.h:707
const Mat3< T > & operator+=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:401
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:123
Mat3 adjoint() const
returns adjoint of "this", i.e. the transpose of the cofactor of "this"
Definition: Mat3.h:492
T & operator()(int i, int j)
Definition: Mat3.h:224
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:700
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:185
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:567
Mat< 3, T > MyBase
Definition: Mat3.h:60
void setIdentity()
Set "this" matrix to identity.
Definition: Mat3.h:330
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:817
Definition: Mat.h:52
#define OPENVDB_VERSION_NAME
Definition: version.h:43
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:164
3x3 matrix class.
Definition: Mat3.h:54
T trace() const
Trace of matrix.
Definition: Mat3.h:542
T * asPointer()
Definition: Mat3.h:218
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:551
T det() const
Determinant of matrix.
Definition: Mat3.h:533
T operator()(int i, int j) const
Definition: Mat3.h:234
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:369
const T * operator[](int i) const
Definition: Mat3.h:215
Mat3(Source *a)
Definition: Mat3.h:109
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:201
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:144
Definition: Exceptions.h:39
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Returns M, where for .
Definition: Mat3.h:615
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:312
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:154
Vec3< typename promote< T, MT >::type > operator*(const Mat3< MT > &_m, const Vec3< T > &_v)
Returns v, where for .
Definition: Mat3.h:665
T ValueType
Definition: Mat3.h:59
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:508
Definition: Exceptions.h:82
void setZero()
Set this matrix to zero.
Definition: Mat3.h:316
Mat3(const Quat< T > &q)
Definition: Mat3.h:66
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:631
T value_type
Data type held by the matrix.
Definition: Mat3.h:58
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:407
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:794
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
static Mat3 symmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Definition: Mat3.h:291
Mat3 cofactor() const
Return the cofactor matrix of "this".
Definition: Mat3.h:477
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:596
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:519
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:307
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:703
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:559
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:134
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:746
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:345
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:355
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:610
Tolerance for floating-point comparison.
Definition: Math.h:125
T mm[SIZE *SIZE]
Definition: Mat.h:157