555 lines
19 KiB
C++
555 lines
19 KiB
C++
/*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
|
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Copyright 2011-2015 Danny Robson <danny@nerdcruft.net>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "point.hpp"
|
|
#include "range.hpp"
|
|
#include "vector.hpp"
|
|
|
|
#include <cstdlib>
|
|
|
|
namespace cruft {
|
|
///////////////////////////////////////////////////////////////////////////
|
|
/// a row-major matrix with parameterised size and type
|
|
///
|
|
/// while sizes and types are arbitrary the library is optimised for
|
|
/// dimensions up to ~4, and value types of float. though others _should_
|
|
/// work depending on what operations you pick and behaviours you expect
|
|
/// (eg, an int will round a little differently to floats...)
|
|
template <
|
|
std::size_t Rows,
|
|
std::size_t Cols,
|
|
typename ValueT
|
|
>
|
|
struct matrix {
|
|
static constexpr auto rows = Rows;
|
|
static constexpr auto cols = Cols;
|
|
|
|
using row_t = cruft::vector<Cols,ValueT>;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////
|
|
constexpr matrix () noexcept = default;
|
|
|
|
constexpr matrix(ValueT fill)
|
|
{
|
|
for (auto &r: values)
|
|
r = fill;
|
|
}
|
|
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr matrix (const ValueT(&_data)[Rows][Cols]) noexcept:
|
|
values {}
|
|
{
|
|
static_assert (sizeof (*this) == sizeof (ValueT) * Rows * Cols);
|
|
for (std::size_t r = 0; r < Rows; ++r)
|
|
for (std::size_t c = 0; c < Cols; ++c)
|
|
values[r][c] = _data[r][c];
|
|
}
|
|
|
|
|
|
//---------------------------------------------------------------------
|
|
template <std::size_t S, typename SelfT>
|
|
constexpr matrix (const cruft::coord::base<S,ValueT,SelfT> (&_data)[Rows]) noexcept
|
|
{
|
|
for (std::size_t r = 0; r < Rows; ++r)
|
|
for (std::size_t c = 0; c < Cols; ++c)
|
|
values[r][c] = _data[r][c];
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////
|
|
// index operators return a pointer into the data array so that
|
|
// multidimensional array syntax can be used transparently on this
|
|
// type.
|
|
constexpr row_t&
|
|
operator[] (std::size_t idx)&
|
|
{
|
|
CHECK_LT (idx, rows);
|
|
return values[idx];
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr const row_t&
|
|
operator[] (std::size_t idx) const&
|
|
{
|
|
CHECK_LT (idx, rows);
|
|
return values[idx];
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////
|
|
constexpr row_t*
|
|
data (void)& noexcept
|
|
{
|
|
return &values[0];
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr const row_t*
|
|
data (void) const& noexcept
|
|
{
|
|
return &values[0];
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr auto
|
|
begin (void) const& noexcept
|
|
{
|
|
return data ();
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr auto
|
|
begin (void)& noexcept
|
|
{
|
|
return data ();
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr row_t*
|
|
end (void)& noexcept
|
|
{
|
|
return &values[Rows];
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr const row_t*
|
|
end (void) const& noexcept
|
|
{
|
|
return &values[Rows];
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr auto
|
|
cbegin (void) const& noexcept
|
|
{
|
|
return begin ();
|
|
}
|
|
|
|
//---------------------------------------------------------------------
|
|
constexpr auto
|
|
cend (void) const& noexcept
|
|
{
|
|
return end ();
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////
|
|
ValueT determinant (void) const;
|
|
|
|
matrix inverse (void) const;
|
|
|
|
matrix
|
|
inverse_affine (void) const
|
|
{
|
|
// TODO: ensure we have specialisations for typical dimensions
|
|
return inverse ();
|
|
}
|
|
|
|
|
|
template <
|
|
typename VectorT,
|
|
typename = std::enable_if_t<is_coord_v<VectorT>>
|
|
>
|
|
auto
|
|
operator* (const VectorT &rhs) const
|
|
{
|
|
if constexpr (VectorT::elements + 1 == Cols) {
|
|
return (
|
|
*this * rhs.homog ()
|
|
).template redim<VectorT::elements> ();
|
|
} else {
|
|
VectorT out;
|
|
for (std::size_t r = 0; r < Rows; ++r)
|
|
out[r] = dot (rhs, values[r]);
|
|
return out;
|
|
}
|
|
}
|
|
|
|
bool is_affine (void) const;
|
|
|
|
template <typename U>
|
|
matrix<Rows,Cols,U>
|
|
cast (void) const noexcept
|
|
{
|
|
matrix out;
|
|
std::copy (cbegin (), cend (), std::begin (out));
|
|
return out;
|
|
}
|
|
|
|
// Constant matrices
|
|
static constexpr
|
|
matrix identity (void) noexcept
|
|
{
|
|
auto m = zeroes ();
|
|
for (std::size_t i = 0; i < Rows; ++i)
|
|
m[i][i] = 1;
|
|
return m;
|
|
}
|
|
|
|
static constexpr
|
|
matrix zeroes (void) noexcept
|
|
{
|
|
matrix ret {};
|
|
std::fill (std::begin (ret), std::end (ret), row_t{0});
|
|
return ret;
|
|
}
|
|
|
|
|
|
private:
|
|
row_t values[Rows];
|
|
};
|
|
|
|
|
|
// Perspective matrices
|
|
template <typename T> matrix<4,4,T> ortho (T left, T right, T bottom, T top, T near, T far);
|
|
template <typename T> matrix<4,4,T> ortho2D (T left, T right, T bottom, T top);
|
|
template <typename T> matrix<4,4,T> perspective (T fov, T aspect, range<T> Z);
|
|
template <typename T> matrix<4,4,T> look_at (point<3,T> eye, point<3,T> target, vector<3,T> up);
|
|
|
|
// Affine matrices
|
|
template <size_t S, typename T>
|
|
matrix<S+1,S+1,T>
|
|
translation (vector<S,T> offset)
|
|
{
|
|
matrix<S+1,S+1,T> res {};
|
|
for (size_t i = 0; i != S; ++i) {
|
|
res[i][i] = 1;
|
|
res[i][S] = offset[i];
|
|
}
|
|
res[S][S] = 1;
|
|
return res;
|
|
}
|
|
|
|
|
|
template <size_t S, typename T>
|
|
matrix<S+1,S+1,T>
|
|
scale (vector<S,T> factor)
|
|
{
|
|
matrix<S+1,S+1,T> res {};
|
|
for (size_t i = 0; i != S; ++i)
|
|
res[i][i] = factor[i];
|
|
res[S][S] = 1;
|
|
return res;
|
|
}
|
|
|
|
//template <typename T> matrix<4,4,T> translation (vector<3,T>);
|
|
//template <typename T> matrix<4,4,T> scale (vector<3,T>);
|
|
//template <typename T> matrix<4,4,T> scale (T);
|
|
template <typename T> matrix<4,4,T> rotation (T angle, vector<3,T> about);
|
|
|
|
|
|
/// Returns a matrix representing the rotation vector specifying the
|
|
/// concatentation of: {yaw,pitch,roll}.
|
|
template <typename ValueT>
|
|
constexpr
|
|
matrix<4,4,ValueT>
|
|
rotation_euler (vector<3,ValueT> angles)
|
|
{
|
|
return rotation (angles[1], {0,1,0}) *
|
|
rotation (angles[0], {1,0,0}) *
|
|
rotation (angles[2], {0,0,1});
|
|
}
|
|
|
|
//template <typename T> matrix<3,3,T> translation (vector<2,T>);
|
|
//template <typename T> matrix<3,3,T> scale (vector<2,T>);
|
|
|
|
|
|
template <size_t Rows, size_t Cols, typename ValueT>
|
|
bool
|
|
relatively_equal (const cruft::matrix<Rows,Cols,ValueT> &a,
|
|
const cruft::matrix<Rows,Cols,ValueT> &b,
|
|
const float percentage)
|
|
{
|
|
for (size_t r = 0; r < Rows; ++r)
|
|
if (!all (relatively_equal (a[r], b[r], percentage)))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
// Convert an affine rotation matrix to euler angles.
|
|
//
|
|
// Results are undefined if the matrix is not purely a rotation matrix,
|
|
// or if the dimension is not 3x3 or 4x4.
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
vector<3,T>
|
|
to_euler (const matrix<Rows, Cols, T>&);
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
// logical operations
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr
|
|
bool
|
|
operator== (const matrix<Rows,Cols,T> &a, const matrix<Rows,Cols,T> &b)
|
|
{
|
|
return std::equal (std::cbegin (a), std::cend (a), std::cbegin (b));
|
|
}
|
|
|
|
|
|
//-------------------------------------------------------------------------
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr
|
|
bool
|
|
operator!= (const matrix<Rows,Cols,T> &a, const matrix<Rows,Cols,T> &b)
|
|
{
|
|
return !(a == b);
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
// element operations
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr
|
|
matrix<Rows,Cols,T>
|
|
operator+ (const matrix<Rows,Cols,T>&, const matrix<Rows,Cols,T>&);
|
|
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr
|
|
matrix<Rows,Cols,T>
|
|
operator- (const matrix<Rows,Cols,T>&, const matrix<Rows,Cols,T>&);
|
|
|
|
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr
|
|
matrix<Rows,Cols,T>
|
|
div (const matrix<Rows,Cols,T> &a, const matrix<Rows,Cols,T> &b)
|
|
{
|
|
matrix<Rows,Cols,T> out {};
|
|
for (std::size_t r = 0; r < Rows; ++r)
|
|
for (std::size_t c = 0; c < Cols; ++c)
|
|
out[r][c] = a[r][c] / b[r][c];
|
|
return out;
|
|
}
|
|
|
|
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr T
|
|
max (const matrix<Rows,Cols,T> &m)
|
|
{
|
|
T val = m[0][0];
|
|
for (std::size_t r = 0; r < Rows; ++r)
|
|
for (std::size_t c = 0; c < Cols; ++c)
|
|
val = max (val, m[r][c]);
|
|
return val;
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
// scalar operations
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator* (const matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator/ (const matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator+ (const matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator- (const matrix<R,C,T>&, T);
|
|
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator* (T, const matrix<R,C,T>&);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator/ (T, const matrix<R,C,T>&);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator+ (T, const matrix<R,C,T>&);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T> operator- (T, const matrix<R,C,T>&);
|
|
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T>& operator*= (matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T>& operator/= (matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T>& operator+= (matrix<R,C,T>&, T);
|
|
template <std::size_t R, std::size_t C, typename T> constexpr matrix<R,C,T>& operator-= (matrix<R,C,T>&, T);
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
// matrix operations
|
|
template <
|
|
std::size_t R1, //int C1,
|
|
std::size_t RC, //int R2,
|
|
std::size_t C2,
|
|
typename T
|
|
>
|
|
constexpr
|
|
matrix<R1,C2,T>
|
|
operator* (const matrix<R1,RC,T>&a, const matrix<RC,C2,T>&b)
|
|
{
|
|
matrix<R1,C2,T> res {};
|
|
|
|
// TODO: iterating over r,c rather than c,r will cause an ICE with
|
|
// clang#xxxx: 'X86 DAG->DAG Instruction Selection'.
|
|
//
|
|
// this is likely related to gold and LTO support. for the time being
|
|
// we switch the orders because it appears to confuse the optimiser
|
|
// sufficiently. :(
|
|
for (std::size_t c = 0; c < C2; ++c) {
|
|
for (std::size_t r = 0; r < R1; ++r) {
|
|
T accum {0};
|
|
|
|
for (std::size_t i = 0; i < RC; ++i)
|
|
accum += a[r][i] * b[i][c];
|
|
|
|
res[r][c] = accum;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
//-------------------------------------------------------------------------
|
|
template <
|
|
std::size_t R1, std::size_t C1,
|
|
std::size_t R2, std::size_t C2,
|
|
typename T
|
|
>
|
|
constexpr
|
|
matrix<R1,C2,T>&
|
|
operator*= (matrix<R1,C1,T> &a, const matrix<R2,C2,T> &b)
|
|
{ return a = a * b; };
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
T
|
|
determinant (const matrix<Rows,Cols,T>&);
|
|
|
|
|
|
//-------------------------------------------------------------------------
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
matrix<Rows,Cols,T>
|
|
inverse (const matrix<Rows,Cols,T>&);
|
|
|
|
|
|
//-------------------------------------------------------------------------
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
matrix<Cols,Rows,T>
|
|
transposed (const matrix<Rows,Cols,T>&);
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
matrix<Rows,Cols,T>
|
|
abs (const matrix<Rows,Cols,T> &src)
|
|
{
|
|
matrix<Rows,Cols,T> dst;
|
|
for (size_t r = 0; r < Rows; ++r)
|
|
dst[r] = abs (src[r]);
|
|
return dst;
|
|
}
|
|
|
|
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
constexpr T
|
|
sum (const matrix<Rows,Cols,T> &src)
|
|
{
|
|
cruft::vector<Rows,T> accum {};
|
|
for (size_t r = 0; r < Rows; ++r)
|
|
accum[r] = sum (src[r]);
|
|
return sum (accum);
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
#define MATRIX_ELEMENT_OP(OP) \
|
|
template <std::size_t Rows, std::size_t Cols, typename T> \
|
|
constexpr \
|
|
matrix<Rows,Cols,T> \
|
|
operator OP ( \
|
|
const matrix<Rows,Cols,T> &a, \
|
|
const matrix<Rows,Cols,T> &b) \
|
|
{ \
|
|
matrix<Rows,Cols,T> res {}; \
|
|
\
|
|
for (std::size_t i = 0; i < a.rows; ++i) \
|
|
for (std::size_t j = 0; j < a.cols; ++j) \
|
|
res[i][j] = a[i][j] OP b[i][j]; \
|
|
\
|
|
return res; \
|
|
}
|
|
|
|
MATRIX_ELEMENT_OP(-)
|
|
MATRIX_ELEMENT_OP(+)
|
|
|
|
#undef MATRIX_ELEMENT_OP
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
#define MATRIX_SCALAR_OP(OP) \
|
|
template <std::size_t Rows, std::size_t Cols, typename T> \
|
|
constexpr \
|
|
matrix<Rows,Cols,T> \
|
|
operator OP (const matrix<Rows,Cols,T> &m, const T t) \
|
|
{ \
|
|
matrix<Rows,Cols,T> res {}; \
|
|
\
|
|
std::transform ( \
|
|
std::cbegin (m), \
|
|
std::cend (m), \
|
|
std::begin (res), \
|
|
[&t] (auto x) { return x OP t; } \
|
|
); \
|
|
\
|
|
return res; \
|
|
} \
|
|
\
|
|
\
|
|
template <std::size_t Rows, std::size_t Cols, typename T> \
|
|
constexpr \
|
|
matrix<Rows,Cols,T> \
|
|
operator OP (const T t, const matrix<Rows,Cols,T> &m) \
|
|
{ \
|
|
return m OP t; \
|
|
} \
|
|
\
|
|
\
|
|
template <std::size_t Rows, std::size_t Cols, typename T> \
|
|
constexpr \
|
|
matrix<Rows,Cols,T>& \
|
|
operator OP##= (matrix<Rows,Cols,T> &m, T t) \
|
|
{ \
|
|
std::transform ( \
|
|
std::cbegin (m), \
|
|
std::cend (m), \
|
|
std::begin (m), \
|
|
[&t] (auto x) { return x OP t; } \
|
|
); \
|
|
\
|
|
return m; \
|
|
}
|
|
|
|
MATRIX_SCALAR_OP(*)
|
|
MATRIX_SCALAR_OP(/)
|
|
MATRIX_SCALAR_OP(+)
|
|
MATRIX_SCALAR_OP(-)
|
|
|
|
#undef MATRIX_SCALAR_OP
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
template <typename T> using matrix3 = matrix<3,3,T>;
|
|
template <typename T> using matrix4 = matrix<4,4,T>;
|
|
|
|
template <std::size_t Rows, std::size_t Cols> using matrixf = matrix<Rows,Cols,float>;
|
|
template <std::size_t Rows, std::size_t Cols> using matrixd = matrix<Rows,Cols,double>;
|
|
|
|
typedef matrix<2,2,float> matrix2f;
|
|
typedef matrix<2,2,double> matrix2d;
|
|
|
|
typedef matrix<3,3,float> matrix3f;
|
|
typedef matrix<3,3,double> matrix3d;
|
|
|
|
typedef matrix<4,4,float> matrix4f;
|
|
typedef matrix<4,4,double> matrix4d;
|
|
}
|
|
|
|
|
|
#include <iosfwd>
|
|
|
|
namespace cruft {
|
|
///////////////////////////////////////////////////////////////////////////
|
|
template <std::size_t Rows, std::size_t Cols, typename T>
|
|
std::ostream&
|
|
operator<< (std::ostream &os, matrix<Rows,Cols,T> const&);
|
|
}
|