4 #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
5 #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
21 template<
typename T>
class Vec3;
22 template<
typename T>
class Mat4;
23 template<
typename T>
class Quat;
50 template<
typename Source>
51 Mat3(Source a, Source b, Source c,
52 Source d, Source e, Source f,
53 Source g, Source h, Source i)
55 MyBase::mm[0] = static_cast<T>(a);
56 MyBase::mm[1] = static_cast<T>(b);
57 MyBase::mm[2] = static_cast<T>(c);
58 MyBase::mm[3] = static_cast<T>(d);
59 MyBase::mm[4] = static_cast<T>(e);
60 MyBase::mm[5] = static_cast<T>(f);
61 MyBase::mm[6] = static_cast<T>(g);
62 MyBase::mm[7] = static_cast<T>(h);
63 MyBase::mm[8] = static_cast<T>(i);
68 template<
typename Source>
72 this->setRows(v1, v2, v3);
74 this->setColumns(v1, v2, v3);
82 template<
typename Source>
85 MyBase::mm[0] = static_cast<T>(a[0]);
86 MyBase::mm[1] = static_cast<T>(a[1]);
87 MyBase::mm[2] = static_cast<T>(a[2]);
88 MyBase::mm[3] = static_cast<T>(a[3]);
89 MyBase::mm[4] = static_cast<T>(a[4]);
90 MyBase::mm[5] = static_cast<T>(a[5]);
91 MyBase::mm[6] = static_cast<T>(a[6]);
92 MyBase::mm[7] = static_cast<T>(a[7]);
93 MyBase::mm[8] = static_cast<T>(a[8]);
99 for (
int i=0; i<3; ++i) {
100 for (
int j=0; j<3; ++j) {
101 MyBase::mm[i*3 + j] = m[i][j];
107 template<
typename Source>
110 for (
int i=0; i<3; ++i) {
111 for (
int j=0; j<3; ++j) {
112 MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
120 for (
int i=0; i<3; ++i) {
121 for (
int j=0; j<3; ++j) {
122 MyBase::mm[i*3 + j] = m[i][j];
153 MyBase::mm[i3+0] = v[0];
154 MyBase::mm[i3+1] = v[1];
155 MyBase::mm[i3+2] = v[2];
162 return Vec3<T>((*
this)(i,0), (*
this)(i,1), (*
this)(i,2));
169 MyBase::mm[0+j] = v[0];
170 MyBase::mm[3+j] = v[1];
171 MyBase::mm[6+j] = v[2];
178 return Vec3<T>((*
this)(0,j), (*
this)(1,j), (*
this)(2,j));
186 T* operator[](
int i) {
return &(MyBase::mm[i*3]); }
189 const T*
operator[](
int i)
const {
return &(MyBase::mm[i*3]); }
202 return MyBase::mm[3*i+j];
212 return MyBase::mm[3*i+j];
218 MyBase::mm[0] = v1[0];
219 MyBase::mm[1] = v1[1];
220 MyBase::mm[2] = v1[2];
221 MyBase::mm[3] = v2[0];
222 MyBase::mm[4] = v2[1];
223 MyBase::mm[5] = v2[2];
224 MyBase::mm[6] = v3[0];
225 MyBase::mm[7] = v3[1];
226 MyBase::mm[8] = v3[2];
232 MyBase::mm[0] = v1[0];
233 MyBase::mm[1] = v2[0];
234 MyBase::mm[2] = v3[0];
235 MyBase::mm[3] = v1[1];
236 MyBase::mm[4] = v2[1];
237 MyBase::mm[5] = v3[1];
238 MyBase::mm[6] = v1[2];
239 MyBase::mm[7] = v2[2];
240 MyBase::mm[8] = v3[2];
246 MyBase::mm[0] = vdiag[0];
247 MyBase::mm[1] = vtri[0];
248 MyBase::mm[2] = vtri[1];
249 MyBase::mm[3] = vtri[0];
250 MyBase::mm[4] = vdiag[1];
251 MyBase::mm[5] = vtri[2];
252 MyBase::mm[6] = vtri[1];
253 MyBase::mm[7] = vtri[2];
254 MyBase::mm[8] = vdiag[2];
261 vdiag[0], vtri[0], vtri[1],
262 vtri[0], vdiag[1], vtri[2],
263 vtri[1], vtri[2], vdiag[2]
275 {*
this = rotation<Mat3<T> >(q);}
280 {*
this = rotation<Mat3<T> >(axis,
angle);}
311 template<
typename Source>
317 std::copy(src, (src + this->numElements()), MyBase::mm);
322 bool eq(
const Mat3 &m, T eps=1.0e-8)
const
339 -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
340 -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
341 -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
351 template <
typename S>
354 MyBase::mm[0] *= scalar;
355 MyBase::mm[1] *= scalar;
356 MyBase::mm[2] *= scalar;
357 MyBase::mm[3] *= scalar;
358 MyBase::mm[4] *= scalar;
359 MyBase::mm[5] *= scalar;
360 MyBase::mm[6] *= scalar;
361 MyBase::mm[7] *= scalar;
362 MyBase::mm[8] *= scalar;
367 template <
typename S>
372 MyBase::mm[0] += s[0];
373 MyBase::mm[1] += s[1];
374 MyBase::mm[2] += s[2];
375 MyBase::mm[3] += s[3];
376 MyBase::mm[4] += s[4];
377 MyBase::mm[5] += s[5];
378 MyBase::mm[6] += s[6];
379 MyBase::mm[7] += s[7];
380 MyBase::mm[8] += s[8];
385 template <
typename S>
390 MyBase::mm[0] -= s[0];
391 MyBase::mm[1] -= s[1];
392 MyBase::mm[2] -= s[2];
393 MyBase::mm[3] -= s[3];
394 MyBase::mm[4] -= s[4];
395 MyBase::mm[5] -= s[5];
396 MyBase::mm[6] -= s[6];
397 MyBase::mm[7] -= s[7];
398 MyBase::mm[8] -= s[8];
403 template <
typename S>
410 MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
413 MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
416 MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
420 MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
423 MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
426 MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
430 MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
433 MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
436 MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
447 MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
448 MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
449 MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
450 MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
451 MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
452 MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
453 MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
454 MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
455 MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
462 MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
463 MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
464 MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
465 MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
466 MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
467 MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
468 MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
469 MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
470 MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
478 MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
479 MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
480 MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
490 const T det = inv.
mm[0]*MyBase::mm[0] + inv.
mm[1]*MyBase::mm[3] + inv.
mm[2]*MyBase::mm[6];
496 return inv * (T(1)/det);
502 const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
503 const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
504 const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
505 return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
511 return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
525 template<
typename T0>
528 return static_cast< Vec3<T0> >(v * *
this);
533 template<
typename T0>
536 return static_cast< Vec3<T0> >(*
this * v);
546 ret.
mm[0] *= diag(0);
547 ret.
mm[1] *= diag(1);
548 ret.
mm[2] *= diag(2);
549 ret.
mm[3] *= diag(0);
550 ret.
mm[4] *= diag(1);
551 ret.
mm[5] *= diag(2);
552 ret.
mm[6] *= diag(0);
553 ret.
mm[7] *= diag(1);
554 ret.
mm[8] *= diag(2);
562 template <
typename T0,
typename T1>
568 for (
int i=0; i<9; ++i) {
576 template <
typename T0,
typename T1>
581 template <
typename S,
typename T>
587 template <
typename S,
typename T>
597 template <
typename T0,
typename T1>
607 template <
typename T0,
typename T1>
617 template <
typename T0,
typename T1>
627 template<
typename T,
typename MT>
633 _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
634 _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
635 _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
640 template<
typename T,
typename MT>
646 _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
647 _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
648 _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
653 template<
typename T,
typename MT>
663 template <
typename T>
666 return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
667 v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
668 v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
675 template<
typename T,
typename T0>
685 namespace mat3_internal {
694 double cotan_of_2_theta;
696 double cosin_of_theta;
702 double Sjj_minus_Sii = D[j] - D[i];
705 tan_of_theta = Sij / Sjj_minus_Sii;
708 cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
710 if (cotan_of_2_theta < 0.) {
712 -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
715 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
719 cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
720 sin_of_theta = cosin_of_theta * tan_of_theta;
721 z = tan_of_theta * Sij;
725 for (
int k = 0; k < i; ++k) {
727 S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
728 S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
730 for (
int k = i+1; k < j; ++k) {
732 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
733 S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
735 for (
int k = j+1; k < n; ++k) {
737 S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
738 S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
740 for (
int k = 0; k < n; ++k)
743 Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
744 Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
759 unsigned int MAX_ITERATIONS=250)
769 for (
int i = 0; i < n; ++i) {
773 unsigned int iterations(0);
780 for (
int i = 0; i < n; ++i) {
781 for (
int j = i+1; j < n; ++j) {
794 for (
int i = 0; i < n; ++i) {
795 for (
int j = i+1; j < n; ++j){
801 if (fabs(S(i,j)) > max_element) {
802 max_element = fabs(S(i,j));
809 }
while (iterations < MAX_ITERATIONS);
822 template<>
inline math::Mat3s zeroVal<math::Mat3s>() {
return math::Mat3s::zero(); }
823 template<>
inline math::Mat3d zeroVal<math::Mat3d>() {
return math::Mat3d::zero(); }
828 #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED