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));
198 const T*
operator[](
int i)
const {
return &(MyBase::mm[i*3]); }
208 T& operator()(
int i,
int j)
212 return MyBase::mm[3*i+j];
218 T operator()(
int i,
int j)
const
222 return MyBase::mm[3*i+j];
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];
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];
258 vdiag[0], vtri[0], vtri[1],
259 vtri[0], vdiag[1], vtri[2],
260 vtri[1], vtri[2], vdiag[2]
273 {*
this = rotation<Mat3<T> >(q);}
278 {*
this = rotation<Mat3<T> >(axis,
angle);}
309 template<
typename Source>
315 std::copy(src, (src + this->numElements()), MyBase::mm);
320 bool eq(
const Mat3 &m, T eps=1.0e-8)
const
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]
349 template <
typename S>
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;
365 template <
typename S>
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];
383 template <
typename S>
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];
401 template <
typename S>
408 MyBase::mm[0] =
static_cast<T
>(s0[0] * s1[0] +
411 MyBase::mm[1] =
static_cast<T
>(s0[0] * s1[1] +
414 MyBase::mm[2] =
static_cast<T
>(s0[0] * s1[2] +
418 MyBase::mm[3] =
static_cast<T
>(s0[3] * s1[0] +
421 MyBase::mm[4] =
static_cast<T
>(s0[3] * s1[1] +
424 MyBase::mm[5] =
static_cast<T
>(s0[3] * s1[2] +
428 MyBase::mm[6] =
static_cast<T
>(s0[6] * s1[0] +
431 MyBase::mm[7] =
static_cast<T
>(s0[6] * s1[1] +
434 MyBase::mm[8] =
static_cast<T
>(s0[6] * s1[2] +
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]);
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]);
472 T det = inv.
mm[0]*MyBase::mm[0] + inv.
mm[1]*MyBase::mm[3] + inv.
mm[2]*MyBase::mm[6];
479 return inv * (T(1)/det);
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;
495 return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
504 return snapBasis(*
this, axis, direction);
509 template<
typename T0>
512 return static_cast< Vec3<T0> >(v * *
this);
517 template<
typename T0>
520 return static_cast< Vec3<T0> >(*
this * v);
527 template<
typename T0>
530 return snapBasis(*
this, axis, direction);
538 #ifdef ALLOW_CAST_TO_POINTER
541 #
if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
542 __attribute__ ((deprecated))
544 {
return MyBase::mm; }
546 #
if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4))
547 __attribute__ ((deprecated))
549 {
return MyBase::mm; }
555 template<
typename T0,
typename T1>
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];
576 template<
typename T0,
typename T1>
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];
596 template<
typename T0,
typename T1>
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];
616 template<
typename T0,
typename T1>
624 MyBase::mm[0] =
static_cast<T
>(src1[0] * src2[0] +
627 MyBase::mm[1] =
static_cast<T
>(src1[0] * src2[1] +
630 MyBase::mm[2] =
static_cast<T
>(src1[0] * src2[2] +
634 MyBase::mm[3] =
static_cast<T
>(src1[3] * src2[0] +
637 MyBase::mm[4] =
static_cast<T
>(src1[3] * src2[1] +
640 MyBase::mm[5] =
static_cast<T
>(src1[3] * src2[2] +
644 MyBase::mm[6] =
static_cast<T
>(src1[6] * src2[0] +
647 MyBase::mm[7] =
static_cast<T
>(src1[6] * src2[1] +
650 MyBase::mm[8] =
static_cast<T
>(src1[6] * src2[2] +
658 static const Mat3<T> sIdentity;
663 template <
typename T>
664 const Mat3<T> Mat3<T>::sIdentity = Mat3<T>(1, 0, 0,
668 template <
typename T>
669 const Mat3<T> Mat3<T>::sZero = Mat3<T>(0, 0, 0,
675 template <
typename T0,
typename T1>
681 for (
int i=0; i<9; ++i) {
689 template <
typename T0,
typename T1>
694 template <
typename S,
typename T>
700 template <
typename S,
typename T>
710 template <
typename T0,
typename T1>
720 template <
typename T0,
typename T1>
733 template <
typename T0,
typename T1>
743 template<
typename T,
typename MT>
744 inline Vec3<typename promote<T, MT>::type>
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]);
756 template<
typename T,
typename MT>
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]);
769 template<
typename T,
typename MT>
779 template <
typename T>
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]));
794 #if DWREAL_IS_DOUBLE == 1
798 #endif // DWREAL_IS_DOUBLE
804 template<
typename T,
typename T0>
816 void pivot(
int i,
int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
818 const int& n = Mat3<T>::size;
821 double cotan_of_2_theta;
823 double cosin_of_theta;
829 double Sjj_minus_Sii = D[j] - D[i];
831 if (fabs(Sjj_minus_Sii) * (10*tolerance<T>::value()) > fabs(Sij)) {
832 tan_of_theta = Sij / Sjj_minus_Sii;
835 cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
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);
840 tan_of_theta = 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
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;
850 for (
int k = 0; k < i; ++k) {
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);
855 for (
int k = i+1; k < j; ++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);
860 for (
int k = j+1; k < n; ++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);
865 for (
int k = 0; k < n; ++k)
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);
890 for (
int i = 0; i < n; ++i) {
895 unsigned int iterations(0);
902 for (
int i = 0; i < n; ++i) {
903 for (
int j = i+1; j < n; ++j) {
916 for (
int i = 0; i < n; ++i) {
917 for (
int j = i+1; j < n; ++j){
923 if (fabs(S(i,j)) > max_element) {
924 max_element = fabs(S(i,j));
930 pivot(ip, jp, S, D, Q);
931 }
while (iterations < MAX_ITERATIONS);
942 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED