31 #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
32 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
37 #include <openvdb/Exceptions.h>
47 template<
typename T>
class Vec3;
48 template<
typename T>
class Mat4;
49 template<
typename T>
class Quat;
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)
93 template<
typename Source>
95 { setBasis(v1, v2, v3); }
101 template<
typename Source>
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];
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];
126 template<
typename Source>
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];
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];
162 MyBase::mm[i3+0] = v[0];
163 MyBase::mm[i3+1] = v[1];
164 MyBase::mm[i3+2] = v[2];
171 return Vec3<T>((*this)(i,0), (*
this)(i,1), (*
this)(i,2));
178 MyBase::mm[0+j] = v[0];
179 MyBase::mm[3+j] = v[1];
180 MyBase::mm[6+j] = v[2];
187 return Vec3<T>((*this)(0,j), (*
this)(1,j), (*
this)(2,j));
195 T* operator[](
int i) {
return &(MyBase::mm[i*3]); }
198 const T*
operator[](
int i)
const {
return &(MyBase::mm[i*3]); }
211 return MyBase::mm[3*i+j];
221 return MyBase::mm[3*i+j];
227 MyBase::mm[0] = v1[0];
228 MyBase::mm[1] = v1[1];
229 MyBase::mm[2] = v1[2];
230 MyBase::mm[3] = v2[0];
231 MyBase::mm[4] = v2[1];
232 MyBase::mm[5] = v2[2];
233 MyBase::mm[6] = v3[0];
234 MyBase::mm[7] = v3[1];
235 MyBase::mm[8] = v3[2];
241 MyBase::mm[0] = vdiag[0];
242 MyBase::mm[1] = vtri[0];
243 MyBase::mm[2] = vtri[1];
244 MyBase::mm[3] = vtri[0];
245 MyBase::mm[4] = vdiag[1];
246 MyBase::mm[5] = vtri[2];
247 MyBase::mm[6] = vtri[1];
248 MyBase::mm[7] = vtri[2];
249 MyBase::mm[8] = vdiag[2];
257 vdiag[0], vtri[0], vtri[1],
258 vtri[0], vdiag[1], vtri[2],
259 vtri[1], vtri[2], vdiag[2]
271 {*
this = rotation<Mat3<T> >(q);}
276 {*
this = rotation<Mat3<T> >(axis,
angle);}
307 template<
typename Source>
313 std::copy(src, (src + this->numElements()), MyBase::mm);
318 bool eq(
const Mat3 &m, T eps=1.0e-8)
const
335 -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
336 -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
337 -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
347 template <
typename S>
350 MyBase::mm[0] *= scalar;
351 MyBase::mm[1] *= scalar;
352 MyBase::mm[2] *= scalar;
353 MyBase::mm[3] *= scalar;
354 MyBase::mm[4] *= scalar;
355 MyBase::mm[5] *= scalar;
356 MyBase::mm[6] *= scalar;
357 MyBase::mm[7] *= scalar;
358 MyBase::mm[8] *= scalar;
363 template <
typename S>
368 MyBase::mm[0] += s[0];
369 MyBase::mm[1] += s[1];
370 MyBase::mm[2] += s[2];
371 MyBase::mm[3] += s[3];
372 MyBase::mm[4] += s[4];
373 MyBase::mm[5] += s[5];
374 MyBase::mm[6] += s[6];
375 MyBase::mm[7] += s[7];
376 MyBase::mm[8] += s[8];
381 template <
typename S>
386 MyBase::mm[0] -= s[0];
387 MyBase::mm[1] -= s[1];
388 MyBase::mm[2] -= s[2];
389 MyBase::mm[3] -= s[3];
390 MyBase::mm[4] -= s[4];
391 MyBase::mm[5] -= s[5];
392 MyBase::mm[6] -= s[6];
393 MyBase::mm[7] -= s[7];
394 MyBase::mm[8] -= s[8];
399 template <
typename S>
406 MyBase::mm[0] =
static_cast<T
>(s0[0] * s1[0] +
409 MyBase::mm[1] =
static_cast<T
>(s0[0] * s1[1] +
412 MyBase::mm[2] =
static_cast<T
>(s0[0] * s1[2] +
416 MyBase::mm[3] =
static_cast<T
>(s0[3] * s1[0] +
419 MyBase::mm[4] =
static_cast<T
>(s0[3] * s1[1] +
422 MyBase::mm[5] =
static_cast<T
>(s0[3] * s1[2] +
426 MyBase::mm[6] =
static_cast<T
>(s0[6] * s1[0] +
429 MyBase::mm[7] =
static_cast<T
>(s0[6] * s1[1] +
432 MyBase::mm[8] =
static_cast<T
>(s0[6] * s1[2] +
443 MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
444 MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
445 MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
446 MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
447 MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
448 MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
449 MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
450 MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
451 MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
458 MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
459 MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
460 MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
470 T det = inv.
mm[0]*MyBase::mm[0] + inv.
mm[1]*MyBase::mm[3] + inv.
mm[2]*MyBase::mm[6];
477 return inv * (T(1)/det);
483 T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
484 T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
485 T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
486 T d = MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
493 return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
502 return snapBasis(*
this, axis, direction);
507 template<
typename T0>
510 return static_cast< Vec3<T0> >(v * *
this);
515 template<
typename T0>
518 return static_cast< Vec3<T0> >(*
this * v);
525 template<
typename T0>
528 return snapBasis(*
this, axis, direction);
532 static const Mat3<T> sIdentity;
537 template <
typename T>
538 const Mat3<T> Mat3<T>::sIdentity = Mat3<T>(1, 0, 0,
542 template <
typename T>
543 const Mat3<T> Mat3<T>::sZero = Mat3<T>(0, 0, 0,
549 template <
typename T0,
typename T1>
555 for (
int i=0; i<9; ++i) {
563 template <
typename T0,
typename T1>
568 template <
typename S,
typename T>
574 template <
typename S,
typename T>
584 template <
typename T0,
typename T1>
594 template <
typename T0,
typename T1>
607 template <
typename T0,
typename T1>
617 template<
typename T,
typename MT>
618 inline Vec3<typename promote<T, MT>::type>
623 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
624 _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
625 _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
630 template<
typename T,
typename MT>
636 _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
637 _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
638 _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
643 template<
typename T,
typename MT>
653 template <
typename T>
659 Vec3<T>(v1[0]*v2[1], v1[1]*v2[1], v1[2]*v2[1]),
660 Vec3<T>(v1[0]*v2[2], v1[1]*v2[2], v1[2]*v2[2]));
668 #if DWREAL_IS_DOUBLE == 1
672 #endif // DWREAL_IS_DOUBLE
678 template<
typename T,
typename T0>
690 void pivot(
int i,
int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
692 const int& n = Mat3<T>::size;
695 double cotan_of_2_theta;
697 double cosin_of_theta;
703 double Sjj_minus_Sii = D[j] - D[i];
706 tan_of_theta = Sij / Sjj_minus_Sii;
709 cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
711 if (cotan_of_2_theta < 0.) {
713 -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
716 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
720 cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
721 sin_of_theta = cosin_of_theta * tan_of_theta;
722 z = tan_of_theta * Sij;
726 for (
int k = 0; k < i; ++k) {
728 S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
729 S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
731 for (
int k = i+1; k < j; ++k) {
733 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
734 S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
736 for (
int k = j+1; k < n; ++k) {
738 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
739 S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
741 for (
int k = 0; k < n; ++k)
744 Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
745 Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
758 unsigned int MAX_ITERATIONS=250)
768 for (
int i = 0; i < n; ++i) {
772 unsigned int iterations(0);
779 for (
int i = 0; i < n; ++i) {
780 for (
int j = i+1; j < n; ++j) {
793 for (
int i = 0; i < n; ++i) {
794 for (
int j = i+1; j < n; ++j){
800 if (fabs(S(i,j)) > max_element) {
801 max_element = fabs(S(i,j));
807 pivot(ip, jp, S, D, Q);
808 }
while (iterations < MAX_ITERATIONS);
817 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition: Mat3.h:175
Mat3< typename promote< S, T >::type > operator*(const Mat3< T > &m, S scalar)
Returns M, where for .
Definition: Mat3.h:575
const T * asPointer() const
Definition: Mat3.h:202
const Mat3< T > & operator-=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:382
const Mat3< T > & operator*=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:400
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition: Mat3.h:157
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition: Mat3.h:348
Mat3s Mat3f
Definition: Mat3.h:671
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:97
Mat3< double > Mat3d
Definition: Mat3.h:666
void setBasis(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:225
Definition: Exceptions.h:78
MatType skew(const Vec3< typename MatType::value_type > &skew)
Definition: Mat.h:689
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition: Mat3.h:239
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:431
Tolerance for floating-point comparison.
Definition: Math.h:116
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition: Mat3.h:168
Mat3()
Trivial constructor, the matrix is NOT initialized.
Definition: Mat3.h:62
const Mat3< T > & operator+=(const Mat3< S > &m1)
Returns m0, where for .
Definition: Mat3.h:364
void setIdentity()
Set "this" matrix to identity.
Definition: Mat3.h:293
Mat3(const Mat< 3, T > &m)
Copy constructor.
Definition: Mat3.h:116
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:595
4x4 -matrix class.
Definition: Mat3.h:48
T * asPointer()
Definition: Mat3.h:201
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition: Mat3.h:500
Mat3 adjoint() const
returns adjoint of m
Definition: Mat3.h:440
T det() const
Determinant of matrix.
Definition: Mat3.h:481
T & operator()(int i, int j)
Definition: Mat3.h:207
Mat< 3, T > MyBase
Definition: Mat3.h:60
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition: Mat3.h:516
#define OPENVDB_VERSION_NAME
Definition: version.h:45
const T * operator[](int i) const
Definition: Mat3.h:198
T mm[SIZE *SIZE]
Definition: Mat.h:145
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition: Mat3.h:332
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition: Mat3.h:184
T operator()(int i, int j) const
Definition: Mat3.h:217
T trace() const
Trace of matrix.
Definition: Mat3.h:491
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left)
Definition: Mat3.h:137
Mat3(Source *a)
Definition: Mat3.h:102
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition: Mat3.h:275
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition: Mat3.h:152
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Returns M, where for .
Definition: Mat3.h:569
Vec3< typename promote< T, MT >::type > operator*(const Mat3< MT > &_m, const Vec3< T > &_v)
Returns v, where for .
Definition: Mat3.h:619
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition: Mat3.h:147
3x3 matrix class.
Definition: Mat3.h:54
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition: Mat3.h:679
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:779
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:353
Mat3(const Quat< T > &q)
Definition: Mat3.h:66
T ValueType
Definition: Mat3.h:59
T value_type
Data type held by the matrix.
Definition: Mat3.h:58
Mat3 transpose() const
returns transpose of this
Definition: Mat3.h:455
Mat3 inverse(T tolerance=0) const
Definition: Mat3.h:466
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:757
static Mat3 symmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Definition: Mat3.h:254
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Returns M, where for .
Definition: Mat3.h:585
Mat3(Source a, Source b, Source c, Source d, Source e, Source f, Source g, Source h, Source i)
Constructor given array of elements, the ordering is in row major form:
Definition: Mat3.h:77
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3)
Construct matrix given basis vectors (columns)
Definition: Mat3.h:94
void setZero()
Set this matrix to zero.
Definition: Mat3.h:279
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition: Mat3.h:127
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:56
Axis
Definition: Math.h:762
void setToRotation(const Quat< T > &q)
Set this matrix to the rotation matrix specified by the quaternion.
Definition: Mat3.h:270
bool isApproxEqual(const Hermite &lhs, const Hermite &rhs)
Definition: Hermite.h:470
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition: Mat3.h:550
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition: Mat3.h:308
Mat3< float > Mat3s
Definition: Mat3.h:665
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition: Mat3.h:654
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:318
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Matrix multiplication.
Definition: Mat3.h:608
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:508
Mat3 snappedBasis(Axis axis, const Vec3< T0 > &direction) const
Definition: Mat3.h:526
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat3< MT > &_m)
Returns v, where for .
Definition: Mat3.h:632
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition: Mat3.h:564
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition: Mat3.h:264