hpr-cpp/source/hpr/math/matrix.hpp
2024-03-14 15:57:16 +05:00

509 lines
18 KiB
C++

#pragma once
#include <hpr/numeric.hpp>
#include <hpr/container/static_array.hpp>
#include <hpr/math/quaternion.hpp>
namespace hpr
{
// forward declarations
template <IsReal Type, Size Rows, Size Cols> requires (Rows >= 0 && Cols >= 0)
class Matrix;
template <IsReal Type, Size Rows, Size Cols>
using SubMatrix = typename std::conditional<(Rows >= 2 && Cols >= 2), Matrix<Type, Rows - 1, Cols - 1>, Matrix<Type, 1, 1>>::type;
// type traits
template <typename T>
struct is_matrix : public std::false_type {};
template <typename T, Size Rows, Size Cols>
struct is_matrix<Matrix<T, Rows, Cols>> : public std::true_type {};
// concepts
template <typename T>
concept IsMatrix = is_matrix<T>::value;
// aliases
template <typename Type, size_t Row, size_t Col>
using mat = Matrix<Type, Row, Col>;
using mat2 = Matrix<scalar, 2, 2>;
using mat3 = Matrix<scalar, 3, 3>;
using mat4 = Matrix<scalar, 4, 4>;
template <IsReal Type, Size Rows, Size Cols> requires (Rows >= 0 && Cols >= 0)
class Matrix : public StaticArray<Type, Rows * Cols>
{
using base = StaticArray<Type, Rows * Cols>;
public:
using value_type = Type;
using size_type = Size;
using pointer = Type*;
using reference = Type&;
using iterator = Iterator<Type>;
using const_reference = Type const&;
using const_iterator = Iterator<const Type>;
protected:
size_type p_rows;
size_type p_cols;
public:
friend constexpr
void swap(Matrix& main, Matrix& other)
{
using std::swap;
swap(static_cast<base&>(main), static_cast<base&>(other));
swap(main.p_rows, other.p_rows);
swap(main.p_cols, other.p_cols);
}
inline
Matrix() :
base {},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(const Matrix& ms) :
base {static_cast<base>(ms)},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(Matrix&& ms) noexcept:
base {std::forward<base>(static_cast<base>(ms))},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix& operator=(const Matrix& ms)
{
//base::operator=(ms);
swap(*this, ms);
return *this;
}
inline explicit
Matrix(const base& vs) :
base {vs},
p_rows {Rows},
p_cols {Cols}
{}
inline explicit
Matrix(base&& vs) noexcept:
base {std::forward<base>(vs)},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(typename base::iterator start, typename base::iterator end) :
base {start, end},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(typename base::const_iterator start, typename base::const_iterator end) :
base {start, end},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(std::initializer_list<value_type> list) :
base {list},
p_rows {Rows},
p_cols {Cols}
{}
template <IsReal... Args>
inline
Matrix(value_type&& v, Args&& ...args) requires (1 + sizeof...(args) == Rows * Cols):
base {v, static_cast<value_type>(std::forward<Args>(args))...},
p_rows {Rows},
p_cols {Cols}
{}
inline
Matrix(const value_type& v) :
base {},
p_rows {Rows},
p_cols {Cols}
{
for (Size n = 0; n < Rows * Cols; ++n)
(*this)[n] = v;
}
inline
Matrix& operator=(const value_type& v)
{
for (Size n = 0; n < Rows * Cols; ++n)
(*this)[n] = v;
return *this;
}
inline explicit
Matrix(const Quaternion& q) requires (Rows == 3 && Cols == 3 || Rows == 4 && Cols == 4) :
base {},
p_rows {Rows},
p_cols {Cols}
{
const scalar s = pow(norm(q), -2);
(*this)(0, 0) = 1 - 2 * s * (q[2] * q[2] + q[3] * q[3]);
(*this)(1, 0) = 2 * s * (q[1] * q[2] - q[3] * q[0]);
(*this)(2, 0) = 2 * s * (q[1] * q[3] - q[2] * q[0]);
(*this)(0, 1) = 2 * s * (q[1] * q[2] + q[3] * q[0]);
(*this)(1, 1) = 1 - 2 * s * (q[1] * q[1] + q[3] * q[3]);
(*this)(2, 1) = 2 * s * (q[2] * q[3] + q[1] * q[0]);
(*this)(0, 2) = 2 * s * (q[1] * q[3] + q[2] * q[0]);
(*this)(1, 2) = 2 * s * (q[2] * q[3] + q[1] * q[0]);
(*this)(2, 2) = 1 - 2 * s * (q[1] * q[1] + q[2] * q[2]);
if constexpr (Rows == 4)
(*this)(3, 3) = 1;
}
// access
inline
reference operator()(size_type row, size_type col)
{
if (row >= p_rows || std::numeric_limits<size_type>::max() - p_rows < row)
throw std::out_of_range("Row index is out of range");
if (col >= p_cols || std::numeric_limits<size_type>::max() - p_cols < col)
throw std::out_of_range("Column index is out of range");
return (*this)[col + p_rows * row];
}
inline
const_reference operator()(size_type row, size_type col) const
{
if (row >= p_rows || std::numeric_limits<size_type>::max() - p_rows < row)
throw std::out_of_range("Row index is out of range");
if (col >= p_cols || std::numeric_limits<size_type>::max() - p_cols < col)
throw std::out_of_range("Column index is out of range");
return (*this)[col + p_rows * row];
}
Vector<value_type, Cols> row(size_type row) const
{
Vector<value_type, Cols> vs;
for (auto n = 0; n < Cols; ++n)
vs[n] = (*this)(row, n);
return vs;
}
void row(size_type row, const Vector<value_type, Cols>& vs)
{
for (auto n = 0; n < Cols; ++n)
(*this)(row, n) = vs[n];
}
Vector<value_type, Rows> col(size_type col) const
{
Vector<value_type, Rows> vs;
for (auto n = 0; n < Rows; ++n)
vs[n] = (*this)(n, col);
return vs;
}
void col(size_type col, const Vector<value_type, Rows>& vs)
{
for (auto n = 0; n < Rows; ++n)
(*this)(n, col) = vs[n];
}
[[nodiscard]] constexpr size_type rows() const { return p_rows; }
[[nodiscard]] constexpr size_type cols() const { return p_cols; }
// member functions
[[nodiscard]]
constexpr
bool is_square() const
{
return p_rows == p_cols;
}
inline
Matrix& fill(value_type value)
{
for (auto n = 0; n < this->size(); ++n)
(*this)[n] = value;
return *this;
}
// Global functions
static inline
Matrix identity()
{
Matrix ms;
for (auto n = 0; n < Rows; ++n)
//for (auto k = 0; k < Cols; ++k)
ms(n, n) = 1;
return ms;
}
};
// global operators
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator+(const Matrix<T, R, C>& lhs) { Matrix<T, R, C> ms; for (Size n = 0; n < lhs.size(); ++n) ms[n] = lhs[n]; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator-(const Matrix<T, R, C>& lhs) { Matrix<T, R, C> ms; for (Size n = 0; n < lhs.size(); ++n) ms[n] = -lhs[n]; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator+=(Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] += rhs[n]; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator-=(Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] -= rhs[n]; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator*=(Matrix<T, R, C>& lhs, const Matrix<T, C, R>& rhs) { Matrix<T, R, C> temp {lhs}; for (Size n = 0; n < R; ++n) for (Size k = 0; k < C; ++k) lhs(n, k) = sum(temp.col(k) * rhs.row(n)); return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator+(const Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] += rhs[n]; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator-(const Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] -= rhs[n]; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator*(const Matrix<T, R, C>& lhs, const Matrix<T, C, R>& rhs) { Matrix<T, R, C> ms; for (Size n = 0; n < R; ++n) for (Size k = 0; k < C; ++k) ms(n, k) = sum(lhs.col(k) * rhs.row(n)); return ms; }
template <IsReal T, Size R, Size C> inline bool operator==(const Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { for (Size n = 0; n < lhs.size(); ++n) if (lhs[n] != rhs[n]) return false; return true; }
template <IsReal T, Size R, Size C> inline bool operator!=(const Matrix<T, R, C>& lhs, const Matrix<T, R, C>& rhs) { for (Size n = 0; n < lhs.size(); ++n) if (lhs[n] == rhs[n]) return false; return true; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator+=(Matrix<T, R, C>& lhs, const T& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] += rhs; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator-=(Matrix<T, R, C>& lhs, const T& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] -= rhs; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator*=(Matrix<T, R, C>& lhs, const T& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] *= rhs; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C>& operator/=(Matrix<T, R, C>& lhs, const T& rhs) { for (Size n = 0; n < lhs.size(); ++n) lhs[n] /= rhs; return lhs; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator+(const Matrix<T, R, C>& lhs, const T& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] += rhs; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator-(const Matrix<T, R, C>& lhs, const T& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] -= rhs; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator*(const Matrix<T, R, C>& lhs, const T& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] *= rhs; return ms; }
template <IsReal T, Size R, Size C> inline Matrix<T, R, C> operator/(const Matrix<T, R, C>& lhs, const T& rhs) { Matrix<T, R, C> ms {lhs}; for (Size n = 0; n < lhs.size(); ++n) ms[n] /= rhs; return ms; }
template <IsReal T, Size R, Size C> inline Vector<T, R> operator*(const Matrix<T, R, C>& ms, const Vector<T, R>& vs) { Vector<T, R> res; for (Size n = 0; n < R; ++n) res[n] = sum(ms.row(n) * vs); return res; }
template <IsReal T, Size R, Size C> inline Vector<T, C> operator*(const Vector<T, R>& vs, const Matrix<T, R, C>& ms) { Vector<T, C> res; for (Size n = 0; n < C; ++n) res[n] = sum(ms.col(n) * vs); return res; }
template <IsReal T, Size R, Size C> inline bool operator==(const Matrix<T, R, C>& lhs, const Vector<T, R * C>& rhs) { return false; }
template <IsReal T, Size R, Size C> inline bool operator!=(const Matrix<T, R, C>& lhs, const Vector<T, R * C>& rhs) { return true; }
// matrix operations
//! Transpose matrix
template <IsReal T, Size R, Size C>
inline
Matrix<T, R, C> transpose(const Matrix<T, R, C>& ms)
{
Matrix<T, R, C> res;
for (Size n = 0; n < R; ++n)
for (Size k = 0; k < C; ++k)
res(k, n) = ms(n, k);
return res;
}
//! Trace of a matrix
template <IsReal T, Size R, Size C>
inline
T trace(const Matrix<T, R, C>& ms) requires (R == C)
{
T res;
for (auto n = 0; n < R; ++n)
res += ms(n, n);
return res;
}
//! Minor of a matrix
template <IsReal T, Size R, Size C>
inline
SubMatrix<T, R, C> minor(const Matrix<T, R, C>& ms, Size row, Size col)
{
if (ms.size() < 4)
throw std::runtime_error("Matrix should be greater 2x2");
SubMatrix<T, R, C> minor;
auto minor_iter = minor.begin();
for (auto n = 0; n < R; ++n)
for (auto k = 0; k < C; ++k)
if (k != col && n != row)
*(minor_iter++) = ms[k + ms.rows() * n];
return minor;
}
//! Determinant of a matrix
template <IsReal T, Size R, Size C>
inline
scalar det(const Matrix<T, R, C>& ms) requires (R == C)
{
if (ms.size() == 1)
return ms[0];
else if (ms.size() == 4)
return ms(0, 0) * ms(1, 1) - ms(0, 1) * ms(1, 0);
else {
scalar res = 0;
for (auto n = 0; n < ms.cols(); ++n)
res += pow(-1, n) * ms(0, n) * det(minor(ms, 0, n));
return res;
}
}
//! Adjoint matrix
template <IsReal T, Size R, Size C>
inline
Matrix<T, R, C> adj(const Matrix<T, R, C>& ms)
{
Matrix<T, R, C> res;
for (auto n = 0; n < R; ++n)
for (auto k = 0; k < C; ++k)
res(n, k) = pow(-1, n + k) * det(minor(ms, n, k));
return transpose(res);
}
//! Inverse matrix
template <IsReal T, Size R, Size C>
inline
Matrix<T, R, C> inv(const Matrix<T, R, C>& ms)
{
return adj(ms) / det(ms);
}
// Transforms
template <IsReal T>
inline
Matrix<T, 4, 4> translate(const Matrix<T, 4, 4>& ms, const Vector<T, 3>& vs)
{
Matrix<T, 4, 4> res {ms};
res.col(3, ms.row(0) * vs[0] + ms.row(1) * vs[1] + ms.row(2) * vs[2] + ms.row(3));
return res;
}
template <IsReal T>
inline
Matrix<T, 4, 4> rotate(const Matrix<T, 4, 4>& ms, const Vector<T, 3>& vs, T angle)
{
const T cosv = cos(angle);
const T sinv = sin(angle);
Vector<T, 3> axis {normalize(vs)};
Vector<T, 3> temp {(static_cast<T>(1) - cosv) * axis};
Matrix<T, 4, 4> rot;
rot(0, 0) = cosv + temp[0] * axis[0];
rot(0, 1) = temp[0] * axis[1] + sinv * axis[2];
rot(0, 2) = temp[0] * axis[2] - sinv * axis[1];
rot(1, 0) = temp[1] * axis[0] - sinv * axis[2];
rot(1, 1) = cosv + temp[1] * axis[1];
rot(1, 2) = temp[1] * axis[2] + sinv * axis[0];
rot(2, 0) = temp[2] * axis[0] + sinv * axis[1];
rot(2, 1) = temp[2] * axis[1] - sinv * axis[0];
rot(2, 2) = cosv + temp[2] * axis[2];
Matrix<T, 4, 4> res {ms};
res.row(0, ms.row(0) * rot(0, 0) + ms.row(1) * rot(0, 1) + ms.row(2) * rot(0, 2));
res.row(1, ms.row(0) * rot(1, 0) + ms.row(1) * rot(1, 1) + ms.row(2) * rot(1, 2));
res.row(2, ms.row(0) * rot(2, 0) + ms.row(1) * rot(2, 1) + ms.row(2) * rot(2, 2));
res.row(3, ms.row(3));
return res;
}
template <IsReal T>
inline
Matrix<T, 4, 4> rotate(const Matrix<T, 4, 4>& ms, const Quaternion& q)
{
return ms * Matrix<T, 4, 4>(q);
}
template <IsReal T>
inline
Matrix<T, 4, 4> scale(const Matrix<T, 4, 4>& ms, const Vector<T, 3>& vs)
{
Matrix<T, 4, 4> res;
res.row(0, ms.row(0) * vs[0]);
res.row(1, ms.row(1) * vs[1]);
res.row(2, ms.row(2) * vs[2]);
res.row(3, ms.row(3));
return res;
}
template <typename T>
inline
Matrix<T, 4, 4> lookAt(const Vector<T, 3>& eye, const Vector<T, 3>& center, const Vector<T, 3>& up)
{
const Vector<T, 3> forward {normalize(center - eye)};
const Vector<T, 3> right {normalize(cross(forward, up))};
const Vector<T, 3> nup {cross(right, forward)};
const Vector<T, 3> translation {dot(right, eye), dot(nup, eye), -dot(forward, eye)};
Matrix<T, 4, 4> res = Matrix<T, 4, 4>::identity();
res.row(0, Vector<T, 4>(right, 0));
res.row(1, Vector<T, 4>(nup, 0));
res.row(2, Vector<T, 4>(-forward, 0));
res.col(3, Vector<T, 4>(-translation, static_cast<T>(1)));
return res;
}
// Clip space
template <IsReal T>
inline
Matrix<T, 4, 4> ortho(T left, T right, T bottom, T top)
{
Matrix<T, 4, 4> ms = Matrix<T, 4, 4>::identity();
ms(0, 0) = static_cast<T>(2) / (right - left);
ms(1, 1) = static_cast<T>(2) / (top - bottom);
ms(2, 2) = -static_cast<T>(1);
ms(3, 0) = -(right + left) / (right - left);
ms(3, 1) = -(top + bottom) / (top - bottom);
return ms;
}
template <IsReal T>
inline
Matrix<T, 4, 4> ortho(T left, T right, T bottom, T top, T zNear, T zFar)
{
Matrix<T, 4, 4> ms = Matrix<T, 4, 4>::identity();
ms(0, 0) = static_cast<T>(2) / (right - left);
ms(1, 1) = static_cast<T>(2) / (top - bottom);
ms(2, 2) = -static_cast<T>(2) / (zFar - zNear);
ms(0, 3) = -(right + left) / (right - left);
ms(1, 3) = -(top + bottom) / (top - bottom);
ms(2, 3) = -(zFar + zNear) / (zFar - zNear);
return ms;
}
template <IsReal T>
inline
Matrix<T, 4, 4> perspective(T fovy, T aspect, T zNear, T zFar)
{
assert(abs(aspect - std::numeric_limits<T>::epsilon()) > 0);
Matrix<T, 4, 4> ms;
const T halfFovyTan = tan(fovy / 2);
ms(0, 0) = static_cast<T>(1) / (aspect * halfFovyTan);
ms(1, 1) = static_cast<T>(1) / halfFovyTan;
ms(2, 2) = -(zFar + zNear) / (zFar - zNear);
ms(3, 2) = -static_cast<T>(1);
ms(2, 3) = -(static_cast<T>(2) * zFar * zNear) / (zFar - zNear);
return ms;
}
} // end namespace hpr