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