34 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
35 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
43 #include <boost/format.hpp>
44 #include <openvdb/Exceptions.h>
55 template<
unsigned SIZE,
typename T>
64 static unsigned numRows() {
return SIZE; }
74 for (
unsigned i(0); i < numElements(); ++i) {
89 str(
unsigned indentation = 0)
const {
95 indent.append(indentation+1,
' ');
100 for (
unsigned i(0); i < SIZE; i++) {
105 for (
unsigned j(0); j < SIZE; j++) {
108 if (j) ret.append(
", ");
109 ret.append((boost::format(
"%1%") % mm[(i*SIZE)+j]).str());
118 ret.append((boost::format(
",\n%1%") % indent).str());
135 void write(std::ostream& os)
const {
136 os.write(reinterpret_cast<const char*>(&mm),
sizeof(T)*SIZE*SIZE);
140 is.read(reinterpret_cast<char*>(&mm),
sizeof(T)*SIZE*SIZE);
149 template<
typename T>
class Quat;
150 template<
typename T>
class Vec3;
156 template<
class MatType>
158 typename MatType::value_type eps = 1.0e-8)
160 typedef typename MatType::value_type T;
183 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
184 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
185 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
187 if(MatType::numColumns() == 4)
padMat4(r);
196 template<
class MatType>
199 typedef typename MatType::value_type T;
200 T c =
static_cast<T
>(cos(angle));
201 T s =
static_cast<T
>(sin(angle));
204 result.setIdentity();
226 throw ValueError(
"Unrecognized rotation axis");
233 template<
class MatType>
236 typename MatType::value_type
angle)
238 typedef typename MatType::value_type T;
239 T txy, txz, tyz, sx, sy, sz;
244 T c(cos(
double(angle)));
245 T s(sin(
double(angle)));
250 result[0][0] = axis[0]*axis[0] * t + c;
251 result[1][1] = axis[1]*axis[1] * t + c;
252 result[2][2] = axis[2]*axis[2] * t + c;
254 txy = axis[0]*axis[1] * t;
257 txz = axis[0]*axis[2] * t;
260 tyz = axis[1]*axis[2] * t;
265 result[0][1] = txy + sz;
266 result[1][0] = txy - sz;
268 result[0][2] = txz - sy;
269 result[2][0] = txz + sy;
271 result[1][2] = tyz + sx;
272 result[2][1] = tyz - sx;
274 if(MatType::numColumns() == 4)
padMat4(result);
275 return MatType(result);
316 template <
class MatType>
320 typename MatType::value_type eps=1.0e-8)
322 typedef typename MatType::value_type ValueType;
324 ValueType phi, theta, psi;
326 switch(rotationOrder)
331 phi = 0.5 * atan2(mat[1][2], mat[1][1]);
335 phi = 0.5 * atan2(mat[1][2], mat[1][1]);
338 psi = atan2(-mat[1][0],mat[0][0]);
339 phi = atan2(-mat[2][1],mat[2][2]);
340 theta = atan2(mat[2][0],
341 sqrt( mat[2][1]*mat[2][1] +
342 mat[2][2]*mat[2][2]));
344 return V(phi, theta, psi);
348 phi = 0.5 * atan2(mat[0][1], mat[0][0]);
352 phi = 0.5 * atan2(mat[0][1],mat[2][1]);
355 psi = atan2(-mat[0][2], mat[2][2]);
356 phi = atan2(-mat[1][0], mat[1][1]);
357 theta = atan2(mat[1][2],
358 sqrt(mat[0][2] * mat[0][2] +
359 mat[2][2] * mat[2][2]));
361 return V(theta, psi, phi);
366 phi = 0.5 * atan2(mat[2][0], mat[2][2]);
370 phi = 0.5 * atan2(mat[2][0], mat[1][0]);
373 psi = atan2(-mat[2][1], mat[1][1]);
374 phi = atan2(-mat[0][2], mat[0][0]);
375 theta = atan2(mat[0][1],
376 sqrt(mat[0][0] * mat[0][0] +
377 mat[0][2] * mat[0][2]));
379 return V(psi, phi, theta);
385 phi = 0.5 * atan2(mat[1][2], mat[1][1]);
389 psi = 0.5 * atan2(mat[2][1], -mat[1][1]);
392 psi = atan2(mat[2][0], -mat[1][0]);
393 phi = atan2(mat[0][2], mat[0][1]);
394 theta = atan2(sqrt(mat[0][1] * mat[0][1] +
395 mat[0][2] * mat[0][2]),
398 return V(phi, psi, theta);
404 phi = 0.5 * atan2(mat[0][1], mat[0][0]);
408 phi = 0.5 * atan2(mat[0][1], mat[0][0]);
411 psi = atan2(mat[0][2], mat[1][2]);
412 phi = atan2(mat[2][0], -mat[2][1]);
413 theta = atan2(sqrt(mat[0][2] * mat[0][2] +
414 mat[1][2] * mat[1][2]),
417 return V(theta, psi, phi);
423 phi = 0.5 * atan2(-mat[1][0], mat[0][0]);
427 phi = 0.5 * atan2(mat[1][0], mat[0][0]);
430 psi = atan2(mat[0][1], mat[1][1]);
431 phi = atan2(mat[2][0], mat[2][2]);
432 theta = atan2(-mat[2][1],
433 sqrt(mat[0][1] * mat[0][1] +
434 mat[1][1] * mat[1][1]));
436 return V(theta, phi, psi);
442 phi = 0.5 * atan2(-mat[1][0], mat[1][1]);
446 phi = 0.5 * atan2(mat[2][1], mat[2][0]);
449 psi = atan2(mat[1][2], mat[2][2]);
450 phi = atan2(mat[0][1], mat[0][0]);
451 theta = atan2(-mat[0][2],
452 sqrt(mat[0][1] * mat[0][1] +
453 mat[0][0] * mat[0][0]));
455 return V(psi, theta, phi);
461 psi = 0.5 * atan2(mat[2][1], mat[2][2]);
465 psi = 0.5 * atan2(- mat[2][1], mat[2][2]);
468 psi = atan2(mat[2][0], mat[0][0]);
469 phi = atan2(mat[1][2], mat[1][1]);
470 theta = atan2(- mat[1][0],
471 sqrt(mat[1][1] * mat[1][1] +
472 mat[1][2] * mat[1][2]));
474 return V(phi, psi, theta);
483 template<
class MatType>
487 typename MatType::value_type eps=1.0e-8)
489 typedef typename MatType::value_type T;
518 Vec3<T> u, v, p(0.0, 0.0, 0.0);
520 double x =
Abs(v1[0]);
521 double y =
Abs(v1[1]);
522 double z =
Abs(v1[2]);
540 double udot = u.
dot(u);
541 double vdot = v.
dot(v);
543 double a = -2 / udot;
544 double b = -2 / vdot;
545 double c = 4 * u.
dot(v) / (udot * vdot);
548 result.setIdentity();
550 for (
int j = 0; j < 3; j++) {
551 for (
int i = 0; i < 3; i++)
553 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i];
559 if(MatType::numColumns() == 4)
padMat4(result);
563 double c = v1.
dot(v2);
564 double a = (1.0 - c) / cross.
dot(cross);
566 double a0 = a * cross[0];
567 double a1 = a * cross[1];
568 double a2 = a * cross[2];
570 double a01 = a0 * cross[1];
571 double a02 = a0 * cross[2];
572 double a12 = a1 * cross[2];
576 r[0][0] = c + a0 * cross[0];
577 r[0][1] = a01 + cross[2];
578 r[0][2] = a02 - cross[1],
579 r[1][0] = a01 - cross[2];
580 r[1][1] = c + a1 * cross[1];
581 r[1][2] = a12 + cross[0];
582 r[2][0] = a02 + cross[1];
583 r[2][1] = a12 - cross[0];
584 r[2][2] = c + a2 * cross[2];
586 if(MatType::numColumns() == 4)
padMat4(r);
594 template<
class MatType>
601 result.setIdentity();
602 result[0][0] = scaling[0];
603 result[1][1] = scaling[1];
604 result[2][2] = scaling[2];
612 template<
class MatType>
613 Vec3<typename MatType::value_type>
618 V(mat[0][0], mat[0][1], mat[0][2]).length(),
619 V(mat[1][0], mat[1][1], mat[1][2]).length(),
620 V(mat[2][0], mat[2][1], mat[2][2]).length());
627 template<
class MatType>
629 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
632 return unit(mat, eps, dud);
640 template<
class MatType>
644 typename MatType::value_type eps,
647 typedef typename MatType::value_type T;
650 for (
int i(0); i < 3; i++) {
653 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
654 for (
int j=0; j<3; j++) result[i][j] = u[j];
656 for (
int j=0; j<3; j++) result[i][j] = 0;
667 template <
class MatType>
671 int index0 =
static_cast<int>(axis0);
672 int index1 =
static_cast<int>(axis1);
675 result.setIdentity();
676 if (axis0 == axis1) {
677 result[index1][index0] = shear + 1;
679 result[index1][index0] =
shear;
687 template<
class MatType>
691 typedef typename MatType::value_type T;
694 r[0][0] = T(0); r[0][1] = skew.
z(); r[0][2] = -skew.
y();
695 r[1][0] = -skew.
z(); r[1][1] = T(0); r[2][1] = skew.
x();
696 r[2][0] = skew.
y(); r[2][1] = -skew.
x(); r[2][2] = T(0);
698 if(MatType::numColumns() == 4)
padMat4(r);
705 template<
class MatType>
710 typedef typename MatType::value_type T;
712 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
713 Vec3<T> up(forward.cross(horizontal).unit());
717 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
718 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
719 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
721 if(MatType::numColumns() == 4)
padMat4(r);
728 template<
class MatType>
732 dest[0][3] = dest[1][3] = dest[2][3] = 0;
733 dest[3][2] = dest[3][1] = dest[3][0] = 0;
743 template <
typename MatType>
745 sqrtSolve(
const MatType &aA, MatType &aB,
double aTol=0.01)
747 unsigned int iterations = (
unsigned int)(log(aTol)/log(0.5));
753 unsigned int current = 0;
756 Z[0] = MatType::identity();
758 unsigned int iteration;
759 for (iteration=0; iteration<iterations; iteration++)
761 unsigned int last = current;
764 invY = Y[last].inverse();
765 invZ = Z[last].inverse();
767 Y[current]=0.5*(Y[last]+invZ);
768 Z[current]=0.5*(Z[last]+invY);
771 MatType &R = Y[current];
777 template <
typename MatType>
779 powSolve(
const MatType &aA, MatType &aB,
double aPower,
double aTol=0.01)
781 unsigned int iterations = (
unsigned int)(log(aTol)/log(0.5));
783 const bool inverted = ( aPower < 0.0 );
789 unsigned int whole = (
unsigned int)aPower;
790 double fraction = aPower - whole;
793 R = MatType::identity();
795 MatType partial = aA;
797 double contribution = 1.0;
799 unsigned int iteration;
801 for (iteration=0; iteration< iterations; iteration++)
806 if (fraction>=contribution)
809 fraction-=contribution;
833 template <
typename MatType>
835 typedef typename MatType::ValueType value_type;
836 return m.eq(MatType::identity());
840 template <
typename MatType>
842 typedef typename MatType::ValueType value_type;
847 template <
typename MatType>
849 return m.eq(m.transpose());
853 template <
typename MatType>
855 typedef typename MatType::ValueType value_type;
856 if (!
isApproxEqual(fabs(m.det()), (value_type)1))
return false;
858 MatType temp = m * m.transpose();
859 return temp.eq(MatType::identity());
863 template <
typename MatType>
865 int n = MatType::size;
866 typename MatType::ValueType temp(0);
867 for (
int i = 0; i < n; ++i) {
868 for (
int j = 0; j < n; ++j) {
870 temp+=std::abs(mat(i,j));
878 template<
typename MatType>
881 int n = MatType::size;
882 typename MatType::ValueType norm = 0;
884 for(
int j = 0; j<n; ++j) {
885 typename MatType::ValueType column_sum = 0;
887 for (
int i = 0; i<n; ++i) {
888 column_sum += fabs(matrix(i,j));
897 template<
typename MatType>
898 typename MatType::ValueType
lOneNorm(
const MatType& matrix)
900 int n = MatType::size;
901 typename MatType::ValueType norm = 0;
903 for(
int i = 0; i<n; ++i) {
904 typename MatType::ValueType row_sum = 0;
906 for (
int j = 0; j<n; ++j) {
907 row_sum += fabs(matrix(i,j));
923 template<
typename MatType>
924 bool polarDecomposition(
const MatType& input, MatType& unitary, MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
929 MatType new_unitary(input);
934 unsigned int iteration(0);
936 typename MatType::ValueType linf_of_u;
937 typename MatType::ValueType l1nm_of_u;
938 typename MatType::ValueType linf_of_u_inv;
939 typename MatType::ValueType l1nm_of_u_inv;
940 typename MatType::ValueType l1_error = 100;
944 unitary_inv = unitary.inverse();
949 l1nm_of_u_inv =
lOneNorm(unitary_inv);
951 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
953 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
956 unitary = new_unitary;
959 if (iteration > MAX_ITERATIONS)
return false;
963 positive_hermitian = unitary.transpose() * input;
971 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED