395 lines
10 KiB
C++
395 lines
10 KiB
C++
/******************************************************************************
|
|
*
|
|
* MantaFlow fluid solver framework
|
|
* Copyright 2015 Kiwon Um, Nils Thuerey
|
|
*
|
|
* This program is free software, distributed under the terms of the
|
|
* GNU General Public License (GPL)
|
|
* http://www.gnu.org/licenses
|
|
*
|
|
* Matrix (3x3) class
|
|
*
|
|
******************************************************************************/
|
|
|
|
#ifndef MATRIXBASE_H
|
|
#define MATRIXBASE_H
|
|
|
|
#include "vectorbase.h"
|
|
|
|
namespace Manta {
|
|
|
|
template<typename T> class Matrix3x3 {
|
|
public:
|
|
// NOTE: default is the identity matrix!
|
|
explicit Matrix3x3(const T &p00 = 1,
|
|
const T &p01 = 0,
|
|
const T &p02 = 0,
|
|
const T &p10 = 0,
|
|
const T &p11 = 1,
|
|
const T &p12 = 0,
|
|
const T &p20 = 0,
|
|
const T &p21 = 0,
|
|
const T &p22 = 1)
|
|
{
|
|
v[0][0] = p00;
|
|
v[0][1] = p01;
|
|
v[0][2] = p02;
|
|
v[1][0] = p10;
|
|
v[1][1] = p11;
|
|
v[1][2] = p12;
|
|
v[2][0] = p20;
|
|
v[2][1] = p21;
|
|
v[2][2] = p22;
|
|
}
|
|
|
|
explicit Matrix3x3(const Vector3D<T> &diag)
|
|
{
|
|
v[0][0] = diag.x;
|
|
v[0][1] = 0;
|
|
v[0][2] = 0;
|
|
v[1][0] = 0;
|
|
v[1][1] = diag.y;
|
|
v[1][2] = 0;
|
|
v[2][0] = 0;
|
|
v[2][1] = 0;
|
|
v[2][2] = diag.z;
|
|
}
|
|
|
|
Matrix3x3(const Vector3D<T> &c0, const Vector3D<T> &c1, const Vector3D<T> &c2)
|
|
{
|
|
v[0][0] = c0.x;
|
|
v[0][1] = c1.x;
|
|
v[0][2] = c2.x;
|
|
v[1][0] = c0.y;
|
|
v[1][1] = c1.y;
|
|
v[1][2] = c2.y;
|
|
v[2][0] = c0.z;
|
|
v[2][1] = c1.z;
|
|
v[2][2] = c2.z;
|
|
}
|
|
|
|
// assignment operators
|
|
Matrix3x3 &operator+=(const Matrix3x3 &m)
|
|
{
|
|
v00 += m.v00;
|
|
v01 += m.v01;
|
|
v02 += m.v02;
|
|
v10 += m.v10;
|
|
v11 += m.v11;
|
|
v12 += m.v12;
|
|
v20 += m.v20;
|
|
v21 += m.v21;
|
|
v22 += m.v22;
|
|
return *this;
|
|
}
|
|
Matrix3x3 &operator-=(const Matrix3x3 &m)
|
|
{
|
|
v00 -= m.v00;
|
|
v01 -= m.v01;
|
|
v02 -= m.v02;
|
|
v10 -= m.v10;
|
|
v11 -= m.v11;
|
|
v12 -= m.v12;
|
|
v20 -= m.v20;
|
|
v21 -= m.v21;
|
|
v22 -= m.v22;
|
|
return *this;
|
|
}
|
|
Matrix3x3 &operator*=(const T s)
|
|
{
|
|
v00 *= s;
|
|
v01 *= s;
|
|
v02 *= s;
|
|
v10 *= s;
|
|
v11 *= s;
|
|
v12 *= s;
|
|
v20 *= s;
|
|
v21 *= s;
|
|
v22 *= s;
|
|
return *this;
|
|
}
|
|
Matrix3x3 &operator/=(const T s)
|
|
{
|
|
v00 /= s;
|
|
v01 /= s;
|
|
v02 /= s;
|
|
v10 /= s;
|
|
v11 /= s;
|
|
v12 /= s;
|
|
v20 /= s;
|
|
v21 /= s;
|
|
v22 /= s;
|
|
return *this;
|
|
}
|
|
|
|
// binary operators
|
|
Matrix3x3 operator+(const Matrix3x3 &m) const
|
|
{
|
|
return Matrix3x3(*this) += m;
|
|
}
|
|
Matrix3x3 operator-(const Matrix3x3 &m) const
|
|
{
|
|
return Matrix3x3(*this) -= m;
|
|
}
|
|
Matrix3x3 operator*(const Matrix3x3 &m) const
|
|
{
|
|
return Matrix3x3(v00 * m.v00 + v01 * m.v10 + v02 * m.v20,
|
|
v00 * m.v01 + v01 * m.v11 + v02 * m.v21,
|
|
v00 * m.v02 + v01 * m.v12 + v02 * m.v22,
|
|
|
|
v10 * m.v00 + v11 * m.v10 + v12 * m.v20,
|
|
v10 * m.v01 + v11 * m.v11 + v12 * m.v21,
|
|
v10 * m.v02 + v11 * m.v12 + v12 * m.v22,
|
|
|
|
v20 * m.v00 + v21 * m.v10 + v22 * m.v20,
|
|
v20 * m.v01 + v21 * m.v11 + v22 * m.v21,
|
|
v20 * m.v02 + v21 * m.v12 + v22 * m.v22);
|
|
}
|
|
Matrix3x3 operator*(const T s) const
|
|
{
|
|
return Matrix3x3(*this) *= s;
|
|
}
|
|
Vector3D<T> operator*(const Vector3D<T> &v) const
|
|
{
|
|
return Vector3D<T>(v00 * v.x + v01 * v.y + v02 * v.z,
|
|
v10 * v.x + v11 * v.y + v12 * v.z,
|
|
v20 * v.x + v21 * v.y + v22 * v.z);
|
|
}
|
|
Vector3D<T> transposedMul(const Vector3D<T> &v) const
|
|
{
|
|
// M^T*v
|
|
return Vector3D<T>(v00 * v.x + v10 * v.y + v20 * v.z,
|
|
v01 * v.x + v11 * v.y + v21 * v.z,
|
|
v02 * v.x + v12 * v.y + v22 * v.z);
|
|
}
|
|
Matrix3x3 transposedMul(const Matrix3x3 &m) const
|
|
{
|
|
// M^T*M
|
|
return Matrix3x3(v00 * m.v00 + v10 * m.v10 + v20 * m.v20,
|
|
v00 * m.v01 + v10 * m.v11 + v20 * m.v21,
|
|
v00 * m.v02 + v10 * m.v12 + v20 * m.v22,
|
|
|
|
v01 * m.v00 + v11 * m.v10 + v21 * m.v20,
|
|
v01 * m.v01 + v11 * m.v11 + v21 * m.v21,
|
|
v01 * m.v02 + v11 * m.v12 + v21 * m.v22,
|
|
|
|
v02 * m.v00 + v12 * m.v10 + v22 * m.v20,
|
|
v02 * m.v01 + v12 * m.v11 + v22 * m.v21,
|
|
v02 * m.v02 + v12 * m.v12 + v22 * m.v22);
|
|
}
|
|
Matrix3x3 mulTranspose(const Matrix3x3 &m) const
|
|
{
|
|
// M*m^T
|
|
return Matrix3x3(v00 * m.v00 + v01 * m.v01 + v02 * m.v02,
|
|
v00 * m.v10 + v01 * m.v11 + v02 * m.v12,
|
|
v00 * m.v20 + v01 * m.v21 + v02 * m.v22,
|
|
|
|
v10 * m.v00 + v11 * m.v01 + v12 * m.v02,
|
|
v10 * m.v10 + v11 * m.v11 + v12 * m.v12,
|
|
v10 * m.v20 + v11 * m.v21 + v12 * m.v22,
|
|
|
|
v20 * m.v00 + v21 * m.v01 + v22 * m.v02,
|
|
v20 * m.v10 + v21 * m.v11 + v22 * m.v12,
|
|
v20 * m.v20 + v21 * m.v21 + v22 * m.v22);
|
|
}
|
|
|
|
bool operator==(const Matrix3x3 &m) const
|
|
{
|
|
return (v00 == m.v00 && v01 == m.v01 && v02 == m.v02 && v10 == m.v10 && v11 == m.v11 &&
|
|
v12 == m.v12 && v20 == m.v20 && v21 == m.v21 && v22 == m.v22);
|
|
}
|
|
|
|
const T &operator()(const int r, const int c) const
|
|
{
|
|
return v[r][c];
|
|
}
|
|
T &operator()(const int r, const int c)
|
|
{
|
|
return const_cast<T &>(const_cast<const Matrix3x3 &>(*this)(r, c));
|
|
}
|
|
|
|
T trace() const
|
|
{
|
|
return v00 + v11 + v22;
|
|
}
|
|
T sumSqr() const
|
|
{
|
|
return (v00 * v00 + v01 * v01 + v02 * v02 + v10 * v10 + v11 * v11 + v12 * v12 + v20 * v20 +
|
|
v21 * v21 + v22 * v22);
|
|
}
|
|
|
|
Real determinant() const
|
|
{
|
|
return (v00 * v11 * v22 - v00 * v12 * v21 + v01 * v12 * v20 - v01 * v10 * v22 +
|
|
v02 * v10 * v21 - v02 * v11 * v20);
|
|
}
|
|
Matrix3x3 &transpose()
|
|
{
|
|
return *this = transposed();
|
|
}
|
|
Matrix3x3 transposed() const
|
|
{
|
|
return Matrix3x3(v00, v10, v20, v01, v11, v21, v02, v12, v22);
|
|
}
|
|
Matrix3x3 &invert()
|
|
{
|
|
return *this = inverse();
|
|
}
|
|
Matrix3x3 inverse() const
|
|
{
|
|
const Real det = determinant(); // FIXME: assert(det);
|
|
const Real idet = 1e0 / det;
|
|
return Matrix3x3(idet * (v11 * v22 - v12 * v21),
|
|
idet * (v02 * v21 - v01 * v22),
|
|
idet * (v01 * v12 - v02 * v11),
|
|
idet * (v12 * v20 - v10 * v22),
|
|
idet * (v00 * v22 - v02 * v20),
|
|
idet * (v02 * v10 - v00 * v12),
|
|
idet * (v10 * v21 - v11 * v20),
|
|
idet * (v01 * v20 - v00 * v21),
|
|
idet * (v00 * v11 - v01 * v10));
|
|
}
|
|
bool getInverse(Matrix3x3 &inv) const
|
|
{
|
|
const Real det = determinant();
|
|
if (det == 0e0)
|
|
return false; // FIXME: is it likely to happen the floating error?
|
|
|
|
const Real idet = 1e0 / det;
|
|
inv.v00 = idet * (v11 * v22 - v12 * v21);
|
|
inv.v01 = idet * (v02 * v21 - v01 * v22);
|
|
inv.v02 = idet * (v01 * v12 - v02 * v11);
|
|
|
|
inv.v10 = idet * (v12 * v20 - v10 * v22);
|
|
inv.v11 = idet * (v00 * v22 - v02 * v20);
|
|
inv.v12 = idet * (v02 * v10 - v00 * v12);
|
|
|
|
inv.v20 = idet * (v10 * v21 - v11 * v20);
|
|
inv.v21 = idet * (v01 * v20 - v00 * v21);
|
|
inv.v22 = idet * (v00 * v11 - v01 * v10);
|
|
|
|
return true;
|
|
}
|
|
|
|
Real normOne() const
|
|
{
|
|
// the maximum absolute column sum of the matrix
|
|
return max(std::fabs(v00) + std::fabs(v10) + std::fabs(v20),
|
|
std::fabs(v01) + std::fabs(v11) + std::fabs(v21),
|
|
std::fabs(v02) + std::fabs(v12) + std::fabs(v22));
|
|
}
|
|
Real normInf() const
|
|
{
|
|
// the maximum absolute row sum of the matrix
|
|
return max(std::fabs(v00) + std::fabs(v01) + std::fabs(v02),
|
|
std::fabs(v10) + std::fabs(v11) + std::fabs(v12),
|
|
std::fabs(v20) + std::fabs(v21) + std::fabs(v22));
|
|
}
|
|
|
|
Vector3D<T> eigenvalues() const
|
|
{
|
|
Vector3D<T> eigen;
|
|
|
|
const Real b = -v00 - v11 - v22;
|
|
const Real c = v00 * (v11 + v22) + v11 * v22 - v12 * v21 - v01 * v10 - v02 * v20;
|
|
Real d = -v00 * (v11 * v22 - v12 * v21) - v20 * (v01 * v12 - v11 * v02) -
|
|
v10 * (v02 * v21 - v22 * v01);
|
|
const Real f = (3.0 * c - b * b) / 3.0;
|
|
const Real g = (2.0 * b * b * b - 9.0 * b * c + 27.0 * d) / 27.0;
|
|
const Real h = g * g / 4.0 + f * f * f / 27.0;
|
|
|
|
Real sign;
|
|
if (h > 0) {
|
|
Real r = -g / 2.0 + std::sqrt(h);
|
|
if (r < 0) {
|
|
r = -r;
|
|
sign = -1.0;
|
|
}
|
|
else
|
|
sign = 1.0;
|
|
Real s = sign * std::pow(r, 1.0 / 3.0);
|
|
Real t = -g / 2.0 - std::sqrt(h);
|
|
if (t < 0) {
|
|
t = -t;
|
|
sign = -1.0;
|
|
}
|
|
else
|
|
sign = 1.0;
|
|
Real u = sign * std::pow(t, 1.0 / 3.0);
|
|
eigen[0] = (s + u) - b / 3.0;
|
|
eigen[1] = eigen[2] = 0;
|
|
}
|
|
else if (h == 0) {
|
|
if (d < 0) {
|
|
d = -d;
|
|
sign = -1.0;
|
|
}
|
|
sign = 1.0;
|
|
eigen[0] = -1.0 * sign * std::pow(d, 1.0 / 3.0);
|
|
eigen[1] = eigen[2] = 0;
|
|
}
|
|
else {
|
|
const Real i = std::sqrt(g * g / 4.0 - h);
|
|
const Real j = std::pow(i, 1.0 / 3.0);
|
|
const Real k = std::acos(-g / (2.0 * i));
|
|
const Real l = -j;
|
|
const Real m = std::cos(k / 3.0);
|
|
const Real n = std::sqrt(3.0) * std::sin(k / 3.0);
|
|
const Real p = -b / 3.0;
|
|
eigen[0] = 2e0 * j * m + p;
|
|
eigen[1] = l * (m + n) + p;
|
|
eigen[2] = l * (m - n) + p;
|
|
}
|
|
|
|
return eigen;
|
|
}
|
|
|
|
static Matrix3x3 I()
|
|
{
|
|
return Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
}
|
|
|
|
#ifdef _WIN32
|
|
# pragma warning(disable : 4201)
|
|
#endif
|
|
union {
|
|
struct {
|
|
T v00, v01, v02, v10, v11, v12, v20, v21, v22;
|
|
};
|
|
T v[3][3];
|
|
T v1[9];
|
|
};
|
|
#ifdef _WIN32
|
|
# pragma warning(default : 4201)
|
|
#endif
|
|
};
|
|
|
|
template<typename T1, typename T> inline Matrix3x3<T> operator*(const T1 s, const Matrix3x3<T> &m)
|
|
{
|
|
return m * static_cast<T>(s);
|
|
}
|
|
|
|
template<typename T> inline Matrix3x3<T> crossProductMatrix(const Vector3D<T> &v)
|
|
{
|
|
return Matrix3x3<T>(0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0);
|
|
}
|
|
|
|
template<typename T> inline Matrix3x3<T> outerProduct(const Vector3D<T> &a, const Vector3D<T> &b)
|
|
{
|
|
return Matrix3x3<T>(a.x * b.x,
|
|
a.x * b.y,
|
|
a.x * b.z,
|
|
a.y * b.x,
|
|
a.y * b.y,
|
|
a.y * b.z,
|
|
a.z * b.x,
|
|
a.z * b.y,
|
|
a.z * b.z);
|
|
}
|
|
|
|
typedef Matrix3x3<Real> Matrix3x3f;
|
|
|
|
} // namespace Manta
|
|
|
|
#endif /* MATRIXBASE_H */
|