7 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 8 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 25 template<
unsigned SIZE,
typename T>
39 static unsigned numRows() {
return SIZE; }
53 str(
unsigned indentation = 0)
const {
59 indent.append(indentation+1,
' ');
64 for (
unsigned i(0); i < SIZE; i++) {
69 for (
unsigned j(0); j < SIZE; j++) {
72 if (j) ret.append(
", ");
73 ret.append(std::to_string(mm[(i*SIZE)+j]));
110 void write(std::ostream& os)
const {
111 os.write(reinterpret_cast<const char*>(&mm),
sizeof(T)*SIZE*SIZE);
115 is.read(reinterpret_cast<char*>(&mm),
sizeof(T)*SIZE*SIZE);
120 T x =
static_cast<T
>(std::fabs(mm[0]));
121 for (
unsigned i = 1; i < numElements(); ++i) {
122 x =
std::max(x, static_cast<T>(std::fabs(mm[i])));
129 for (
unsigned i = 0; i < numElements(); ++i) {
137 for (
unsigned i = 0; i < numElements(); ++i) {
145 for (
unsigned i = 0; i < numElements(); ++i) {
153 for (
unsigned i = 0; i < numElements(); ++i) {
164 template<
typename T>
class Quat;
165 template<
typename T>
class Vec3;
170 template<
class MatType>
173 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
175 using T =
typename MatType::value_type;
198 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
199 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
200 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
202 if(MatType::numColumns() == 4)
padMat4(r);
211 template<
class MatType>
215 using T =
typename MatType::value_type;
216 T c =
static_cast<T
>(cos(angle));
217 T s =
static_cast<T
>(sin(angle));
220 result.setIdentity();
242 throw ValueError(
"Unrecognized rotation axis");
249 template<
class MatType>
253 using T =
typename MatType::value_type;
254 T txy, txz, tyz, sx, sy, sz;
259 T c(cos(
double(angle)));
260 T s(sin(
double(angle)));
265 result[0][0] = axis[0]*axis[0] * t + c;
266 result[1][1] = axis[1]*axis[1] * t + c;
267 result[2][2] = axis[2]*axis[2] * t + c;
269 txy = axis[0]*axis[1] * t;
272 txz = axis[0]*axis[2] * t;
275 tyz = axis[1]*axis[2] * t;
280 result[0][1] = txy + sz;
281 result[1][0] = txy - sz;
283 result[0][2] = txz - sy;
284 result[2][0] = txz + sy;
286 result[1][2] = tyz + sx;
287 result[2][1] = tyz - sx;
289 if(MatType::numColumns() == 4)
padMat4(result);
290 return MatType(result);
331 template<
class MatType>
336 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
338 using ValueType =
typename MatType::value_type;
340 ValueType phi, theta, psi;
342 switch(rotationOrder)
346 theta = ValueType(math::pi<double>() / 2.0);
347 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
350 theta = ValueType(-math::pi<double>() / 2.0);
351 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
354 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
355 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
356 theta = ValueType(atan2(mat[2][0],
357 sqrt( mat[2][1]*mat[2][1] +
358 mat[2][2]*mat[2][2])));
360 return V(phi, theta, psi);
363 theta = ValueType(math::pi<double>() / 2.0);
364 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
367 theta = ValueType(-math::pi<double>() / 2.0);
368 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
371 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
372 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
373 theta = ValueType(atan2(mat[1][2],
374 sqrt(mat[0][2] * mat[0][2] +
375 mat[2][2] * mat[2][2])));
377 return V(theta, psi, phi);
381 theta = ValueType(math::pi<double>() / 2.0);
382 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
385 theta = ValueType(-math::pi<double>() / 2.0);
386 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
389 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
390 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
391 theta = ValueType(atan2(mat[0][1],
392 sqrt(mat[0][0] * mat[0][0] +
393 mat[0][2] * mat[0][2])));
395 return V(psi, phi, theta);
400 theta = ValueType(0.0);
401 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
404 theta = ValueType(math::pi<double>());
405 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
408 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
409 phi = ValueType(atan2(mat[0][2], mat[0][1]));
410 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
411 mat[0][2] * mat[0][2]),
414 return V(phi, psi, theta);
419 theta = ValueType(0.0);
420 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
423 theta = ValueType(math::pi<double>());
424 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
427 psi = ValueType(atan2(mat[0][2], mat[1][2]));
428 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
429 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
430 mat[1][2] * mat[1][2]),
433 return V(theta, psi, phi);
438 theta = ValueType(-math::pi<double>() / 2.0);
439 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
442 theta = ValueType(math::pi<double>() / 2.0);
443 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
446 psi = ValueType(atan2(mat[0][1], mat[1][1]));
447 phi = ValueType(atan2(mat[2][0], mat[2][2]));
448 theta = ValueType(atan2(-mat[2][1],
449 sqrt(mat[0][1] * mat[0][1] +
450 mat[1][1] * mat[1][1])));
452 return V(theta, phi, psi);
457 theta = ValueType(-math::pi<double>() / 2.0);
458 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
461 theta = ValueType(math::pi<double>() / 2.0);
462 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
465 psi = ValueType(atan2(mat[1][2], mat[2][2]));
466 phi = ValueType(atan2(mat[0][1], mat[0][0]));
467 theta = ValueType(atan2(-mat[0][2],
468 sqrt(mat[0][1] * mat[0][1] +
469 mat[0][0] * mat[0][0])));
471 return V(psi, theta, phi);
476 theta = ValueType(math::pi<double>() / 2.0);
477 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
480 theta = ValueType(-math::pi<double>() / 2.0);
481 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
484 psi = ValueType(atan2(mat[2][0], mat[0][0]));
485 phi = ValueType(atan2(mat[1][2], mat[1][1]));
486 theta = ValueType(atan2(- mat[1][0],
487 sqrt(mat[1][1] * mat[1][1] +
488 mat[1][2] * mat[1][2])));
490 return V(phi, psi, theta);
500 template<
typename MatType,
typename ValueType1,
typename ValueType2>
505 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
507 using T =
typename MatType::value_type;
537 Vec3<T> u, v, p(0.0, 0.0, 0.0);
539 double x =
Abs(v1[0]);
540 double y =
Abs(v1[1]);
541 double z =
Abs(v1[2]);
559 double udot = u.
dot(u);
560 double vdot = v.
dot(v);
562 double a = -2 / udot;
563 double b = -2 / vdot;
564 double c = 4 * u.
dot(v) / (udot * vdot);
567 result.setIdentity();
569 for (
int j = 0; j < 3; j++) {
570 for (
int i = 0; i < 3; i++)
571 result[i][j] = static_cast<T>(
572 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
578 if(MatType::numColumns() == 4)
padMat4(result);
582 double c = v1.
dot(v2);
583 double a = (1.0 - c) / cross.
dot(cross);
585 double a0 = a * cross[0];
586 double a1 = a * cross[1];
587 double a2 = a * cross[2];
589 double a01 = a0 * cross[1];
590 double a02 = a0 * cross[2];
591 double a12 = a1 * cross[2];
595 r[0][0] =
static_cast<T
>(c + a0 * cross[0]);
596 r[0][1] =
static_cast<T
>(a01 + cross[2]);
597 r[0][2] =
static_cast<T
>(a02 - cross[1]);
598 r[1][0] =
static_cast<T
>(a01 - cross[2]);
599 r[1][1] =
static_cast<T
>(c + a1 * cross[1]);
600 r[1][2] =
static_cast<T
>(a12 + cross[0]);
601 r[2][0] =
static_cast<T
>(a02 + cross[1]);
602 r[2][1] =
static_cast<T
>(a12 - cross[0]);
603 r[2][2] =
static_cast<T
>(c + a2 * cross[2]);
605 if(MatType::numColumns() == 4)
padMat4(r);
613 template<
class MatType>
621 result.setIdentity();
631 template<
class MatType>
637 V(mat[0][0], mat[0][1], mat[0][2]).length(),
638 V(mat[1][0], mat[1][1], mat[1][2]).length(),
639 V(mat[2][0], mat[2][1], mat[2][2]).length());
646 template<
class MatType>
648 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
651 return unit(mat, eps, dud);
659 template<
class MatType>
663 typename MatType::value_type eps,
666 using T =
typename MatType::value_type;
669 for (
int i(0); i < 3; i++) {
672 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
673 for (
int j=0; j<3; j++) result[i][j] = u[j];
675 for (
int j=0; j<3; j++) result[i][j] = 0;
686 template <
class MatType>
690 int index0 =
static_cast<int>(axis0);
691 int index1 =
static_cast<int>(axis1);
694 result.setIdentity();
695 if (axis0 == axis1) {
696 result[index1][index0] = shear + 1;
698 result[index1][index0] =
shear;
706 template<
class MatType>
710 using T =
typename MatType::value_type;
713 r[0][0] = T(0); r[0][1] = skew.
z(); r[0][2] = -skew.
y();
714 r[1][0] = -skew.
z(); r[1][1] = T(0); r[2][1] = skew.
x();
715 r[2][0] = skew.
y(); r[2][1] = -skew.
x(); r[2][2] = T(0);
717 if(MatType::numColumns() == 4)
padMat4(r);
724 template<
class MatType>
729 using T =
typename MatType::value_type;
731 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
732 Vec3<T> up(forward.cross(horizontal).unit());
736 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
737 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
738 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
740 if(MatType::numColumns() == 4)
padMat4(r);
749 template<
class MatType>
753 using T =
typename MatType::value_type;
756 Vec3<T> ourUnitAxis(source.row(axis).unit());
759 T parallel = unitDir.
dot(ourUnitAxis);
769 T angleBetween(
angle(unitDir, ourUnitAxis));
774 rotation.setToRotation(rotationAxis, angleBetween);
781 template<
class MatType>
785 dest[0][3] = dest[1][3] = dest[2][3] = 0;
786 dest[3][2] = dest[3][1] = dest[3][0] = 0;
795 template<
typename MatType>
797 sqrtSolve(
const MatType& aA, MatType& aB,
double aTol=0.01)
799 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
803 Z[0] = MatType::identity();
805 unsigned int current = 0;
806 for (
unsigned int iteration=0; iteration < iterations; iteration++) {
807 unsigned int last = current;
810 MatType invY = Y[last].inverse();
811 MatType invZ = Z[last].inverse();
813 Y[current] = 0.5 * (Y[last] + invZ);
814 Z[current] = 0.5 * (Z[last] + invY);
820 template<
typename MatType>
822 powSolve(
const MatType& aA, MatType& aB,
double aPower,
double aTol=0.01)
824 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
826 const bool inverted = (aPower < 0.0);
827 if (inverted) { aPower = -aPower; }
829 unsigned int whole =
static_cast<unsigned int>(aPower);
830 double fraction = aPower - whole;
832 MatType R = MatType::identity();
833 MatType partial = aA;
835 double contribution = 1.0;
836 for (
unsigned int iteration = 0; iteration < iterations; iteration++) {
839 if (fraction >= contribution) {
841 fraction -= contribution;
847 if (whole & 1) { R *= partial; }
849 if (whole) { partial *= partial; }
852 if (inverted) { aB = R.inverse(); }
858 template<
typename MatType>
862 return m.eq(MatType::identity());
867 template<
typename MatType>
871 using ValueType =
typename MatType::ValueType;
878 template<
typename MatType>
882 return m.eq(m.transpose());
887 template<
typename MatType>
891 using ValueType =
typename MatType::ValueType;
892 if (!
isApproxEqual(std::abs(m.det()), ValueType(1.0)))
return false;
894 MatType temp = m * m.transpose();
895 return temp.eq(MatType::identity());
900 template<
typename MatType>
904 int n = MatType::size;
905 typename MatType::ValueType temp(0);
906 for (
int i = 0; i < n; ++i) {
907 for (
int j = 0; j < n; ++j) {
909 temp += std::abs(mat(i,j));
913 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
918 template<
typename MatType>
919 typename MatType::ValueType
922 int n = MatType::size;
923 typename MatType::ValueType norm = 0;
925 for(
int j = 0; j<n; ++j) {
926 typename MatType::ValueType column_sum = 0;
928 for (
int i = 0; i<n; ++i) {
929 column_sum += std::fabs(matrix(i,j));
939 template<
typename MatType>
940 typename MatType::ValueType
943 int n = MatType::size;
944 typename MatType::ValueType norm = 0;
946 for(
int i = 0; i<n; ++i) {
947 typename MatType::ValueType row_sum = 0;
949 for (
int j = 0; j<n; ++j) {
950 row_sum += std::fabs(matrix(i,j));
966 template<
typename MatType>
969 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
972 MatType new_unitary(input);
977 unsigned int iteration(0);
979 typename MatType::ValueType linf_of_u;
980 typename MatType::ValueType l1nm_of_u;
981 typename MatType::ValueType linf_of_u_inv;
982 typename MatType::ValueType l1nm_of_u_inv;
983 typename MatType::ValueType l1_error = 100;
987 unitary_inv = unitary.inverse();
992 l1nm_of_u_inv =
lOneNorm(unitary_inv);
994 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
996 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
999 unitary = new_unitary;
1002 if (iteration > MAX_ITERATIONS)
return false;
1006 positive_hermitian = unitary.transpose() * input;
1013 template<
unsigned SIZE,
typename T>
1019 constexpr
unsigned size = SIZE*SIZE;
1020 for (
unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1027 template<
unsigned SIZE,
typename T>
1033 constexpr
unsigned size = SIZE*SIZE;
1034 for (
unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1044 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:220
Definition: Exceptions.h:65
static unsigned numElements()
Definition: Mat.h:41
SIZE_
Definition: Mat.h:31
RotationOrder
Definition: Math.h:908
bool isNan(const float x)
Return true if x is a NaN (Not-A-Number) value.
Definition: Math.h:395
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:708
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:136
const T * asPointer() const
Definition: Mat.h:102
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:74
bool isFinite(const float x)
Return true if x is finite.
Definition: Math.h:375
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:443
Definition: Exceptions.h:56
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:615
T value_type
Definition: Mat.h:29
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:902
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:128
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:144
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:119
T * operator[](int i)
Array style reference to ith row.
Definition: Mat.h:106
const T * operator[](int i) const
Array style reference to ith row.
Definition: Mat.h:107
static unsigned numColumns()
Definition: Mat.h:40
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
Coord Abs(const Coord &xyz)
Definition: Coord.h:517
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:152
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition: Mat.h:1029
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 sem...
Definition: Mat.h:968
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the L∞ norm of an N×N matrix.
Definition: Mat.h:920
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:337
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:860
T & y()
Definition: Vec3.h:86
bool cwiseLessThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition: Mat.h:1015
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:374
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:889
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:191
Axis
Definition: Math.h:901
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:751
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:333
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:822
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition: Math.h:406
Definition: Exceptions.h:13
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:451
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:797
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 o...
Definition: Mat.h:661
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:869
T ValueType
Definition: Mat.h:30
T & z()
Definition: Vec3.h:87
void write(std::ostream &os) const
Definition: Mat.h:110
T & w()
Definition: Quat.h:196
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:726
Definition: Exceptions.h:61
MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition: Mat.h:783
MatType::ValueType lOneNorm(const MatType &matrix)
Return the L1 norm of an N×N matrix.
Definition: Mat.h:941
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:633
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:92
static unsigned numRows()
Definition: Mat.h:39
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:193
Tolerance for floating-point comparison.
Definition: Math.h:148
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:880
void read(std::istream &is)
Definition: Mat.h:114
T * asPointer()
Direct access to the internal data.
Definition: Mat.h:101
bool isInfinite(const float x)
Return true if x is an infinity value (either positive infinity or negative infinity).
Definition: Math.h:385
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h.in:121
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:502
std::string str(unsigned indentation=0) const
Definition: Mat.h:53
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:362
T & y()
Definition: Quat.h:194
T & z()
Definition: Quat.h:195
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:85
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h.in:212
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:688