34 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 35 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 52 template<
unsigned SIZE,
typename T>
61 static unsigned numRows() {
return SIZE; }
71 for (
unsigned i(0); i < numElements(); ++i) {
78 for (
unsigned i = 0; i < numElements(); ++i) {
95 str(
unsigned indentation = 0)
const {
101 indent.append(indentation+1,
' ');
106 for (
unsigned i(0); i < SIZE; i++) {
111 for (
unsigned j(0); j < SIZE; j++) {
114 if (j) ret.append(
", ");
115 ret.append(std::to_string(mm[(i*SIZE)+j]));
142 void write(std::ostream& os)
const {
143 os.write(reinterpret_cast<const char*>(&mm),
sizeof(T)*SIZE*SIZE);
147 is.read(reinterpret_cast<char*>(&mm),
sizeof(T)*SIZE*SIZE);
152 T x = static_cast<T>(std::fabs(mm[0]));
153 for (
unsigned i = 1; i < numElements(); ++i) {
154 x =
std::max(x, static_cast<T>(std::fabs(mm[i])));
161 for (
unsigned i = 0; i < numElements(); ++i) {
162 if (std::isnan(mm[i]))
return true;
169 for (
unsigned i = 0; i < numElements(); ++i) {
170 if (std::isinf(mm[i]))
return true;
177 for (
unsigned i = 0; i < numElements(); ++i) {
178 if (!std::isfinite(mm[i]))
return false;
185 for (
unsigned i = 0; i < numElements(); ++i) {
196 template<
typename T>
class Quat;
197 template<
typename T>
class Vec3;
202 template<
class MatType>
205 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
207 using T =
typename MatType::value_type;
230 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
231 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
232 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
234 if(MatType::numColumns() == 4)
padMat4(r);
243 template<
class MatType>
247 using T =
typename MatType::value_type;
248 T c = static_cast<T>(cos(
angle));
249 T s = static_cast<T>(sin(
angle));
252 result.setIdentity();
274 throw ValueError(
"Unrecognized rotation axis");
281 template<
class MatType>
285 using T =
typename MatType::value_type;
286 T txy, txz, tyz, sx, sy, sz;
291 T c(cos(
double(
angle)));
292 T s(sin(
double(
angle)));
297 result[0][0] = axis[0]*axis[0] * t + c;
298 result[1][1] = axis[1]*axis[1] * t + c;
299 result[2][2] = axis[2]*axis[2] * t + c;
301 txy = axis[0]*axis[1] * t;
304 txz = axis[0]*axis[2] * t;
307 tyz = axis[1]*axis[2] * t;
312 result[0][1] = txy + sz;
313 result[1][0] = txy - sz;
315 result[0][2] = txz - sy;
316 result[2][0] = txz + sy;
318 result[1][2] = tyz + sx;
319 result[2][1] = tyz - sx;
321 if(MatType::numColumns() == 4)
padMat4(result);
322 return MatType(result);
363 template<
class MatType>
368 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
370 using ValueType =
typename MatType::value_type;
372 ValueType phi, theta, psi;
374 switch(rotationOrder)
378 theta = ValueType(M_PI_2);
379 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
382 theta = ValueType(-M_PI_2);
383 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
386 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
387 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
388 theta = ValueType(atan2(mat[2][0],
389 sqrt( mat[2][1]*mat[2][1] +
390 mat[2][2]*mat[2][2])));
392 return V(phi, theta, psi);
395 theta = ValueType(M_PI_2);
396 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
399 theta = ValueType(-M_PI/2);
400 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
403 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
404 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
405 theta = ValueType(atan2(mat[1][2],
406 sqrt(mat[0][2] * mat[0][2] +
407 mat[2][2] * mat[2][2])));
409 return V(theta, psi, phi);
413 theta = ValueType(M_PI_2);
414 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
417 theta = ValueType(-M_PI/2);
418 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
421 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
422 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
423 theta = ValueType(atan2(mat[0][1],
424 sqrt(mat[0][0] * mat[0][0] +
425 mat[0][2] * mat[0][2])));
427 return V(psi, phi, theta);
432 theta = ValueType(0.0);
433 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
436 theta = ValueType(M_PI);
437 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
440 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
441 phi = ValueType(atan2(mat[0][2], mat[0][1]));
442 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
443 mat[0][2] * mat[0][2]),
446 return V(phi, psi, theta);
451 theta = ValueType(0.0);
452 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
455 theta = ValueType(M_PI);
456 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
459 psi = ValueType(atan2(mat[0][2], mat[1][2]));
460 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
461 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
462 mat[1][2] * mat[1][2]),
465 return V(theta, psi, phi);
470 theta = ValueType(-M_PI_2);
471 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
474 theta = ValueType(M_PI_2);
475 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
478 psi = ValueType(atan2(mat[0][1], mat[1][1]));
479 phi = ValueType(atan2(mat[2][0], mat[2][2]));
480 theta = ValueType(atan2(-mat[2][1],
481 sqrt(mat[0][1] * mat[0][1] +
482 mat[1][1] * mat[1][1])));
484 return V(theta, phi, psi);
489 theta = ValueType(-M_PI_2);
490 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
493 theta = ValueType(M_PI_2);
494 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
497 psi = ValueType(atan2(mat[1][2], mat[2][2]));
498 phi = ValueType(atan2(mat[0][1], mat[0][0]));
499 theta = ValueType(atan2(-mat[0][2],
500 sqrt(mat[0][1] * mat[0][1] +
501 mat[0][0] * mat[0][0])));
503 return V(psi, theta, phi);
508 theta = ValueType(M_PI_2);
509 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
512 theta = ValueType(-M_PI_2);
513 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
516 psi = ValueType(atan2(mat[2][0], mat[0][0]));
517 phi = ValueType(atan2(mat[1][2], mat[1][1]));
518 theta = ValueType(atan2(- mat[1][0],
519 sqrt(mat[1][1] * mat[1][1] +
520 mat[1][2] * mat[1][2])));
522 return V(phi, psi, theta);
532 template<
typename MatType,
typename ValueType1,
typename ValueType2>
537 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
539 using T =
typename MatType::value_type;
569 Vec3<T> u, v, p(0.0, 0.0, 0.0);
571 double x =
Abs(v1[0]);
572 double y =
Abs(v1[1]);
573 double z =
Abs(v1[2]);
591 double udot = u.
dot(u);
592 double vdot = v.
dot(v);
594 double a = -2 / udot;
595 double b = -2 / vdot;
596 double c = 4 * u.
dot(v) / (udot * vdot);
599 result.setIdentity();
601 for (
int j = 0; j < 3; j++) {
602 for (
int i = 0; i < 3; i++)
603 result[i][j] = static_cast<T>(
604 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
610 if(MatType::numColumns() == 4)
padMat4(result);
614 double c = v1.
dot(v2);
615 double a = (1.0 - c) / cross.
dot(cross);
617 double a0 = a * cross[0];
618 double a1 = a * cross[1];
619 double a2 = a * cross[2];
621 double a01 = a0 * cross[1];
622 double a02 = a0 * cross[2];
623 double a12 = a1 * cross[2];
627 r[0][0] = static_cast<T>(c + a0 * cross[0]);
628 r[0][1] = static_cast<T>(a01 + cross[2]);
629 r[0][2] = static_cast<T>(a02 - cross[1]);
630 r[1][0] = static_cast<T>(a01 - cross[2]);
631 r[1][1] = static_cast<T>(c + a1 * cross[1]);
632 r[1][2] = static_cast<T>(a12 + cross[0]);
633 r[2][0] = static_cast<T>(a02 + cross[1]);
634 r[2][1] = static_cast<T>(a12 - cross[0]);
635 r[2][2] = static_cast<T>(c + a2 * cross[2]);
637 if(MatType::numColumns() == 4)
padMat4(r);
645 template<
class MatType>
653 result.setIdentity();
663 template<
class MatType>
669 V(mat[0][0], mat[0][1], mat[0][2]).length(),
670 V(mat[1][0], mat[1][1], mat[1][2]).length(),
671 V(mat[2][0], mat[2][1], mat[2][2]).length());
678 template<
class MatType>
680 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
683 return unit(mat, eps, dud);
691 template<
class MatType>
695 typename MatType::value_type eps,
698 using T =
typename MatType::value_type;
701 for (
int i(0); i < 3; i++) {
704 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
705 for (
int j=0; j<3; j++) result[i][j] = u[j];
707 for (
int j=0; j<3; j++) result[i][j] = 0;
718 template <
class MatType>
722 int index0 = static_cast<int>(axis0);
723 int index1 = static_cast<int>(axis1);
726 result.setIdentity();
727 if (axis0 == axis1) {
728 result[index1][index0] =
shear + 1;
730 result[index1][index0] =
shear;
738 template<
class MatType>
742 using T =
typename MatType::value_type;
745 r[0][0] = T(0); r[0][1] =
skew.z(); r[0][2] = -
skew.y();
746 r[1][0] = -
skew.z(); r[1][1] = T(0); r[2][1] =
skew.x();
747 r[2][0] =
skew.y(); r[2][1] = -
skew.x(); r[2][2] = T(0);
749 if(MatType::numColumns() == 4)
padMat4(r);
756 template<
class MatType>
761 using T =
typename MatType::value_type;
763 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
764 Vec3<T> up(forward.cross(horizontal).unit());
768 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
769 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
770 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
772 if(MatType::numColumns() == 4)
padMat4(r);
781 template<
class MatType>
785 using T =
typename MatType::value_type;
788 Vec3<T> ourUnitAxis(source.row(axis).unit());
791 T parallel = unitDir.
dot(ourUnitAxis);
801 T angleBetween(
angle(unitDir, ourUnitAxis));
806 rotation.setToRotation(rotationAxis, angleBetween);
813 template<
class MatType>
817 dest[0][3] = dest[1][3] = dest[2][3] = 0;
818 dest[3][2] = dest[3][1] = dest[3][0] = 0;
827 template<
typename MatType>
829 sqrtSolve(
const MatType& aA, MatType& aB,
double aTol=0.01)
831 unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
835 Z[0] = MatType::identity();
837 unsigned int current = 0;
838 for (
unsigned int iteration=0; iteration < iterations; iteration++) {
839 unsigned int last = current;
842 MatType invY = Y[last].inverse();
843 MatType invZ = Z[last].inverse();
845 Y[current] = 0.5 * (Y[last] + invZ);
846 Z[current] = 0.5 * (Z[last] + invY);
852 template<
typename MatType>
854 powSolve(
const MatType& aA, MatType& aB,
double aPower,
double aTol=0.01)
856 unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
858 const bool inverted = (aPower < 0.0);
859 if (inverted) { aPower = -aPower; }
861 unsigned int whole = static_cast<unsigned int>(aPower);
862 double fraction = aPower - whole;
864 MatType R = MatType::identity();
865 MatType partial = aA;
867 double contribution = 1.0;
868 for (
unsigned int iteration = 0; iteration < iterations; iteration++) {
871 if (fraction >= contribution) {
873 fraction -= contribution;
879 if (whole & 1) { R *= partial; }
881 if (whole) { partial *= partial; }
884 if (inverted) { aB = R.inverse(); }
890 template<
typename MatType>
894 return m.eq(MatType::identity());
899 template<
typename MatType>
903 using ValueType =
typename MatType::ValueType;
910 template<
typename MatType>
914 return m.eq(m.transpose());
919 template<
typename MatType>
923 using ValueType =
typename MatType::ValueType;
924 if (!
isApproxEqual(std::abs(m.det()), ValueType(1.0)))
return false;
926 MatType temp = m * m.transpose();
927 return temp.eq(MatType::identity());
932 template<
typename MatType>
936 int n = MatType::size;
937 typename MatType::ValueType temp(0);
938 for (
int i = 0; i < n; ++i) {
939 for (
int j = 0; j < n; ++j) {
941 temp += std::abs(mat(i,j));
945 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
950 template<
typename MatType>
951 typename MatType::ValueType
954 int n = MatType::size;
955 typename MatType::ValueType norm = 0;
957 for(
int j = 0; j<n; ++j) {
958 typename MatType::ValueType column_sum = 0;
960 for (
int i = 0; i<n; ++i) {
961 column_sum += fabs(matrix(i,j));
971 template<
typename MatType>
972 typename MatType::ValueType
975 int n = MatType::size;
976 typename MatType::ValueType norm = 0;
978 for(
int i = 0; i<n; ++i) {
979 typename MatType::ValueType row_sum = 0;
981 for (
int j = 0; j<n; ++j) {
982 row_sum += fabs(matrix(i,j));
998 template<
typename MatType>
1001 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
1004 MatType new_unitary(input);
1005 MatType unitary_inv;
1009 unsigned int iteration(0);
1011 typename MatType::ValueType linf_of_u;
1012 typename MatType::ValueType l1nm_of_u;
1013 typename MatType::ValueType linf_of_u_inv;
1014 typename MatType::ValueType l1nm_of_u_inv;
1015 typename MatType::ValueType l1_error = 100;
1019 unitary_inv = unitary.inverse();
1024 l1nm_of_u_inv =
lOneNorm(unitary_inv);
1026 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1028 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1031 unitary = new_unitary;
1034 if (iteration > MAX_ITERATIONS)
return false;
1038 positive_hermitian = unitary.transpose() * input;
1046 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED std::string str(unsigned indentation=0) const
Definition: Mat.h:95
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:216
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:377
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:493
static MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition: Mat.h:815
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:168
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:901
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:934
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:109
RotationOrder
Definition: Math.h:863
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:160
T & z()
Definition: Quat.h:226
T & y()
Definition: Quat.h:225
SIZE_
Definition: Mat.h:58
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:245
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:921
bool polarDecomposition(const MatType &input, MatType &unitary, MatType &positive_hermitian, unsigned int MAX_ITERATIONS=100)
Decompose an invertible 3×3 matrix into a unitary matrix followed by a symmetric matrix (positive se...
Definition: Mat.h:1000
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:892
Mat & operator=(Mat const &src)
Definition: Mat.h:76
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:854
Axis
Definition: Math.h:856
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h:136
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:308
T mm[SIZE *SIZE]
Definition: Mat.h:192
T & z()
Definition: Vec3.h:112
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:224
void read(std::istream &is)
Definition: Mat.h:146
T & y()
Definition: Vec3.h:111
Definition: Exceptions.h:40
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:912
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the L∞ norm of an N×N matrix.
Definition: Mat.h:952
T value_type
Definition: Mat.h:56
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:134
MatType aim(const Vec3< typename MatType::value_type > &direction, const Vec3< typename MatType::value_type > &vertical)
Return an orientation matrix such that z points along direction, and y is along the direction / verti...
Definition: Mat.h:758
bool isApproxEqual(const Type &a, const Type &b)
Return true if a is equal to b to within the default floating-point comparison tolerance.
Definition: Math.h:358
Tolerance for floating-point comparison.
Definition: Math.h:117
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:647
MatType shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
Set the matrix to a shear along axis0 by a fraction of axis1.
Definition: Mat.h:720
void write(std::ostream &os) const
Definition: Mat.h:142
MatType snapMatBasis(const MatType &source, Axis axis, const Vec3< typename MatType::value_type > &direction)
This function snaps a specific axis to a specific direction, preserving scaling.
Definition: Mat.h:783
Coord Abs(const Coord &xyz)
Definition: Coord.h:513
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:472
static unsigned numElements()
Definition: Mat.h:63
MatType unit(const MatType &in, typename MatType::value_type eps, Vec3< typename MatType::value_type > &scaling)
Return a copy of the given matrix with its upper 3×3 rows normalized, and return the length of each ...
Definition: Mat.h:693
Mat()
Definition: Mat.h:67
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:389
static unsigned numColumns()
Definition: Mat.h:62
Definition: Exceptions.h:92
static unsigned numRows()
Definition: Mat.h:61
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:70
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:740
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:188
Vec3< typename MatType::value_type > eulerAngles(const MatType &mat, RotationOrder rotationOrder, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the Euler angles composing the given rotation matrix.
Definition: Mat.h:365
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:176
MatType::ValueType lOneNorm(const MatType &matrix)
Return the L1 norm of an N×N matrix.
Definition: Mat.h:973
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:184
T ValueType
Definition: Mat.h:57
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:829
Definition: Exceptions.h:88
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:151
MatType rotation(const Vec3< ValueType1 > &_v1, const Vec3< ValueType2 > &_v2, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return a rotation matrix that maps v1 onto v2 about the cross product of v1 and v2.
Definition: Mat.h:534
Vec3< typename MatType::value_type > getScale(const MatType &mat)
Return a Vec3 representing the lengths of the passed matrix's upper 3×3's rows.
Definition: Mat.h:665
Definition: Exceptions.h:83
T & w()
Definition: Quat.h:227
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:110