libcruft-util/matrix.hpp

552 lines
19 KiB
C++
Raw Normal View History

/*
2018-08-04 15:14:06 +10:00
* 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/.
*
2015-10-30 23:40:13 +11:00
* Copyright 2011-2015 Danny Robson <danny@nerdcruft.net>
*/
2011-05-23 17:18:52 +10:00
2018-03-13 22:33:54 +11:00
#ifndef CRUFT_UTIL_MATRIX_HPP
#define CRUFT_UTIL_MATRIX_HPP
2011-05-23 17:18:52 +10:00
#include "point.hpp"
#include "range.hpp"
#include "vector.hpp"
2017-11-24 17:19:04 +11:00
#include "iterator.hpp"
2011-05-23 17:18:52 +10:00
2017-02-21 21:19:28 +11:00
#include <cstdlib>
2017-11-24 17:19:04 +11:00
#include <ostream>
2011-05-23 17:18:52 +10:00
namespace cruft {
2018-03-13 22:33:54 +11:00
///////////////////////////////////////////////////////////////////////////
/// 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 {
2017-02-21 21:19:28 +11:00
static constexpr auto rows = Rows;
static constexpr auto cols = Cols;
2011-05-23 17:18:52 +10:00
using row_t = cruft::vector<Cols,ValueT>;
2017-11-24 13:08:56 +11:00
///////////////////////////////////////////////////////////////////////
constexpr matrix () noexcept = default;
2018-04-11 18:23:57 +10:00
constexpr matrix(ValueT fill)
{
for (auto &r: values)
r = fill;
}
2017-11-24 13:08:56 +11:00
//---------------------------------------------------------------------
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];
}
2017-11-24 13:08:56 +11:00
//---------------------------------------------------------------------
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];
}
2014-08-19 20:45:28 +10:00
2017-11-23 17:24:11 +11:00
///////////////////////////////////////////////////////////////////////
// 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];
}
///////////////////////////////////////////////////////////////////////
2017-11-24 13:08:56 +11:00
constexpr row_t*
data (void)& noexcept
{
2017-11-24 13:08:56 +11:00
return &values[0];
}
//---------------------------------------------------------------------
2017-11-24 13:08:56 +11:00
constexpr const row_t*
data (void) const& noexcept
{
2017-11-24 13:08:56 +11:00
return &values[0];
}
2017-01-05 19:49:43 +11:00
//---------------------------------------------------------------------
2017-11-23 17:24:11 +11:00
constexpr auto
begin (void) const& noexcept
{
2017-11-24 13:08:56 +11:00
return data ();
}
//---------------------------------------------------------------------
2017-11-23 17:24:11 +11:00
constexpr auto
2017-11-24 13:08:56 +11:00
begin (void)& noexcept
{
2017-11-24 13:08:56 +11:00
return data ();
}
//---------------------------------------------------------------------
2017-11-24 13:08:56 +11:00
constexpr row_t*
end (void)& noexcept
{
2017-11-24 13:08:56 +11:00
return &values[Rows];
}
//---------------------------------------------------------------------
2017-11-24 13:08:56 +11:00
constexpr const row_t*
end (void) const& noexcept
{
2017-11-24 13:08:56 +11:00
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 ();
}
2014-08-19 20:45:28 +10:00
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;
}
}
2014-08-19 20:46:00 +10:00
bool is_affine (void) const;
2011-05-23 17:18:52 +10:00
2015-02-19 13:24:37 +11:00
template <typename U>
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,U>
2017-11-23 17:24:11 +11:00
cast (void) const noexcept
{
matrix out;
std::copy (cbegin (), cend (), std::begin (out));
return out;
}
2015-02-19 13:24:37 +11:00
2014-12-15 13:44:33 +11:00
// 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 {};
2017-11-24 13:08:56 +11:00
std::fill (std::begin (ret), std::end (ret), row_t{0});
return ret;
}
2017-11-24 13:08:56 +11:00
private:
row_t values[Rows];
2011-05-23 17:18:52 +10:00
};
// 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);
//template <typename T> matrix<3,3,T> translation (vector<2,T>);
//template <typename T> matrix<3,3,T> scale (vector<2,T>);
2018-04-11 18:24:10 +10:00
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,
2018-04-11 18:24:10 +10:00
const float percentage)
{
for (size_t r = 0; r < Rows; ++r)
if (!all (relatively_equal (a[r], b[r], percentage)))
return false;
return true;
}
2016-09-14 17:53:34 +10:00
///////////////////////////////////////////////////////////////////////////
// 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>
2016-09-14 17:53:34 +10:00
vector<3,T>
2017-02-21 21:19:28 +11:00
to_euler (const matrix<Rows, Cols, T>&);
2016-09-14 17:53:34 +10:00
2016-08-15 20:32:24 +10:00
///////////////////////////////////////////////////////////////////////////
// logical operations
template <std::size_t Rows, std::size_t Cols, typename T>
2016-08-15 20:32:24 +10:00
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));
}
2016-08-15 20:32:24 +10:00
//-------------------------------------------------------------------------
template <std::size_t Rows, std::size_t Cols, typename T>
2016-08-15 20:32:24 +10:00
constexpr
bool
operator!= (const matrix<Rows,Cols,T> &a, const matrix<Rows,Cols,T> &b)
{
return !(a == b);
}
2016-08-15 20:32:24 +10:00
///////////////////////////////////////////////////////////////////////////
// element operations
template <std::size_t Rows, std::size_t Cols, typename T>
2016-08-15 18:53:52 +10:00
constexpr
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,T>
operator+ (const matrix<Rows,Cols,T>&, const matrix<Rows,Cols,T>&);
2016-08-15 18:53:52 +10:00
template <std::size_t Rows, std::size_t Cols, typename T>
2016-08-15 18:53:52 +10:00
constexpr
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,T>
operator- (const matrix<Rows,Cols,T>&, const matrix<Rows,Cols,T>&);
2016-08-15 18:53:52 +10:00
2016-08-15 20:32:24 +10:00
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;
}
2016-08-15 20:32:24 +10:00
///////////////////////////////////////////////////////////////////////////
// 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);
2017-02-21 21:19:28 +11:00
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>&);
2016-08-15 20:32:24 +10:00
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);
2016-08-15 20:32:24 +10:00
2017-02-21 21:19:28 +11:00
///////////////////////////////////////////////////////////////////////////
// matrix operations
template <
std::size_t R1, //int C1,
std::size_t RC, //int R2,
std::size_t C2,
2017-02-21 21:19:28 +11:00
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; };
2016-08-15 20:32:24 +10:00
///////////////////////////////////////////////////////////////////////////
template <std::size_t Rows, std::size_t Cols, typename T>
2016-08-15 18:53:52 +10:00
T
2017-02-21 21:19:28 +11:00
determinant (const matrix<Rows,Cols,T>&);
//-------------------------------------------------------------------------
template <std::size_t Rows, std::size_t Cols, typename T>
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,T>
inverse (const matrix<Rows,Cols,T>&);
//-------------------------------------------------------------------------
template <std::size_t Rows, std::size_t Cols, typename T>
2017-02-21 21:19:28 +11:00
matrix<Cols,Rows,T>
transposed (const matrix<Rows,Cols,T>&);
2016-09-14 17:46:03 +10:00
///////////////////////////////////////////////////////////////////////////
template <std::size_t Rows, std::size_t Cols, typename T>
2017-02-21 21:19:28 +11:00
matrix<Rows,Cols,T>
abs (const matrix<Rows,Cols,T> &src)
{
matrix<Rows,Cols,T> dst;
2017-11-24 13:08:56 +11:00
for (size_t r = 0; r < Rows; ++r)
dst[r] = abs (src[r]);
return dst;
}
2016-08-15 18:44:27 +10:00
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 {};
2017-11-24 13:08:56 +11:00
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
2016-09-14 17:53:34 +10:00
///////////////////////////////////////////////////////////////////////////
2017-02-21 21:19:28 +11:00
template <typename T> using matrix3 = matrix<3,3,T>;
template <typename T> using matrix4 = matrix<4,4,T>;
2015-07-13 16:27:54 +10:00
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>;
2017-02-21 21:19:28 +11:00
typedef matrix<2,2,float> matrix2f;
typedef matrix<2,2,double> matrix2d;
2017-02-21 21:19:28 +11:00
typedef matrix<3,3,float> matrix3f;
typedef matrix<3,3,double> matrix3d;
2017-02-21 21:19:28 +11:00
typedef matrix<4,4,float> matrix4f;
typedef matrix<4,4,double> matrix4d;
2015-10-30 23:40:13 +11:00
2016-09-14 17:53:34 +10:00
///////////////////////////////////////////////////////////////////////////
template <std::size_t Rows, std::size_t Cols, typename T>
2017-11-24 17:19:04 +11:00
std::ostream&
operator<< (std::ostream &os, const matrix<Rows,Cols,T> &m)
{
os << '[';
std::copy (
std::cbegin (m),
std::cend (m),
cruft::infix_iterator<decltype(m[0])> (os, ", ")
2017-11-24 17:19:04 +11:00
);
return os << ']';
}
};
2011-05-23 17:18:52 +10:00
#endif