Loading...
Searching...
No Matches
Mat.h
Go to the documentation of this file.
// Copyright Contributors to the OpenVDB Project
// SPDX-License-Identifier: MPL-2.0
//
/// @file Mat.h
/// @author Joshua Schpok
#ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
#define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
#include "Math.h"
#include <openvdb/Exceptions.h>
#include <algorithm> // for std::max()
#include <cmath>
#include <iostream>
#include <string>
namespace openvdb {
OPENVDB_USE_VERSION_NAMESPACE
namespace OPENVDB_VERSION_NAME {
namespace math {
/// @class Mat "Mat.h"
/// A base class for square matrices.
template<unsigned SIZE, typename T>
class Mat
{
public:
using value_type = T;
using ValueType = T;
enum SIZE_ { size = SIZE };
/// 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)
Mat() = default;
// Number of cols, rows, elements
static unsigned numRows() { return SIZE; }
static unsigned numColumns() { return SIZE; }
static unsigned numElements() { return SIZE*SIZE; }
/// @return string representation of matrix
/// Since output is multiline, optional indentation argument prefixes
/// each newline with that much white space. It does not indent
/// the first line, since you might be calling this inline:
///
/// cout << "matrix: " << mat.str(7)
///
/// matrix: [[1 2]
/// [3 4]]
std::string
str(unsigned indentation = 0) const {
std::string ret;
std::string indent;
// We add +1 since we're indenting one for the first '['
indent.append(indentation+1, ' ');
ret.append("[");
// For each row,
for (unsigned i(0); i < SIZE; i++) {
ret.append("[");
// For each column
for (unsigned j(0); j < SIZE; j++) {
// Put a comma after everything except the last
if (j) ret.append(", ");
ret.append(std::to_string(mm[(i*SIZE)+j]));
}
ret.append("]");
// At the end of every row (except the last)...
if (i < SIZE - 1) {
// ...suffix the row bracket with a comma, newline, and advance indentation.
ret.append(",\n");
ret.append(indent);
}
}
ret.append("]");
return ret;
}
/// Write a Mat to an output stream
friend std::ostream& operator<<(
std::ostream& ostr,
const Mat<SIZE, T>& m)
{
ostr << m.str();
return ostr;
}
/// Direct access to the internal data
T* asPointer() { return mm; }
const T* asPointer() const { return mm; }
//@{
/// Array style reference to ith row
T* operator[](int i) { return &(mm[i*SIZE]); }
const T* operator[](int i) const { return &(mm[i*SIZE]); }
//@}
void write(std::ostream& os) const {
os.write(reinterpret_cast<const char*>(&mm), sizeof(T)*SIZE*SIZE);
}
void read(std::istream& is) {
is.read(reinterpret_cast<char*>(&mm), sizeof(T)*SIZE*SIZE);
}
/// Return the maximum of the absolute of all elements in this matrix
T absMax() const {
T x = static_cast<T>(std::fabs(mm[0]));
for (unsigned i = 1; i < numElements(); ++i) {
x = std::max(x, static_cast<T>(std::fabs(mm[i])));
}
return x;
}
/// True if a Nan is present in this matrix
bool isNan() const {
for (unsigned i = 0; i < numElements(); ++i) {
if (math::isNan(mm[i])) return true;
}
return false;
}
/// True if an Inf is present in this matrix
bool isInfinite() const {
for (unsigned i = 0; i < numElements(); ++i) {
if (math::isInfinite(mm[i])) return true;
}
return false;
}
/// True if no Nan or Inf values are present
bool isFinite() const {
for (unsigned i = 0; i < numElements(); ++i) {
if (!math::isFinite(mm[i])) return false;
}
return true;
}
/// True if all elements are exactly zero
bool isZero() const {
for (unsigned i = 0; i < numElements(); ++i) {
if (!math::isZero(mm[i])) return false;
}
return true;
}
protected:
T mm[SIZE*SIZE];
};
template<typename T> class Quat;
template<typename T> class Vec3;
/// @brief Return the rotation matrix specified by the given quaternion.
/// @details The quaternion is normalized and used to construct the matrix.
/// Note that the matrix is transposed to match post-multiplication semantics.
template<class MatType>
MatType
rotation(const Quat<typename MatType::value_type> &q,
typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
{
using T = typename MatType::value_type;
T qdot(q.dot(q));
T s(0);
if (!isApproxEqual(qdot, T(0.0),eps)) {
s = T(2.0 / qdot);
}
T x = s*q.x();
T y = s*q.y();
T z = s*q.z();
T wx = x*q.w();
T wy = y*q.w();
T wz = z*q.w();
T xx = x*q.x();
T xy = y*q.x();
T xz = z*q.x();
T yy = y*q.y();
T yz = z*q.y();
T zz = z*q.z();
MatType r;
r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
if(MatType::numColumns() == 4) padMat4(r);
return r;
}
/// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
/// @param axis The axis (one of X, Y, Z) to rotate about.
/// @param angle The rotation angle, in radians.
template<class MatType>
MatType
rotation(Axis axis, typename MatType::value_type angle)
{
using T = typename MatType::value_type;
T c = static_cast<T>(cos(angle));
T s = static_cast<T>(sin(angle));
MatType result;
result.setIdentity();
switch (axis) {
case X_AXIS:
result[1][1] = c;
result[1][2] = s;
result[2][1] = -s;
result[2][2] = c;
return result;
case Y_AXIS:
result[0][0] = c;
result[0][2] = -s;
result[2][0] = s;
result[2][2] = c;
return result;
case Z_AXIS:
result[0][0] = c;
result[0][1] = s;
result[1][0] = -s;
result[1][1] = c;
return result;
default:
throw ValueError("Unrecognized rotation axis");
}
}
/// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
/// @note The axis must be a unit vector.
template<class MatType>
MatType
rotation(const Vec3<typename MatType::value_type> &_axis, typename MatType::value_type angle)
{
using T = typename MatType::value_type;
T txy, txz, tyz, sx, sy, sz;
Vec3<T> axis(_axis.unit());
// compute trig properties of angle:
T c(cos(double(angle)));
T s(sin(double(angle)));
T t(1 - c);
MatType result;
// handle diagonal elements
result[0][0] = axis[0]*axis[0] * t + c;
result[1][1] = axis[1]*axis[1] * t + c;
result[2][2] = axis[2]*axis[2] * t + c;
txy = axis[0]*axis[1] * t;
sz = axis[2] * s;
txz = axis[0]*axis[2] * t;
sy = axis[1] * s;
tyz = axis[1]*axis[2] * t;
sx = axis[0] * s;
// right handed space
// Contribution from rotation about 'z'
result[0][1] = txy + sz;
result[1][0] = txy - sz;
// Contribution from rotation about 'y'
result[0][2] = txz - sy;
result[2][0] = txz + sy;
// Contribution from rotation about 'x'
result[1][2] = tyz + sx;
result[2][1] = tyz - sx;
if(MatType::numColumns() == 4) padMat4(result);
return MatType(result);
}
/// @brief Return the Euler angles composing the given rotation matrix.
/// @details Optional axes arguments describe in what order elementary rotations
/// are applied. Note that in our convention, XYZ means Rz * Ry * Rx.
/// Because we are using rows rather than columns to represent the
/// local axes of a coordinate frame, the interpretation from a local
/// reference point of view is to first rotate about the x axis, then
/// about the newly rotated y axis, and finally by the new local z axis.
/// From a fixed reference point of view, the interpretation is to
/// rotate about the stationary world z, y, and x axes respectively.
///
/// Irrespective of the Euler angle convention, in the case of distinct
/// axes, eulerAngles() returns the x, y, and z angles in the corresponding
/// x, y, z components of the returned Vec3. For the XZX convention, the
/// left X value is returned in Vec3.x, and the right X value in Vec3.y.
/// For the ZXZ convention the left Z value is returned in Vec3.z and
/// the right Z value in Vec3.y
///
/// Examples of reconstructing r from its Euler angle decomposition
///
/// v = eulerAngles(r, ZYX_ROTATION);
/// rx.setToRotation(Vec3d(1,0,0), v[0]);
/// ry.setToRotation(Vec3d(0,1,0), v[1]);
/// rz.setToRotation(Vec3d(0,0,1), v[2]);
/// r = rx * ry * rz;
///
/// v = eulerAngles(r, ZXZ_ROTATION);
/// rz1.setToRotation(Vec3d(0,0,1), v[2]);
/// rx.setToRotation (Vec3d(1,0,0), v[0]);
/// rz2.setToRotation(Vec3d(0,0,1), v[1]);
/// r = rz2 * rx * rz1;
///
/// v = eulerAngles(r, XZX_ROTATION);
/// rx1.setToRotation (Vec3d(1,0,0), v[0]);
/// rx2.setToRotation (Vec3d(1,0,0), v[1]);
/// rz.setToRotation (Vec3d(0,0,1), v[2]);
/// r = rx2 * rz * rx1;
///
template<class MatType>
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))
{
using ValueType = typename MatType::value_type;
using V = Vec3<ValueType>;
ValueType phi, theta, psi;
switch(rotationOrder)
{
case XYZ_ROTATION:
if (isApproxEqual(mat[2][0], ValueType(1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
psi = phi;
} else if (isApproxEqual(mat[2][0], ValueType(-1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
psi = -phi;
} else {
psi = ValueType(atan2(-mat[1][0],mat[0][0]));
phi = ValueType(atan2(-mat[2][1],mat[2][2]));
theta = ValueType(atan2(mat[2][0],
sqrt( mat[2][1]*mat[2][1] +
mat[2][2]*mat[2][2])));
}
return V(phi, theta, psi);
case ZXY_ROTATION:
if (isApproxEqual(mat[1][2], ValueType(1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
psi = phi;
} else if (isApproxEqual(mat[1][2], ValueType(-1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
psi = -phi;
} else {
psi = ValueType(atan2(-mat[0][2], mat[2][2]));
phi = ValueType(atan2(-mat[1][0], mat[1][1]));
theta = ValueType(atan2(mat[1][2],
sqrt(mat[0][2] * mat[0][2] +
mat[2][2] * mat[2][2])));
}
return V(theta, psi, phi);
case YZX_ROTATION:
if (isApproxEqual(mat[0][1], ValueType(1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
psi = phi;
} else if (isApproxEqual(mat[0][1], ValueType(-1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
psi = -phi;
} else {
psi = ValueType(atan2(-mat[2][1], mat[1][1]));
phi = ValueType(atan2(-mat[0][2], mat[0][0]));
theta = ValueType(atan2(mat[0][1],
sqrt(mat[0][0] * mat[0][0] +
mat[0][2] * mat[0][2])));
}
return V(psi, phi, theta);
case XZX_ROTATION:
if (isApproxEqual(mat[0][0], ValueType(1.0), eps)) {
theta = ValueType(0.0);
phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
psi = phi;
} else if (isApproxEqual(mat[0][0], ValueType(-1.0), eps)) {
theta = ValueType(math::pi<double>());
psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
phi = - psi;
} else {
psi = ValueType(atan2(mat[2][0], -mat[1][0]));
phi = ValueType(atan2(mat[0][2], mat[0][1]));
theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
mat[0][2] * mat[0][2]),
mat[0][0]));
}
return V(phi, psi, theta);
case ZXZ_ROTATION:
if (isApproxEqual(mat[2][2], ValueType(1.0), eps)) {
theta = ValueType(0.0);
phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
psi = phi;
} else if (isApproxEqual(mat[2][2], ValueType(-1.0), eps)) {
theta = ValueType(math::pi<double>());
phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
psi = -phi;
} else {
psi = ValueType(atan2(mat[0][2], mat[1][2]));
phi = ValueType(atan2(mat[2][0], -mat[2][1]));
theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
mat[1][2] * mat[1][2]),
mat[2][2]));
}
return V(theta, psi, phi);
case YXZ_ROTATION:
if (isApproxEqual(mat[2][1], ValueType(1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
psi = phi;
} else if (isApproxEqual(mat[2][1], ValueType(-1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
psi = -phi;
} else {
psi = ValueType(atan2(mat[0][1], mat[1][1]));
phi = ValueType(atan2(mat[2][0], mat[2][2]));
theta = ValueType(atan2(-mat[2][1],
sqrt(mat[0][1] * mat[0][1] +
mat[1][1] * mat[1][1])));
}
return V(theta, phi, psi);
case ZYX_ROTATION:
if (isApproxEqual(mat[0][2], ValueType(1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
psi = phi;
} else if (isApproxEqual(mat[0][2], ValueType(-1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
psi = -phi;
} else {
psi = ValueType(atan2(mat[1][2], mat[2][2]));
phi = ValueType(atan2(mat[0][1], mat[0][0]));
theta = ValueType(atan2(-mat[0][2],
sqrt(mat[0][1] * mat[0][1] +
mat[0][0] * mat[0][0])));
}
return V(psi, theta, phi);
case XZY_ROTATION:
if (isApproxEqual(mat[1][0], ValueType(-1.0), eps)) {
theta = ValueType(math::pi<double>() / 2.0);
psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
phi = -psi;
} else if (isApproxEqual(mat[1][0], ValueType(1.0), eps)) {
theta = ValueType(-math::pi<double>() / 2.0);
psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
phi = psi;
} else {
psi = ValueType(atan2(mat[2][0], mat[0][0]));
phi = ValueType(atan2(mat[1][2], mat[1][1]));
theta = ValueType(atan2(- mat[1][0],
sqrt(mat[1][1] * mat[1][1] +
mat[1][2] * mat[1][2])));
}
return V(phi, psi, theta);
}
OPENVDB_THROW(NotImplementedError, "Euler extraction sequence not implemented");
}
/// @brief Return a rotation matrix that maps @a v1 onto @a v2
/// about the cross product of @a v1 and @a v2.
/// <a name="rotation_v1_v2"></a>
template<typename MatType, typename ValueType1, typename ValueType2>
inline MatType
rotation(
const Vec3<ValueType1>& _v1,
const Vec3<ValueType2>& _v2,
typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
{
using T = typename MatType::value_type;
Vec3<T> v1(_v1);
Vec3<T> v2(_v2);
// Check if v1 and v2 are unit length
if (!isApproxEqual(T(1), v1.dot(v1), eps)) {
v1.normalize();
}
if (!isApproxEqual(T(1), v2.dot(v2), eps)) {
v2.normalize();
}
Vec3<T> cross;
cross.cross(v1, v2);
if (isApproxEqual(cross[0], zeroVal<T>(), eps) &&
isApproxEqual(cross[1], zeroVal<T>(), eps) &&
isApproxEqual(cross[2], zeroVal<T>(), eps)) {
// Given two unit vectors v1 and v2 that are nearly parallel, build a
// rotation matrix that maps v1 onto v2. First find which principal axis
// p is closest to perpendicular to v1. Find a reflection that exchanges
// v1 and p, and find a reflection that exchanges p2 and v2. The desired
// rotation matrix is the composition of these two reflections. See the
// paper "Efficiently Building a Matrix to Rotate One Vector to
// Another" by Tomas Moller and John Hughes in Journal of Graphics
// Tools Vol 4, No 4 for details.
Vec3<T> u, v, p(0.0, 0.0, 0.0);
double x = Abs(v1[0]);
double y = Abs(v1[1]);
double z = Abs(v1[2]);
if (x < y) {
if (z < x) {
p[2] = 1;
} else {
p[0] = 1;
}
} else {
if (z < y) {
p[2] = 1;
} else {
p[1] = 1;
}
}
u = p - v1;
v = p - v2;
double udot = u.dot(u);
double vdot = v.dot(v);
double a = -2 / udot;
double b = -2 / vdot;
double c = 4 * u.dot(v) / (udot * vdot);
MatType result;
result.setIdentity();
for (int j = 0; j < 3; j++) {
for (int i = 0; i < 3; i++)
result[i][j] = static_cast<T>(
a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
}
result[0][0] += 1.0;
result[1][1] += 1.0;
result[2][2] += 1.0;
if(MatType::numColumns() == 4) padMat4(result);
return result;
} else {
double c = v1.dot(v2);
double a = (1.0 - c) / cross.dot(cross);
double a0 = a * cross[0];
double a1 = a * cross[1];
double a2 = a * cross[2];
double a01 = a0 * cross[1];
double a02 = a0 * cross[2];
double a12 = a1 * cross[2];
MatType r;
r[0][0] = static_cast<T>(c + a0 * cross[0]);
r[0][1] = static_cast<T>(a01 + cross[2]);
r[0][2] = static_cast<T>(a02 - cross[1]);
r[1][0] = static_cast<T>(a01 - cross[2]);
r[1][1] = static_cast<T>(c + a1 * cross[1]);
r[1][2] = static_cast<T>(a12 + cross[0]);
r[2][0] = static_cast<T>(a02 + cross[1]);
r[2][1] = static_cast<T>(a12 - cross[0]);
r[2][2] = static_cast<T>(c + a2 * cross[2]);
if(MatType::numColumns() == 4) padMat4(r);
return r;
}
}
/// Return a matrix that scales by @a s.
template<class MatType>
MatType
scale(const Vec3<typename MatType::value_type>& s)
{
// Gets identity, then sets top 3 diagonal
// Inefficient by 3 sets.
MatType result;
result.setIdentity();
result[0][0] = s[0];
result[1][1] = s[1];
result[2][2] = s[2];
return result;
}
/// Return a Vec3 representing the lengths of the passed matrix's upper 3&times;3's rows.
template<class MatType>
Vec3<typename MatType::value_type>
getScale(const MatType &mat)
{
using V = Vec3<typename MatType::value_type>;
return V(
V(mat[0][0], mat[0][1], mat[0][2]).length(),
V(mat[1][0], mat[1][1], mat[1][2]).length(),
V(mat[2][0], mat[2][1], mat[2][2]).length());
}
/// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized.
/// @details This can be geometrically interpreted as a matrix with no scaling
/// along its major axes.
template<class MatType>
MatType
unit(const MatType &mat, typename MatType::value_type eps = 1.0e-8)
{
Vec3<typename MatType::value_type> dud;
return unit(mat, eps, dud);
}
/// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized,
/// and return the length of each of these rows in @a scaling.
/// @details This can be geometrically interpretted as a matrix with no scaling
/// along its major axes, and the scaling in the input vector
template<class MatType>
MatType
unit(
const MatType &in,
typename MatType::value_type eps,
Vec3<typename MatType::value_type>& scaling)
{
using T = typename MatType::value_type;
MatType result(in);
for (int i(0); i < 3; i++) {
try {
const Vec3<T> u(
Vec3<T>(in[i][0], in[i][1], in[i][2]).unit(eps, scaling[i]));
for (int j=0; j<3; j++) result[i][j] = u[j];
} catch (ArithmeticError&) {
for (int j=0; j<3; j++) result[i][j] = 0;
}
}
return result;
}
/// @brief Set the matrix to a shear along @a axis0 by a fraction of @a axis1.
/// @param axis0 The fixed axis of the shear.
/// @param axis1 The shear axis.
/// @param shear The shear factor.
template <class MatType>
MatType
shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
{
int index0 = static_cast<int>(axis0);
int index1 = static_cast<int>(axis1);
MatType result;
result.setIdentity();
if (axis0 == axis1) {
result[index1][index0] = shear + 1;
} else {
result[index1][index0] = shear;
}
return result;
}
/// Return a matrix as the cross product of the given vector.
template<class MatType>
MatType
skew(const Vec3<typename MatType::value_type> &skew)
{
using T = typename MatType::value_type;
MatType r;
r[0][0] = T(0); r[0][1] = skew.z(); r[0][2] = -skew.y();
r[1][0] = -skew.z(); r[1][1] = T(0); r[2][1] = skew.x();
r[2][0] = skew.y(); r[2][1] = -skew.x(); r[2][2] = T(0);
if(MatType::numColumns() == 4) padMat4(r);
return r;
}
/// @brief Return an orientation matrix such that z points along @a direction,
/// and y is along the @a direction / @a vertical plane.
template<class MatType>
MatType
aim(const Vec3<typename MatType::value_type>& direction,
const Vec3<typename MatType::value_type>& vertical)
{
using T = typename MatType::value_type;
Vec3<T> forward(direction.unit());
Vec3<T> horizontal(vertical.unit().cross(forward).unit());
Vec3<T> up(forward.cross(horizontal).unit());
MatType r;
r[0][0]=horizontal.x(); r[0][1]=horizontal.y(); r[0][2]=horizontal.z();
r[1][0]=up.x(); r[1][1]=up.y(); r[1][2]=up.z();
r[2][0]=forward.x(); r[2][1]=forward.y(); r[2][2]=forward.z();
if(MatType::numColumns() == 4) padMat4(r);
return r;
}
/// @brief This function snaps a specific axis to a specific direction,
/// preserving scaling.
/// @details It does this using minimum energy, thus posing a unique solution if
/// basis & direction aren't parallel.
/// @note @a direction need not be unit.
template<class MatType>
inline MatType
snapMatBasis(const MatType& source, Axis axis, const Vec3<typename MatType::value_type>& direction)
{
using T = typename MatType::value_type;
Vec3<T> unitDir(direction.unit());
Vec3<T> ourUnitAxis(source.row(axis).unit());
// Are the two parallel?
T parallel = unitDir.dot(ourUnitAxis);
// Already snapped!
if (isApproxEqual(parallel, T(1.0))) return source;
if (isApproxEqual(parallel, T(-1.0))) {
OPENVDB_THROW(ValueError, "Cannot snap to inverse axis");
}
// Find angle between our basis and the one specified
T angleBetween(angle(unitDir, ourUnitAxis));
// Caclulate axis to rotate along
Vec3<T> rotationAxis = unitDir.cross(ourUnitAxis);
MatType rotation;
rotation.setToRotation(rotationAxis, angleBetween);
return source * rotation;
}
/// @brief Write 0s along Mat4's last row and column, and a 1 on its diagonal.
/// @details Useful initialization when we're initializing just the 3&times;3 block.
template<class MatType>
inline MatType&
padMat4(MatType& dest)
{
dest[0][3] = dest[1][3] = dest[2][3] = 0;
dest[3][2] = dest[3][1] = dest[3][0] = 0;
dest[3][3] = 1;
return dest;
}
/// @brief Solve for A=B*B, given A.
/// @details Denman-Beavers square root iteration
template<typename MatType>
inline void
sqrtSolve(const MatType& aA, MatType& aB, double aTol=0.01)
{
unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
MatType Y[2], Z[2];
Y[0] = aA;
Z[0] = MatType::identity();
unsigned int current = 0;
for (unsigned int iteration=0; iteration < iterations; iteration++) {
unsigned int last = current;
current = !current;
MatType invY = Y[last].inverse();
MatType invZ = Z[last].inverse();
Y[current] = 0.5 * (Y[last] + invZ);
Z[current] = 0.5 * (Z[last] + invY);
}
aB = Y[current];
}
template<typename MatType>
inline void
powSolve(const MatType& aA, MatType& aB, double aPower, double aTol=0.01)
{
unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
const bool inverted = (aPower < 0.0);
if (inverted) { aPower = -aPower; }
unsigned int whole = static_cast<unsigned int>(aPower);
double fraction = aPower - whole;
MatType R = MatType::identity();
MatType partial = aA;
double contribution = 1.0;
for (unsigned int iteration = 0; iteration < iterations; iteration++) {
sqrtSolve(partial, partial, aTol);
contribution *= 0.5;
if (fraction >= contribution) {
R *= partial;
fraction -= contribution;
}
}
partial = aA;
while (whole) {
if (whole & 1) { R *= partial; }
whole >>= 1;
if (whole) { partial *= partial; }
}
if (inverted) { aB = R.inverse(); }
else { aB = R; }
}
/// @brief Determine if a matrix is an identity matrix.
template<typename MatType>
inline bool
isIdentity(const MatType& m)
{
return m.eq(MatType::identity());
}
/// @brief Determine if a matrix is invertible.
template<typename MatType>
inline bool
isInvertible(const MatType& m)
{
using ValueType = typename MatType::ValueType;
return !isApproxEqual(m.det(), ValueType(0));
}
/// @brief Determine if a matrix is symmetric.
/// @details This implicitly uses math::isApproxEqual() to determine equality.
template<typename MatType>
inline bool
isSymmetric(const MatType& m)
{
return m.eq(m.transpose());
}
/// Determine if a matrix is unitary (i.e., rotation or reflection).
template<typename MatType>
inline bool
isUnitary(const MatType& m)
{
using ValueType = typename MatType::ValueType;
if (!isApproxEqual(std::abs(m.det()), ValueType(1.0))) return false;
// check that the matrix transpose is the inverse
MatType temp = m * m.transpose();
return temp.eq(MatType::identity());
}
/// Determine if a matrix is diagonal.
template<typename MatType>
inline bool
isDiagonal(const MatType& mat)
{
int n = MatType::size;
typename MatType::ValueType temp(0);
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
if (i != j) {
temp += std::abs(mat(i,j));
}
}
}
return isApproxEqual(temp, typename MatType::ValueType(0.0));
}
/// Return the <i>L</i><sub>&infin;</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
template<typename MatType>
typename MatType::ValueType
lInfinityNorm(const MatType& matrix)
{
int n = MatType::size;
typename MatType::ValueType norm = 0;
for( int j = 0; j<n; ++j) {
typename MatType::ValueType column_sum = 0;
for (int i = 0; i<n; ++i) {
column_sum += std::fabs(matrix(i,j));
}
norm = std::max(norm, column_sum);
}
return norm;
}
/// Return the <i>L</i><sub>1</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
template<typename MatType>
typename MatType::ValueType
lOneNorm(const MatType& matrix)
{
int n = MatType::size;
typename MatType::ValueType norm = 0;
for( int i = 0; i<n; ++i) {
typename MatType::ValueType row_sum = 0;
for (int j = 0; j<n; ++j) {
row_sum += std::fabs(matrix(i,j));
}
norm = std::max(norm, row_sum);
}
return norm;
}
/// @brief Decompose an invertible 3&times;3 matrix into a unitary matrix
/// followed by a symmetric matrix (positive semi-definite Hermitian),
/// i.e., M = U * S.
/// @details If det(U) = 1 it is a rotation, otherwise det(U) = -1,
/// meaning there is some part reflection.
/// See "Computing the polar decomposition with applications"
/// Higham, N.J. - SIAM J. Sc. Stat Comput 7(4):1160-1174
template<typename MatType>
bool
polarDecomposition(const MatType& input, MatType& unitary,
MatType& positive_hermitian, unsigned int MAX_ITERATIONS=100)
{
unitary = input;
MatType new_unitary(input);
MatType unitary_inv;
if (fabs(unitary.det()) < math::Tolerance<typename MatType::ValueType>::value()) return false;
unsigned int iteration(0);
typename MatType::ValueType linf_of_u;
typename MatType::ValueType l1nm_of_u;
typename MatType::ValueType linf_of_u_inv;
typename MatType::ValueType l1nm_of_u_inv;
typename MatType::ValueType l1_error = 100;
double gamma;
do {
unitary_inv = unitary.inverse();
linf_of_u = lInfinityNorm(unitary);
l1nm_of_u = lOneNorm(unitary);
linf_of_u_inv = lInfinityNorm(unitary_inv);
l1nm_of_u_inv = lOneNorm(unitary_inv);
gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
l1_error = lInfinityNorm(unitary - new_unitary);
unitary = new_unitary;
/// this generally converges in less than ten iterations
if (iteration > MAX_ITERATIONS) return false;
iteration++;
} while (l1_error > math::Tolerance<typename MatType::ValueType>::value());
positive_hermitian = unitary.transpose() * input;
return true;
}
////////////////////////////////////////
/// @return true if m0 < m1, comparing components in order of significance.
template<unsigned SIZE, typename T>
inline bool
cwiseLessThan(const Mat<SIZE, T>& m0, const Mat<SIZE, T>& m1)
{
const T* m0p = m0.asPointer();
const T* m1p = m1.asPointer();
constexpr unsigned size = SIZE*SIZE;
for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p < *m1p;
}
return *m0p < *m1p;
}
/// @return true if m0 > m1, comparing components in order of significance.
template<unsigned SIZE, typename T>
inline bool
cwiseGreaterThan(const Mat<SIZE, T>& m0, const Mat<SIZE, T>& m1)
{
const T* m0p = m0.asPointer();
const T* m1p = m1.asPointer();
constexpr unsigned size = SIZE*SIZE;
for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p > *m1p;
}
return *m0p > *m1p;
}
} // namespace math
} // namespace OPENVDB_VERSION_NAME
} // namespace openvdb
#endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
Math.h
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
openvdb::v10_0::ArithmeticError
Definition Exceptions.h:56
openvdb::v10_0::NotImplementedError
Definition Exceptions.h:61
openvdb::v10_0::ValueError
Definition Exceptions.h:65
openvdb::v10_0::math::Mat
Definition Mat.h:27
openvdb::v10_0::math::Mat< 3, T >::mm
T mm[SIZE *SIZE]
Definition Mat.h:160
openvdb::v10_0::math::Mat::isZero
bool isZero() const
True if all elements are exactly zero.
Definition Mat.h:152
openvdb::v10_0::math::Mat::operator[]
T * operator[](int i)
Array style reference to ith row.
Definition Mat.h:106
openvdb::v10_0::math::Mat::operator[]
const T * operator[](int i) const
Definition Mat.h:107
openvdb::v10_0::math::Mat::Mat
Mat()=default
openvdb::v10_0::math::Mat::write
void write(std::ostream &os) const
Definition Mat.h:110
openvdb::v10_0::math::Mat::numColumns
static unsigned numColumns()
Definition Mat.h:40
openvdb::v10_0::math::Mat::asPointer
T * asPointer()
Direct access to the internal data.
Definition Mat.h:101
openvdb::v10_0::math::Mat::numRows
static unsigned numRows()
Definition Mat.h:39
openvdb::v10_0::math::Mat::str
std::string str(unsigned indentation=0) const
Definition Mat.h:53
openvdb::v10_0::math::Mat::absMax
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition Mat.h:119
openvdb::v10_0::math::Mat::isInfinite
bool isInfinite() const
True if an Inf is present in this matrix.
Definition Mat.h:136
openvdb::v10_0::math::Mat::asPointer
const T * asPointer() const
Definition Mat.h:102
openvdb::v10_0::math::Mat::operator<<
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition Mat.h:92
openvdb::v10_0::math::Mat< 3, T >::SIZE_
SIZE_
Definition Mat.h:31
openvdb::v10_0::math::Mat< 3, T >::size
@ size
Definition Mat.h:31
openvdb::v10_0::math::Mat::numElements
static unsigned numElements()
Definition Mat.h:41
openvdb::v10_0::math::Mat::ValueType
T ValueType
Definition Mat.h:30
openvdb::v10_0::math::Mat::isNan
bool isNan() const
True if a Nan is present in this matrix.
Definition Mat.h:128
openvdb::v10_0::math::Mat::value_type
T value_type
Definition Mat.h:29
openvdb::v10_0::math::Mat::read
void read(std::istream &is)
Definition Mat.h:114
openvdb::v10_0::math::Mat::isFinite
bool isFinite() const
True if no Nan or Inf values are present.
Definition Mat.h:144
openvdb::v10_0::math::Quat
Definition Quat.h:79
openvdb::v10_0::math::Quat::x
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition Quat.h:193
openvdb::v10_0::math::Quat::y
T & y()
Definition Quat.h:194
openvdb::v10_0::math::Quat::z
T & z()
Definition Quat.h:195
openvdb::v10_0::math::Quat::w
T & w()
Definition Quat.h:196
openvdb::v10_0::math::Quat::dot
T dot(const Quat &q) const
Dot product.
Definition Quat.h:451
openvdb::v10_0::math::Vec3
Definition Vec3.h:24
openvdb::v10_0::math::Vec3::x
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition Vec3.h:85
openvdb::v10_0::math::Vec3::dot
T dot(const Vec3< T > &v) const
Dot product.
Definition Vec3.h:191
openvdb::v10_0::math::Vec3::y
T & y()
Definition Vec3.h:86
openvdb::v10_0::math::Vec3::z
T & z()
Definition Vec3.h:87
openvdb::v10_0::math::Vec3::cross
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition Vec3.h:220
openvdb::v10_0::math::Vec3::unit
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition Vec3.h:374
openvdb::v10_0::math::Vec3::normalize
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition Vec3.h:362
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::isInfinite
bool isInfinite(const float x)
Return true if x is an infinity value (either positive infinity or negative infinity).
Definition Math.h:385
openvdb::v10_0::math::eulerAngles
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
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::pi
constexpr T pi()
Pi constant taken from Boost to match old behaviour.
Definition Math.h:119
openvdb::v10_0::math::isDiagonal
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition Mat.h:902
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::isInvertible
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition Mat.h:869
openvdb::v10_0::math::padMat4
MatType & padMat4(MatType &dest)
Write 0s along Mat4's last row and column, and a 1 on its diagonal.
Definition Mat.h:783
openvdb::v10_0::math::isFinite
bool isFinite(const float x)
Return true if x is finite.
Definition Math.h:375
openvdb::v10_0::math::lInfinityNorm
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the L∞ norm of an N×N matrix.
Definition Mat.h:920
openvdb::v10_0::math::scale
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition Mat.h:615
openvdb::v10_0::math::isUnitary
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition Mat.h:889
openvdb::v10_0::math::isSymmetric
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition Mat.h:880
openvdb::v10_0::math::lOneNorm
MatType::ValueType lOneNorm(const MatType &matrix)
Return the L1 norm of an N×N matrix.
Definition Mat.h:941
openvdb::v10_0::math::isNan
bool isNan(const float x)
Return true if x is a NaN (Not-A-Number) value.
Definition Math.h:395
openvdb::v10_0::math::unit
MatType unit(const MatType &mat, typename MatType::value_type eps=1.0e-8)
Return a copy of the given matrix with its upper 3×3 rows normalized.
Definition Mat.h:648
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::Abs
Coord Abs(const Coord &xyz)
Definition Coord.h:515
openvdb::v10_0::math::polarDecomposition
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
openvdb::v10_0::math::aim
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
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::isZero
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition Math.h:337
openvdb::v10_0::math::isIdentity
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition Mat.h:860
openvdb::v10_0::math::shear
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
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::Z_AXIS
@ Z_AXIS
Definition Math.h:904
openvdb::v10_0::math::X_AXIS
@ X_AXIS
Definition Math.h:902
openvdb::v10_0::math::Y_AXIS
@ Y_AXIS
Definition Math.h:903
openvdb::v10_0::math::cwiseGreaterThan
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1029
openvdb::v10_0::math::sqrtSolve
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition Mat.h:797
openvdb::v10_0::math::RotationOrder
RotationOrder
Definition Math.h:908
openvdb::v10_0::math::YXZ_ROTATION
@ YXZ_ROTATION
Definition Math.h:911
openvdb::v10_0::math::ZXY_ROTATION
@ ZXY_ROTATION
Definition Math.h:913
openvdb::v10_0::math::YZX_ROTATION
@ YZX_ROTATION
Definition Math.h:912
openvdb::v10_0::math::ZXZ_ROTATION
@ ZXZ_ROTATION
Definition Math.h:916
openvdb::v10_0::math::XZX_ROTATION
@ XZX_ROTATION
Definition Math.h:915
openvdb::v10_0::math::XYZ_ROTATION
@ XYZ_ROTATION
Definition Math.h:909
openvdb::v10_0::math::ZYX_ROTATION
@ ZYX_ROTATION
Definition Math.h:914
openvdb::v10_0::math::XZY_ROTATION
@ XZY_ROTATION
Definition Math.h:910
openvdb::v10_0::math::getScale
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
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