31 #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
32 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
34 #include <openvdb/Exceptions.h>
48 template<
typename T>
class Vec3;
49 template<
typename T>
class Mat4;
50 template<
typename T>
class Quat;
77 template<
typename Source>
78 Mat3(Source a, Source b, Source c,
79 Source d, Source e, Source f,
80 Source g, Source h, Source i)
82 MyBase::mm[0] = static_cast<ValueType>(a);
83 MyBase::mm[1] = static_cast<ValueType>(b);
84 MyBase::mm[2] = static_cast<ValueType>(c);
85 MyBase::mm[3] = static_cast<ValueType>(d);
86 MyBase::mm[4] = static_cast<ValueType>(e);
87 MyBase::mm[5] = static_cast<ValueType>(f);
88 MyBase::mm[6] = static_cast<ValueType>(g);
89 MyBase::mm[7] = static_cast<ValueType>(h);
90 MyBase::mm[8] = static_cast<ValueType>(i);
95 template<
typename Source>
99 this->setRows(v1, v2, v3);
101 this->setColumns(v1, v2, v3);
109 template<
typename Source>
112 MyBase::mm[0] = a[0];
113 MyBase::mm[1] = a[1];
114 MyBase::mm[2] = a[2];
115 MyBase::mm[3] = a[3];
116 MyBase::mm[4] = a[4];
117 MyBase::mm[5] = a[5];
118 MyBase::mm[6] = a[6];
119 MyBase::mm[7] = a[7];
120 MyBase::mm[8] = a[8];
126 for (
int i=0; i<3; ++i) {
127 for (
int j=0; j<3; ++j) {
128 MyBase::mm[i*3 + j] = m[i][j];
134 template<
typename Source>
137 for (
int i=0; i<3; ++i) {
138 for (
int j=0; j<3; ++j) {
139 MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
147 for (
int i=0; i<3; ++i) {
148 for (
int j=0; j<3; ++j) {
149 MyBase::mm[i*3 + j] = m[i][j];
180 MyBase::mm[i3+0] = v[0];
181 MyBase::mm[i3+1] = v[1];
182 MyBase::mm[i3+2] = v[2];
189 return Vec3<T>((*
this)(i,0), (*
this)(i,1), (*
this)(i,2));
196 MyBase::mm[0+j] = v[0];
197 MyBase::mm[3+j] = v[1];
198 MyBase::mm[6+j] = v[2];
205 return Vec3<T>((*
this)(0,j), (*
this)(1,j), (*
this)(2,j));
213 T* operator[](
int i) {
return &(MyBase::mm[i*3]); }
216 const T*
operator[](
int i)
const {
return &(MyBase::mm[i*3]); }
229 return MyBase::mm[3*i+j];
239 return MyBase::mm[3*i+j];
245 MyBase::mm[0] = v1[0];
246 MyBase::mm[1] = v1[1];
247 MyBase::mm[2] = v1[2];
248 MyBase::mm[3] = v2[0];
249 MyBase::mm[4] = v2[1];
250 MyBase::mm[5] = v2[2];
251 MyBase::mm[6] = v3[0];
252 MyBase::mm[7] = v3[1];
253 MyBase::mm[8] = v3[2];
259 MyBase::mm[0] = v1[0];
260 MyBase::mm[1] = v2[0];
261 MyBase::mm[2] = v3[0];
262 MyBase::mm[3] = v1[1];
263 MyBase::mm[4] = v2[1];
264 MyBase::mm[5] = v3[1];
265 MyBase::mm[6] = v1[2];
266 MyBase::mm[7] = v2[2];
267 MyBase::mm[8] = v3[2];
273 MyBase::mm[0] = vdiag[0];
274 MyBase::mm[1] = vtri[0];
275 MyBase::mm[2] = vtri[1];
276 MyBase::mm[3] = vtri[0];
277 MyBase::mm[4] = vdiag[1];
278 MyBase::mm[5] = vtri[2];
279 MyBase::mm[6] = vtri[1];
280 MyBase::mm[7] = vtri[2];
281 MyBase::mm[8] = vdiag[2];
288 vdiag[0], vtri[0], vtri[1],
289 vtri[0], vdiag[1], vtri[2],
290 vtri[1], vtri[2], vdiag[2]
302 {*
this = rotation<Mat3<T> >(q);}
307 {*
this = rotation<Mat3<T> >(axis,
angle);}
338 template<
typename Source>
344 std::copy(src, (src + this->numElements()), MyBase::mm);
349 bool eq(
const Mat3 &m, T eps=1.0e-8)
const
366 -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
367 -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
368 -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
378 template <
typename S>
381 MyBase::mm[0] *= scalar;
382 MyBase::mm[1] *= scalar;
383 MyBase::mm[2] *= scalar;
384 MyBase::mm[3] *= scalar;
385 MyBase::mm[4] *= scalar;
386 MyBase::mm[5] *= scalar;
387 MyBase::mm[6] *= scalar;
388 MyBase::mm[7] *= scalar;
389 MyBase::mm[8] *= scalar;
394 template <
typename S>
399 MyBase::mm[0] += s[0];
400 MyBase::mm[1] += s[1];
401 MyBase::mm[2] += s[2];
402 MyBase::mm[3] += s[3];
403 MyBase::mm[4] += s[4];
404 MyBase::mm[5] += s[5];
405 MyBase::mm[6] += s[6];
406 MyBase::mm[7] += s[7];
407 MyBase::mm[8] += s[8];
412 template <
typename S>
417 MyBase::mm[0] -= s[0];
418 MyBase::mm[1] -= s[1];
419 MyBase::mm[2] -= s[2];
420 MyBase::mm[3] -= s[3];
421 MyBase::mm[4] -= s[4];
422 MyBase::mm[5] -= s[5];
423 MyBase::mm[6] -= s[6];
424 MyBase::mm[7] -= s[7];
425 MyBase::mm[8] -= s[8];
430 template <
typename S>
437 MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
440 MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
443 MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
447 MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
450 MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
453 MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
457 MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
460 MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
463 MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
474 MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
475 MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
476 MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
477 MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
478 MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
479 MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
480 MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
481 MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
482 MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
489 MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
490 MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
491 MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
492 MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
493 MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
494 MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
495 MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
496 MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
497 MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
505 MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
506 MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
507 MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
517 const T det = inv.
mm[0]*MyBase::mm[0] + inv.
mm[1]*MyBase::mm[3] + inv.
mm[2]*MyBase::mm[6];
523 return inv * (T(1)/det);
529 const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
530 const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
531 const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
532 return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
538 return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
552 template<
typename T0>
555 return static_cast< Vec3<T0> >(v * *
this);
560 template<
typename T0>
563 return static_cast< Vec3<T0> >(*
this * v);
573 ret.
mm[0] *= diag(0);
574 ret.
mm[1] *= diag(1);
575 ret.
mm[2] *= diag(2);
576 ret.
mm[3] *= diag(0);
577 ret.
mm[4] *= diag(1);
578 ret.
mm[5] *= diag(2);
579 ret.
mm[6] *= diag(0);
580 ret.
mm[7] *= diag(1);
581 ret.
mm[8] *= diag(2);
589 template <
typename T0,
typename T1>
595 for (
int i=0; i<9; ++i) {
603 template <
typename T0,
typename T1>
608 template <
typename S,
typename T>
614 template <
typename S,
typename T>
624 template <
typename T0,
typename T1>
634 template <
typename T0,
typename T1>
644 template <
typename T0,
typename T1>
654 template<
typename T,
typename MT>
660 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
661 _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
662 _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
667 template<
typename T,
typename MT>
673 _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
674 _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
675 _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
680 template<
typename T,
typename MT>
690 template <
typename T>
693 return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
694 v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
695 v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
702 template<
typename T,
typename T0>
712 namespace mat3_internal {
721 double cotan_of_2_theta;
723 double cosin_of_theta;
729 double Sjj_minus_Sii = D[j] - D[i];
732 tan_of_theta = Sij / Sjj_minus_Sii;
735 cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
737 if (cotan_of_2_theta < 0.) {
739 -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
742 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
746 cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
747 sin_of_theta = cosin_of_theta * tan_of_theta;
748 z = tan_of_theta * Sij;
752 for (
int k = 0; k < i; ++k) {
754 S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
755 S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
757 for (
int k = i+1; k < j; ++k) {
759 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
760 S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
762 for (
int k = j+1; k < n; ++k) {
764 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
765 S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
767 for (
int k = 0; k < n; ++k)
770 Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
771 Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
786 unsigned int MAX_ITERATIONS=250)
796 for (
int i = 0; i < n; ++i) {
800 unsigned int iterations(0);
807 for (
int i = 0; i < n; ++i) {
808 for (
int j = i+1; j < n; ++j) {
821 for (
int i = 0; i < n; ++i) {
822 for (
int j = i+1; j < n; ++j){
828 if (fabs(S(i,j)) > max_element) {
829 max_element = fabs(S(i,j));
836 }
while (iterations < MAX_ITERATIONS);
849 template<>
inline math::Mat3s zeroVal<math::Mat3s>() {
return math::Mat3s::zero(); }
850 template<>
inline math::Mat3d zeroVal<math::Mat3d>() {
return math::Mat3d::zero(); }
855 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED