OpenVDB  12.0.0
Mat3.h
Go to the documentation of this file.
1 // Copyright Contributors to the OpenVDB Project
2 // SPDX-License-Identifier: Apache-2.0
3 
4 #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
5 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
6 
7 #include <openvdb/Exceptions.h>
8 #include <openvdb/util/Assert.h>
9 #include "Vec3.h"
10 #include "Mat.h"
11 #include <algorithm> // for std::copy()
12 #include <cmath>
13 #include <iomanip>
14 
15 
16 namespace openvdb {
18 namespace OPENVDB_VERSION_NAME {
19 namespace math {
20 
21 template<typename T> class Vec3;
22 template<typename T> class Mat4;
23 template<typename T> class Quat;
24 
25 /// @class Mat3 Mat3.h
26 /// @brief 3x3 matrix class.
27 template<typename T>
28 class Mat3: public Mat<3, T>
29 {
30 public:
31  /// Data type held by the matrix.
32  using value_type = T;
33  using ValueType = T;
34  using MyBase = Mat<3, T>;
35 
36  /// Trivial constructor, the matrix is NOT initialized
37  /// @note destructor, copy constructor, assignment operator and
38  /// move constructor are left to be defined by the compiler (default)
39  Mat3() = default;
40 
41  /// Constructor given the quaternion rotation, e.g. Mat3f m(q);
42  /// The quaternion is normalized and used to construct the matrix
43  Mat3(const Quat<T> &q)
44  { setToRotation(q); }
45 
46 
47  /// Constructor given array of elements, the ordering is in row major form:
48  /** @verbatim
49  a b c
50  d e f
51  g h i
52  @endverbatim */
53  template<typename Source>
54  Mat3(Source a, Source b, Source c,
55  Source d, Source e, Source f,
56  Source g, Source h, Source i)
57  {
58  MyBase::mm[0] = static_cast<T>(a);
59  MyBase::mm[1] = static_cast<T>(b);
60  MyBase::mm[2] = static_cast<T>(c);
61  MyBase::mm[3] = static_cast<T>(d);
62  MyBase::mm[4] = static_cast<T>(e);
63  MyBase::mm[5] = static_cast<T>(f);
64  MyBase::mm[6] = static_cast<T>(g);
65  MyBase::mm[7] = static_cast<T>(h);
66  MyBase::mm[8] = static_cast<T>(i);
67  } // constructor1Test
68 
69  /// Construct matrix from rows or columns vectors (defaults to rows
70  /// for historical reasons)
71  template<typename Source>
72  Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true)
73  {
74  if (rows) {
75  this->setRows(v1, v2, v3);
76  } else {
77  this->setColumns(v1, v2, v3);
78  }
79  }
80 
81  /// Constructor given array of elements, the ordering is in row major form:\n
82  /// a[0] a[1] a[2]\n
83  /// a[3] a[4] a[5]\n
84  /// a[6] a[7] a[8]\n
85  template<typename Source>
86  Mat3(Source *a)
87  {
88  MyBase::mm[0] = static_cast<T>(a[0]);
89  MyBase::mm[1] = static_cast<T>(a[1]);
90  MyBase::mm[2] = static_cast<T>(a[2]);
91  MyBase::mm[3] = static_cast<T>(a[3]);
92  MyBase::mm[4] = static_cast<T>(a[4]);
93  MyBase::mm[5] = static_cast<T>(a[5]);
94  MyBase::mm[6] = static_cast<T>(a[6]);
95  MyBase::mm[7] = static_cast<T>(a[7]);
96  MyBase::mm[8] = static_cast<T>(a[8]);
97  } // constructor1Test
98 
99  /// Conversion constructor
100  template<typename Source>
101  explicit Mat3(const Mat3<Source> &m)
102  {
103  for (int i=0; i<3; ++i) {
104  for (int j=0; j<3; ++j) {
105  MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
106  }
107  }
108  }
109 
110  /// Conversion from Mat4 (copies top left)
111  explicit Mat3(const Mat4<T> &m)
112  {
113  for (int i=0; i<3; ++i) {
114  for (int j=0; j<3; ++j) {
115  MyBase::mm[i*3 + j] = m[i][j];
116  }
117  }
118  }
119 
120  /// Predefined constant for identity matrix
121  static const Mat3<T>& identity() {
122  static const Mat3<T> sIdentity = Mat3<T>(
123  1, 0, 0,
124  0, 1, 0,
125  0, 0, 1
126  );
127  return sIdentity;
128  }
129 
130  /// Predefined constant for zero matrix
131  static const Mat3<T>& zero() {
132  static const Mat3<T> sZero = Mat3<T>(
133  0, 0, 0,
134  0, 0, 0,
135  0, 0, 0
136  );
137  return sZero;
138  }
139 
140  /// Set ith row to vector v
141  void setRow(int i, const Vec3<T> &v)
142  {
143  OPENVDB_ASSERT(i>=0 && i<3);
144  int i3 = i * 3;
145 
146  MyBase::mm[i3+0] = v[0];
147  MyBase::mm[i3+1] = v[1];
148  MyBase::mm[i3+2] = v[2];
149  } // rowColumnTest
150 
151  /// Get ith row, e.g. Vec3d v = m.row(1);
152  Vec3<T> row(int i) const
153  {
154  OPENVDB_ASSERT(i>=0 && i<3);
155  return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
156  } // rowColumnTest
157 
158  /// Set jth column to vector v
159  void setCol(int j, const Vec3<T>& v)
160  {
161  OPENVDB_ASSERT(j>=0 && j<3);
162  MyBase::mm[0+j] = v[0];
163  MyBase::mm[3+j] = v[1];
164  MyBase::mm[6+j] = v[2];
165  } // rowColumnTest
166 
167  /// Get jth column, e.g. Vec3d v = m.col(0);
168  Vec3<T> col(int j) const
169  {
170  OPENVDB_ASSERT(j>=0 && j<3);
171  return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
172  } // rowColumnTest
173 
174  /// Alternative indexed reference to the elements
175  /// Note that the indices are row first and column second.
176  /// e.g. m(0,0) = 1;
177  T& operator()(int i, int j)
178  {
179  OPENVDB_ASSERT(i>=0 && i<3);
180  OPENVDB_ASSERT(j>=0 && j<3);
181  return MyBase::mm[3*i+j];
182  } // trivial
183 
184  /// Alternative indexed constant reference to the elements,
185  /// Note that the indices are row first and column second.
186  /// e.g. float f = m(1,0);
187  T operator()(int i, int j) const
188  {
189  OPENVDB_ASSERT(i>=0 && i<3);
190  OPENVDB_ASSERT(j>=0 && j<3);
191  return MyBase::mm[3*i+j];
192  } // trivial
193 
194  /// Set the rows of this matrix to the vectors v1, v2, v3
195  void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
196  {
197  MyBase::mm[0] = v1[0];
198  MyBase::mm[1] = v1[1];
199  MyBase::mm[2] = v1[2];
200  MyBase::mm[3] = v2[0];
201  MyBase::mm[4] = v2[1];
202  MyBase::mm[5] = v2[2];
203  MyBase::mm[6] = v3[0];
204  MyBase::mm[7] = v3[1];
205  MyBase::mm[8] = v3[2];
206  } // setRows
207 
208  /// Set the columns of this matrix to the vectors v1, v2, v3
209  void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
210  {
211  MyBase::mm[0] = v1[0];
212  MyBase::mm[1] = v2[0];
213  MyBase::mm[2] = v3[0];
214  MyBase::mm[3] = v1[1];
215  MyBase::mm[4] = v2[1];
216  MyBase::mm[5] = v3[1];
217  MyBase::mm[6] = v1[2];
218  MyBase::mm[7] = v2[2];
219  MyBase::mm[8] = v3[2];
220  } // setColumns
221 
222  /// Set diagonal and symmetric triangular components
223  void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
224  {
225  MyBase::mm[0] = vdiag[0];
226  MyBase::mm[1] = vtri[0];
227  MyBase::mm[2] = vtri[1];
228  MyBase::mm[3] = vtri[0];
229  MyBase::mm[4] = vdiag[1];
230  MyBase::mm[5] = vtri[2];
231  MyBase::mm[6] = vtri[1];
232  MyBase::mm[7] = vtri[2];
233  MyBase::mm[8] = vdiag[2];
234  } // setSymmetricTest
235 
236  /// Return a matrix with the prescribed diagonal and symmetric triangular components.
237  static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
238  {
239  return Mat3(
240  vdiag[0], vtri[0], vtri[1],
241  vtri[0], vdiag[1], vtri[2],
242  vtri[1], vtri[2], vdiag[2]
243  );
244  }
245 
246  /// Set the matrix as cross product of the given vector
247  void setSkew(const Vec3<T> &v)
248  {*this = skew(v);}
249 
250  /// @brief Set this matrix to the rotation matrix specified by the quaternion
251  /// @details The quaternion is normalized and used to construct the matrix.
252  /// Note that the matrix is transposed to match post-multiplication semantics.
253  void setToRotation(const Quat<T> &q)
254  {*this = rotation<Mat3<T> >(q);}
255 
256  /// @brief Set this matrix to the rotation specified by @a axis and @a angle
257  /// @details The axis must be unit vector
258  void setToRotation(const Vec3<T> &axis, T angle)
259  {*this = rotation<Mat3<T> >(axis, angle);}
260 
261  /// Set this matrix to zero
262  void setZero()
263  {
264  MyBase::mm[0] = 0;
265  MyBase::mm[1] = 0;
266  MyBase::mm[2] = 0;
267  MyBase::mm[3] = 0;
268  MyBase::mm[4] = 0;
269  MyBase::mm[5] = 0;
270  MyBase::mm[6] = 0;
271  MyBase::mm[7] = 0;
272  MyBase::mm[8] = 0;
273  } // trivial
274 
275  /// Set this matrix to identity
276  void setIdentity()
277  {
278  MyBase::mm[0] = 1;
279  MyBase::mm[1] = 0;
280  MyBase::mm[2] = 0;
281  MyBase::mm[3] = 0;
282  MyBase::mm[4] = 1;
283  MyBase::mm[5] = 0;
284  MyBase::mm[6] = 0;
285  MyBase::mm[7] = 0;
286  MyBase::mm[8] = 1;
287  } // trivial
288 
289  /// Assignment operator
290  template<typename Source>
291  const Mat3& operator=(const Mat3<Source> &m)
292  {
293  const Source *src = m.asPointer();
294 
295  // don't suppress type conversion warnings
296  std::copy(src, (src + this->numElements()), MyBase::mm);
297  return *this;
298  } // opEqualToTest
299 
300  /// Return @c true if this matrix is equivalent to @a m within a tolerance of @a eps.
301  bool eq(const Mat3 &m, T eps=1.0e-8) const
302  {
303  return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
304  isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
305  isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
306  isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
307  isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
308  isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
309  isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
310  isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
311  isApproxEqual(MyBase::mm[8],m.mm[8],eps));
312  } // trivial
313 
314  /// Negation operator, for e.g. m1 = -m2;
316  {
317  return Mat3<T>(
318  -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
319  -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
320  -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
321  );
322  } // trivial
323 
324  /// Multiplication operator, e.g. M = scalar * M;
325  // friend Mat3 operator*(T scalar, const Mat3& m) {
326  // return m*scalar;
327  // }
328 
329  /// Multiply each element of this matrix by @a scalar.
330  template <typename S>
331  const Mat3<T>& operator*=(S scalar)
332  {
333  MyBase::mm[0] *= scalar;
334  MyBase::mm[1] *= scalar;
335  MyBase::mm[2] *= scalar;
336  MyBase::mm[3] *= scalar;
337  MyBase::mm[4] *= scalar;
338  MyBase::mm[5] *= scalar;
339  MyBase::mm[6] *= scalar;
340  MyBase::mm[7] *= scalar;
341  MyBase::mm[8] *= scalar;
342  return *this;
343  }
344 
345  /// Add each element of the given matrix to the corresponding element of this matrix.
346  template <typename S>
347  const Mat3<T> &operator+=(const Mat3<S> &m1)
348  {
349  const S *s = m1.asPointer();
350 
351  MyBase::mm[0] += s[0];
352  MyBase::mm[1] += s[1];
353  MyBase::mm[2] += s[2];
354  MyBase::mm[3] += s[3];
355  MyBase::mm[4] += s[4];
356  MyBase::mm[5] += s[5];
357  MyBase::mm[6] += s[6];
358  MyBase::mm[7] += s[7];
359  MyBase::mm[8] += s[8];
360  return *this;
361  }
362 
363  /// Subtract each element of the given matrix from the corresponding element of this matrix.
364  template <typename S>
365  const Mat3<T> &operator-=(const Mat3<S> &m1)
366  {
367  const S *s = m1.asPointer();
368 
369  MyBase::mm[0] -= s[0];
370  MyBase::mm[1] -= s[1];
371  MyBase::mm[2] -= s[2];
372  MyBase::mm[3] -= s[3];
373  MyBase::mm[4] -= s[4];
374  MyBase::mm[5] -= s[5];
375  MyBase::mm[6] -= s[6];
376  MyBase::mm[7] -= s[7];
377  MyBase::mm[8] -= s[8];
378  return *this;
379  }
380 
381  /// Multiply this matrix by the given matrix.
382  template <typename S>
383  const Mat3<T> &operator*=(const Mat3<S> &m1)
384  {
385  Mat3<T> m0(*this);
386  const T* s0 = m0.asPointer();
387  const S* s1 = m1.asPointer();
388 
389  MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
390  s0[1] * s1[3] +
391  s0[2] * s1[6]);
392  MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
393  s0[1] * s1[4] +
394  s0[2] * s1[7]);
395  MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
396  s0[1] * s1[5] +
397  s0[2] * s1[8]);
398 
399  MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
400  s0[4] * s1[3] +
401  s0[5] * s1[6]);
402  MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
403  s0[4] * s1[4] +
404  s0[5] * s1[7]);
405  MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
406  s0[4] * s1[5] +
407  s0[5] * s1[8]);
408 
409  MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
410  s0[7] * s1[3] +
411  s0[8] * s1[6]);
412  MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
413  s0[7] * s1[4] +
414  s0[8] * s1[7]);
415  MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
416  s0[7] * s1[5] +
417  s0[8] * s1[8]);
418 
419  return *this;
420  }
421 
422  /// @brief Return the cofactor matrix of this matrix.
423  Mat3 cofactor() const
424  {
425  return Mat3<T>(
426  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
427  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
428  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
429  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
430  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
431  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
432  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
433  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
434  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
435  }
436 
437  /// Return the adjoint of this matrix, i.e., the transpose of its cofactor.
438  Mat3 adjoint() const
439  {
440  return Mat3<T>(
441  MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
442  MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
443  MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
444  MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
445  MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
446  MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
447  MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
448  MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
449  MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
450 
451  } // adjointTest
452 
453  /// returns transpose of this
454  Mat3 transpose() const
455  {
456  return Mat3<T>(
457  MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
458  MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
459  MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
460 
461  } // transposeTest
462 
463  /// returns inverse of this
464  /// @throws ArithmeticError if singular
465  Mat3 inverse(T tolerance = 0) const
466  {
467  Mat3<T> inv(this->adjoint());
468 
469  const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
470 
471  // If the determinant is 0, m was singular and the result will be invalid.
472  if (isApproxEqual(det,T(0.0),tolerance)) {
473  OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
474  }
475  return inv * (T(1)/det);
476  } // invertTest
477 
478  /// Determinant of matrix
479  T det() const
480  {
481  const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
482  const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
483  const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
484  return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
485  } // determinantTest
486 
487  /// Trace of matrix
488  T trace() const
489  {
490  return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
491  }
492 
493  /// This function snaps a specific axis to a specific direction,
494  /// preserving scaling. It does this using minimum energy, thus
495  /// posing a unique solution if basis & direction arent parralel.
496  /// Direction need not be unit.
497  Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
498  {
499  return snapMatBasis(*this, axis, direction);
500  }
501 
502  /// Return the transformed vector by this matrix.
503  /// This function is equivalent to post-multiplying the matrix.
504  template<typename T0>
505  Vec3<T0> transform(const Vec3<T0> &v) const
506  {
507  return static_cast< Vec3<T0> >(v * *this);
508  } // xformVectorTest
509 
510  /// Return the transformed vector by transpose of this matrix.
511  /// This function is equivalent to pre-multiplying the matrix.
512  template<typename T0>
514  {
515  return static_cast< Vec3<T0> >(*this * v);
516  } // xformTVectorTest
517 
518 
519  /// @brief Treat @a diag as a diagonal matrix and return the product
520  /// of this matrix with @a diag (from the right).
521  Mat3 timesDiagonal(const Vec3<T>& diag) const
522  {
523  Mat3 ret(*this);
524 
525  ret.mm[0] *= diag(0);
526  ret.mm[1] *= diag(1);
527  ret.mm[2] *= diag(2);
528  ret.mm[3] *= diag(0);
529  ret.mm[4] *= diag(1);
530  ret.mm[5] *= diag(2);
531  ret.mm[6] *= diag(0);
532  ret.mm[7] *= diag(1);
533  ret.mm[8] *= diag(2);
534  return ret;
535  }
536 }; // class Mat3
537 
538 
539 /// @relates Mat3
540 /// @brief Equality operator, does exact floating point comparisons
541 template <typename T0, typename T1>
542 bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
543 {
544  const T0 *t0 = m0.asPointer();
545  const T1 *t1 = m1.asPointer();
546 
547  for (int i=0; i<9; ++i) {
548  if (!isExactlyEqual(t0[i], t1[i])) return false;
549  }
550  return true;
551 }
552 
553 /// @relates Mat3
554 /// @brief Inequality operator, does exact floating point comparisons
555 template <typename T0, typename T1>
556 bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
557 
558 /// @relates Mat3
559 /// @brief Multiply each element of the given matrix by @a scalar and return the result.
560 template <typename S, typename T>
562 { return m*scalar; }
563 
564 /// @relates Mat3
565 /// @brief Multiply each element of the given matrix by @a scalar and return the result.
566 template <typename S, typename T>
568 {
570  result *= scalar;
571  return result;
572 }
573 
574 /// @relates Mat3
575 /// @brief Add corresponding elements of @a m0 and @a m1 and return the result.
576 template <typename T0, typename T1>
578 {
580  result += m1;
581  return result;
582 }
583 
584 /// @relates Mat3
585 /// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result.
586 template <typename T0, typename T1>
588 {
590  result -= m1;
591  return result;
592 }
593 
594 
595 /// @brief Multiply @a m0 by @a m1 and return the resulting matrix.
596 template <typename T0, typename T1>
598 {
600  result *= m1;
601  return result;
602 }
603 
604 /// @relates Mat3
605 /// @brief Multiply @a _m by @a _v and return the resulting vector.
606 template<typename T, typename MT>
608 operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
609 {
610  MT const *m = _m.asPointer();
612  _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
613  _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
614  _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
615 }
616 
617 /// @relates Mat3
618 /// @brief Multiply @a _v by @a _m and return the resulting vector.
619 template<typename T, typename MT>
621 operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
622 {
623  MT const *m = _m.asPointer();
625  _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
626  _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
627  _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
628 }
629 
630 /// @relates Mat3
631 /// @brief Multiply @a _v by @a _m and replace @a _v with the resulting vector.
632 template<typename T, typename MT>
633 inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
634 {
635  Vec3<T> mult = _v * _m;
636  _v = mult;
637  return _v;
638 }
639 
640 /// Returns outer product of v1, v2, i.e. v1 v2^T if v1 and v2 are
641 /// column vectors, e.g. M = Mat3f::outerproduct(v1,v2);
642 template <typename T>
643 Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
644 {
645  return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
646  v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
647  v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
648 }// outerProduct
649 
650 
651 /// Interpolate the rotation between m1 and m2 using Mat::powSolve.
652 /// Unlike slerp, translation is not treated independently.
653 /// This results in smoother animation results.
654 template<typename T, typename T0>
655 Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
656 {
657  Mat3<T> x = m1.inverse() * m2;
658  powSolve(x, x, t);
659  Mat3<T> m = m1 * x;
660  return m;
661 }
662 
663 
664 namespace mat3_internal {
665 
666 template<typename T>
667 inline void
668 pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
669 {
670  const int& n = Mat3<T>::size; // should be 3
671  T temp;
672  /// scratch variables used in pivoting
673  double cotan_of_2_theta;
674  double tan_of_theta;
675  double cosin_of_theta;
676  double sin_of_theta;
677  double z;
678 
679  double Sij = S(i,j);
680 
681  double Sjj_minus_Sii = D[j] - D[i];
682 
683  if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
684  tan_of_theta = Sij / Sjj_minus_Sii;
685  } else {
686  /// pivot on Sij
687  cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
688 
689  if (cotan_of_2_theta < 0.) {
690  tan_of_theta =
691  -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
692  } else {
693  tan_of_theta =
694  1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
695  }
696  }
697 
698  cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
699  sin_of_theta = cosin_of_theta * tan_of_theta;
700  z = tan_of_theta * Sij;
701  S(i,j) = 0;
702  D[i] -= z;
703  D[j] += z;
704  for (int k = 0; k < i; ++k) {
705  temp = S(k,i);
706  S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
707  S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
708  }
709  for (int k = i+1; k < j; ++k) {
710  temp = S(i,k);
711  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
712  S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
713  }
714  for (int k = j+1; k < n; ++k) {
715  temp = S(i,k);
716  S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
717  S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
718  }
719  for (int k = 0; k < n; ++k)
720  {
721  temp = Q(k,i);
722  Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
723  Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
724  }
725 }
726 
727 } // namespace mat3_internal
728 
729 
730 /// @brief Use Jacobi iterations to decompose a symmetric 3x3 matrix
731 /// (diagonalize and compute eigenvectors)
732 /// @details This is based on the "Efficient numerical diagonalization of Hermitian 3x3 matrices"
733 /// Joachim Kopp. arXiv.org preprint: physics/0610206
734 /// with the addition of largest pivot
735 template<typename T>
736 inline bool
738  unsigned int MAX_ITERATIONS=250)
739 {
740  /// use Givens rotation matrix to eliminate off-diagonal entries.
741  /// initialize the rotation matrix as idenity
742  Q = Mat3<T>::identity();
743  int n = Mat3<T>::size; // should be 3
744 
745  /// temp matrix. Assumed to be symmetric
746  Mat3<T> S(input);
747 
748  for (int i = 0; i < n; ++i) {
749  D[i] = S(i,i);
750  }
751 
752  unsigned int iterations(0);
753  /// Just iterate over all the non-diagonal enteries
754  /// using the largest as a pivot.
755  do {
756  /// check for absolute convergence
757  /// are symmetric off diagonals all zero
758  double er = 0;
759  for (int i = 0; i < n; ++i) {
760  for (int j = i+1; j < n; ++j) {
761  er += fabs(S(i,j));
762  }
763  }
764  if (std::abs(er) < math::Tolerance<T>::value()) {
765  return true;
766  }
767  iterations++;
768 
769  T max_element = 0;
770  int ip = 0;
771  int jp = 0;
772  /// loop over all the off-diagonals above the diagonal
773  for (int i = 0; i < n; ++i) {
774  for (int j = i+1; j < n; ++j){
775 
776  if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
777  /// value too small to pivot on
778  S(i,j) = 0;
779  }
780  if (fabs(S(i,j)) > max_element) {
781  max_element = fabs(S(i,j));
782  ip = i;
783  jp = j;
784  }
785  }
786  }
787  mat3_internal::pivot(ip, jp, S, D, Q);
788  } while (iterations < MAX_ITERATIONS);
789 
790  return false;
791 }
792 
793 template<typename T>
794 inline Mat3<T>
795 Abs(const Mat3<T>& m)
796 {
797  Mat3<T> out;
798  const T* ip = m.asPointer();
799  T* op = out.asPointer();
800  for (unsigned i = 0; i < 9; ++i, ++op, ++ip) *op = math::Abs(*ip);
801  return out;
802 }
803 
804 template<typename Type1, typename Type2>
805 inline Mat3<Type1>
806 cwiseAdd(const Mat3<Type1>& m, const Type2 s)
807 {
808  Mat3<Type1> out;
809  const Type1* ip = m.asPointer();
810  Type1* op = out.asPointer();
811  for (unsigned i = 0; i < 9; ++i, ++op, ++ip) {
813  *op = *ip + s;
815  }
816  return out;
817 }
818 
819 template<typename T>
820 inline bool
821 cwiseLessThan(const Mat3<T>& m0, const Mat3<T>& m1)
822 {
823  return cwiseLessThan<3, T>(m0, m1);
824 }
825 
826 template<typename T>
827 inline bool
828 cwiseGreaterThan(const Mat3<T>& m0, const Mat3<T>& m1)
829 {
830  return cwiseGreaterThan<3, T>(m0, m1);
831 }
832 
835 using Mat3f = Mat3d;
836 
839 
840 } // namespace math
841 
842 
843 template<> inline math::Mat3s zeroVal<math::Mat3s>() { return math::Mat3s::zero(); }
844 template<> inline math::Mat3d zeroVal<math::Mat3d>() { return math::Mat3d::zero(); }
845 
846 } // namespace OPENVDB_VERSION_NAME
847 } // namespace openvdb
848 
849 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
Bracket code with OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN/_END, to inhibit warnings about type conve...
Definition: Platform.h:221
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:291
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:443
Mat3< double > Mat3d
Definition: Mat3.h:834
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:822
Tolerance for floating-point comparison.
Definition: Math.h:148
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:209
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:74
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:621
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:195
Mat3(const Quat< T > &q)
Definition: Mat3.h:43
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:141
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:54
3x3 matrix class.
Definition: Mat3.h:28
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:121
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:159
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:505
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition: Math.h:406
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:454
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:223
Definition: Mat.h:164
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:561
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:315
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:542
OutGridT XformOp & op
Definition: ValueTransformer.h:139
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:708
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_END
Definition: Platform.h:222
Mat3 adjoint() const
Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Definition: Mat3.h:438
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:131
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:597
Mat3< float > Mat3s
Definition: Mat3.h:833
void setZero()
Set this matrix to zero.
Definition: Mat3.h:262
Definition: Mat.h:26
4x4 -matrix class.
Definition: Mat3.h:22
T value_type
Definition: Mat.h:29
bool cwiseLessThan(const Mat3< T > &m0, const Mat3< T > &m1)
Definition: Mat3.h:821
T & operator()(int i, int j)
Definition: Mat3.h:177
#define OPENVDB_ASSERT(X)
Definition: Assert.h:41
Axis
Definition: Math.h:901
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:168
Mat3< Type1 > cwiseAdd(const Mat3< Type1 > &m, const Type2 s)
Definition: Mat3.h:806
T det() const
Determinant of matrix.
Definition: Mat3.h:479
T mm[SIZE *SIZE]
Definition: Mat.h:160
T operator()(int i, int j) const
Definition: Mat3.h:187
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:237
Definition: Exceptions.h:13
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:365
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:655
constexpr T zeroVal()
Return the value of type T that corresponds to zero.
Definition: Math.h:70
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:513
T trace() const
Trace of matrix.
Definition: Mat3.h:488
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3, bool rows=true)
Definition: Mat3.h:72
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:577
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition: Mat3.h:668
Definition: Mat.h:165
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:152
Mat3 cofactor() const
Return the cofactor matrix of this matrix.
Definition: Mat3.h:423
Mat3< T > Abs(const Mat3< T > &m)
Definition: Mat3.h:795
T ValueType
Definition: Mat.h:30
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:301
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:111
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:567
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:465
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:331
Definition: Exceptions.h:56
void setIdentity()
Set this matrix to identity.
Definition: Mat3.h:276
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:556
Mat3(Source *a)
Definition: Mat3.h:86
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:737
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:751
bool cwiseGreaterThan(const Mat3< T > &m0, const Mat3< T > &m1)
Definition: Mat3.h:828
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:608
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:258
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:247
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h.in:121
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:587
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:253
const Mat3< T > & operator*=(const Mat3< S > &m1)
Multiply this matrix by the given matrix.
Definition: Mat3.h:383
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition: Mat3.h:497
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:521
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:643
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:347
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:101
T * asPointer()
Direct access to the internal data.
Definition: Mat.h:101
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h.in:218
#define OPENVDB_IS_POD(Type)
Definition: Math.h:56