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