7 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
8 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
25 template<
unsigned SIZE,
typename T>
34 static unsigned numRows() {
return SIZE; }
44 for (
unsigned i(0); i < numElements(); ++i) {
51 for (
unsigned i = 0; i < numElements(); ++i) {
68 str(
unsigned indentation = 0)
const {
74 indent.append(indentation+1,
' ');
79 for (
unsigned i(0); i < SIZE; i++) {
84 for (
unsigned j(0); j < SIZE; j++) {
87 if (j) ret.append(
", ");
88 ret.append(std::to_string(mm[(i*SIZE)+j]));
120 T* operator[](
int i) {
return &(mm[i*SIZE]); }
125 void write(std::ostream& os)
const {
126 os.write(
reinterpret_cast<const char*
>(&mm),
sizeof(T)*SIZE*SIZE);
130 is.read(
reinterpret_cast<char*
>(&mm),
sizeof(T)*SIZE*SIZE);
135 T x =
static_cast<T
>(std::fabs(mm[0]));
136 for (
unsigned i = 1; i < numElements(); ++i) {
137 x =
std::max(x,
static_cast<T
>(std::fabs(mm[i])));
144 for (
unsigned i = 0; i < numElements(); ++i) {
152 for (
unsigned i = 0; i < numElements(); ++i) {
160 for (
unsigned i = 0; i < numElements(); ++i) {
168 for (
unsigned i = 0; i < numElements(); ++i) {
179 template<
typename T>
class Quat;
180 template<
typename T>
class Vec3;
185 template<
class MatType>
188 typename MatType::value_type eps =
static_cast<typename MatType::value_type
>(1.0e-8))
190 using T =
typename MatType::value_type;
213 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
214 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
215 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
217 if(MatType::numColumns() == 4)
padMat4(r);
226 template<
class MatType>
230 using T =
typename MatType::value_type;
231 T c =
static_cast<T
>(cos(
angle));
232 T s =
static_cast<T
>(sin(
angle));
235 result.setIdentity();
257 throw ValueError(
"Unrecognized rotation axis");
264 template<
class MatType>
268 using T =
typename MatType::value_type;
269 T txy, txz, tyz, sx, sy, sz;
274 T c(cos(
double(
angle)));
275 T s(sin(
double(
angle)));
280 result[0][0] = axis[0]*axis[0] * t + c;
281 result[1][1] = axis[1]*axis[1] * t + c;
282 result[2][2] = axis[2]*axis[2] * t + c;
284 txy = axis[0]*axis[1] * t;
287 txz = axis[0]*axis[2] * t;
290 tyz = axis[1]*axis[2] * t;
295 result[0][1] = txy + sz;
296 result[1][0] = txy - sz;
298 result[0][2] = txz - sy;
299 result[2][0] = txz + sy;
301 result[1][2] = tyz + sx;
302 result[2][1] = tyz - sx;
304 if(MatType::numColumns() == 4)
padMat4(result);
305 return MatType(result);
346 template<
class MatType>
351 typename MatType::value_type eps =
static_cast<typename MatType::value_type
>(1.0e-8))
353 using ValueType =
typename MatType::value_type;
355 ValueType phi, theta, psi;
357 switch(rotationOrder)
361 theta = ValueType(M_PI_2);
362 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
365 theta = ValueType(-M_PI_2);
366 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
369 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
370 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
371 theta = ValueType(atan2(mat[2][0],
372 sqrt( mat[2][1]*mat[2][1] +
373 mat[2][2]*mat[2][2])));
375 return V(phi, theta, psi);
378 theta = ValueType(M_PI_2);
379 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
382 theta = ValueType(-M_PI/2);
383 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
386 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
387 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
388 theta = ValueType(atan2(mat[1][2],
389 sqrt(mat[0][2] * mat[0][2] +
390 mat[2][2] * mat[2][2])));
392 return V(theta, psi, phi);
396 theta = ValueType(M_PI_2);
397 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
400 theta = ValueType(-M_PI/2);
401 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
404 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
405 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
406 theta = ValueType(atan2(mat[0][1],
407 sqrt(mat[0][0] * mat[0][0] +
408 mat[0][2] * mat[0][2])));
410 return V(psi, phi, theta);
415 theta = ValueType(0.0);
416 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
419 theta = ValueType(M_PI);
420 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
423 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
424 phi = ValueType(atan2(mat[0][2], mat[0][1]));
425 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
426 mat[0][2] * mat[0][2]),
429 return V(phi, psi, theta);
434 theta = ValueType(0.0);
435 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
438 theta = ValueType(M_PI);
439 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
442 psi = ValueType(atan2(mat[0][2], mat[1][2]));
443 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
444 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
445 mat[1][2] * mat[1][2]),
448 return V(theta, psi, phi);
453 theta = ValueType(-M_PI_2);
454 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
457 theta = ValueType(M_PI_2);
458 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
461 psi = ValueType(atan2(mat[0][1], mat[1][1]));
462 phi = ValueType(atan2(mat[2][0], mat[2][2]));
463 theta = ValueType(atan2(-mat[2][1],
464 sqrt(mat[0][1] * mat[0][1] +
465 mat[1][1] * mat[1][1])));
467 return V(theta, phi, psi);
472 theta = ValueType(-M_PI_2);
473 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
476 theta = ValueType(M_PI_2);
477 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
480 psi = ValueType(atan2(mat[1][2], mat[2][2]));
481 phi = ValueType(atan2(mat[0][1], mat[0][0]));
482 theta = ValueType(atan2(-mat[0][2],
483 sqrt(mat[0][1] * mat[0][1] +
484 mat[0][0] * mat[0][0])));
486 return V(psi, theta, phi);
491 theta = ValueType(M_PI_2);
492 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
495 theta = ValueType(-M_PI_2);
496 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
499 psi = ValueType(atan2(mat[2][0], mat[0][0]));
500 phi = ValueType(atan2(mat[1][2], mat[1][1]));
501 theta = ValueType(atan2(- mat[1][0],
502 sqrt(mat[1][1] * mat[1][1] +
503 mat[1][2] * mat[1][2])));
505 return V(phi, psi, theta);
515 template<
typename MatType,
typename ValueType1,
typename ValueType2>
520 typename MatType::value_type eps =
static_cast<typename MatType::value_type
>(1.0e-8))
522 using T =
typename MatType::value_type;
552 Vec3<T> u, v, p(0.0, 0.0, 0.0);
554 double x =
Abs(v1[0]);
555 double y =
Abs(v1[1]);
556 double z =
Abs(v1[2]);
574 double udot = u.
dot(u);
575 double vdot = v.
dot(v);
577 double a = -2 / udot;
578 double b = -2 / vdot;
579 double c = 4 * u.
dot(v) / (udot * vdot);
582 result.setIdentity();
584 for (
int j = 0; j < 3; j++) {
585 for (
int i = 0; i < 3; i++)
586 result[i][j] =
static_cast<T
>(
587 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
593 if(MatType::numColumns() == 4)
padMat4(result);
597 double c = v1.
dot(v2);
598 double a = (1.0 - c) / cross.
dot(cross);
600 double a0 = a * cross[0];
601 double a1 = a * cross[1];
602 double a2 = a * cross[2];
604 double a01 = a0 * cross[1];
605 double a02 = a0 * cross[2];
606 double a12 = a1 * cross[2];
610 r[0][0] =
static_cast<T
>(c + a0 * cross[0]);
611 r[0][1] =
static_cast<T
>(a01 + cross[2]);
612 r[0][2] =
static_cast<T
>(a02 - cross[1]);
613 r[1][0] =
static_cast<T
>(a01 - cross[2]);
614 r[1][1] =
static_cast<T
>(c + a1 * cross[1]);
615 r[1][2] =
static_cast<T
>(a12 + cross[0]);
616 r[2][0] =
static_cast<T
>(a02 + cross[1]);
617 r[2][1] =
static_cast<T
>(a12 - cross[0]);
618 r[2][2] =
static_cast<T
>(c + a2 * cross[2]);
620 if(MatType::numColumns() == 4)
padMat4(r);
628 template<
class MatType>
636 result.setIdentity();
646 template<
class MatType>
652 V(mat[0][0], mat[0][1], mat[0][2]).length(),
653 V(mat[1][0], mat[1][1], mat[1][2]).length(),
654 V(mat[2][0], mat[2][1], mat[2][2]).length());
661 template<
class MatType>
663 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
666 return unit(mat, eps, dud);
674 template<
class MatType>
678 typename MatType::value_type eps,
681 using T =
typename MatType::value_type;
684 for (
int i(0); i < 3; i++) {
687 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
688 for (
int j=0; j<3; j++) result[i][j] = u[j];
690 for (
int j=0; j<3; j++) result[i][j] = 0;
701 template <
class MatType>
705 int index0 =
static_cast<int>(axis0);
706 int index1 =
static_cast<int>(axis1);
709 result.setIdentity();
710 if (axis0 == axis1) {
711 result[index1][index0] =
shear + 1;
713 result[index1][index0] =
shear;
721 template<
class MatType>
725 using T =
typename MatType::value_type;
728 r[0][0] = T(0); r[0][1] =
skew.z(); r[0][2] = -
skew.y();
729 r[1][0] = -
skew.z(); r[1][1] = T(0); r[2][1] =
skew.x();
730 r[2][0] =
skew.y(); r[2][1] = -
skew.x(); r[2][2] = T(0);
732 if(MatType::numColumns() == 4)
padMat4(r);
739 template<
class MatType>
744 using T =
typename MatType::value_type;
746 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
751 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
752 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
753 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
755 if(MatType::numColumns() == 4)
padMat4(r);
764 template<
class MatType>
768 using T =
typename MatType::value_type;
771 Vec3<T> ourUnitAxis(source.row(axis).unit());
774 T parallel = unitDir.
dot(ourUnitAxis);
784 T angleBetween(
angle(unitDir, ourUnitAxis));
789 rotation.setToRotation(rotationAxis, angleBetween);
796 template<
class MatType>
800 dest[0][3] = dest[1][3] = dest[2][3] = 0;
801 dest[3][2] = dest[3][1] = dest[3][0] = 0;
810 template<
typename MatType>
812 sqrtSolve(
const MatType& aA, MatType& aB,
double aTol=0.01)
814 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
818 Z[0] = MatType::identity();
820 unsigned int current = 0;
821 for (
unsigned int iteration=0; iteration < iterations; iteration++) {
822 unsigned int last = current;
825 MatType invY = Y[last].inverse();
826 MatType invZ = Z[last].inverse();
828 Y[current] = 0.5 * (Y[last] + invZ);
829 Z[current] = 0.5 * (Z[last] + invY);
835 template<
typename MatType>
837 powSolve(
const MatType& aA, MatType& aB,
double aPower,
double aTol=0.01)
839 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
841 const bool inverted = (aPower < 0.0);
842 if (inverted) { aPower = -aPower; }
844 unsigned int whole =
static_cast<unsigned int>(aPower);
845 double fraction = aPower - whole;
847 MatType R = MatType::identity();
848 MatType partial = aA;
850 double contribution = 1.0;
851 for (
unsigned int iteration = 0; iteration < iterations; iteration++) {
854 if (fraction >= contribution) {
856 fraction -= contribution;
862 if (whole & 1) { R *= partial; }
864 if (whole) { partial *= partial; }
867 if (inverted) { aB = R.inverse(); }
873 template<
typename MatType>
877 return m.eq(MatType::identity());
882 template<
typename MatType>
886 using ValueType =
typename MatType::ValueType;
893 template<
typename MatType>
897 return m.eq(m.transpose());
902 template<
typename MatType>
906 using ValueType =
typename MatType::ValueType;
907 if (!
isApproxEqual(std::abs(m.det()), ValueType(1.0)))
return false;
909 MatType temp = m * m.transpose();
910 return temp.eq(MatType::identity());
915 template<
typename MatType>
919 int n = MatType::size;
920 typename MatType::ValueType temp(0);
921 for (
int i = 0; i < n; ++i) {
922 for (
int j = 0; j < n; ++j) {
924 temp += std::abs(mat(i,j));
928 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
933 template<
typename MatType>
934 typename MatType::ValueType
937 int n = MatType::size;
938 typename MatType::ValueType norm = 0;
940 for(
int j = 0; j<n; ++j) {
941 typename MatType::ValueType column_sum = 0;
943 for (
int i = 0; i<n; ++i) {
944 column_sum += std::fabs(matrix(i,j));
954 template<
typename MatType>
955 typename MatType::ValueType
958 int n = MatType::size;
959 typename MatType::ValueType norm = 0;
961 for(
int i = 0; i<n; ++i) {
962 typename MatType::ValueType row_sum = 0;
964 for (
int j = 0; j<n; ++j) {
965 row_sum += std::fabs(matrix(i,j));
981 template<
typename MatType>
984 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
987 MatType new_unitary(input);
992 unsigned int iteration(0);
994 typename MatType::ValueType linf_of_u;
995 typename MatType::ValueType l1nm_of_u;
996 typename MatType::ValueType linf_of_u_inv;
997 typename MatType::ValueType l1nm_of_u_inv;
998 typename MatType::ValueType l1_error = 100;
1002 unitary_inv = unitary.inverse();
1007 l1nm_of_u_inv =
lOneNorm(unitary_inv);
1009 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1011 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1014 unitary = new_unitary;
1017 if (iteration > MAX_ITERATIONS)
return false;
1021 positive_hermitian = unitary.transpose() * input;
1028 template<
unsigned SIZE,
typename T>
1034 constexpr
unsigned size = SIZE*SIZE;
1035 for (
unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1042 template<
unsigned SIZE,
typename T>
1048 constexpr
unsigned size = SIZE*SIZE;
1049 for (
unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1059 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED