34 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 35 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED 39 #include <boost/format.hpp> 51 template<
unsigned SIZE,
typename T>
60 static unsigned numRows() {
return SIZE; }
70 for (
unsigned i(0);
i < numElements(); ++
i) {
77 for (
unsigned i = 0;
i < numElements(); ++
i) {
94 str(
unsigned indentation = 0)
const {
100 indent.append(indentation+1,
' ');
105 for (
unsigned i(0);
i < SIZE;
i++) {
110 for (
unsigned j(0); j < SIZE; j++) {
113 if (j) ret.append(
", ");
114 ret.append((boost::format(
"%1%") % mm[(
i*SIZE)+j]).str());
123 ret.append((boost::format(
",\n%1%") % indent).str());
140 void write(std::ostream& os)
const {
141 os.write(reinterpret_cast<const char*>(&mm),
sizeof(T)*SIZE*SIZE);
145 is.read(reinterpret_cast<char*>(&mm),
sizeof(T)*SIZE*SIZE);
150 T x =
static_cast<T
>(std::fabs(mm[0]));
151 for (
unsigned i = 1;
i < numElements(); ++
i) {
152 x =
std::max(x, static_cast<T>(std::fabs(mm[
i])));
159 for (
unsigned i = 0;
i < numElements(); ++
i) {
160 if (std::isnan(mm[
i]))
return true;
167 for (
unsigned i = 0;
i < numElements(); ++
i) {
168 if (std::isinf(mm[
i]))
return true;
175 for (
unsigned i = 0;
i < numElements(); ++
i) {
176 if (!std::isfinite(mm[
i]))
return false;
183 for (
unsigned i = 0;
i < numElements(); ++
i) {
184 if (!
isZero(mm[
i]))
return false;
194 template<
typename T>
class Quat;
195 template<
typename T>
class Vec3;
200 template<
class MatType>
203 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
205 typedef typename MatType::value_type T;
228 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
229 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
230 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
232 if(MatType::numColumns() == 4)
padMat4(r);
241 template<
class MatType>
245 typedef typename MatType::value_type T;
246 T c =
static_cast<T
>(cos(
angle));
247 T s =
static_cast<T
>(sin(
angle));
250 result.setIdentity();
272 throw ValueError(
"Unrecognized rotation axis");
279 template<
class MatType>
283 typedef typename MatType::value_type T;
284 T txy, txz, tyz, sx, sy, sz;
289 T c(cos(
double(
angle)));
290 T s(sin(
double(
angle)));
295 result[0][0] = axis[0]*axis[0] * t + c;
296 result[1][1] = axis[1]*axis[1] * t + c;
297 result[2][2] = axis[2]*axis[2] * t + c;
299 txy = axis[0]*axis[1] * t;
302 txz = axis[0]*axis[2] * t;
305 tyz = axis[1]*axis[2] * t;
310 result[0][1] = txy + sz;
311 result[1][0] = txy - sz;
313 result[0][2] = txz - sy;
314 result[2][0] = txz + sy;
316 result[1][2] = tyz + sx;
317 result[2][1] = tyz - sx;
319 if(MatType::numColumns() == 4)
padMat4(result);
320 return MatType(result);
361 template<
class MatType>
366 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
368 typedef typename MatType::value_type ValueType;
370 ValueType phi, theta, psi;
372 switch(rotationOrder)
376 theta = ValueType(M_PI_2);
377 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
380 theta = ValueType(-M_PI_2);
381 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
384 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
385 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
386 theta = ValueType(atan2(mat[2][0],
387 sqrt( mat[2][1]*mat[2][1] +
388 mat[2][2]*mat[2][2])));
390 return V(phi, theta, psi);
393 theta = ValueType(M_PI_2);
394 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
397 theta = ValueType(-M_PI/2);
398 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
401 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
402 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
403 theta = ValueType(atan2(mat[1][2],
404 sqrt(mat[0][2] * mat[0][2] +
405 mat[2][2] * mat[2][2])));
407 return V(theta, psi, phi);
411 theta = ValueType(M_PI_2);
412 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
415 theta = ValueType(-M_PI/2);
416 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
419 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
420 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
421 theta = ValueType(atan2(mat[0][1],
422 sqrt(mat[0][0] * mat[0][0] +
423 mat[0][2] * mat[0][2])));
425 return V(psi, phi, theta);
430 theta = ValueType(0.0);
431 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
434 theta = ValueType(M_PI);
435 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
438 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
439 phi = ValueType(atan2(mat[0][2], mat[0][1]));
440 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
441 mat[0][2] * mat[0][2]),
444 return V(phi, psi, theta);
449 theta = ValueType(0.0);
450 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
453 theta = ValueType(M_PI);
454 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
457 psi = ValueType(atan2(mat[0][2], mat[1][2]));
458 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
459 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
460 mat[1][2] * mat[1][2]),
463 return V(theta, psi, phi);
468 theta = ValueType(-M_PI_2);
469 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
472 theta = ValueType(M_PI_2);
473 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
476 psi = ValueType(atan2(mat[0][1], mat[1][1]));
477 phi = ValueType(atan2(mat[2][0], mat[2][2]));
478 theta = ValueType(atan2(-mat[2][1],
479 sqrt(mat[0][1] * mat[0][1] +
480 mat[1][1] * mat[1][1])));
482 return V(theta, phi, psi);
487 theta = ValueType(-M_PI_2);
488 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
491 theta = ValueType(M_PI_2);
492 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
495 psi = ValueType(atan2(mat[1][2], mat[2][2]));
496 phi = ValueType(atan2(mat[0][1], mat[0][0]));
497 theta = ValueType(atan2(-mat[0][2],
498 sqrt(mat[0][1] * mat[0][1] +
499 mat[0][0] * mat[0][0])));
501 return V(psi, theta, phi);
506 theta = ValueType(M_PI_2);
507 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
510 theta = ValueType(-M_PI_2);
511 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
514 psi = ValueType(atan2(mat[2][0], mat[0][0]));
515 phi = ValueType(atan2(mat[1][2], mat[1][1]));
516 theta = ValueType(atan2(- mat[1][0],
517 sqrt(mat[1][1] * mat[1][1] +
518 mat[1][2] * mat[1][2])));
520 return V(phi, psi, theta);
529 template<
class MatType>
534 typename MatType::value_type eps=1.0e-8)
536 typedef typename MatType::value_type T;
565 Vec3<T> u, v, p(0.0, 0.0, 0.0);
567 double x =
Abs(v1[0]);
568 double y =
Abs(v1[1]);
569 double z =
Abs(v1[2]);
587 double udot = u.
dot(u);
588 double vdot = v.
dot(v);
590 double a = -2 / udot;
591 double b = -2 / vdot;
592 double c = 4 * u.
dot(v) / (udot * vdot);
595 result.setIdentity();
597 for (
int j = 0; j < 3; j++) {
598 for (
int i = 0;
i < 3;
i++)
600 a * u[
i] * u[j] + b * v[
i] * v[j] + c * v[j] * u[
i];
606 if(MatType::numColumns() == 4)
padMat4(result);
610 double c = v1.
dot(v2);
611 double a = (1.0 - c) / cross.
dot(cross);
613 double a0 = a * cross[0];
614 double a1 = a * cross[1];
615 double a2 = a * cross[2];
617 double a01 = a0 * cross[1];
618 double a02 = a0 * cross[2];
619 double a12 = a1 * cross[2];
623 r[0][0] = c + a0 * cross[0];
624 r[0][1] = a01 + cross[2];
625 r[0][2] = a02 - cross[1],
626 r[1][0] = a01 - cross[2];
627 r[1][1] = c + a1 * cross[1];
628 r[1][2] = a12 + cross[0];
629 r[2][0] = a02 + cross[1];
630 r[2][1] = a12 - cross[0];
631 r[2][2] = c + a2 * cross[2];
633 if(MatType::numColumns() == 4)
padMat4(r);
641 template<
class MatType>
649 result.setIdentity();
659 template<
class MatType>
665 V(mat[0][0], mat[0][1], mat[0][2]).length(),
666 V(mat[1][0], mat[1][1], mat[1][2]).length(),
667 V(mat[2][0], mat[2][1], mat[2][2]).length());
674 template<
class MatType>
676 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
679 return unit(mat, eps, dud);
687 template<
class MatType>
691 typename MatType::value_type eps,
694 typedef typename MatType::value_type T;
697 for (
int i(0);
i < 3;
i++) {
701 for (
int j=0; j<3; j++) result[
i][j] = u[j];
703 for (
int j=0; j<3; j++) result[
i][j] = 0;
714 template <
class MatType>
718 int index0 =
static_cast<int>(axis0);
719 int index1 =
static_cast<int>(axis1);
722 result.setIdentity();
723 if (axis0 == axis1) {
724 result[index1][index0] =
shear + 1;
726 result[index1][index0] =
shear;
734 template<
class MatType>
738 typedef typename MatType::value_type T;
741 r[0][0] = T(0); r[0][1] =
skew.z(); r[0][2] = -
skew.y();
742 r[1][0] = -
skew.z(); r[1][1] = T(0); r[2][1] =
skew.x();
743 r[2][0] =
skew.y(); r[2][1] = -
skew.x(); r[2][2] = T(0);
745 if(MatType::numColumns() == 4)
padMat4(r);
752 template<
class MatType>
757 typedef typename MatType::value_type T;
759 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
760 Vec3<T> up(forward.cross(horizontal).unit());
764 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
765 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
766 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
768 if(MatType::numColumns() == 4)
padMat4(r);
777 template<
class MatType>
781 typedef typename MatType::value_type T;
784 Vec3<T> ourUnitAxis(source.row(axis).unit());
787 T parallel = unitDir.
dot(ourUnitAxis);
797 T angleBetween(
angle(unitDir, ourUnitAxis));
802 rotation.setToRotation(rotationAxis, angleBetween);
809 template<
class MatType>
813 dest[0][3] = dest[1][3] = dest[2][3] = 0;
814 dest[3][2] = dest[3][1] = dest[3][0] = 0;
823 template<
typename MatType>
825 sqrtSolve(
const MatType& aA, MatType& aB,
double aTol=0.01)
827 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
831 Z[0] = MatType::identity();
833 unsigned int current = 0;
834 for (
unsigned int iteration=0; iteration < iterations; iteration++) {
835 unsigned int last = current;
838 MatType invY = Y[last].inverse();
839 MatType invZ = Z[last].inverse();
841 Y[current] = 0.5 * (Y[last] + invZ);
842 Z[current] = 0.5 * (Z[last] + invY);
848 template<
typename MatType>
850 powSolve(
const MatType& aA, MatType& aB,
double aPower,
double aTol=0.01)
852 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
854 const bool inverted = (aPower < 0.0);
855 if (inverted) { aPower = -aPower; }
857 unsigned int whole =
static_cast<unsigned int>(aPower);
858 double fraction = aPower - whole;
860 MatType R = MatType::identity();
861 MatType partial = aA;
863 double contribution = 1.0;
864 for (
unsigned int iteration = 0; iteration < iterations; iteration++) {
867 if (fraction >= contribution) {
869 fraction -= contribution;
875 if (whole & 1) { R *= partial; }
877 if (whole) { partial *= partial; }
880 if (inverted) { aB = R.inverse(); }
886 template<
typename MatType>
890 return m.eq(MatType::identity());
895 template<
typename MatType>
899 typedef typename MatType::ValueType ValueType;
906 template<
typename MatType>
910 return m.eq(m.transpose());
915 template<
typename MatType>
919 typedef typename MatType::ValueType value_type;
920 if (!
isApproxEqual(std::abs(m.det()), value_type(1.0)))
return false;
922 MatType temp = m * m.transpose();
923 return temp.eq(MatType::identity());
928 template<
typename MatType>
933 typename MatType::ValueType temp(0);
934 for (
int i = 0;
i < n; ++
i) {
935 for (
int j = 0; j < n; ++j) {
937 temp+=std::abs(mat(
i,j));
941 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
946 template<
typename MatType>
947 typename MatType::ValueType
951 typename MatType::ValueType norm = 0;
953 for(
int j = 0; j<n; ++j) {
954 typename MatType::ValueType column_sum = 0;
956 for (
int i = 0;
i<n; ++
i) {
957 column_sum += fabs(matrix(
i,j));
967 template<
typename MatType>
968 typename MatType::ValueType
972 typename MatType::ValueType norm = 0;
974 for(
int i = 0;
i<n; ++
i) {
975 typename MatType::ValueType row_sum = 0;
977 for (
int j = 0; j<n; ++j) {
978 row_sum += fabs(matrix(
i,j));
994 template<
typename MatType>
997 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
1000 MatType new_unitary(input);
1001 MatType unitary_inv;
1005 unsigned int iteration(0);
1007 typename MatType::ValueType linf_of_u;
1008 typename MatType::ValueType l1nm_of_u;
1009 typename MatType::ValueType linf_of_u_inv;
1010 typename MatType::ValueType l1nm_of_u_inv;
1011 typename MatType::ValueType l1_error = 100;
1015 unitary_inv = unitary.inverse();
1020 l1nm_of_u_inv =
lOneNorm(unitary_inv);
1022 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1024 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1027 unitary = new_unitary;
1030 if (iteration > MAX_ITERATIONS)
return false;
1034 positive_hermitian = unitary.transpose() * input;
1042 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
bool polarDecomposition(const MatType &input, MatType &unitary, MatType &positive_hermitian, unsigned int MAX_ITERATIONS=100)
Decompose an invertible 3x3 matrix into a unitary matrix followed by a symmetric matrix (positive sem...
Definition: Mat.h:996
Tolerance for floating-point comparison.
Definition: Math.h:117
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:897
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:969
T & w()
Definition: Quat.h:226
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
T & y()
Definition: Quat.h:224
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:471
Definition: Exceptions.h:88
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:215
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:888
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:182
T value_type
Definition: Mat.h:55
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:754
T & z()
Definition: Vec3.h:111
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:101
SIZE_
Definition: Mat.h:57
tbb::atomic< Index32 > i
Definition: LeafBuffer.h:71
Definition: Exceptions.h:82
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:354
void read(std::istream &is)
Definition: Mat.h:144
Vec3< typename MatType::value_type > getScale(const MatType &mat)
Return a Vec3 representing the lengths of the passed matrix's upper 3x3's rows.
Definition: Mat.h:661
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:388
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:174
static MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition: Mat.h:811
MatType rotation(const Vec3< typename MatType::value_type > &_v1, const Vec3< typename MatType::value_type > &_v2, typename MatType::value_type eps=1.0e-8)
Return a rotation matrix that maps v1 onto v2 about the cross product of v1 and v2.
Definition: Mat.h:531
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:643
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:825
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:149
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:930
static unsigned numColumns()
Definition: Mat.h:61
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:850
std::string str(unsigned indentation=0) const
Definition: Mat.h:94
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:779
static unsigned numElements()
Definition: Mat.h:62
#define OPENVDB_VERSION_NAME
Definition: version.h:43
T mm[SIZE *SIZE]
Definition: Mat.h:190
RotationOrder
Definition: Math.h:859
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:308
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
Coord Abs(const Coord &xyz)
Definition: Coord.h:509
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:376
Definition: Exceptions.h:39
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:244
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:166
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:132
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:908
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:109
T & y()
Definition: Vec3.h:110
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:736
Definition: Exceptions.h:92
void write(std::ostream &os) const
Definition: Mat.h:140
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:716
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 3x3 rows normalized, and return the length of each o...
Definition: Mat.h:689
Mat()
Definition: Mat.h:66
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:948
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
static unsigned numRows()
Definition: Mat.h:60
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
Axis
Definition: Math.h:852
Mat & operator=(Mat const &src)
Definition: Mat.h:75
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:363
T & z()
Definition: Quat.h:225
static constexpr size_t size
The size of a LeafBuffer when LeafBuffer::mOutOfCore is atomic.
Definition: LeafBuffer.h:85
T ValueType
Definition: Mat.h:56
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:917
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:158