OpenVDB  0.104.0
Mat3.h
Go to the documentation of this file.
1 
2 //
3 // Copyright (c) 2012 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] = a;
82  MyBase::mm[1] = b;
83  MyBase::mm[2] = c;
84  MyBase::mm[3] = d;
85  MyBase::mm[4] = e;
86  MyBase::mm[5] = f;
87  MyBase::mm[6] = g;
88  MyBase::mm[7] = h;
89  MyBase::mm[8] = i;
90  } // constructor1Test
91 
93  template<typename Source>
94  Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3)
95  { setBasis(v1, v2, v3); }
96 
101  template<typename Source>
102  Mat3(Source *a)
103  {
104  MyBase::mm[0] = a[0];
105  MyBase::mm[1] = a[1];
106  MyBase::mm[2] = a[2];
107  MyBase::mm[3] = a[3];
108  MyBase::mm[4] = a[4];
109  MyBase::mm[5] = a[5];
110  MyBase::mm[6] = a[6];
111  MyBase::mm[7] = a[7];
112  MyBase::mm[8] = a[8];
113  } // constructor1Test
114 
116  Mat3(const Mat<3, T> &m)
117  {
118  for (int i=0; i<3; ++i) {
119  for (int j=0; j<3; ++j) {
120  MyBase::mm[i*3 + j] = m[i][j];
121  }
122  }
123  }
124 
126  template<typename Source>
127  explicit Mat3(const Mat3<Source> &m)
128  {
129  for (int i=0; i<3; ++i) {
130  for (int j=0; j<3; ++j) {
131  MyBase::mm[i*3 + j] = m[i][j];
132  }
133  }
134  }
135 
137  explicit Mat3(const Mat4<T> &m)
138  {
139  for (int i=0; i<3; ++i) {
140  for (int j=0; j<3; ++j) {
141  MyBase::mm[i*3 + j] = m[i][j];
142  }
143  }
144  }
145 
147  static const Mat3<T>& identity() {
148  return sIdentity;
149  }
150 
152  static const Mat3<T>& zero() {
153  return sZero;
154  }
155 
157  void setRow(int i, const Vec3<T> &v)
158  {
159  // assert(i>=0 && i<3);
160  int i3 = i * 3;
161 
162  MyBase::mm[i3+0] = v[0];
163  MyBase::mm[i3+1] = v[1];
164  MyBase::mm[i3+2] = v[2];
165  } // rowColumnTest
166 
168  Vec3<T> row(int i) const
169  {
170  // assert(i>=0 && i<3);
171  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
172  } // rowColumnTest
173 
175  void setCol(int j, const Vec3<T>& v)
176  {
177  // assert(j>=0 && j<3);
178  MyBase::mm[0+j] = v[0];
179  MyBase::mm[3+j] = v[1];
180  MyBase::mm[6+j] = v[2];
181  } // rowColumnTest
182 
184  Vec3<T> col(int j) const
185  {
186  // assert(j>=0 && j<3);
187  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
188  } // rowColumnTest
189 
190  // NB: The following two methods were changed to
191  // work around a gccWS5 compiler issue related to strict
192  // aliasing (see FX-475).
193 
195 
196 
197  T* operator[](int i) { return &(MyBase::mm[i*3]); }
198  const T* operator[](int i) const { return &(MyBase::mm[i*3]); }
200 
201  T* asPointer() {return MyBase::mm;}
202  const T* asPointer() const {return MyBase::mm;}
203 
204 
208  T& operator()(int i, int j)
209  {
210  // assert(i>=0 && i<3);
211  // assert(j>=0 && j<3);
212  return MyBase::mm[3*i+j];
213  } // trivial
214 
218  T operator()(int i, int j) const
219  {
220  // assert(i>=0 && i<3);
221  // assert(j>=0 && j<3);
222  return MyBase::mm[3*i+j];
223  } // trivial
224 
226  void setBasis(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
227  {
228  MyBase::mm[0] = v1[0];
229  MyBase::mm[1] = v1[1];
230  MyBase::mm[2] = v1[2];
231  MyBase::mm[3] = v2[0];
232  MyBase::mm[4] = v2[1];
233  MyBase::mm[5] = v2[2];
234  MyBase::mm[6] = v3[0];
235  MyBase::mm[7] = v3[1];
236  MyBase::mm[8] = v3[2];
237  } // setBasisTest
238 
240  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
241  {
242  MyBase::mm[0] = vdiag[0];
243  MyBase::mm[1] = vtri[0];
244  MyBase::mm[2] = vtri[1];
245  MyBase::mm[3] = vtri[0];
246  MyBase::mm[4] = vdiag[1];
247  MyBase::mm[5] = vtri[2];
248  MyBase::mm[6] = vtri[1];
249  MyBase::mm[7] = vtri[2];
250  MyBase::mm[8] = vdiag[2];
251  } // setSymmetricTest
252 
255  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
256  {
257  return Mat3(
258  vdiag[0], vtri[0], vtri[1],
259  vtri[0], vdiag[1], vtri[2],
260  vtri[1], vtri[2], vdiag[2]
261  );
262  }
263 
265  void setSkew(const Vec3<T> &v)
266  {*this = skew(v);}
267 
272  void setToRotation(const Quat<T> &q)
273  {*this = rotation<Mat3<T> >(q);}
274 
277  void setToRotation(const Vec3<T> &axis, T angle)
278  {*this = rotation<Mat3<T> >(axis, angle);}
279 
280  // Set "this" matrix to zero
281  void setZero()
282  {
283  MyBase::mm[0] = 0;
284  MyBase::mm[1] = 0;
285  MyBase::mm[2] = 0;
286  MyBase::mm[3] = 0;
287  MyBase::mm[4] = 0;
288  MyBase::mm[5] = 0;
289  MyBase::mm[6] = 0;
290  MyBase::mm[7] = 0;
291  MyBase::mm[8] = 0;
292  } // trivial
293 
295  void setIdentity()
296  {
297  MyBase::mm[0] = 1;
298  MyBase::mm[1] = 0;
299  MyBase::mm[2] = 0;
300  MyBase::mm[3] = 0;
301  MyBase::mm[4] = 1;
302  MyBase::mm[5] = 0;
303  MyBase::mm[6] = 0;
304  MyBase::mm[7] = 0;
305  MyBase::mm[8] = 1;
306  } // trivial
307 
309  template<typename Source>
310  const Mat3& operator=(const Mat3<Source> &m)
311  {
312  const Source *src = m.asPointer();
313 
314  // don't suppress type conversion warnings
315  std::copy(src, (src + this->numElements()), MyBase::mm);
316  return *this;
317  } // opEqualToTest
318 
320  bool eq(const Mat3 &m, T eps=1.0e-8) const
321  {
322  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
323  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
324  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
325  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
326  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
327  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
328  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
329  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
330  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
331  } // trivial
332 
335  {
336  return Mat3<T>(
337  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
338  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
339  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
340  );
341  } // trivial
342 
344  // friend Mat3 operator*(T scalar, const Mat3& m) {
345  // return m*scalar;
346  // }
347 
349  template <typename S>
350  const Mat3<T>& operator*=(S scalar)
351  {
352  MyBase::mm[0] *= scalar;
353  MyBase::mm[1] *= scalar;
354  MyBase::mm[2] *= scalar;
355  MyBase::mm[3] *= scalar;
356  MyBase::mm[4] *= scalar;
357  MyBase::mm[5] *= scalar;
358  MyBase::mm[6] *= scalar;
359  MyBase::mm[7] *= scalar;
360  MyBase::mm[8] *= scalar;
361  return *this;
362  }
363 
365  template <typename S>
366  const Mat3<T> &operator+=(const Mat3<S> &m1)
367  {
368  const S *s = m1.asPointer();
369 
370  MyBase::mm[0] += s[0];
371  MyBase::mm[1] += s[1];
372  MyBase::mm[2] += s[2];
373  MyBase::mm[3] += s[3];
374  MyBase::mm[4] += s[4];
375  MyBase::mm[5] += s[5];
376  MyBase::mm[6] += s[6];
377  MyBase::mm[7] += s[7];
378  MyBase::mm[8] += s[8];
379  return *this;
380  }
381 
383  template <typename S>
384  const Mat3<T> &operator-=(const Mat3<S> &m1)
385  {
386  const S *s = m1.asPointer();
387 
388  MyBase::mm[0] -= s[0];
389  MyBase::mm[1] -= s[1];
390  MyBase::mm[2] -= s[2];
391  MyBase::mm[3] -= s[3];
392  MyBase::mm[4] -= s[4];
393  MyBase::mm[5] -= s[5];
394  MyBase::mm[6] -= s[6];
395  MyBase::mm[7] -= s[7];
396  MyBase::mm[8] -= s[8];
397  return *this;
398  }
399 
401  template <typename S>
402  const Mat3<T> &operator*=(const Mat3<S> &m1)
403  {
404  Mat3<T> m0(*this);
405  const T* s0 = m0.asPointer();
406  const S* s1 = m1.asPointer();
407 
408  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
409  s0[1] * s1[3] +
410  s0[2] * s1[6]);
411  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
412  s0[1] * s1[4] +
413  s0[2] * s1[7]);
414  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
415  s0[1] * s1[5] +
416  s0[2] * s1[8]);
417 
418  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
419  s0[4] * s1[3] +
420  s0[5] * s1[6]);
421  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
422  s0[4] * s1[4] +
423  s0[5] * s1[7]);
424  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
425  s0[4] * s1[5] +
426  s0[5] * s1[8]);
427 
428  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
429  s0[7] * s1[3] +
430  s0[8] * s1[6]);
431  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
432  s0[7] * s1[4] +
433  s0[8] * s1[7]);
434  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
435  s0[7] * s1[5] +
436  s0[8] * s1[8]);
437 
438  return *this;
439  }
440 
442  Mat3 adjoint() const
443  {
444  return Mat3<T>(
445  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
446  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
447  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
448  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
449  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
450  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
451  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
452  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
453  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
454  } // adjointTest
455 
457  Mat3 transpose() const
458  {
459  return Mat3<T>(
460  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
461  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
462  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
463 
464  } // transposeTest
465 
468  Mat3 inverse(T tolerance = 0) const
469  {
470  Mat3<T> inv(adjoint());
471 
472  T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
473 
474  // If the determinant is 0, m was singular and "this" will contain junk.
475  if (isApproxEqual(det,0.0,tolerance))
476  {
477  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
478  }
479  return inv * (T(1)/det);
480  } // invertTest
481 
483  T det() const
484  {
485  T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
486  T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
487  T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
488  T d = MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
489  return d;
490  } // determinantTest
491 
493  T trace() const
494  {
495  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
496  }
497 
502  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
503  {
504  return snapBasis(*this, axis, direction);
505  }
506 
509  template<typename T0>
510  Vec3<T0> transform(const Vec3<T0> &v) const
511  {
512  return static_cast< Vec3<T0> >(v * *this);
513  } // xformVectorTest
514 
517  template<typename T0>
518  Vec3<T0> pretransform(const Vec3<T0> &v) const
519  {
520  return static_cast< Vec3<T0> >(*this * v);
521  } // xformTVectorTest
522 
527  template<typename T0>
528  Mat3 snappedBasis(Axis axis, const Vec3<T0>& direction) const
529  {
530  return snapBasis(*this, axis, direction);
531  }
532 
533 
538 #ifdef ALLOW_CAST_TO_POINTER
539 
540 OPENVDB_DEPRECATED operator T*()
541 #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
542  __attribute__ ((deprecated))
543 #endif
544  { return MyBase::mm; } // trivial
545 OPENVDB_DEPRECATED operator const T*() const
546 #if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
547  __attribute__ ((deprecated))
548 #endif
549  { return MyBase::mm; } // trivial
550 #endif
551 
552 
555  template<typename T0, typename T1>
556 OPENVDB_DEPRECATED const Mat3 &add(const Mat3<T0> &m1, const Mat3<T1> &m2)
557  {
558  const T0 *src1 = m1.asPointer();
559  const T1 *src2 = m2.asPointer();
560 
561  MyBase::mm[0] = src1[0] + src2[0];
562  MyBase::mm[1] = src1[1] + src2[1];
563  MyBase::mm[2] = src1[2] + src2[2];
564  MyBase::mm[3] = src1[3] + src2[3];
565  MyBase::mm[4] = src1[4] + src2[4];
566  MyBase::mm[5] = src1[5] + src2[5];
567  MyBase::mm[6] = src1[6] + src2[6];
568  MyBase::mm[7] = src1[7] + src2[7];
569  MyBase::mm[8] = src1[8] + src2[8];
570 
571  return *this;
572  } // opPlusTest
573 
576  template<typename T0, typename T1>
577 OPENVDB_DEPRECATED const Mat3 &sub(const Mat3<T0> &m1, const Mat3<T1> &m2)
578  {
579  const T0 *src1 = m1.asPointer();
580  const T1 *src2 = m2.asPointer();
581 
582  MyBase::mm[0] = src1[0] - src2[0];
583  MyBase::mm[1] = src1[1] - src2[1];
584  MyBase::mm[2] = src1[2] - src2[2];
585  MyBase::mm[3] = src1[3] - src2[3];
586  MyBase::mm[4] = src1[4] - src2[4];
587  MyBase::mm[5] = src1[5] - src2[5];
588  MyBase::mm[6] = src1[6] - src2[6];
589  MyBase::mm[7] = src1[7] - src2[7];
590  MyBase::mm[8] = src1[8] - src2[8];
591 
592  return *this;
593  } // opMinusTest
594 
596  template<typename T0, typename T1>
597 OPENVDB_DEPRECATED const Mat3 &scale(T0 scalar, const Mat3<T1> &m)
598  {
599  const T1 *src = m.asPointer();
600 
601  MyBase::mm[0] = scalar * src[0];
602  MyBase::mm[1] = scalar * src[1];
603  MyBase::mm[2] = scalar * src[2];
604  MyBase::mm[3] = scalar * src[3];
605  MyBase::mm[4] = scalar * src[4];
606  MyBase::mm[5] = scalar * src[5];
607  MyBase::mm[6] = scalar * src[6];
608  MyBase::mm[7] = scalar * src[7];
609  MyBase::mm[8] = scalar * src[8];
610 
611  return *this;
612  }
613 
616  template<typename T0, typename T1>
617 OPENVDB_DEPRECATED const Mat3 &mult(const Mat3<T0> &m1, const Mat3<T1> &m2)
618  {
619  const T0 *src1 = m1.asPointer();
620  const T1 *src2 = m2.asPointer();
621  // assert(this!=&m1);
622  // assert(this!=&m2);
623 
624  MyBase::mm[0] = static_cast<T>(src1[0] * src2[0] +
625  src1[1] * src2[3] +
626  src1[2] * src2[6]);
627  MyBase::mm[1] = static_cast<T>(src1[0] * src2[1] +
628  src1[1] * src2[4] +
629  src1[2] * src2[7]);
630  MyBase::mm[2] = static_cast<T>(src1[0] * src2[2] +
631  src1[1] * src2[5] +
632  src1[2] * src2[8]);
633 
634  MyBase::mm[3] = static_cast<T>(src1[3] * src2[0] +
635  src1[4] * src2[3] +
636  src1[5] * src2[6]);
637  MyBase::mm[4] = static_cast<T>(src1[3] * src2[1] +
638  src1[4] * src2[4] +
639  src1[5] * src2[7]);
640  MyBase::mm[5] = static_cast<T>(src1[3] * src2[2] +
641  src1[4] * src2[5] +
642  src1[5] * src2[8]);
643 
644  MyBase::mm[6] = static_cast<T>(src1[6] * src2[0] +
645  src1[7] * src2[3] +
646  src1[8] * src2[6]);
647  MyBase::mm[7] = static_cast<T>(src1[6] * src2[1] +
648  src1[7] * src2[4] +
649  src1[8] * src2[7]);
650  MyBase::mm[8] = static_cast<T>(src1[6] * src2[2] +
651  src1[7] * src2[5] +
652  src1[8] * src2[8]);
653  return *this;
654  } // multTest
655 
656 
657 private:
658  static const Mat3<T> sIdentity;
659  static const Mat3<T> sZero;
660 }; // class Mat3
661 
662 
663 template <typename T>
664 const Mat3<T> Mat3<T>::sIdentity = Mat3<T>(1, 0, 0,
665  0, 1, 0,
666  0, 0, 1);
667 
668 template <typename T>
669 const Mat3<T> Mat3<T>::sZero = Mat3<T>(0, 0, 0,
670  0, 0, 0,
671  0, 0, 0);
672 
675 template <typename T0, typename T1>
676 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
677 {
678  const T0 *t0 = m0.asPointer();
679  const T1 *t1 = m1.asPointer();
680 
681  for (int i=0; i<9; ++i) {
682  if (!isExactlyEqual(t0[i], t1[i])) return false;
683  }
684  return true;
685 }
686 
689 template <typename T0, typename T1>
690 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
691 
694 template <typename S, typename T>
696 { return m*scalar; }
697 
700 template <typename S, typename T>
702 {
704  result *= scalar;
705  return result;
706 }
707 
710 template <typename T0, typename T1>
712 {
714  result += m1;
715  return result;
716 }
717 
720 template <typename T0, typename T1>
722 {
724  result -= m1;
725  return result;
726 }
727 
728 
733 template <typename T0, typename T1>
735 {
737  result *= m1;
738  return result;
739 }
740 
743 template<typename T, typename MT>
744 inline Vec3<typename promote<T, MT>::type>
745 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
746 {
747  MT const *m = _m.asPointer();
749  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
750  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
751  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
752 }
753 
756 template<typename T, typename MT>
758 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
759 {
760  MT const *m = _m.asPointer();
762  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
763  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
764  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
765 }
766 
769 template<typename T, typename MT>
770 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
771 {
772  Vec3<T> mult = _v * _m;
773  _v = mult;
774  return _v;
775 }
776 
779 template <typename T>
780 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
781 {
782  Mat3<T> m;
783 
784  m.setBasis(Vec3<T>(v1[0]*v2[0], v1[1]*v2[0], v1[2]*v2[0]),
785  Vec3<T>(v1[0]*v2[1], v1[1]*v2[1], v1[2]*v2[1]),
786  Vec3<T>(v1[0]*v2[2], v1[1]*v2[2], v1[2]*v2[2]));
787 
788  return m;
789 } // outerproductTest
790 
793 
794 #if DWREAL_IS_DOUBLE == 1
795 typedef Mat3d Mat3f;
796 #else
797 typedef Mat3s Mat3f;
798 #endif // DWREAL_IS_DOUBLE
799 
800 
804 template<typename T, typename T0>
805 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
806 {
807  Mat3<T> x = m1.inverse() * m2;
808  powSolve(x, x, t);
809  Mat3<T> m = m1 * x;
810  return m;
811 }
812 
813 
814 namespace {
815  template<typename T>
816  void pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
817  {
818  const int& n = Mat3<T>::size; // should be 3
819  T temp;
821  double cotan_of_2_theta;
822  double tan_of_theta;
823  double cosin_of_theta;
824  double sin_of_theta;
825  double z;
826 
827  double Sij = S(i,j);
828 
829  double Sjj_minus_Sii = D[j] - D[i];
830 
831  if (fabs(Sjj_minus_Sii) * (10*tolerance<T>::value()) > fabs(Sij)) {
832  tan_of_theta = Sij / Sjj_minus_Sii;
833  } else {
835  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
836 
837  if (cotan_of_2_theta < 0.) {
838  tan_of_theta = -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
839  } else {
840  tan_of_theta = 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
841  }
842  }
843 
844  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
845  sin_of_theta = cosin_of_theta * tan_of_theta;
846  z = tan_of_theta * Sij;
847  S(i,j) = 0;
848  D[i] -= z;
849  D[j] += z;
850  for (int k = 0; k < i; ++k) {
851  temp = S(k,i);
852  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
853  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
854  }
855  for (int k = i+1; k < j; ++k) {
856  temp = S(i,k);
857  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
858  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
859  }
860  for (int k = j+1; k < n; ++k) {
861  temp = S(i,k);
862  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
863  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
864  }
865  for (int k = 0; k < n; ++k)
866  {
867  temp = Q(k,i);
868  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
869  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
870  }
871  }
872 }
873 
874 
879 template<typename T>
880 bool diagonalizeSymmetricMatrix(const Mat3<T>& input, Mat3<T>& Q, Vec3<T>& D, unsigned int MAX_ITERATIONS=250)
881 {
884  Q = Mat3<T>::identity();
885  int n = Mat3<T>::size; // should be 3
886 
888  Mat3<T> S(input);
889 
890  for (int i = 0; i < n; ++i) {
891  D[i] = S(i,i);
892  }
893 
894 
895  unsigned int iterations(0);
898  do {
901  double er = 0;
902  for (int i = 0; i < n; ++i) {
903  for (int j = i+1; j < n; ++j) {
904  er += fabs(S(i,j));
905  }
906  }
907  if (std::abs(er) < tolerance<T>::value()) {
908  return true;
909  }
910  iterations++;
911 
912  T max_element = 0;
913  int ip = 0;
914  int jp = 0;
916  for (int i = 0; i < n; ++i) {
917  for (int j = i+1; j < n; ++j){
918 
919  if ( fabs(D[i]) * (10*tolerance<T>::value()) > fabs(S(i,j))) {
921  S(i,j) = 0;
922  }
923  if (fabs(S(i,j)) > max_element) {
924  max_element = fabs(S(i,j));
925  ip = i;
926  jp = j;
927  }
928  }
929  }
930  pivot(ip, jp, S, D, Q);
931  } while (iterations < MAX_ITERATIONS);
932 
933  return false;
934 }
935 
936 
937 } // namespace math
938 } // namespace OPENVDB_VERSION_NAME
939 } // namespace openvdb
940 
941 
942 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
943 
944 // Copyright (c) 2012 DreamWorks Animation LLC
945 // All rights reserved. This software is distributed under the
946 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )