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) {
85 str(
unsigned indentation = 0)
const {
91 indent.append(indentation+1,
' ');
96 for (
unsigned i(0); i < SIZE; i++) {
101 for (
unsigned j(0); j < SIZE; j++) {
104 if (j) ret.append(
", ");
105 ret.append((boost::format(
"%1%") % mm[(i*SIZE)+j]).str());
114 ret.append((boost::format(
",\n%1%") % indent).str());
131 void write(std::ostream& os)
const {
132 os.write(reinterpret_cast<const char*>(&mm),
sizeof(T)*SIZE*SIZE);
136 is.read(reinterpret_cast<char*>(&mm),
sizeof(T)*SIZE*SIZE);
145 template<
typename T>
class Quat;
146 template<
typename T>
class Vec3;
151 template<
class MatType>
154 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
156 typedef typename MatType::value_type T;
179 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
180 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
181 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
183 if(MatType::numColumns() == 4)
padMat4(r);
192 template<
class MatType>
196 typedef typename MatType::value_type T;
197 T c =
static_cast<T
>(cos(angle));
198 T s =
static_cast<T
>(sin(angle));
201 result.setIdentity();
223 throw ValueError(
"Unrecognized rotation axis");
230 template<
class MatType>
234 typedef typename MatType::value_type T;
235 T txy, txz, tyz, sx, sy, sz;
240 T c(cos(
double(angle)));
241 T s(sin(
double(angle)));
246 result[0][0] = axis[0]*axis[0] * t + c;
247 result[1][1] = axis[1]*axis[1] * t + c;
248 result[2][2] = axis[2]*axis[2] * t + c;
250 txy = axis[0]*axis[1] * t;
253 txz = axis[0]*axis[2] * t;
256 tyz = axis[1]*axis[2] * t;
261 result[0][1] = txy + sz;
262 result[1][0] = txy - sz;
264 result[0][2] = txz - sy;
265 result[2][0] = txz + sy;
267 result[1][2] = tyz + sx;
268 result[2][1] = tyz - sx;
270 if(MatType::numColumns() == 4)
padMat4(result);
271 return MatType(result);
312 template<
class MatType>
317 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
319 typedef typename MatType::value_type ValueType;
321 ValueType phi, theta, psi;
323 switch(rotationOrder)
327 theta = ValueType(M_PI_2);
328 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
331 theta = ValueType(-M_PI_2);
332 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
335 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
336 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
337 theta = ValueType(atan2(mat[2][0],
338 sqrt( mat[2][1]*mat[2][1] +
339 mat[2][2]*mat[2][2])));
341 return V(phi, theta, psi);
344 theta = ValueType(M_PI_2);
345 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
348 theta = ValueType(-M_PI/2);
349 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
352 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
353 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
354 theta = ValueType(atan2(mat[1][2],
355 sqrt(mat[0][2] * mat[0][2] +
356 mat[2][2] * mat[2][2])));
358 return V(theta, psi, phi);
362 theta = ValueType(M_PI_2);
363 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
366 theta = ValueType(-M_PI/2);
367 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
370 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
371 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
372 theta = ValueType(atan2(mat[0][1],
373 sqrt(mat[0][0] * mat[0][0] +
374 mat[0][2] * mat[0][2])));
376 return V(psi, phi, theta);
381 theta = ValueType(0.0);
382 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
385 theta = ValueType(M_PI);
386 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
389 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
390 phi = ValueType(atan2(mat[0][2], mat[0][1]));
391 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
392 mat[0][2] * mat[0][2]),
395 return V(phi, psi, theta);
400 theta = ValueType(0.0);
401 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
404 theta = ValueType(M_PI);
405 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
408 psi = ValueType(atan2(mat[0][2], mat[1][2]));
409 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
410 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
411 mat[1][2] * mat[1][2]),
414 return V(theta, psi, phi);
419 theta = ValueType(-M_PI_2);
420 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
423 theta = ValueType(M_PI_2);
424 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
427 psi = ValueType(atan2(mat[0][1], mat[1][1]));
428 phi = ValueType(atan2(mat[2][0], mat[2][2]));
429 theta = ValueType(atan2(-mat[2][1],
430 sqrt(mat[0][1] * mat[0][1] +
431 mat[1][1] * mat[1][1])));
433 return V(theta, phi, psi);
438 theta = ValueType(-M_PI_2);
439 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
442 theta = ValueType(M_PI_2);
443 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
446 psi = ValueType(atan2(mat[1][2], mat[2][2]));
447 phi = ValueType(atan2(mat[0][1], mat[0][0]));
448 theta = ValueType(atan2(-mat[0][2],
449 sqrt(mat[0][1] * mat[0][1] +
450 mat[0][0] * mat[0][0])));
452 return V(psi, theta, phi);
457 theta = ValueType(M_PI_2);
458 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
461 theta = ValueType(-M_PI_2);
462 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
465 psi = ValueType(atan2(mat[2][0], mat[0][0]));
466 phi = ValueType(atan2(mat[1][2], mat[1][1]));
467 theta = ValueType(atan2(- mat[1][0],
468 sqrt(mat[1][1] * mat[1][1] +
469 mat[1][2] * mat[1][2])));
471 return V(phi, psi, theta);
480 template<
class MatType>
485 typename MatType::value_type eps=1.0e-8)
487 typedef typename MatType::value_type T;
516 Vec3<T> u, v, p(0.0, 0.0, 0.0);
518 double x =
Abs(v1[0]);
519 double y =
Abs(v1[1]);
520 double z =
Abs(v1[2]);
538 double udot = u.
dot(u);
539 double vdot = v.
dot(v);
541 double a = -2 / udot;
542 double b = -2 / vdot;
543 double c = 4 * u.
dot(v) / (udot * vdot);
546 result.setIdentity();
548 for (
int j = 0; j < 3; j++) {
549 for (
int i = 0; i < 3; i++)
551 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i];
557 if(MatType::numColumns() == 4)
padMat4(result);
561 double c = v1.
dot(v2);
562 double a = (1.0 - c) / cross.
dot(cross);
564 double a0 = a * cross[0];
565 double a1 = a * cross[1];
566 double a2 = a * cross[2];
568 double a01 = a0 * cross[1];
569 double a02 = a0 * cross[2];
570 double a12 = a1 * cross[2];
574 r[0][0] = c + a0 * cross[0];
575 r[0][1] = a01 + cross[2];
576 r[0][2] = a02 - cross[1],
577 r[1][0] = a01 - cross[2];
578 r[1][1] = c + a1 * cross[1];
579 r[1][2] = a12 + cross[0];
580 r[2][0] = a02 + cross[1];
581 r[2][1] = a12 - cross[0];
582 r[2][2] = c + a2 * cross[2];
584 if(MatType::numColumns() == 4)
padMat4(r);
592 template<
class MatType>
600 result.setIdentity();
610 template<
class MatType>
616 V(mat[0][0], mat[0][1], mat[0][2]).length(),
617 V(mat[1][0], mat[1][1], mat[1][2]).length(),
618 V(mat[2][0], mat[2][1], mat[2][2]).length());
625 template<
class MatType>
627 unit(
const MatType &mat,
typename MatType::value_type eps = 1.0e-8)
630 return unit(mat, eps, dud);
638 template<
class MatType>
642 typename MatType::value_type eps,
645 typedef typename MatType::value_type T;
648 for (
int i(0); i < 3; i++) {
651 Vec3<T>(in[i][0], in[i][1], in[i][2]).
unit(eps, scaling[i]));
652 for (
int j=0; j<3; j++) result[i][j] = u[j];
654 for (
int j=0; j<3; j++) result[i][j] = 0;
665 template <
class MatType>
669 int index0 =
static_cast<int>(axis0);
670 int index1 =
static_cast<int>(axis1);
673 result.setIdentity();
674 if (axis0 == axis1) {
675 result[index1][index0] = shear + 1;
677 result[index1][index0] =
shear;
685 template<
class MatType>
689 typedef typename MatType::value_type T;
692 r[0][0] = T(0); r[0][1] = skew.
z(); r[0][2] = -skew.
y();
693 r[1][0] = -skew.
z(); r[1][1] = T(0); r[2][1] = skew.
x();
694 r[2][0] = skew.
y(); r[2][1] = -skew.
x(); r[2][2] = T(0);
696 if(MatType::numColumns() == 4)
padMat4(r);
703 template<
class MatType>
708 typedef typename MatType::value_type T;
710 Vec3<T> horizontal(vertical.
unit().cross(forward).unit());
711 Vec3<T> up(forward.cross(horizontal).unit());
715 r[0][0]=horizontal.
x(); r[0][1]=horizontal.
y(); r[0][2]=horizontal.
z();
716 r[1][0]=up.
x(); r[1][1]=up.
y(); r[1][2]=up.
z();
717 r[2][0]=forward.
x(); r[2][1]=forward.
y(); r[2][2]=forward.
z();
719 if(MatType::numColumns() == 4)
padMat4(r);
728 template<
class MatType>
732 typedef typename MatType::value_type T;
735 Vec3<T> ourUnitAxis(source.row(axis).unit());
738 T parallel = unitDir.
dot(ourUnitAxis);
748 T angleBetween(
angle(unitDir, ourUnitAxis));
753 rotation.setToRotation(rotationAxis, angleBetween);
760 template<
class MatType>
764 dest[0][3] = dest[1][3] = dest[2][3] = 0;
765 dest[3][2] = dest[3][1] = dest[3][0] = 0;
774 template <
typename MatType>
776 sqrtSolve(
const MatType &aA, MatType &aB,
double aTol=0.01)
778 unsigned int iterations = (
unsigned int)(log(aTol)/log(0.5));
784 unsigned int current = 0;
787 Z[0] = MatType::identity();
789 unsigned int iteration;
790 for (iteration=0; iteration<iterations; iteration++)
792 unsigned int last = current;
795 invY = Y[last].inverse();
796 invZ = Z[last].inverse();
798 Y[current]=0.5*(Y[last]+invZ);
799 Z[current]=0.5*(Z[last]+invY);
802 MatType &R = Y[current];
808 template <
typename MatType>
810 powSolve(
const MatType &aA, MatType &aB,
double aPower,
double aTol=0.01)
812 unsigned int iterations = (
unsigned int)(log(aTol)/log(0.5));
814 const bool inverted = ( aPower < 0.0 );
820 unsigned int whole = (
unsigned int)aPower;
821 double fraction = aPower - whole;
824 R = MatType::identity();
826 MatType partial = aA;
828 double contribution = 1.0;
830 unsigned int iteration;
832 for (iteration=0; iteration< iterations; iteration++)
837 if (fraction>=contribution)
840 fraction-=contribution;
866 template<
typename MatType>
870 return m.eq(MatType::identity());
875 template<
typename MatType>
879 typedef typename MatType::ValueType value_type;
886 template<
typename MatType>
890 return m.eq(m.transpose());
895 template<
typename MatType>
899 typedef typename MatType::ValueType value_type;
900 if (!
isApproxEqual(std::abs(m.det()), value_type(1.0)))
return false;
902 MatType temp = m * m.transpose();
903 return temp.eq(MatType::identity());
908 template<
typename MatType>
912 int n = MatType::size;
913 typename MatType::ValueType temp(0);
914 for (
int i = 0; i < n; ++i) {
915 for (
int j = 0; j < n; ++j) {
917 temp+=std::abs(mat(i,j));
921 return isApproxEqual(temp,
typename MatType::ValueType(0.0));
926 template<
typename MatType>
927 typename MatType::ValueType
930 int n = MatType::size;
931 typename MatType::ValueType norm = 0;
933 for(
int j = 0; j<n; ++j) {
934 typename MatType::ValueType column_sum = 0;
936 for (
int i = 0; i<n; ++i) {
937 column_sum += fabs(matrix(i,j));
947 template<
typename MatType>
948 typename MatType::ValueType
951 int n = MatType::size;
952 typename MatType::ValueType norm = 0;
954 for(
int i = 0; i<n; ++i) {
955 typename MatType::ValueType row_sum = 0;
957 for (
int j = 0; j<n; ++j) {
958 row_sum += fabs(matrix(i,j));
974 template<
typename MatType>
977 MatType& positive_hermitian,
unsigned int MAX_ITERATIONS=100)
980 MatType new_unitary(input);
985 unsigned int iteration(0);
987 typename MatType::ValueType linf_of_u;
988 typename MatType::ValueType l1nm_of_u;
989 typename MatType::ValueType linf_of_u_inv;
990 typename MatType::ValueType l1nm_of_u_inv;
991 typename MatType::ValueType l1_error = 100;
995 unitary_inv = unitary.inverse();
1000 l1nm_of_u_inv =
lOneNorm(unitary_inv);
1002 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1004 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1007 unitary = new_unitary;
1010 if (iteration > MAX_ITERATIONS)
return false;
1014 positive_hermitian = unitary.transpose() * input;
1022 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
T & z()
Definition: Quat.h:225
T & y()
Definition: Quat.h:224
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:776
Coord Abs(const Coord &xyz)
Definition: Coord.h:247
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:203
std::string str(unsigned indentation=0) const
Definition: Mat.h:85
T & z()
Definition: Vec3.h:99
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:482
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:360
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:97
T mm[SIZE *SIZE]
Definition: Mat.h:141
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:897
T & w()
Definition: Quat.h:226
Definition: Exceptions.h:78
T ValueType
Definition: Mat.h:56
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:928
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:667
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:123
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
static unsigned numRows()
Definition: Mat.h:60
RotationOrder
Definition: Math.h:863
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:640
#define OPENVDB_VERSION_NAME
Definition: version.h:43
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:97
T value_type
Definition: Mat.h:55
Definition: Exceptions.h:39
Tolerance for floating-point comparison.
Definition: Math.h:125
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:594
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
Mat()
Definition: Mat.h:66
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:949
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:348
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:612
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:232
void read(std::istream &is)
Definition: Mat.h:135
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:877
T & y()
Definition: Vec3.h:98
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:705
static unsigned numColumns()
Definition: Mat.h:61
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:314
Definition: Exceptions.h:84
Definition: Exceptions.h:88
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:730
Axis
Definition: Math.h:856
SIZE_
Definition: Mat.h:57
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:687
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:810
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
static MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition: Mat.h:762
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:888
void write(std::ostream &os) const
Definition: Mat.h:131
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:910
static unsigned numElements()
Definition: Mat.h:62
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:868
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:976