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 (
int i = 1; i < SIZE*SIZE; ++i)
152 x =
std::max(x, static_cast<T>(std::fabs(mm[i])));
161 template<
typename T>
class Quat;
162 template<
typename T>
class Vec3;
167 template<
class MatType>
170 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
172 typedef typename MatType::value_type T;
195 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
196 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
197 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
199 if(MatType::numColumns() == 4)
padMat4(r);
208 template<
class MatType>
212 typedef typename MatType::value_type T;
213 T c =
static_cast<T
>(cos(angle));
214 T s =
static_cast<T
>(sin(angle));
217 result.setIdentity();
239 throw ValueError(
"Unrecognized rotation axis");
246 template<
class MatType>
250 typedef typename MatType::value_type T;
251 T txy, txz, tyz, sx, sy, sz;
256 T c(cos(
double(angle)));
257 T s(sin(
double(angle)));
262 result[0][0] = axis[0]*axis[0] * t + c;
263 result[1][1] = axis[1]*axis[1] * t + c;
264 result[2][2] = axis[2]*axis[2] * t + c;
266 txy = axis[0]*axis[1] * t;
269 txz = axis[0]*axis[2] * t;
272 tyz = axis[1]*axis[2] * t;
277 result[0][1] = txy + sz;
278 result[1][0] = txy - sz;
280 result[0][2] = txz - sy;
281 result[2][0] = txz + sy;
283 result[1][2] = tyz + sx;
284 result[2][1] = tyz - sx;
286 if(MatType::numColumns() == 4)
padMat4(result);
287 return MatType(result);
328 template<
class MatType>
333 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
335 typedef typename MatType::value_type ValueType;
337 ValueType phi, theta, psi;
339 switch(rotationOrder)
343 theta = ValueType(M_PI_2);
344 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
347 theta = ValueType(-M_PI_2);
348 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
351 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
352 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
353 theta = ValueType(atan2(mat[2][0],
354 sqrt( mat[2][1]*mat[2][1] +
355 mat[2][2]*mat[2][2])));
357 return V(phi, theta, psi);
360 theta = ValueType(M_PI_2);
361 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
364 theta = ValueType(-M_PI/2);
365 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
368 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
369 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
370 theta = ValueType(atan2(mat[1][2],
371 sqrt(mat[0][2] * mat[0][2] +
372 mat[2][2] * mat[2][2])));
374 return V(theta, psi, phi);
378 theta = ValueType(M_PI_2);
379 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
382 theta = ValueType(-M_PI/2);
383 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
386 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
387 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
388 theta = ValueType(atan2(mat[0][1],
389 sqrt(mat[0][0] * mat[0][0] +
390 mat[0][2] * mat[0][2])));
392 return V(psi, phi, theta);
397 theta = ValueType(0.0);
398 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
401 theta = ValueType(M_PI);
402 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
405 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
406 phi = ValueType(atan2(mat[0][2], mat[0][1]));
407 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
408 mat[0][2] * mat[0][2]),
411 return V(phi, psi, theta);
416 theta = ValueType(0.0);
417 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
420 theta = ValueType(M_PI);
421 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
424 psi = ValueType(atan2(mat[0][2], mat[1][2]));
425 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
426 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
427 mat[1][2] * mat[1][2]),
430 return V(theta, psi, phi);
435 theta = ValueType(-M_PI_2);
436 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
439 theta = ValueType(M_PI_2);
440 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
443 psi = ValueType(atan2(mat[0][1], mat[1][1]));
444 phi = ValueType(atan2(mat[2][0], mat[2][2]));
445 theta = ValueType(atan2(-mat[2][1],
446 sqrt(mat[0][1] * mat[0][1] +
447 mat[1][1] * mat[1][1])));
449 return V(theta, phi, psi);
454 theta = ValueType(-M_PI_2);
455 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
458 theta = ValueType(M_PI_2);
459 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
462 psi = ValueType(atan2(mat[1][2], mat[2][2]));
463 phi = ValueType(atan2(mat[0][1], mat[0][0]));
464 theta = ValueType(atan2(-mat[0][2],
465 sqrt(mat[0][1] * mat[0][1] +
466 mat[0][0] * mat[0][0])));
468 return V(psi, theta, phi);
473 theta = ValueType(M_PI_2);
474 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
477 theta = ValueType(-M_PI_2);
478 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
481 psi = ValueType(atan2(mat[2][0], mat[0][0]));
482 phi = ValueType(atan2(mat[1][2], mat[1][1]));
483 theta = ValueType(atan2(- mat[1][0],
484 sqrt(mat[1][1] * mat[1][1] +
485 mat[1][2] * mat[1][2])));
487 return V(phi, psi, theta);
496 template<
class MatType>
501 typename MatType::value_type eps=1.0e-8)
503 typedef typename MatType::value_type T;
532 Vec3<T> u, v, p(0.0, 0.0, 0.0);
534 double x =
Abs(v1[0]);
535 double y =
Abs(v1[1]);
536 double z =
Abs(v1[2]);
554 double udot = u.
dot(u);
555 double vdot = v.
dot(v);
557 double a = -2 / udot;
558 double b = -2 / vdot;
559 double c = 4 * u.
dot(v) / (udot * vdot);
562 result.setIdentity();
564 for (
int j = 0; j < 3; j++) {
565 for (
int i = 0; i < 3; i++)
567 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i];
573 if(MatType::numColumns() == 4)
padMat4(result);
577 double c = v1.
dot(v2);
578 double a = (1.0 - c) / cross.
dot(cross);
580 double a0 = a * cross[0];
581 double a1 = a * cross[1];
582 double a2 = a * cross[2];
584 double a01 = a0 * cross[1];
585 double a02 = a0 * cross[2];
586 double a12 = a1 * cross[2];
590 r[0][0] = c + a0 * cross[0];
591 r[0][1] = a01 + cross[2];
592 r[0][2] = a02 - cross[1],
593 r[1][0] = a01 - cross[2];
594 r[1][1] = c + a1 * cross[1];
595 r[1][2] = a12 + cross[0];
596 r[2][0] = a02 + cross[1];
597 r[2][1] = a12 - cross[0];
598 r[2][2] = c + a2 * cross[2];
600 if(MatType::numColumns() == 4)
padMat4(r);
608 template<
class MatType>
616 result.setIdentity();
626 template<
class MatType>
632 V(mat[0][0], mat[0][1], mat[0][2]).length(),
633 V(mat[1][0], mat[1][1], mat[1][2]).length(),
634 V(mat[2][0], mat[2][1], mat[2][2]).length());
641 template<
class MatType>
643 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
646 return unit(mat, eps, dud);
654 template<
class MatType>
658 typename MatType::value_type eps,
661 typedef typename MatType::value_type T;
664 for (
int i(0); i < 3; i++) {
667 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
668 for (
int j=0; j<3; j++) result[i][j] = u[j];
670 for (
int j=0; j<3; j++) result[i][j] = 0;
681 template <
class MatType>
685 int index0 =
static_cast<int>(axis0);
686 int index1 =
static_cast<int>(axis1);
689 result.setIdentity();
690 if (axis0 == axis1) {
691 result[index1][index0] = shear + 1;
693 result[index1][index0] =
shear;
701 template<
class MatType>
705 typedef typename MatType::value_type T;
708 r[0][0] = T(0); r[0][1] = skew.
z(); r[0][2] = -skew.
y();
709 r[1][0] = -skew.
z(); r[1][1] = T(0); r[2][1] = skew.
x();
710 r[2][0] = skew.
y(); r[2][1] = -skew.
x(); r[2][2] = T(0);
712 if(MatType::numColumns() == 4)
padMat4(r);
719 template<
class MatType>
724 typedef typename MatType::value_type T;
726 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
727 Vec3<T> up(forward.cross(horizontal).unit());
731 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
732 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
733 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
735 if(MatType::numColumns() == 4)
padMat4(r);
744 template<
class MatType>
748 typedef typename MatType::value_type T;
751 Vec3<T> ourUnitAxis(source.row(axis).unit());
754 T parallel = unitDir.
dot(ourUnitAxis);
764 T angleBetween(
angle(unitDir, ourUnitAxis));
769 rotation.setToRotation(rotationAxis, angleBetween);
776 template<
class MatType>
780 dest[0][3] = dest[1][3] = dest[2][3] = 0;
781 dest[3][2] = dest[3][1] = dest[3][0] = 0;
790 template<
typename MatType>
792 sqrtSolve(
const MatType& aA, MatType& aB,
double aTol=0.01)
794 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
798 Z[0] = MatType::identity();
800 unsigned int current = 0;
801 for (
unsigned int iteration=0; iteration < iterations; iteration++) {
802 unsigned int last = current;
805 MatType invY = Y[last].inverse();
806 MatType invZ = Z[last].inverse();
808 Y[current] = 0.5 * (Y[last] + invZ);
809 Z[current] = 0.5 * (Z[last] + invY);
815 template<
typename MatType>
817 powSolve(
const MatType& aA, MatType& aB,
double aPower,
double aTol=0.01)
819 unsigned int iterations =
static_cast<unsigned int>(log(aTol)/log(0.5));
821 const bool inverted = (aPower < 0.0);
822 if (inverted) { aPower = -aPower; }
824 unsigned int whole =
static_cast<unsigned int>(aPower);
825 double fraction = aPower - whole;
827 MatType R = MatType::identity();
828 MatType partial = aA;
830 double contribution = 1.0;
831 for (
unsigned int iteration = 0; iteration < iterations; iteration++) {
834 if (fraction >= contribution) {
836 fraction -= contribution;
842 if (whole & 1) { R *= partial; }
844 if (whole) { partial *= partial; }
847 if (inverted) { aB = R.inverse(); }
853 template<
typename MatType>
857 return m.eq(MatType::identity());
862 template<
typename MatType>
866 typedef typename MatType::ValueType ValueType;
873 template<
typename MatType>
877 return m.eq(m.transpose());
882 template<
typename MatType>
886 typedef typename MatType::ValueType value_type;
887 if (!
isApproxEqual(std::abs(m.det()), value_type(1.0)))
return false;
889 MatType temp = m * m.transpose();
890 return temp.eq(MatType::identity());
895 template<
typename MatType>
899 int n = MatType::size;
900 typename MatType::ValueType temp(0);
901 for (
int i = 0; i < n; ++i) {
902 for (
int j = 0; j < n; ++j) {
904 temp+=std::abs(mat(i,j));
908 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
913 template<
typename MatType>
914 typename MatType::ValueType
917 int n = MatType::size;
918 typename MatType::ValueType norm = 0;
920 for(
int j = 0; j<n; ++j) {
921 typename MatType::ValueType column_sum = 0;
923 for (
int i = 0; i<n; ++i) {
924 column_sum += fabs(matrix(i,j));
934 template<
typename MatType>
935 typename MatType::ValueType
938 int n = MatType::size;
939 typename MatType::ValueType norm = 0;
941 for(
int i = 0; i<n; ++i) {
942 typename MatType::ValueType row_sum = 0;
944 for (
int j = 0; j<n; ++j) {
945 row_sum += fabs(matrix(i,j));
961 template<
typename MatType>
964 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
967 MatType new_unitary(input);
972 unsigned int iteration(0);
974 typename MatType::ValueType linf_of_u;
975 typename MatType::ValueType l1nm_of_u;
976 typename MatType::ValueType linf_of_u_inv;
977 typename MatType::ValueType l1nm_of_u_inv;
978 typename MatType::ValueType l1_error = 100;
982 unitary_inv = unitary.inverse();
987 l1nm_of_u_inv =
lOneNorm(unitary_inv);
989 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
991 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
994 unitary = new_unitary;
997 if (iteration > MAX_ITERATIONS)
return false;
1001 positive_hermitian = unitary.transpose() * input;
1009 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:875
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:963
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:370
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:498
T & w()
Definition: Quat.h:226
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
Coord Abs(const Coord &xyz)
Definition: Coord.h:254
T & y()
Definition: Quat.h:224
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:132
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:480
T & z()
Definition: Vec3.h:111
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:101
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:683
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
static unsigned numRows()
Definition: Mat.h:60
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:855
Mat & operator=(Mat const &src)
Definition: Mat.h:75
static MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition: Mat.h:778
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:628
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:610
void write(std::ostream &os) const
Definition: Mat.h:140
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:817
RotationOrder
Definition: Math.h:863
T ValueType
Definition: Mat.h:56
Mat()
Definition: Mat.h:66
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:388
#define OPENVDB_VERSION_NAME
Definition: version.h:43
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:215
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:376
Definition: Exceptions.h:39
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:109
T & y()
Definition: Vec3.h:110
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:792
SIZE_
Definition: Mat.h:57
Definition: Exceptions.h:82
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:330
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:884
Definition: Exceptions.h:92
T value_type
Definition: Mat.h:55
Definition: Exceptions.h:88
static unsigned numElements()
Definition: Mat.h:62
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:915
Axis
Definition: Math.h:856
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:703
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:149
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:721
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:656
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:897
void read(std::istream &is)
Definition: Mat.h:144
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:864
T & z()
Definition: Quat.h:225
static unsigned numColumns()
Definition: Mat.h:61
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:746
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:936
Tolerance for floating-point comparison.
Definition: Math.h:125
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:244
T mm[SIZE *SIZE]
Definition: Mat.h:157