Loading...
Searching...
No Matches
Mat3.h
Go to the documentation of this file.
// Copyright Contributors to the OpenVDB Project
// SPDX-License-Identifier: MPL-2.0
#ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
#define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
#include <openvdb/Exceptions.h>
#include "Vec3.h"
#include "Mat.h"
#include <algorithm> // for std::copy()
#include <cassert>
#include <cmath>
#include <iomanip>
namespace openvdb {
OPENVDB_USE_VERSION_NAMESPACE
namespace OPENVDB_VERSION_NAME {
namespace math {
template<typename T> class Vec3;
template<typename T> class Mat4;
template<typename T> class Quat;
/// @class Mat3 Mat3.h
/// @brief 3x3 matrix class.
template<typename T>
class Mat3: public Mat<3, T>
{
public:
/// Data type held by the matrix.
using value_type = T;
using ValueType = T;
using MyBase = Mat<3, T>;
/// Trivial constructor, the matrix is NOT initialized
/// @note destructor, copy constructor, assignment operator and
/// move constructor are left to be defined by the compiler (default)
Mat3() = default;
/// Constructor given the quaternion rotation, e.g. Mat3f m(q);
/// The quaternion is normalized and used to construct the matrix
Mat3(const Quat<T> &q)
{ setToRotation(q); }
/// Constructor given array of elements, the ordering is in row major form:
/** @verbatim
a b c
d e f
g h i
@endverbatim */
template<typename Source>
Mat3(Source a, Source b, Source c,
Source d, Source e, Source f,
Source g, Source h, Source i)
{
MyBase::mm[0] = static_cast<T>(a);
MyBase::mm[1] = static_cast<T>(b);
MyBase::mm[2] = static_cast<T>(c);
MyBase::mm[3] = static_cast<T>(d);
MyBase::mm[4] = static_cast<T>(e);
MyBase::mm[5] = static_cast<T>(f);
MyBase::mm[6] = static_cast<T>(g);
MyBase::mm[7] = static_cast<T>(h);
MyBase::mm[8] = static_cast<T>(i);
} // constructor1Test
/// Construct matrix from rows or columns vectors (defaults to rows
/// for historical reasons)
template<typename Source>
Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true)
{
if (rows) {
this->setRows(v1, v2, v3);
} else {
this->setColumns(v1, v2, v3);
}
}
/// Constructor given array of elements, the ordering is in row major form:\n
/// a[0] a[1] a[2]\n
/// a[3] a[4] a[5]\n
/// a[6] a[7] a[8]\n
template<typename Source>
Mat3(Source *a)
{
MyBase::mm[0] = static_cast<T>(a[0]);
MyBase::mm[1] = static_cast<T>(a[1]);
MyBase::mm[2] = static_cast<T>(a[2]);
MyBase::mm[3] = static_cast<T>(a[3]);
MyBase::mm[4] = static_cast<T>(a[4]);
MyBase::mm[5] = static_cast<T>(a[5]);
MyBase::mm[6] = static_cast<T>(a[6]);
MyBase::mm[7] = static_cast<T>(a[7]);
MyBase::mm[8] = static_cast<T>(a[8]);
} // constructor1Test
/// Conversion constructor
template<typename Source>
explicit Mat3(const Mat3<Source> &m)
{
for (int i=0; i<3; ++i) {
for (int j=0; j<3; ++j) {
MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]);
}
}
}
/// Conversion from Mat4 (copies top left)
explicit Mat3(const Mat4<T> &m)
{
for (int i=0; i<3; ++i) {
for (int j=0; j<3; ++j) {
MyBase::mm[i*3 + j] = m[i][j];
}
}
}
/// Predefined constant for identity matrix
static const Mat3<T>& identity() {
static const Mat3<T> sIdentity = Mat3<T>(
1, 0, 0,
0, 1, 0,
0, 0, 1
);
return sIdentity;
}
/// Predefined constant for zero matrix
static const Mat3<T>& zero() {
static const Mat3<T> sZero = Mat3<T>(
0, 0, 0,
0, 0, 0,
0, 0, 0
);
return sZero;
}
/// Set ith row to vector v
void setRow(int i, const Vec3<T> &v)
{
// assert(i>=0 && i<3);
int i3 = i * 3;
MyBase::mm[i3+0] = v[0];
MyBase::mm[i3+1] = v[1];
MyBase::mm[i3+2] = v[2];
} // rowColumnTest
/// Get ith row, e.g. Vec3d v = m.row(1);
Vec3<T> row(int i) const
{
// assert(i>=0 && i<3);
return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2));
} // rowColumnTest
/// Set jth column to vector v
void setCol(int j, const Vec3<T>& v)
{
// assert(j>=0 && j<3);
MyBase::mm[0+j] = v[0];
MyBase::mm[3+j] = v[1];
MyBase::mm[6+j] = v[2];
} // rowColumnTest
/// Get jth column, e.g. Vec3d v = m.col(0);
Vec3<T> col(int j) const
{
// assert(j>=0 && j<3);
return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j));
} // rowColumnTest
/// Alternative indexed reference to the elements
/// Note that the indices are row first and column second.
/// e.g. m(0,0) = 1;
T& operator()(int i, int j)
{
// assert(i>=0 && i<3);
// assert(j>=0 && j<3);
return MyBase::mm[3*i+j];
} // trivial
/// Alternative indexed constant reference to the elements,
/// Note that the indices are row first and column second.
/// e.g. float f = m(1,0);
T operator()(int i, int j) const
{
// assert(i>=0 && i<3);
// assert(j>=0 && j<3);
return MyBase::mm[3*i+j];
} // trivial
/// Set the rows of this matrix to the vectors v1, v2, v3
void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
{
MyBase::mm[0] = v1[0];
MyBase::mm[1] = v1[1];
MyBase::mm[2] = v1[2];
MyBase::mm[3] = v2[0];
MyBase::mm[4] = v2[1];
MyBase::mm[5] = v2[2];
MyBase::mm[6] = v3[0];
MyBase::mm[7] = v3[1];
MyBase::mm[8] = v3[2];
} // setRows
/// Set the columns of this matrix to the vectors v1, v2, v3
void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3)
{
MyBase::mm[0] = v1[0];
MyBase::mm[1] = v2[0];
MyBase::mm[2] = v3[0];
MyBase::mm[3] = v1[1];
MyBase::mm[4] = v2[1];
MyBase::mm[5] = v3[1];
MyBase::mm[6] = v1[2];
MyBase::mm[7] = v2[2];
MyBase::mm[8] = v3[2];
} // setColumns
/// Set diagonal and symmetric triangular components
void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
{
MyBase::mm[0] = vdiag[0];
MyBase::mm[1] = vtri[0];
MyBase::mm[2] = vtri[1];
MyBase::mm[3] = vtri[0];
MyBase::mm[4] = vdiag[1];
MyBase::mm[5] = vtri[2];
MyBase::mm[6] = vtri[1];
MyBase::mm[7] = vtri[2];
MyBase::mm[8] = vdiag[2];
} // setSymmetricTest
/// Return a matrix with the prescribed diagonal and symmetric triangular components.
static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri)
{
return Mat3(
vdiag[0], vtri[0], vtri[1],
vtri[0], vdiag[1], vtri[2],
vtri[1], vtri[2], vdiag[2]
);
}
/// Set the matrix as cross product of the given vector
void setSkew(const Vec3<T> &v)
{*this = skew(v);}
/// @brief Set this matrix to the rotation matrix specified by the quaternion
/// @details The quaternion is normalized and used to construct the matrix.
/// Note that the matrix is transposed to match post-multiplication semantics.
void setToRotation(const Quat<T> &q)
{*this = rotation<Mat3<T> >(q);}
/// @brief Set this matrix to the rotation specified by @a axis and @a angle
/// @details The axis must be unit vector
void setToRotation(const Vec3<T> &axis, T angle)
{*this = rotation<Mat3<T> >(axis, angle);}
/// Set this matrix to zero
void setZero()
{
MyBase::mm[0] = 0;
MyBase::mm[1] = 0;
MyBase::mm[2] = 0;
MyBase::mm[3] = 0;
MyBase::mm[4] = 0;
MyBase::mm[5] = 0;
MyBase::mm[6] = 0;
MyBase::mm[7] = 0;
MyBase::mm[8] = 0;
} // trivial
/// Set this matrix to identity
void setIdentity()
{
MyBase::mm[0] = 1;
MyBase::mm[1] = 0;
MyBase::mm[2] = 0;
MyBase::mm[3] = 0;
MyBase::mm[4] = 1;
MyBase::mm[5] = 0;
MyBase::mm[6] = 0;
MyBase::mm[7] = 0;
MyBase::mm[8] = 1;
} // trivial
/// Assignment operator
template<typename Source>
const Mat3& operator=(const Mat3<Source> &m)
{
const Source *src = m.asPointer();
// don't suppress type conversion warnings
std::copy(src, (src + this->numElements()), MyBase::mm);
return *this;
} // opEqualToTest
/// Return @c true if this matrix is equivalent to @a m within a tolerance of @a eps.
bool eq(const Mat3 &m, T eps=1.0e-8) const
{
return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) &&
isApproxEqual(MyBase::mm[1],m.mm[1],eps) &&
isApproxEqual(MyBase::mm[2],m.mm[2],eps) &&
isApproxEqual(MyBase::mm[3],m.mm[3],eps) &&
isApproxEqual(MyBase::mm[4],m.mm[4],eps) &&
isApproxEqual(MyBase::mm[5],m.mm[5],eps) &&
isApproxEqual(MyBase::mm[6],m.mm[6],eps) &&
isApproxEqual(MyBase::mm[7],m.mm[7],eps) &&
isApproxEqual(MyBase::mm[8],m.mm[8],eps));
} // trivial
/// Negation operator, for e.g. m1 = -m2;
Mat3<T> operator-() const
{
return Mat3<T>(
-MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2],
-MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5],
-MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8]
);
} // trivial
/// Multiplication operator, e.g. M = scalar * M;
// friend Mat3 operator*(T scalar, const Mat3& m) {
// return m*scalar;
// }
/// Multiply each element of this matrix by @a scalar.
template <typename S>
const Mat3<T>& operator*=(S scalar)
{
MyBase::mm[0] *= scalar;
MyBase::mm[1] *= scalar;
MyBase::mm[2] *= scalar;
MyBase::mm[3] *= scalar;
MyBase::mm[4] *= scalar;
MyBase::mm[5] *= scalar;
MyBase::mm[6] *= scalar;
MyBase::mm[7] *= scalar;
MyBase::mm[8] *= scalar;
return *this;
}
/// Add each element of the given matrix to the corresponding element of this matrix.
template <typename S>
const Mat3<T> &operator+=(const Mat3<S> &m1)
{
const S *s = m1.asPointer();
MyBase::mm[0] += s[0];
MyBase::mm[1] += s[1];
MyBase::mm[2] += s[2];
MyBase::mm[3] += s[3];
MyBase::mm[4] += s[4];
MyBase::mm[5] += s[5];
MyBase::mm[6] += s[6];
MyBase::mm[7] += s[7];
MyBase::mm[8] += s[8];
return *this;
}
/// Subtract each element of the given matrix from the corresponding element of this matrix.
template <typename S>
const Mat3<T> &operator-=(const Mat3<S> &m1)
{
const S *s = m1.asPointer();
MyBase::mm[0] -= s[0];
MyBase::mm[1] -= s[1];
MyBase::mm[2] -= s[2];
MyBase::mm[3] -= s[3];
MyBase::mm[4] -= s[4];
MyBase::mm[5] -= s[5];
MyBase::mm[6] -= s[6];
MyBase::mm[7] -= s[7];
MyBase::mm[8] -= s[8];
return *this;
}
/// Multiply this matrix by the given matrix.
template <typename S>
const Mat3<T> &operator*=(const Mat3<S> &m1)
{
Mat3<T> m0(*this);
const T* s0 = m0.asPointer();
const S* s1 = m1.asPointer();
MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] +
s0[1] * s1[3] +
s0[2] * s1[6]);
MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] +
s0[1] * s1[4] +
s0[2] * s1[7]);
MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] +
s0[1] * s1[5] +
s0[2] * s1[8]);
MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] +
s0[4] * s1[3] +
s0[5] * s1[6]);
MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] +
s0[4] * s1[4] +
s0[5] * s1[7]);
MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] +
s0[4] * s1[5] +
s0[5] * s1[8]);
MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] +
s0[7] * s1[3] +
s0[8] * s1[6]);
MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] +
s0[7] * s1[4] +
s0[8] * s1[7]);
MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] +
s0[7] * s1[5] +
s0[8] * s1[8]);
return *this;
}
/// @brief Return the cofactor matrix of this matrix.
Mat3 cofactor() const
{
return Mat3<T>(
MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
}
/// Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Mat3 adjoint() const
{
return Mat3<T>(
MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7],
MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8],
MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4],
MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8],
MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6],
MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5],
MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6],
MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7],
MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]);
} // adjointTest
/// returns transpose of this
Mat3 transpose() const
{
return Mat3<T>(
MyBase::mm[0], MyBase::mm[3], MyBase::mm[6],
MyBase::mm[1], MyBase::mm[4], MyBase::mm[7],
MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]);
} // transposeTest
/// returns inverse of this
/// @throws ArithmeticError if singular
Mat3 inverse(T tolerance = 0) const
{
Mat3<T> inv(this->adjoint());
const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6];
// If the determinant is 0, m was singular and the result will be invalid.
if (isApproxEqual(det,T(0.0),tolerance)) {
OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix");
}
return inv * (T(1)/det);
} // invertTest
/// Determinant of matrix
T det() const
{
const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7];
const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8];
const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6];
return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20;
} // determinantTest
/// Trace of matrix
T trace() const
{
return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8];
}
/// This function snaps a specific axis to a specific direction,
/// preserving scaling. It does this using minimum energy, thus
/// posing a unique solution if basis & direction arent parralel.
/// Direction need not be unit.
Mat3 snapBasis(Axis axis, const Vec3<T> &direction)
{
return snapMatBasis(*this, axis, direction);
}
/// Return the transformed vector by this matrix.
/// This function is equivalent to post-multiplying the matrix.
template<typename T0>
Vec3<T0> transform(const Vec3<T0> &v) const
{
return static_cast< Vec3<T0> >(v * *this);
} // xformVectorTest
/// Return the transformed vector by transpose of this matrix.
/// This function is equivalent to pre-multiplying the matrix.
template<typename T0>
Vec3<T0> pretransform(const Vec3<T0> &v) const
{
return static_cast< Vec3<T0> >(*this * v);
} // xformTVectorTest
/// @brief Treat @a diag as a diagonal matrix and return the product
/// of this matrix with @a diag (from the right).
Mat3 timesDiagonal(const Vec3<T>& diag) const
{
Mat3 ret(*this);
ret.mm[0] *= diag(0);
ret.mm[1] *= diag(1);
ret.mm[2] *= diag(2);
ret.mm[3] *= diag(0);
ret.mm[4] *= diag(1);
ret.mm[5] *= diag(2);
ret.mm[6] *= diag(0);
ret.mm[7] *= diag(1);
ret.mm[8] *= diag(2);
return ret;
}
}; // class Mat3
/// @relates Mat3
/// @brief Equality operator, does exact floating point comparisons
template <typename T0, typename T1>
bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1)
{
const T0 *t0 = m0.asPointer();
const T1 *t1 = m1.asPointer();
for (int i=0; i<9; ++i) {
if (!isExactlyEqual(t0[i], t1[i])) return false;
}
return true;
}
/// @relates Mat3
/// @brief Inequality operator, does exact floating point comparisons
template <typename T0, typename T1>
bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); }
/// @relates Mat3
/// @brief Multiply each element of the given matrix by @a scalar and return the result.
template <typename S, typename T>
Mat3<typename promote<S, T>::type> operator*(S scalar, const Mat3<T> &m)
{ return m*scalar; }
/// @relates Mat3
/// @brief Multiply each element of the given matrix by @a scalar and return the result.
template <typename S, typename T>
Mat3<typename promote<S, T>::type> operator*(const Mat3<T> &m, S scalar)
{
Mat3<typename promote<S, T>::type> result(m);
result *= scalar;
return result;
}
/// @relates Mat3
/// @brief Add corresponding elements of @a m0 and @a m1 and return the result.
template <typename T0, typename T1>
Mat3<typename promote<T0, T1>::type> operator+(const Mat3<T0> &m0, const Mat3<T1> &m1)
{
Mat3<typename promote<T0, T1>::type> result(m0);
result += m1;
return result;
}
/// @relates Mat3
/// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result.
template <typename T0, typename T1>
Mat3<typename promote<T0, T1>::type> operator-(const Mat3<T0> &m0, const Mat3<T1> &m1)
{
Mat3<typename promote<T0, T1>::type> result(m0);
result -= m1;
return result;
}
/// @brief Multiply @a m0 by @a m1 and return the resulting matrix.
template <typename T0, typename T1>
Mat3<typename promote<T0, T1>::type>operator*(const Mat3<T0> &m0, const Mat3<T1> &m1)
{
Mat3<typename promote<T0, T1>::type> result(m0);
result *= m1;
return result;
}
/// @relates Mat3
/// @brief Multiply @a _m by @a _v and return the resulting vector.
template<typename T, typename MT>
inline Vec3<typename promote<T, MT>::type>
operator*(const Mat3<MT> &_m, const Vec3<T> &_v)
{
MT const *m = _m.asPointer();
return Vec3<typename promote<T, MT>::type>(
_v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2],
_v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5],
_v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]);
}
/// @relates Mat3
/// @brief Multiply @a _v by @a _m and return the resulting vector.
template<typename T, typename MT>
inline Vec3<typename promote<T, MT>::type>
operator*(const Vec3<T> &_v, const Mat3<MT> &_m)
{
MT const *m = _m.asPointer();
return Vec3<typename promote<T, MT>::type>(
_v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6],
_v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7],
_v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]);
}
/// @relates Mat3
/// @brief Multiply @a _v by @a _m and replace @a _v with the resulting vector.
template<typename T, typename MT>
inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m)
{
Vec3<T> mult = _v * _m;
_v = mult;
return _v;
}
/// Returns outer product of v1, v2, i.e. v1 v2^T if v1 and v2 are
/// column vectors, e.g. M = Mat3f::outerproduct(v1,v2);
template <typename T>
Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2)
{
return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2],
v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2],
v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]);
}// outerProduct
/// Interpolate the rotation between m1 and m2 using Mat::powSolve.
/// Unlike slerp, translation is not treated independently.
/// This results in smoother animation results.
template<typename T, typename T0>
Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t)
{
Mat3<T> x = m1.inverse() * m2;
powSolve(x, x, t);
Mat3<T> m = m1 * x;
return m;
}
namespace mat3_internal {
template<typename T>
inline void
pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q)
{
const int& n = Mat3<T>::size; // should be 3
T temp;
/// scratch variables used in pivoting
double cotan_of_2_theta;
double tan_of_theta;
double cosin_of_theta;
double sin_of_theta;
double z;
double Sij = S(i,j);
double Sjj_minus_Sii = D[j] - D[i];
if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) {
tan_of_theta = Sij / Sjj_minus_Sii;
} else {
/// pivot on Sij
cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ;
if (cotan_of_2_theta < 0.) {
tan_of_theta =
-1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta);
} else {
tan_of_theta =
1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta);
}
}
cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta);
sin_of_theta = cosin_of_theta * tan_of_theta;
z = tan_of_theta * Sij;
S(i,j) = 0;
D[i] -= z;
D[j] += z;
for (int k = 0; k < i; ++k) {
temp = S(k,i);
S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j);
S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j);
}
for (int k = i+1; k < j; ++k) {
temp = S(i,k);
S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j);
S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j);
}
for (int k = j+1; k < n; ++k) {
temp = S(i,k);
S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k);
S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k);
}
for (int k = 0; k < n; ++k)
{
temp = Q(k,i);
Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j);
Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j);
}
}
} // namespace mat3_internal
/// @brief Use Jacobi iterations to decompose a symmetric 3x3 matrix
/// (diagonalize and compute eigenvectors)
/// @details This is based on the "Efficient numerical diagonalization of Hermitian 3x3 matrices"
/// Joachim Kopp. arXiv.org preprint: physics/0610206
/// with the addition of largest pivot
template<typename T>
inline bool
diagonalizeSymmetricMatrix(const Mat3<T>& input, Mat3<T>& Q, Vec3<T>& D,
unsigned int MAX_ITERATIONS=250)
{
/// use Givens rotation matrix to eliminate off-diagonal entries.
/// initialize the rotation matrix as idenity
Q = Mat3<T>::identity();
int n = Mat3<T>::size; // should be 3
/// temp matrix. Assumed to be symmetric
Mat3<T> S(input);
for (int i = 0; i < n; ++i) {
D[i] = S(i,i);
}
unsigned int iterations(0);
/// Just iterate over all the non-diagonal enteries
/// using the largest as a pivot.
do {
/// check for absolute convergence
/// are symmetric off diagonals all zero
double er = 0;
for (int i = 0; i < n; ++i) {
for (int j = i+1; j < n; ++j) {
er += fabs(S(i,j));
}
}
if (std::abs(er) < math::Tolerance<T>::value()) {
return true;
}
iterations++;
T max_element = 0;
int ip = 0;
int jp = 0;
/// loop over all the off-diagonals above the diagonal
for (int i = 0; i < n; ++i) {
for (int j = i+1; j < n; ++j){
if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) {
/// value too small to pivot on
S(i,j) = 0;
}
if (fabs(S(i,j)) > max_element) {
max_element = fabs(S(i,j));
ip = i;
jp = j;
}
}
}
mat3_internal::pivot(ip, jp, S, D, Q);
} while (iterations < MAX_ITERATIONS);
return false;
}
template<typename T>
inline Mat3<T>
Abs(const Mat3<T>& m)
{
Mat3<T> out;
const T* ip = m.asPointer();
T* op = out.asPointer();
for (unsigned i = 0; i < 9; ++i, ++op, ++ip) *op = math::Abs(*ip);
return out;
}
template<typename Type1, typename Type2>
inline Mat3<Type1>
cwiseAdd(const Mat3<Type1>& m, const Type2 s)
{
Mat3<Type1> out;
const Type1* ip = m.asPointer();
Type1* op = out.asPointer();
for (unsigned i = 0; i < 9; ++i, ++op, ++ip) {
OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
*op = *ip + s;
OPENVDB_NO_TYPE_CONVERSION_WARNING_END
}
return out;
}
template<typename T>
inline bool
cwiseLessThan(const Mat3<T>& m0, const Mat3<T>& m1)
{
return cwiseLessThan<3, T>(m0, m1);
}
template<typename T>
inline bool
cwiseGreaterThan(const Mat3<T>& m0, const Mat3<T>& m1)
{
return cwiseGreaterThan<3, T>(m0, m1);
}
using Mat3s = Mat3<float>;
using Mat3d = Mat3<double>;
using Mat3f = Mat3d;
OPENVDB_IS_POD(Mat3s)
OPENVDB_IS_POD(Mat3d)
} // namespace math
template<> inline math::Mat3s zeroVal<math::Mat3s>() { return math::Mat3s::zero(); }
template<> inline math::Mat3d zeroVal<math::Mat3d>() { return math::Mat3d::zero(); }
} // namespace OPENVDB_VERSION_NAME
} // namespace openvdb
#endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED
Mat.h
OPENVDB_IS_POD
#define OPENVDB_IS_POD(Type)
Definition Math.h:56
OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN
Bracket code with OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN/_END, to inhibit warnings about type conve...
Definition Platform.h:204
OPENVDB_NO_TYPE_CONVERSION_WARNING_END
#define OPENVDB_NO_TYPE_CONVERSION_WARNING_END
Definition Platform.h:205
Vec3.h
openvdb::v10_0::ArithmeticError
Definition Exceptions.h:56
openvdb::v10_0::math::Mat3
3x3 matrix class.
Definition Mat3.h:29
openvdb::v10_0::math::Mat3::Mat3
Mat3(Source *a)
Definition Mat3.h:86
openvdb::v10_0::math::Mat3::Mat3
Mat3(const Mat4< T > &m)
Conversion from Mat4 (copies top left).
Definition Mat3.h:111
openvdb::v10_0::math::Mat3::operator-=
const Mat3< T > & operator-=(const Mat3< S > &m1)
Subtract each element of the given matrix from the corresponding element of this matrix.
Definition Mat3.h:365
openvdb::v10_0::math::Mat3< Real >::setRows
void setRows(const Vec3< Real > &v1, const Vec3< Real > &v2, const Vec3< Real > &v3)
Definition Mat3.h:195
openvdb::v10_0::math::Mat3::operator*
Mat3< typename promote< S, T >::type > operator*(S scalar, const Mat3< T > &m)
Multiply each element of the given matrix by scalar and return the result.
Definition Mat3.h:561
openvdb::v10_0::math::Mat3::operator!=
bool operator!=(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Inequality operator, does exact floating point comparisons.
Definition Mat3.h:556
openvdb::v10_0::math::Mat3::Mat3
Mat3()=default
openvdb::v10_0::math::Mat3::operator+
Mat3< typename promote< T0, T1 >::type > operator+(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Add corresponding elements of m0 and m1 and return the result.
Definition Mat3.h:577
openvdb::v10_0::math::Mat3::operator*
Mat3< typename promote< S, T >::type > operator*(const Mat3< T > &m, S scalar)
Multiply each element of the given matrix by scalar and return the result.
Definition Mat3.h:567
openvdb::v10_0::math::Mat3::setIdentity
void setIdentity()
Set this matrix to identity.
Definition Mat3.h:276
openvdb::v10_0::math::Mat3::Mat3
Mat3(Source a, Source b, Source c, Source d, Source e, Source f, Source g, Source h, Source i)
Constructor given array of elements, the ordering is in row major form:
Definition Mat3.h:54
openvdb::v10_0::math::Mat3::Mat3
Mat3(const Mat3< Source > &m)
Conversion constructor.
Definition Mat3.h:101
openvdb::v10_0::math::Mat3::setToRotation
void setToRotation(const Vec3< T > &axis, T angle)
Set this matrix to the rotation specified by axis and angle.
Definition Mat3.h:258
openvdb::v10_0::math::Mat3::setZero
void setZero()
Set this matrix to zero.
Definition Mat3.h:262
openvdb::v10_0::math::Mat3::operator=
const Mat3 & operator=(const Mat3< Source > &m)
Assignment operator.
Definition Mat3.h:291
openvdb::v10_0::math::Mat3::Mat3
Mat3(const Quat< T > &q)
Definition Mat3.h:43
openvdb::v10_0::math::Mat3::operator+=
const Mat3< T > & operator+=(const Mat3< S > &m1)
Add each element of the given matrix to the corresponding element of this matrix.
Definition Mat3.h:347
openvdb::v10_0::math::Mat3::pretransform
Vec3< T0 > pretransform(const Vec3< T0 > &v) const
Definition Mat3.h:513
openvdb::v10_0::math::Mat3::eq
bool eq(const Mat3 &m, T eps=1.0e-8) const
Return true if this matrix is equivalent to m within a tolerance of eps.
Definition Mat3.h:301
openvdb::v10_0::math::Mat3< Real >::setToRotation
void setToRotation(const Quat< Real > &q)
Definition Mat3.h:253
openvdb::v10_0::math::Mat3::inverse
Mat3 inverse(T tolerance=0) const
Definition Mat3.h:465
openvdb::v10_0::math::Mat3::trace
T trace() const
Trace of matrix.
Definition Mat3.h:488
openvdb::v10_0::math::Mat3::operator-
Mat3< T > operator-() const
Negation operator, for e.g. m1 = -m2;.
Definition Mat3.h:315
openvdb::v10_0::math::Mat3::setCol
void setCol(int j, const Vec3< T > &v)
Set jth column to vector v.
Definition Mat3.h:159
openvdb::v10_0::math::Mat3::adjoint
Mat3 adjoint() const
Return the adjoint of this matrix, i.e., the transpose of its cofactor.
Definition Mat3.h:438
openvdb::v10_0::math::Mat3::setSkew
void setSkew(const Vec3< T > &v)
Set the matrix as cross product of the given vector.
Definition Mat3.h:247
openvdb::v10_0::math::Mat3::operator*=
const Mat3< T > & operator*=(S scalar)
Multiplication operator, e.g. M = scalar * M;.
Definition Mat3.h:331
openvdb::v10_0::math::Mat3< Real >::MyBase
Mat< 3, Real > MyBase
Definition Mat3.h:34
openvdb::v10_0::math::Mat3::operator*
Vec3< typename promote< T, MT >::type > operator*(const Mat3< MT > &_m, const Vec3< T > &_v)
Multiply _m by _v and return the resulting vector.
Definition Mat3.h:608
openvdb::v10_0::math::Mat3::snapBasis
Mat3 snapBasis(Axis axis, const Vec3< T > &direction)
Definition Mat3.h:497
openvdb::v10_0::math::Mat3::transpose
Mat3 transpose() const
returns transpose of this
Definition Mat3.h:454
openvdb::v10_0::math::Mat3::timesDiagonal
Mat3 timesDiagonal(const Vec3< T > &diag) const
Treat diag as a diagonal matrix and return the product of this matrix with diag (from the right).
Definition Mat3.h:521
openvdb::v10_0::math::Mat3< Real >::det
Real det() const
Definition Mat3.h:479
openvdb::v10_0::math::Mat3::zero
static const Mat3< T > & zero()
Predefined constant for zero matrix.
Definition Mat3.h:131
openvdb::v10_0::math::Mat3::col
Vec3< T > col(int j) const
Get jth column, e.g. Vec3d v = m.col(0);.
Definition Mat3.h:168
openvdb::v10_0::math::Mat3::operator()
T & operator()(int i, int j)
Definition Mat3.h:177
openvdb::v10_0::math::Mat3::symmetric
static Mat3 symmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Return a matrix with the prescribed diagonal and symmetric triangular components.
Definition Mat3.h:237
openvdb::v10_0::math::Mat3::transform
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition Mat3.h:505
openvdb::v10_0::math::Mat3::identity
static const Mat3< T > & identity()
Predefined constant for identity matrix.
Definition Mat3.h:121
openvdb::v10_0::math::Mat3::operator()
T operator()(int i, int j) const
Definition Mat3.h:187
openvdb::v10_0::math::Mat3::operator==
bool operator==(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Equality operator, does exact floating point comparisons.
Definition Mat3.h:542
openvdb::v10_0::math::Mat3::operator*=
const Mat3< T > & operator*=(const Mat3< S > &m1)
Multiply this matrix by the given matrix.
Definition Mat3.h:383
openvdb::v10_0::math::Mat3::row
Vec3< T > row(int i) const
Get ith row, e.g. Vec3d v = m.row(1);.
Definition Mat3.h:152
openvdb::v10_0::math::Mat3::setRow
void setRow(int i, const Vec3< T > &v)
Set ith row to vector v.
Definition Mat3.h:141
openvdb::v10_0::math::Mat3::operator*
Vec3< typename promote< T, MT >::type > operator*(const Vec3< T > &_v, const Mat3< MT > &_m)
Multiply _v by _m and return the resulting vector.
Definition Mat3.h:621
openvdb::v10_0::math::Mat3< Real >::ValueType
Real ValueType
Definition Mat3.h:33
openvdb::v10_0::math::Mat3< Real >::setColumns
void setColumns(const Vec3< Real > &v1, const Vec3< Real > &v2, const Vec3< Real > &v3)
Definition Mat3.h:209
openvdb::v10_0::math::Mat3::cofactor
Mat3 cofactor() const
Return the cofactor matrix of this matrix.
Definition Mat3.h:423
openvdb::v10_0::math::Mat3::operator-
Mat3< typename promote< T0, T1 >::type > operator-(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Subtract corresponding elements of m0 and m1 and return the result.
Definition Mat3.h:587
openvdb::v10_0::math::Mat3::Mat3
Mat3(const Vec3< Source > &v1, const Vec3< Source > &v2, const Vec3< Source > &v3, bool rows=true)
Definition Mat3.h:72
openvdb::v10_0::math::Mat3::setSymmetric
void setSymmetric(const Vec3< T > &vdiag, const Vec3< T > &vtri)
Set diagonal and symmetric triangular components.
Definition Mat3.h:223
openvdb::v10_0::math::Mat3< Real >::value_type
Real value_type
Definition Mat3.h:32
openvdb::v10_0::math::Mat4
4x4 -matrix class.
Definition Mat4.h:31
openvdb::v10_0::math::Mat::mm
T mm[SIZE *SIZE]
Definition Mat.h:160
openvdb::v10_0::math::Mat< 3, T >::Mat
Mat()=default
openvdb::v10_0::math::Mat::asPointer
T * asPointer()
Direct access to the internal data.
Definition Mat.h:101
openvdb::v10_0::math::Mat< 3, T >::size
@ size
Definition Mat.h:31
openvdb::v10_0::math::Mat3< Real >::numElements
static unsigned numElements()
Definition Mat.h:41
openvdb::v10_0::math::Quat
Definition Quat.h:79
openvdb::v10_0::math::Vec3
Definition Vec3.h:24
openvdb::v10_0::math::mat3_internal
Definition Mat3.h:664
openvdb::v10_0::math::mat3_internal::pivot
void pivot(int i, int j, Mat3< T > &S, Vec3< T > &D, Mat3< T > &Q)
Definition Mat3.h:668
openvdb::v10_0::math::snapMatBasis
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
openvdb::v10_0::math::diagonalizeSymmetricMatrix
bool diagonalizeSymmetricMatrix(const Mat3< T > &input, Mat3< T > &Q, Vec3< T > &D, unsigned int MAX_ITERATIONS=250)
Use Jacobi iterations to decompose a symmetric 3x3 matrix (diagonalize and compute eigenvectors).
Definition Mat3.h:737
openvdb::v10_0::math::operator*
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Multiply m0 by m1 and return the resulting matrix.
Definition Mat3.h:597
openvdb::v10_0::math::Mat3d
Mat3< double > Mat3d
Definition Mat3.h:834
openvdb::v10_0::math::cwiseLessThan
bool cwiseLessThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1015
openvdb::v10_0::math::isApproxEqual
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
openvdb::v10_0::math::cwiseAdd
Mat3< Type1 > cwiseAdd(const Mat3< Type1 > &m, const Type2 s)
Definition Mat3.h:806
openvdb::v10_0::math::Mat3s
Mat3< float > Mat3s
Definition Mat3.h:833
openvdb::v10_0::math::rotation
MatType rotation(const Quat< typename MatType::value_type > &q, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the rotation matrix specified by the given quaternion.
Definition Mat.h:172
openvdb::v10_0::math::powLerp
Mat3< T > powLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Definition Mat3.h:655
openvdb::v10_0::math::Mat3f
Mat3d Mat3f
Definition Mat3.h:835
openvdb::v10_0::math::Abs
Coord Abs(const Coord &xyz)
Definition Coord.h:515
openvdb::v10_0::math::outerProduct
Mat3< T > outerProduct(const Vec3< T > &v1, const Vec3< T > &v2)
Definition Mat3.h:643
openvdb::v10_0::math::angle
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition Vec2.h:446
openvdb::v10_0::math::powSolve
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition Mat.h:822
openvdb::v10_0::math::isExactlyEqual
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition Math.h:443
openvdb::v10_0::math::skew
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition Mat.h:708
openvdb::v10_0::math::Axis
Axis
Definition Math.h:901
openvdb::v10_0::math::cwiseGreaterThan
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1029
openvdb::v10_0::zeroVal
T zeroVal()
Return the value of type T that corresponds to zero.
Definition Math.h:70
openvdb
Definition Exceptions.h:13
Exceptions.h
OPENVDB_THROW
#define OPENVDB_THROW(exception, message)
Definition Exceptions.h:74
openvdb::v10_0::math::Tolerance::value
static T value()
Definition Math.h:148
OPENVDB_VERSION_NAME
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition version.h.in:121
OPENVDB_USE_VERSION_NAMESPACE
#define OPENVDB_USE_VERSION_NAMESPACE
Definition version.h.in:212