1101 lines
33 KiB
C++
1101 lines
33 KiB
C++
//=============================================================================
|
|
// Copyright (C) 2011-2018 The pmp-library developers
|
|
// Copyright (C) 2001-2005 by Computer Graphics Group, RWTH Aachen
|
|
// All rights reserved.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistributions of source code must retain the above copyright notice, this
|
|
// list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
// * Neither the name of the copyright holder nor the names of its
|
|
// contributors may be used to endorse or promote products derived from
|
|
// this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
// POSSIBILITY OF SUCH DAMAGE.
|
|
//=============================================================================
|
|
#pragma once
|
|
//=============================================================================
|
|
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include <assert.h>
|
|
#include <limits>
|
|
|
|
//=============================================================================
|
|
|
|
namespace pmp {
|
|
|
|
//=============================================================================
|
|
|
|
/// Base class for MxN matrix
|
|
template <typename Scalar, int M, int N>
|
|
class Matrix
|
|
{
|
|
public:
|
|
//! the scalar type of the vector
|
|
typedef Scalar value_type;
|
|
|
|
//! returns number of rows of the matrix
|
|
static constexpr int rows() { return M; }
|
|
//! returns number of columns of the matrix
|
|
static constexpr int cols() { return N; }
|
|
//! returns the dimension of the vector (or size of the matrix, rows*cols)
|
|
static constexpr int size() { return M * N; }
|
|
|
|
/// empty default constructor
|
|
Matrix() {}
|
|
|
|
/// construct with all entries being a given scalar (matrix and vector)
|
|
explicit Matrix(Scalar s)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] = s;
|
|
}
|
|
|
|
/// constructor for 2D vectors
|
|
explicit Matrix(Scalar x, Scalar y)
|
|
{
|
|
static_assert(M == 2 && N == 1, "only for 2D vectors");
|
|
data_[0] = x;
|
|
data_[1] = y;
|
|
}
|
|
|
|
/// constructor for 3D vectors
|
|
explicit Matrix(Scalar x, Scalar y, Scalar z)
|
|
{
|
|
static_assert(M == 3 && N == 1, "only for 3D vectors");
|
|
data_[0] = x;
|
|
data_[1] = y;
|
|
data_[2] = z;
|
|
}
|
|
|
|
/// constructor for 4D vectors
|
|
explicit Matrix(Scalar x, Scalar y, Scalar z, Scalar w)
|
|
{
|
|
static_assert(M == 4 && N == 1, "only for 4D vectors");
|
|
data_[0] = x;
|
|
data_[1] = y;
|
|
data_[2] = z;
|
|
data_[3] = w;
|
|
}
|
|
|
|
/// constructor for 4D vectors
|
|
explicit Matrix(Matrix<Scalar, 3, 1> xyz, Scalar w)
|
|
{
|
|
static_assert(M == 4 && N == 1, "only for 4D vectors");
|
|
data_[0] = xyz[0];
|
|
data_[1] = xyz[1];
|
|
data_[2] = xyz[2];
|
|
data_[3] = w;
|
|
}
|
|
|
|
/// construct 4x4 matrix from 4 column vectors
|
|
Matrix(Matrix<Scalar, 4, 1> c0,
|
|
Matrix<Scalar, 4, 1> c1,
|
|
Matrix<Scalar, 4, 1> c2,
|
|
Matrix<Scalar, 4, 1> c3)
|
|
{
|
|
static_assert(M == 4 && N == 4, "only for 4x4 matrices");
|
|
(*this)(0,0) = c0[0]; (*this)(0,1) = c1[0]; (*this)(0,2) = c2[0]; (*this)(0,3) = c3[0];
|
|
(*this)(1,0) = c0[1]; (*this)(1,1) = c1[1]; (*this)(1,2) = c2[1]; (*this)(1,3) = c3[1];
|
|
(*this)(2,0) = c0[2]; (*this)(2,1) = c1[2]; (*this)(2,2) = c2[2]; (*this)(2,3) = c3[2];
|
|
(*this)(3,0) = c0[3]; (*this)(3,1) = c1[3]; (*this)(3,2) = c2[3]; (*this)(3,3) = c3[3];
|
|
}
|
|
|
|
/// construct from 16 (row-wise) entries
|
|
Matrix(Scalar m00, Scalar m01, Scalar m02, Scalar m03,
|
|
Scalar m10, Scalar m11, Scalar m12, Scalar m13,
|
|
Scalar m20, Scalar m21, Scalar m22, Scalar m23,
|
|
Scalar m30, Scalar m31, Scalar m32, Scalar m33)
|
|
{
|
|
static_assert(M == 4 && N == 4, "only for 4x4 matrices");
|
|
(*this)(0,0) = m00; (*this)(0,1) = m01; (*this)(0,2) = m02; (*this)(0,3) = m03;
|
|
(*this)(1,0) = m10; (*this)(1,1) = m11; (*this)(1,2) = m12; (*this)(1,3) = m13;
|
|
(*this)(2,0) = m20; (*this)(2,1) = m21; (*this)(2,2) = m22; (*this)(2,3) = m23;
|
|
(*this)(3,0) = m30; (*this)(3,1) = m31; (*this)(3,2) = m32; (*this)(3,3) = m33;
|
|
}
|
|
|
|
/// copy constructor from other scalar type
|
|
/// is also invoked for type-casting
|
|
template <typename OtherScalarType>
|
|
explicit Matrix(const Matrix<OtherScalarType, M, N>& m)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] = static_cast<Scalar>(m[i]);
|
|
}
|
|
|
|
/// return identity matrix (only for square matrices, N==M)
|
|
static Matrix<Scalar, M, N> identity();
|
|
|
|
/// access entry at row i and column j
|
|
Scalar& operator()(unsigned int i, unsigned int j)
|
|
{
|
|
assert(i < M && j < N);
|
|
return data_[M * j + i];
|
|
}
|
|
|
|
/// const-access entry at row i and column j
|
|
const Scalar& operator()(unsigned int i, unsigned int j) const
|
|
{
|
|
assert(i < M && j < N);
|
|
return data_[M * j + i];
|
|
}
|
|
|
|
/// access i'th entry (use for vectors)
|
|
Scalar& operator[](unsigned int i)
|
|
{
|
|
assert(i < M * N);
|
|
return data_[i];
|
|
}
|
|
|
|
/// const-access i'th entry (use for vectors)
|
|
Scalar operator[](unsigned int i) const
|
|
{
|
|
assert(i < M * N);
|
|
return data_[i];
|
|
}
|
|
|
|
/// const-access as scalar array
|
|
const Scalar* data() const { return data_; }
|
|
|
|
/// access as scalar array
|
|
Scalar* data() { return data_; }
|
|
|
|
/// normalize matrix/vector by dividing through Frobenius/Euclidean norm
|
|
void normalize() { *this /= norm(*this); }
|
|
|
|
/// divide matrix by scalar
|
|
Matrix<Scalar, M, N>& operator/=(const Scalar s)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] /= s;
|
|
return *this;
|
|
}
|
|
|
|
/// multply matrix by scalar
|
|
Matrix<Scalar, M, N>& operator*=(const Scalar s)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] *= s;
|
|
return *this;
|
|
}
|
|
|
|
// add other matrix to this matrix
|
|
Matrix<Scalar, M, N>& operator+=(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] += m.data_[i];
|
|
return *this;
|
|
}
|
|
|
|
/// subtract other matrix from this matrix
|
|
Matrix<Scalar, M, N>& operator-=(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
data_[i] -= m.data_[i];
|
|
return *this;
|
|
}
|
|
|
|
//! component-wise comparison
|
|
bool operator==(const Matrix<Scalar, M, N>& other) const
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
if (data_[i] != other.data_[i])
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
//! component-wise comparison
|
|
bool operator!=(const Matrix<Scalar, M, N>& other) const
|
|
{
|
|
for (int i = 0; i < size(); ++i)
|
|
if (data_[i] != other.data_[i])
|
|
return true;
|
|
return false;
|
|
}
|
|
|
|
//! return matrix with minimum of this and other in each component
|
|
//Matrix<Scalar,M,N> minimize(const Matrix<Scalar,M,N>& other)
|
|
//{
|
|
//for (int i=0; i<size(); ++i)
|
|
//if (other[i] < data_[i])
|
|
//data_[i] = other[i];
|
|
//return *this;
|
|
//}
|
|
|
|
//! return matrix with maximum of this and other in each component
|
|
//Matrix<Scalar,M,N> maximize(const Matrix<Scalar,M,N>& other)
|
|
//{
|
|
//for (int i=0; i<size(); ++i)
|
|
//if (other[i] > data_[i])
|
|
//data_[i] = other[i];
|
|
//return *this;
|
|
//}
|
|
|
|
protected:
|
|
Scalar data_[N * M];
|
|
};
|
|
|
|
//== TEMPLATE SPECIALIZATIONS =================================================
|
|
|
|
/// template specialization for Vector as Nx1 matrix
|
|
template <typename Scalar, int M>
|
|
using Vector = Matrix<Scalar, M, 1>;
|
|
|
|
/// template specialization for 4x4 matrices
|
|
template <typename Scalar>
|
|
using Mat4 = Matrix<Scalar, 4, 4>;
|
|
|
|
/// template specialization for 3x3 matrices
|
|
template <typename Scalar>
|
|
using Mat3 = Matrix<Scalar, 3, 3>;
|
|
|
|
/// template specialization for 2x2 matrices
|
|
template <typename Scalar>
|
|
using Mat2 = Matrix<Scalar, 2, 2>;
|
|
|
|
//== TYPEDEFS =================================================================
|
|
|
|
typedef Vector<float, 2> vec2;
|
|
typedef Vector<double, 2> dvec2;
|
|
typedef Vector<bool, 2> bvec2;
|
|
typedef Vector<int, 2> ivec2;
|
|
typedef Vector<unsigned int, 2> uvec2;
|
|
|
|
typedef Vector<float, 3> vec3;
|
|
typedef Vector<double, 3> dvec3;
|
|
typedef Vector<bool, 3> bvec3;
|
|
typedef Vector<int, 3> ivec3;
|
|
typedef Vector<unsigned int, 3> uvec3;
|
|
|
|
typedef Vector<float, 4> vec4;
|
|
typedef Vector<double, 4> dvec4;
|
|
typedef Vector<bool, 4> bvec4;
|
|
typedef Vector<int, 4> ivec4;
|
|
typedef Vector<unsigned int, 4> uvec4;
|
|
|
|
typedef Vector<float, 8> vec8;
|
|
typedef Vector<double, 8> dvec8;
|
|
typedef Vector<bool, 8> bvec8;
|
|
typedef Vector<int, 8> ivec8;
|
|
typedef Vector<unsigned int, 8> uvec8;
|
|
|
|
typedef Mat2<float> mat2;
|
|
typedef Mat2<double> dmat2;
|
|
typedef Mat3<float> mat3;
|
|
typedef Mat3<double> dmat3;
|
|
typedef Mat4<float> mat4;
|
|
typedef Mat4<double> dmat4;
|
|
|
|
//== GENERAL MATRIX FUNCTIONS =================================================
|
|
|
|
//! output a matrix by printing its space-separated compontens
|
|
template <typename Scalar, int M, int N>
|
|
inline std::ostream& operator<<(std::ostream& os, const Matrix<Scalar, M, N>& m)
|
|
{
|
|
for (int i=0; i<M; ++i)
|
|
{
|
|
for (int j=0; j<N; ++j)
|
|
os << m(i,j) << " ";
|
|
os << std::endl;
|
|
}
|
|
return os;
|
|
}
|
|
|
|
/// matrix-matrix multiplication
|
|
template <typename Scalar, int M, int N, int K>
|
|
Matrix<Scalar, M, N> operator*(const Matrix<Scalar, M, K>& m1,
|
|
const Matrix<Scalar, K, N>& m2)
|
|
{
|
|
Matrix<Scalar, M, N> m;
|
|
int i, j, k;
|
|
|
|
for (i = 0; i < M; ++i)
|
|
{
|
|
for (j = 0; j < N; ++j)
|
|
{
|
|
m(i, j) = Scalar(0);
|
|
for (k = 0; k < K; ++k)
|
|
m(i, j) += m1(i, k) * m2(k, j);
|
|
}
|
|
}
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
/// component-wise multiplication
|
|
template <typename Scalar, int M, int N>
|
|
Matrix<Scalar, M, N> cmult(const Matrix<Scalar, M, N>& m1,
|
|
const Matrix<Scalar, M, N>& m2)
|
|
{
|
|
Matrix<Scalar, M, N> m;
|
|
int i, j;
|
|
|
|
for (i = 0; i < M; ++i)
|
|
for (j = 0; j < N; ++j)
|
|
m(i,j) = m1(i,j) * m2(i,j);
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar, int M, int N>
|
|
Matrix<Scalar, N, M> transpose(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
Matrix<Scalar, N, M> result;
|
|
|
|
for (int j = 0; j < M; ++j)
|
|
for (int i = 0; i < N; ++i)
|
|
result(i, j) = m(j, i);
|
|
|
|
return result;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar, int M, int N>
|
|
Matrix<Scalar, M, N> Matrix<Scalar, M, N>::identity()
|
|
{
|
|
static_assert(M == N, "only for square matrices");
|
|
|
|
Matrix<Scalar, N, N> m;
|
|
|
|
for (int j = 0; j < N; ++j)
|
|
for (int i = 0; i < N; ++i)
|
|
m(i, j) = 0.0;
|
|
|
|
for (int i = 0; i < N; ++i)
|
|
m(i, i) = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! matrix + matrix
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator+(const Matrix<Scalar, M, N>& m1,
|
|
const Matrix<Scalar, M, N>& m2)
|
|
{
|
|
return Matrix<Scalar, M, N>(m1) += m2;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! matrix - matrix
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator-(const Matrix<Scalar, M, N>& m1,
|
|
const Matrix<Scalar, M, N>& m2)
|
|
{
|
|
return Matrix<Scalar, M, N>(m1) -= m2;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! negate matrix
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator-(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
Matrix<Scalar, M, N> result;
|
|
for (int i = 0; i < result.size(); ++i)
|
|
result[i] = -m[i];
|
|
return result;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! scalar * matrix
|
|
template <typename Scalar, typename Scalar2, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator*(const Scalar2 s,
|
|
const Matrix<Scalar, M, N>& m)
|
|
{
|
|
return Matrix<Scalar, M, N>(m) *= s;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! matrix * scalar
|
|
template <typename Scalar, typename Scalar2, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator*(const Matrix<Scalar, M, N>& m,
|
|
const Scalar2 s)
|
|
{
|
|
return Matrix<Scalar, M, N>(m) *= s;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! matrix / scalar
|
|
template <typename Scalar, typename Scalar2, int M, int N>
|
|
inline Matrix<Scalar, M, N> operator/(const Matrix<Scalar, M, N>& m,
|
|
const Scalar2 s)
|
|
{
|
|
return Matrix<Scalar, M, N>(m) /= s;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! compute the Frobenius norm of a matrix (or Euclidean norm of a vector)
|
|
template <typename Scalar, int M, int N>
|
|
inline Scalar norm(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
return sqrt(sqrnorm(m));
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! compute the squared Frobenius norm of a matrix (or squared Euclidean norm of a vector)
|
|
template <typename Scalar, int M, int N>
|
|
inline Scalar sqrnorm(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
Scalar s(0.0);
|
|
for (int i = 0; i < m.size(); ++i)
|
|
s += m[i] * m[i];
|
|
return s;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! return a normalized copy of a vector
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> normalize(const Matrix<Scalar, M, N>& m)
|
|
{
|
|
Scalar n = norm(m);
|
|
n = (n > std::numeric_limits<Scalar>::min()) ? 1.0 / n : 0.0;
|
|
return m * n;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! return component-wise minimum
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> min(const Matrix<Scalar, M, N>& m1,
|
|
const Matrix<Scalar, M, N>& m2)
|
|
{
|
|
Matrix<Scalar, M, N> result;
|
|
for (int i = 0; i < result.size(); ++i)
|
|
result[i] = std::min(m1[i], m2[i]);
|
|
return result;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
//! return component-wise maximum
|
|
template <typename Scalar, int M, int N>
|
|
inline Matrix<Scalar, M, N> max(const Matrix<Scalar, M, N>& m1,
|
|
const Matrix<Scalar, M, N>& m2)
|
|
{
|
|
Matrix<Scalar, M, N> result;
|
|
for (int i = 0; i < result.size(); ++i)
|
|
result[i] = std::max(m1[i], m2[i]);
|
|
return result;
|
|
}
|
|
|
|
//== Mat4 functions ===========================================================
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> viewportMatrix(Scalar l, Scalar b, Scalar w, Scalar h)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
|
|
m(0, 0) = 0.5 * w;
|
|
m(0, 3) = 0.5 * w + l;
|
|
m(1, 1) = 0.5 * h;
|
|
m(1, 3) = 0.5 * h + b;
|
|
m(2, 2) = 0.5;
|
|
m(2, 3) = 0.5;
|
|
m(3, 3) = 1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> inverseViewportMatrix(Scalar l, Scalar b, Scalar w, Scalar h)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
|
|
m(0, 0) = 2.0 / w;
|
|
m(0, 3) = -1.0 - (l + l) / w;
|
|
m(1, 1) = 2.0 / h;
|
|
m(1, 3) = -1.0 - (b + b) / h;
|
|
m(2, 2) = 2.0;
|
|
m(2, 3) = -1.0;
|
|
m(3, 3) = 1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> frustumMatrix(Scalar l, Scalar r, Scalar b, Scalar t, Scalar n,
|
|
Scalar f)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
|
|
m(0, 0) = (n + n) / (r - l);
|
|
m(0, 2) = (r + l) / (r - l);
|
|
m(1, 1) = (n + n) / (t - b);
|
|
m(1, 2) = (t + b) / (t - b);
|
|
m(2, 2) = -(f + n) / (f - n);
|
|
m(2, 3) = -f * (n + n) / (f - n);
|
|
m(3, 2) = -1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> inverseFrustumMatrix(Scalar l, Scalar r, Scalar b, Scalar t,
|
|
Scalar n, Scalar f)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
|
|
const Scalar nn = n + n;
|
|
|
|
m(0, 0) = (r - l) / nn;
|
|
m(0, 3) = (r + l) / nn;
|
|
m(1, 1) = (t - b) / nn;
|
|
m(1, 3) = (t + b) / nn;
|
|
m(2, 3) = -1.0;
|
|
m(3, 2) = (n - f) / (nn * f);
|
|
m(3, 3) = (n + f) / (nn * f);
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> perspectiveMatrix(Scalar fovy, Scalar aspect, Scalar zNear,
|
|
Scalar zFar)
|
|
{
|
|
Scalar t = Scalar(zNear) * tan(fovy * M_PI / 360.0);
|
|
Scalar b = -t;
|
|
Scalar l = b * aspect;
|
|
Scalar r = t * aspect;
|
|
|
|
return frustumMatrix(l, r, b, t, Scalar(zNear), Scalar(zFar));
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> inversePerspectiveMatrix(Scalar fovy, Scalar aspect, Scalar zNear,
|
|
Scalar zFar)
|
|
{
|
|
Scalar t = zNear * tan(fovy * M_PI / 360.0);
|
|
Scalar b = -t;
|
|
Scalar l = b * aspect;
|
|
Scalar r = t * aspect;
|
|
|
|
return inverseFrustumMatrix(l, r, b, t, zNear, zFar);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> orthoMatrix(Scalar left, Scalar right, Scalar bottom, Scalar top,
|
|
Scalar zNear=-1, Scalar zFar=1)
|
|
{
|
|
Mat4<Scalar> m(0.0);
|
|
|
|
m(0, 0) = Scalar(2) / (right - left);
|
|
m(1, 1) = Scalar(2) / (top - bottom);
|
|
m(2, 2) = -Scalar(2) / (zFar - zNear);
|
|
m(0, 3) = -(right + left) / (right - left);
|
|
m(1, 3) = -(top + bottom) / (top - bottom);
|
|
m(2, 3) = -(zFar + zNear) / (zFar - zNear);
|
|
m(3, 3) = Scalar(1);
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> lookAtMatrix(const Vector<Scalar, 3>& eye,
|
|
const Vector<Scalar, 3>& center,
|
|
const Vector<Scalar, 3>& up)
|
|
{
|
|
|
|
Vector<Scalar, 3> z = normalize(eye - center);
|
|
Vector<Scalar, 3> x = normalize(cross(up, z));
|
|
Vector<Scalar, 3> y = normalize(cross(z, x));
|
|
|
|
// clang-format off
|
|
Mat4<Scalar> m;
|
|
m(0, 0) = x[0]; m(0, 1) = x[1]; m(0, 2) = x[2]; m(0, 3) = -dot(x, eye);
|
|
m(1, 0) = y[0]; m(1, 1) = y[1]; m(1, 2) = y[2]; m(1, 3) = -dot(y, eye);
|
|
m(2, 0) = z[0]; m(2, 1) = z[1]; m(2, 2) = z[2]; m(2, 3) = -dot(z, eye);
|
|
m(3, 0) = 0.0; m(3, 1) = 0.0; m(3, 2) = 0.0; m(3, 3) = 1.0;
|
|
// clang-format on
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> translationMatrix(const Vector<Scalar, 3>& t)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
m(0, 0) = m(1, 1) = m(2, 2) = m(3, 3) = 1.0f;
|
|
m(0, 3) = t[0];
|
|
m(1, 3) = t[1];
|
|
m(2, 3) = t[2];
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> rotationMatrixX(Scalar angle)
|
|
{
|
|
Scalar ca = cos(angle * (M_PI / 180.0));
|
|
Scalar sa = sin(angle * (M_PI / 180.0));
|
|
|
|
Mat4<Scalar> m(0.0);
|
|
m(0, 0) = 1.0;
|
|
m(1, 1) = ca;
|
|
m(1, 2) = -sa;
|
|
m(2, 2) = ca;
|
|
m(2, 1) = sa;
|
|
m(3, 3) = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> rotationMatrixY(Scalar angle)
|
|
{
|
|
Scalar ca = cos(angle * (M_PI / 180.0));
|
|
Scalar sa = sin(angle * (M_PI / 180.0));
|
|
|
|
Mat4<Scalar> m(0.0);
|
|
m(0, 0) = ca;
|
|
m(0, 2) = sa;
|
|
m(1, 1) = 1.0;
|
|
m(2, 0) = -sa;
|
|
m(2, 2) = ca;
|
|
m(3, 3) = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> rotationMatrixZ(Scalar angle)
|
|
{
|
|
Scalar ca = cos(angle * (M_PI / 180.0));
|
|
Scalar sa = sin(angle * (M_PI / 180.0));
|
|
|
|
Mat4<Scalar> m(0.0);
|
|
m(0, 0) = ca;
|
|
m(0, 1) = -sa;
|
|
m(1, 0) = sa;
|
|
m(1, 1) = ca;
|
|
m(2, 2) = 1.0;
|
|
m(3, 3) = 1.0;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> rotationMatrix(const Vector<Scalar, 3>& axis, Scalar angle)
|
|
{
|
|
Mat4<Scalar> m(Scalar(0));
|
|
Scalar a = angle * (M_PI / 180.0f);
|
|
Scalar c = cosf(a);
|
|
Scalar s = sinf(a);
|
|
Scalar one_m_c = Scalar(1) - c;
|
|
Vector<Scalar, 3> ax = normalize(axis);
|
|
|
|
m(0, 0) = ax[0] * ax[0] * one_m_c + c;
|
|
m(0, 1) = ax[0] * ax[1] * one_m_c - ax[2] * s;
|
|
m(0, 2) = ax[0] * ax[2] * one_m_c + ax[1] * s;
|
|
|
|
m(1, 0) = ax[1] * ax[0] * one_m_c + ax[2] * s;
|
|
m(1, 1) = ax[1] * ax[1] * one_m_c + c;
|
|
m(1, 2) = ax[1] * ax[2] * one_m_c - ax[0] * s;
|
|
|
|
m(2, 0) = ax[2] * ax[0] * one_m_c - ax[1] * s;
|
|
m(2, 1) = ax[2] * ax[1] * one_m_c + ax[0] * s;
|
|
m(2, 2) = ax[2] * ax[2] * one_m_c + c;
|
|
|
|
m(3, 3) = 1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> rotationMatrix(const Vector<Scalar, 4>& quat)
|
|
{
|
|
Mat4<Scalar> m(0.0f);
|
|
Scalar s1(1);
|
|
Scalar s2(2);
|
|
|
|
m(0,0) = s1 - s2 * quat[1] * quat[1] - s2 * quat[2] * quat[2];
|
|
m(1,0) = s2 * quat[0] * quat[1] + s2 * quat[3] * quat[2];
|
|
m(2,0) = s2 * quat[0] * quat[2] - s2 * quat[3] * quat[1];
|
|
|
|
m(0,1) = s2 * quat[0] * quat[1] - s2 * quat[3] * quat[2];
|
|
m(1,1) = s1 - s2 * quat[0] * quat[0] - s2 * quat[2] * quat[2];
|
|
m(2,1) = s2 * quat[1] * quat[2] + s2 * quat[3] * quat[0];
|
|
|
|
m(0,2) = s2 * quat[0] * quat[2] + s2 * quat[3] * quat[1];
|
|
m(1,2) = s2 * quat[1] * quat[2] - s2 * quat[3] * quat[0];
|
|
m(2,2) = s1 - s2 * quat[0] * quat[0] - s2 * quat[1] * quat[1];
|
|
|
|
m(3,3) = 1.0f;
|
|
|
|
return m;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat3<Scalar> linearPart(const Mat4<Scalar>& m)
|
|
{
|
|
Mat3<Scalar> result;
|
|
for (int j = 0; j < 3; ++j)
|
|
for (int i = 0; i < 3; ++i)
|
|
result(i, j) = m(i, j);
|
|
return result;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Vector<Scalar, 3> projectiveTransform(const Mat4<Scalar>& m,
|
|
const Vector<Scalar, 3>& v)
|
|
{
|
|
const Scalar x = m(0, 0) * v[0] + m(0, 1) * v[1] + m(0, 2) * v[2] + m(0, 3);
|
|
const Scalar y = m(1, 0) * v[0] + m(1, 1) * v[1] + m(1, 2) * v[2] + m(1, 3);
|
|
const Scalar z = m(2, 0) * v[0] + m(2, 1) * v[1] + m(2, 2) * v[2] + m(2, 3);
|
|
const Scalar w = m(3, 0) * v[0] + m(3, 1) * v[1] + m(3, 2) * v[2] + m(3, 3);
|
|
return Vector<Scalar, 3>(x / w, y / w, z / w);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
// transforms a position `v` by an affine transformation. (linear*v + translation)
|
|
template <typename Scalar>
|
|
Vector<Scalar, 3> affineTransform(const Mat4<Scalar>& m,
|
|
const Vector<Scalar, 3>& v)
|
|
{
|
|
const Scalar x = m(0, 0) * v[0] + m(0, 1) * v[1] + m(0, 2) * v[2] + m(0, 3);
|
|
const Scalar y = m(1, 0) * v[0] + m(1, 1) * v[1] + m(1, 2) * v[2] + m(1, 3);
|
|
const Scalar z = m(2, 0) * v[0] + m(2, 1) * v[1] + m(2, 2) * v[2] + m(2, 3);
|
|
return Vector<Scalar, 3>(x, y, z);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
// transforms a vector `v` by the linear part of an affine transformation.
|
|
template <typename Scalar>
|
|
Vector<Scalar, 3> linearTransform(const Mat4<Scalar>& m,
|
|
const Vector<Scalar, 3>& v)
|
|
{
|
|
const Scalar x = m(0, 0) * v[0] + m(0, 1) * v[1] + m(0, 2) * v[2];
|
|
const Scalar y = m(1, 0) * v[0] + m(1, 1) * v[1] + m(1, 2) * v[2];
|
|
const Scalar z = m(2, 0) * v[0] + m(2, 1) * v[1] + m(2, 2) * v[2];
|
|
return Vector<Scalar, 3>(x, y, z);
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
Mat4<Scalar> inverse(const Mat4<Scalar>& m)
|
|
{
|
|
Scalar Coef00 = m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2);
|
|
Scalar Coef02 = m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1);
|
|
Scalar Coef03 = m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1);
|
|
|
|
Scalar Coef04 = m(1, 2) * m(3, 3) - m(1, 3) * m(3, 2);
|
|
Scalar Coef06 = m(1, 1) * m(3, 3) - m(1, 3) * m(3, 1);
|
|
Scalar Coef07 = m(1, 1) * m(3, 2) - m(1, 2) * m(3, 1);
|
|
|
|
Scalar Coef08 = m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2);
|
|
Scalar Coef10 = m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1);
|
|
Scalar Coef11 = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
|
|
|
|
Scalar Coef12 = m(0, 2) * m(3, 3) - m(0, 3) * m(3, 2);
|
|
Scalar Coef14 = m(0, 1) * m(3, 3) - m(0, 3) * m(3, 1);
|
|
Scalar Coef15 = m(0, 1) * m(3, 2) - m(0, 2) * m(3, 1);
|
|
|
|
Scalar Coef16 = m(0, 2) * m(2, 3) - m(0, 3) * m(2, 2);
|
|
Scalar Coef18 = m(0, 1) * m(2, 3) - m(0, 3) * m(2, 1);
|
|
Scalar Coef19 = m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1);
|
|
|
|
Scalar Coef20 = m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2);
|
|
Scalar Coef22 = m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1);
|
|
Scalar Coef23 = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
|
|
|
|
Vector<Scalar, 4> const SignA(+1, -1, +1, -1);
|
|
Vector<Scalar, 4> const SignB(-1, +1, -1, +1);
|
|
|
|
Vector<Scalar, 4> Fac0(Coef00, Coef00, Coef02, Coef03);
|
|
Vector<Scalar, 4> Fac1(Coef04, Coef04, Coef06, Coef07);
|
|
Vector<Scalar, 4> Fac2(Coef08, Coef08, Coef10, Coef11);
|
|
Vector<Scalar, 4> Fac3(Coef12, Coef12, Coef14, Coef15);
|
|
Vector<Scalar, 4> Fac4(Coef16, Coef16, Coef18, Coef19);
|
|
Vector<Scalar, 4> Fac5(Coef20, Coef20, Coef22, Coef23);
|
|
|
|
Vector<Scalar, 4> Vec0(m(0, 1), m(0, 0), m(0, 0), m(0, 0));
|
|
Vector<Scalar, 4> Vec1(m(1, 1), m(1, 0), m(1, 0), m(1, 0));
|
|
Vector<Scalar, 4> Vec2(m(2, 1), m(2, 0), m(2, 0), m(2, 0));
|
|
Vector<Scalar, 4> Vec3(m(3, 1), m(3, 0), m(3, 0), m(3, 0));
|
|
|
|
Vector<Scalar, 4> Inv0 = cmult(SignA, (cmult(Vec1, Fac0) - cmult(Vec2, Fac1) + cmult(Vec3, Fac2)));
|
|
Vector<Scalar, 4> Inv1 = cmult(SignB, (cmult(Vec0, Fac0) - cmult(Vec2, Fac3) + cmult(Vec3, Fac4)));
|
|
Vector<Scalar, 4> Inv2 = cmult(SignA, (cmult(Vec0, Fac1) - cmult(Vec1, Fac3) + cmult(Vec3, Fac5)));
|
|
Vector<Scalar, 4> Inv3 = cmult(SignB, (cmult(Vec0, Fac2) - cmult(Vec1, Fac4) + cmult(Vec2, Fac5)));
|
|
|
|
Mat4<Scalar> Inverse(Inv0, Inv1, Inv2, Inv3);
|
|
|
|
Vector<Scalar, 4> Row0(Inverse(0, 0), Inverse(1, 0), Inverse(2, 0),
|
|
Inverse(3, 0));
|
|
Vector<Scalar, 4> Col0(m(0, 0), m(0, 1), m(0, 2), m(0, 3));
|
|
|
|
Scalar Determinant = dot(Col0, Row0);
|
|
|
|
Inverse /= Determinant;
|
|
|
|
return Inverse;
|
|
}
|
|
|
|
//== Mat3 functions ===========================================================
|
|
|
|
template <typename Scalar>
|
|
Mat3<Scalar> inverse(const Mat3<Scalar>& m)
|
|
{
|
|
Scalar det = (-m(0, 0) * m(1, 1) * m(2, 2) + m(0, 0) * m(1, 2) * m(2, 1) +
|
|
m(1, 0) * m(0, 1) * m(2, 2) - m(1, 0) * m(0, 2) * m(2, 1) -
|
|
m(2, 0) * m(0, 1) * m(1, 2) + m(2, 0) * m(0, 2) * m(1, 1));
|
|
|
|
Mat3<Scalar> inv;
|
|
inv(0, 0) = (m(1, 2) * m(2, 1) - m(1, 1) * m(2, 2)) / det;
|
|
inv(0, 1) = (m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) / det;
|
|
inv(0, 2) = (m(0, 2) * m(1, 1) - m(0, 1) * m(1, 2)) / det;
|
|
inv(1, 0) = (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) / det;
|
|
inv(1, 1) = (m(0, 2) * m(2, 0) - m(0, 0) * m(2, 2)) / det;
|
|
inv(1, 2) = (m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) / det;
|
|
inv(2, 0) = (m(1, 1) * m(2, 0) - m(1, 0) * m(2, 1)) / det;
|
|
inv(2, 1) = (m(0, 0) * m(2, 1) - m(0, 1) * m(2, 0)) / det;
|
|
inv(2, 2) = (m(0, 1) * m(1, 0) - m(0, 0) * m(1, 1)) / det;
|
|
|
|
return inv;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
|
|
template <typename Scalar>
|
|
bool symmetricEigendecomposition(const Mat3<Scalar>& m, Scalar& eval1,
|
|
Scalar& eval2, Scalar& eval3,
|
|
Vector<Scalar, 3>& evec1,
|
|
Vector<Scalar, 3>& evec2,
|
|
Vector<Scalar, 3>& evec3)
|
|
{
|
|
unsigned int i, j;
|
|
Scalar theta, t, c, s;
|
|
Mat3<Scalar> V = Mat3<Scalar>::identity();
|
|
Mat3<Scalar> R;
|
|
Mat3<Scalar> A = m;
|
|
const Scalar eps = 1e-10; //0.000001;
|
|
|
|
int iterations = 100;
|
|
while (iterations--)
|
|
{
|
|
// find largest off-diagonal elem
|
|
if (fabs(A(0, 1)) < fabs(A(0, 2)))
|
|
{
|
|
if (fabs(A(0, 2)) < fabs(A(1, 2)))
|
|
{
|
|
i = 1, j = 2;
|
|
}
|
|
else
|
|
{
|
|
i = 0, j = 2;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (fabs(A(0, 1)) < fabs(A(1, 2)))
|
|
{
|
|
i = 1, j = 2;
|
|
}
|
|
else
|
|
{
|
|
i = 0, j = 1;
|
|
}
|
|
}
|
|
|
|
// converged?
|
|
if (fabs(A(i, j)) < eps)
|
|
break;
|
|
|
|
// compute Jacobi-Rotation
|
|
theta = 0.5 * (A(j, j) - A(i, i)) / A(i, j);
|
|
t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
|
|
if (theta < 0.0)
|
|
t = -t;
|
|
|
|
c = 1.0 / sqrt(1.0 + t * t);
|
|
s = t * c;
|
|
|
|
R = Mat3<Scalar>::identity();
|
|
R(i, i) = R(j, j) = c;
|
|
R(i, j) = s;
|
|
R(j, i) = -s;
|
|
|
|
A = transpose(R) * A * R;
|
|
V = V * R;
|
|
}
|
|
|
|
if (iterations > 0)
|
|
{
|
|
|
|
// sort and return
|
|
int sorted[3];
|
|
Scalar d[3] = {A(0, 0), A(1, 1), A(2, 2)};
|
|
|
|
if (d[0] > d[1])
|
|
{
|
|
if (d[1] > d[2])
|
|
{
|
|
sorted[0] = 0, sorted[1] = 1, sorted[2] = 2;
|
|
}
|
|
else
|
|
{
|
|
if (d[0] > d[2])
|
|
{
|
|
sorted[0] = 0, sorted[1] = 2, sorted[2] = 1;
|
|
}
|
|
else
|
|
{
|
|
sorted[0] = 2, sorted[1] = 0, sorted[2] = 1;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (d[0] > d[2])
|
|
{
|
|
sorted[0] = 1, sorted[1] = 0, sorted[2] = 2;
|
|
}
|
|
else
|
|
{
|
|
if (d[1] > d[2])
|
|
{
|
|
sorted[0] = 1, sorted[1] = 2, sorted[2] = 0;
|
|
}
|
|
else
|
|
{
|
|
sorted[0] = 2, sorted[1] = 1, sorted[2] = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
eval1 = d[sorted[0]];
|
|
eval2 = d[sorted[1]];
|
|
eval3 = d[sorted[2]];
|
|
|
|
evec1 = Vector<Scalar, 3>(V(0, sorted[0]), V(1, sorted[0]),
|
|
V(2, sorted[0]));
|
|
evec2 = Vector<Scalar, 3>(V(0, sorted[1]), V(1, sorted[1]),
|
|
V(2, sorted[1]));
|
|
evec3 = normalize(cross(evec1, evec2));
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//== Vector functions =========================================================
|
|
|
|
//! read the space-separated components of a vector from a stream
|
|
template <typename Scalar, int N>
|
|
inline std::istream& operator>>(std::istream& is, Vector<Scalar, N>& vec)
|
|
{
|
|
for (int i = 0; i < N; ++i)
|
|
is >> vec[i];
|
|
return is;
|
|
}
|
|
|
|
//! output a vector by printing its space-separated compontens
|
|
template <typename Scalar, int N>
|
|
inline std::ostream& operator<<(std::ostream& os, const Vector<Scalar, N>& vec)
|
|
{
|
|
for (int i = 0; i < N - 1; ++i)
|
|
os << vec[i] << " ";
|
|
os << vec[N - 1];
|
|
return os;
|
|
}
|
|
|
|
//! compute the dot product of two vectors
|
|
template <typename Scalar, int N>
|
|
inline Scalar dot(const Vector<Scalar, N>& v0, const Vector<Scalar, N>& v1)
|
|
{
|
|
Scalar p = v0[0] * v1[0];
|
|
for (int i = 1; i < N; ++i)
|
|
p += v0[i] * v1[i];
|
|
return p;
|
|
}
|
|
|
|
//! compute the Euclidean distance between two points
|
|
template <typename Scalar, int N>
|
|
inline Scalar distance(const Vector<Scalar, N>& v0, const Vector<Scalar, N>& v1)
|
|
{
|
|
Scalar dist(0), d;
|
|
for (int i = 0; i < N; ++i)
|
|
{
|
|
d = v0[i] - v1[i];
|
|
d *= d;
|
|
dist += d;
|
|
}
|
|
return (Scalar)sqrt(dist);
|
|
}
|
|
|
|
//! compute the cross product of two vectors (only valid for 3D vectors)
|
|
template <typename Scalar>
|
|
inline Vector<Scalar, 3> cross(const Vector<Scalar, 3>& v0,
|
|
const Vector<Scalar, 3>& v1)
|
|
{
|
|
return Vector<Scalar, 3>(v0[1] * v1[2] - v0[2] * v1[1],
|
|
v0[2] * v1[0] - v0[0] * v1[2],
|
|
v0[0] * v1[1] - v0[1] * v1[0]);
|
|
}
|
|
|
|
//=============================================================================
|
|
} // namespace
|
|
//=============================================================================
|