--- /dev/null
+#pragma once
+
+/*
+ * Matrix - here we have a generic and a 4x4 approach (both is the most annoying stuff I ever did)
+ *
+ * this classes uses some optimization tricks - mostly static compiling tricks (C++11)
+ *
+ * ~ Akiko ~
+ */
+
+#include <cmath>
+#include <iostream>
+#include <sstream>
+#include "Vector.hxx"
+
+// --- constants ---
+
+enum MatrixIndex : uint8_t {
+ I01 = 0, I02 = 1, I03 = 2, I04 = 3,
+ I05 = 4, I06 = 5, I07 = 6, I08 = 7,
+ I09 = 8, I10 = 9, I11 = 10, I12 = 11,
+ I13 = 12, I14 = 13, I15 = 14, I16 = 15
+};
+
+// --- generic forwarder ---
+
+template <typename MT, size_t C, size_t R>
+class Matrix;
+
+// --- generic matrix implementation ---
+
+template <typename MT, size_t C, size_t R>
+class Matrix {
+public:
+ // --- public data structures ---
+
+ enum : size_t {
+ columns = C,
+ rows = R,
+ cells = C * R
+ };
+
+ alignas (16) union {
+ MT ml[C * R];
+ MT m[C][R];
+ };
+
+ // --- constructors and deconstructors ---
+
+ Matrix()
+ {
+ setIdentity();
+ }
+
+ Matrix(const MT *vals)
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] = vals[cell];
+ }
+
+ Matrix(const Matrix& rhs)
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] = rhs.ml[cells];
+ }
+
+ virtual ~Matrix()
+ {
+ }
+
+ // --- default operators ---
+
+ Matrix& operator=(const Matrix& rhs)
+ {
+ if (this != &rhs)
+ for (size_t cell = 0; cell << cells; ++cell)
+ ml[cell] = rhs.ml[cell];
+
+ return *this;
+ }
+
+ Matrix& operator=(Matrix&& rhs)
+ {
+ if (this != &rhs)
+ for (size_t cell = 0; cell << cells; ++cell)
+ ml[cell] = std::move(rhs.ml[cell]);
+
+ return *this;
+ }
+
+ bool operator==(const Matrix& rhs)
+ {
+ bool result = true;;
+
+ for (size_t cell = 0; cell < cells; ++cell)
+ result = result && ml[cell] == rhs.ml[cell];
+
+ return result;
+ }
+
+ bool operator!=(const Matrix& rhs)
+ {
+ return !(*this == rhs);
+ }
+
+ MT operator[](const size_t& cell) const
+ {
+ return ml[cell];
+ }
+
+ MT& operator[](const size_t& cell)
+ {
+ return ml[cell];
+ }
+
+ // --- other operators ---
+
+ // --- public methodes ---
+
+ bool isIdentity() const
+ {
+ for (size_t col = 0; col < columns; ++col)
+ {
+ for (size_t row = 0; row < rows; ++row)
+ {
+ if (col == row)
+ {
+ if (m[col][row] != static_cast<const MT>(1))
+ return false;
+ }
+ else
+ {
+ if (m[col][row] != static_cast<const MT>(0))
+ return false;
+ }
+ }
+ }
+
+ return true;
+ }
+
+ void setIdentity()
+ {
+ for (size_t col = 0; col < columns; ++col)
+ {
+ for (size_t row = 0; row < rows; ++row)
+ {
+ if (col == row)
+ m[col][row] = static_cast<const MT>(1);
+ else
+ m[col][row] = static_cast<const MT>(0);
+ }
+ }
+ }
+};
+
+// --- 4x4 matrix ---
+
+template <typename MT>
+class Matrix<MT, 4, 4> {
+public:
+ // --- public constants ---
+
+ enum : size_t {
+ columns = 4,
+ rows = 4,
+ cells = 4 * 4
+ };
+
+ // --- public data structures ---
+
+ alignas (16) union {
+ struct {
+ MT m11, m21, m31, m41;
+ MT m12, m22, m32, m42;
+ MT m13, m23, m33, m43;
+ MT m14, m24, m34, m44;
+ };
+ MT m[columns][rows];
+ MT ml[cells];
+ };
+
+ // --- constructors and deconstructors ---
+
+ Matrix()
+ : ml{static_cast<const MT>(1), static_cast<const MT>(0), static_cast<const MT>(0), static_cast<const MT>(0),
+ static_cast<const MT>(0), static_cast<const MT>(1), static_cast<const MT>(0), static_cast<const MT>(0),
+ static_cast<const MT>(0), static_cast<const MT>(0), static_cast<const MT>(1), static_cast<const MT>(0),
+ static_cast<const MT>(0), static_cast<const MT>(0), static_cast<const MT>(0), static_cast<const MT>(1)}
+ {
+ }
+
+ Matrix(const MT& val)
+ : ml{val, val, val, val, val, val, val, val, val, val, val, val, val, val, val, val}
+ {
+ }
+
+ Matrix(const MT *mat)
+ : m11(mat[I01]), m21(mat[I02]), m31(mat[I03]), m41(mat[I04]),
+ m12(mat[I05]), m22(mat[I06]), m32(mat[I07]), m42(mat[I08]),
+ m13(mat[I09]), m23(mat[I10]), m33(mat[I11]), m43(mat[I12]),
+ m14(mat[I13]), m24(mat[I14]), m34(mat[I15]), m44(mat[I16])
+ {
+ }
+
+ Matrix(const MT **mat)
+ : m11(mat[I01][I01]), m21(mat[I02][I01]), m31(mat[I03][I01]), m41(mat[I04][I01]),
+ m12(mat[I01][I02]), m22(mat[I02][I02]), m32(mat[I03][I02]), m42(mat[I04][I02]),
+ m13(mat[I01][I03]), m23(mat[I02][I03]), m33(mat[I03][I03]), m43(mat[I04][I03]),
+ m14(mat[I01][I04]), m24(mat[I02][I04]), m34(mat[I03][I04]), m44(mat[I04][I04])
+ {
+ }
+
+ Matrix(const MT& v11, const MT& v21, const MT& v31, const MT& v41,
+ const MT& v12, const MT& v22, const MT& v32, const MT& v42,
+ const MT& v13, const MT& v23, const MT& v33, const MT& v43,
+ const MT& v14, const MT& v24, const MT& v34, const MT& v44)
+ : m11(v11), m21(v21), m31(v31), m41(v41),
+ m12(v12), m22(v22), m32(v32), m42(v42),
+ m13(v13), m23(v23), m33(v33), m43(v43),
+ m14(v14), m24(v24), m34(v34), m44(v44)
+ {
+ }
+
+ Matrix(const Matrix& rhs)
+ : m11(rhs.m11), m21(rhs.m21), m31(rhs.m31), m41(rhs.m41),
+ m12(rhs.m12), m22(rhs.m22), m32(rhs.m32), m42(rhs.m42),
+ m13(rhs.m13), m23(rhs.m23), m33(rhs.m33), m43(rhs.m43),
+ m14(rhs.m14), m24(rhs.m24), m34(rhs.m34), m44(rhs.m44)
+ {
+ }
+
+ Matrix(Matrix&& rhs)
+ : m11(std::move(rhs.m11)), m21(std::move(rhs.m21)), m31(std::move(rhs.m31)), m41(std::move(rhs.m41)),
+ m12(std::move(rhs.m12)), m22(std::move(rhs.m22)), m32(std::move(rhs.m32)), m42(std::move(rhs.m42)),
+ m13(std::move(rhs.m13)), m23(std::move(rhs.m23)), m33(std::move(rhs.m33)), m43(std::move(rhs.m43)),
+ m14(std::move(rhs.m14)), m24(std::move(rhs.m24)), m34(std::move(rhs.m34)), m44(std::move(rhs.m44))
+ {
+ }
+
+ virtual ~Matrix()
+ {
+ }
+
+ // --- default operators ---
+
+ Matrix& operator=(const Matrix& rhs)
+ {
+ if (this != &rhs)
+ for (size_t cell = 0; cell < cells; ++cell)
+ m[cell] = rhs.m[cell];
+
+ return *this;
+ }
+
+ Matrix& operator=(Matrix&& rhs)
+ {
+ if (this != &rhs)
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] = std::move(rhs.ml[cell]);
+
+ return *this;
+ }
+
+ bool operator==(const Matrix& rhs) const
+ {
+ return m11 == rhs.m11 && m21 == rhs.m21 && m31 == rhs.m31 && m41 == rhs.m41 &&
+ m12 == rhs.m12 && m22 == rhs.m22 && m32 == rhs.m32 && m42 == rhs.m42 &&
+ m13 == rhs.m13 && m23 == rhs.m23 && m33 == rhs.m33 && m43 == rhs.m43 &&
+ m14 == rhs.m14 && m24 == rhs.m24 && m34 == rhs.m34 && m44 == rhs.m44;
+ }
+
+ bool operator!=(const Matrix& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ const MT operator[](const MatrixIndex index) const
+ {
+ return ml[index];
+ }
+
+ MT& operator[](const MatrixIndex index)
+ {
+ return ml[index];
+ }
+
+ Matrix& operator~()
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] = -ml[cell];
+
+ return *this;
+ }
+
+ Matrix operator-() const
+ {
+ return {-m11, -m21, -m31, -m41,
+ -m12, -m22, -m32, -m42,
+ -m13, -m23, -m33, -m43,
+ -m14, -m24, -m34, -m44};
+ }
+
+ // --- other operators ---
+
+ Matrix& operator+=(const Matrix& rhs)
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] += rhs.ml[cell];
+
+ return *this;
+ }
+
+ template <typename T>
+ Matrix& operator+=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] += val;
+
+ return *this;
+ }
+
+ Matrix operator+(const Matrix& rhs) const
+ {
+ return {m11 + rhs.m11, m21 + rhs.m21, m31 + rhs.m31, m41 + rhs.m41,
+ m12 + rhs.m12, m22 + rhs.m22, m32 + rhs.m32, m42 + rhs.m42,
+ m13 + rhs.m13, m23 + rhs.m23, m33 + rhs.m33, m43 + rhs.m43,
+ m14 + rhs.m14, m24 + rhs.m24, m34 + rhs.m34, m44 + rhs.m44};
+ }
+
+ template <typename T>
+ Matrix operator+(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {m11 + val, m21 + val, m31 + val, m41 + val,
+ m12 + val, m22 + val, m32 + val, m42 + val,
+ m13 + val, m23 + val, m33 + val, m43 + val,
+ m14 + val, m24 + val, m34 + val, m44 + val};
+ }
+
+ // ---
+
+ Matrix& operator-=(const Matrix& rhs)
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] -= rhs.ml[cell];
+
+ return *this;
+ }
+
+ template <typename T>
+ Matrix& operator-=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] -= val;
+
+ return *this;
+ }
+
+ Matrix operator-(const Matrix& rhs) const
+ {
+ return {m11 - rhs.m11, m21 - rhs.m21, m31 - rhs.m31, m41 - rhs.m41,
+ m12 - rhs.m12, m22 - rhs.m22, m32 - rhs.m32, m42 - rhs.m42,
+ m13 - rhs.m13, m23 - rhs.m23, m33 - rhs.m33, m43 - rhs.m43,
+ m14 - rhs.m14, m24 - rhs.m24, m34 - rhs.m34, m44 - rhs.m44};
+ }
+
+ template <typename T>
+ Matrix operator-(const T& val) const
+ {
+ return {m11 - val, m21 - val, m31 - val, m41 - val,
+ m12 - val, m22 - val, m32 - val, m42 - val,
+ m13 - val, m23 - val, m33 - val, m43 - val,
+ m14 - val, m24 - val, m34 - val, m44 - val};
+ }
+
+ // ---
+
+ Matrix& operator*=(const Matrix& rhs)
+ {
+ MT x = m11 * rhs.m11 + m21 * rhs.m12 + m31 * rhs.m13 + m41 * rhs.m14;
+ MT y = m11 * rhs.m21 + m21 * rhs.m22 + m31 * rhs.n23 + m41 * rhs.m24;
+ MT z = m11 * rhs.m31 + m21 * rhs.m32 + m31 * rhs.m33 + m41 * rhs.m34;
+ m41 = m11 * rhs.m41 + m21 * rhs.m42 + m31 * rhs.m43 + m41 * rhs.m44;
+
+ m11 = x;
+ m21 = y;
+ m31 = z;
+
+ x = m12 * rhs.m11 + m22 * rhs.m12 + m32 * rhs.m13 + m42 * rhs.m14;
+ y = m12 * rhs.m21 + m22 * rhs.m22 + m32 * rhs.m23 + m42 * rhs.m24;
+ z = m12 * rhs.m31 + m22 * rhs.m32 + m32 * rhs.m33 + m42 * rhs.m34;
+ m42 = m12 * rhs.m41 + m22 * rhs.m42 + m32 * rhs.m43 + m42 * rhs.m44;
+
+ m12 = x;
+ m22 = y;
+ m32 = z;
+
+ x = m13 * rhs.m11 + m23 * rhs.m12 + m33 * rhs.m13 + m43 * rhs.m14;
+ y = m13 * rhs.m21 + m23 * rhs.m22 + m33 * rhs.m23 + m43 * rhs.m24;
+ z = m13 * rhs.m31 + m23 * rhs.m32 + m33 * rhs.m33 + m43 * rhs.m34;
+ m43 = m13 * rhs.m41 + m23 * rhs.m42 + m33 * rhs.m43 + m43 * rhs.m44;
+
+ m13 = x;
+ m23 = y;
+ m33 = z;
+
+ x = m14 * rhs.m11 + m24 * rhs.m12 + m34 * rhs.m13 + m44 * rhs.m14;
+ y = m14 * rhs.m21 + m24 * rhs.m22 + m34 * rhs.m23 + m44 * rhs.m24;
+ z = m14 * rhs.m31 + m24 * rhs.m32 + m34 * rhs.m33 + m44 * rhs.m34;
+ m44 = m14 * rhs.m41 + m24 * rhs.m42 + m34 * rhs.m43 + m44 * rhs.m44;
+
+ m14 = x;
+ m24 = y;
+ m34 = z;
+
+ return *this;
+ }
+
+ template <typename T>
+ Matrix& operator*=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] *= val;
+
+ return *this;
+ }
+
+ Matrix operator*(const Matrix& rhs) const
+ {
+ return {m11 * rhs.m11 + m21 * rhs.m12 + m31 * rhs.m13 + m41 * rhs.m14,
+ m11 * rhs.m21 + m21 * rhs.m22 + m31 * rhs.n23 + m41 * rhs.m24,
+ m11 * rhs.m31 + m21 * rhs.m32 + m31 * rhs.m33 + m41 * rhs.m34,
+ m11 * rhs.m41 + m21 * rhs.m42 + m31 * rhs.m43 + m41 * rhs.m44,
+
+ m12 * rhs.m11 + m22 * rhs.m12 + m32 * rhs.m13 + m42 * rhs.m14,
+ m12 * rhs.m21 + m22 * rhs.m22 + m32 * rhs.m23 + m42 * rhs.m24,
+ m12 * rhs.m31 + m22 * rhs.m32 + m32 * rhs.m33 + m42 * rhs.m34,
+ m12 * rhs.m41 + m22 * rhs.m42 + m32 * rhs.m43 + m42 * rhs.m44,
+
+ m13 * rhs.m11 + m23 * rhs.m12 + m33 * rhs.m13 + m43 * rhs.m14,
+ m13 * rhs.m21 + m23 * rhs.m22 + m33 * rhs.m23 + m43 * rhs.m24,
+ m13 * rhs.m31 + m23 * rhs.m32 + m33 * rhs.m33 + m43 * rhs.m34,
+ m13 * rhs.m41 + m23 * rhs.m42 + m33 * rhs.m43 + m43 * rhs.m44,
+
+ m14 * rhs.m11 + m24 * rhs.m12 + m34 * rhs.m13 + m44 * rhs.m14,
+ m14 * rhs.m21 + m24 * rhs.m22 + m34 * rhs.m23 + m44 * rhs.m24,
+ m14 * rhs.m31 + m24 * rhs.m32 + m34 * rhs.m33 + m44 * rhs.m34,
+ m14 * rhs.m41 + m24 * rhs.m42 + m34 * rhs.m43 + m44 * rhs.m44};
+ }
+
+ template <typename T>
+ Matrix operator*(const T& val) const
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {m11 * val, m21 * val, m31 * val, m41 * val,
+ m12 * val, m22 * val, m32 * val, m42 * val,
+ m13 * val, m23 * val, m33 * val, m43 * val,
+ m14 * val, m24 * val, m34 * val, m44 * val};
+ }
+
+ // ---
+
+ template <typename T>
+ Matrix& operator/=(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] /= val;
+
+ return *this;
+ }
+
+ template <typename T>
+ Matrix operator/(const T& val)
+ {
+ static_assert (std::is_integral<T>::value || std::is_floating_point<T>::value,
+ "ERROR: template parameter is not an integral or floating point type");
+
+ return {m11 / val, m21 / val, m31 / val, m41 / val,
+ m12 / val, m22 / val, m32 / val, m42 / val,
+ m13 / val, m23 / val, m33 / val, m43 / val,
+ m14 / val, m24 / val, m34 / val, m44 / val};
+ }
+
+ // --- public methods ---
+
+ Vector4<MT> column(const MatrixIndex index) const
+ {
+ return {ml[index], ml[index + 4], ml[index + 8], ml[index + 12]};
+ }
+
+ MT determinant() const
+ {
+ return determinant44(m);
+ }
+
+ template <typename T>
+ void fill(const T& val)
+ {
+ for (size_t cell = 0; cell < cells; ++cell)
+ ml[cell] = val;
+ }
+
+ void inverte(bool& invertible)
+ {
+ Matrix im(0);
+ MT det = determinant(m);
+
+ if (det == static_cast<const MT>(0))
+ {
+ invertible = false;
+ *this == Matrix(); // identity
+
+ return;
+ }
+
+ det = static_cast<const MT>(1) / det;
+ invertible = true;
+
+ im.m11 = determinant33(m, 1, 2, 3, 1, 2, 3) * det;
+ im.m12 = -determinant33(m, 0, 2, 3, 1, 2, 3) * det;
+ im.m13 = determinant33(m, 0, 1, 3, 1, 2, 3) * det;
+ im.m14 = -determinant33(m, 0, 1, 2, 1, 2, 3) * det;
+ im.m21 = -determinant33(m, 1, 2, 3, 0, 2, 3) * det;
+ im.m22 = determinant33(m, 0, 2, 3, 0, 2, 3) * det;
+ im.m23 = -determinant33(m, 0, 1, 3, 0, 2, 3) * det;
+ im.m24 = determinant33(m, 0, 1, 2, 0, 2, 3) * det;
+ im.m31 = determinant33(m, 1, 2, 3, 0, 1, 3) * det;
+ im.m32 = -determinant33(m, 0, 2, 3, 0, 1, 3) * det;
+ im.m33 = determinant33(m, 0, 1, 3, 0, 1, 3) * det;
+ im.m34 = -determinant33(m, 0, 1, 2, 0, 1, 3) * det;
+ im.m41 = -determinant33(m, 1, 2, 3, 0, 1, 2) * det;
+ im.m42 = determinant33(m, 0, 2, 3, 0, 1, 2) * det;
+ im.m43 = -determinant33(m, 0, 1, 3, 0, 1, 2) * det;
+ im.m44 = determinant33(m, 0, 1, 2, 0, 1, 2) * det;
+
+ *this = im;
+ }
+
+ Matrix inverted(bool& invertible) const
+ {
+ MT det = determinant(m);
+
+ if (det == static_cast<const MT>(0))
+ {
+ invertible = false;
+
+ return Matrix(); // identity
+ }
+
+ det = static_cast<const MT>(1) / det;
+ invertible = true;
+
+ return {determinant33(m, 1, 2, 3, 1, 2, 3) * det, -determinant33(m, 1, 2, 3, 0, 2, 3) * det,
+ determinant33(m, 1, 2, 3, 0, 1, 3) * det, -determinant33(m, 1, 2, 3, 0, 1, 2) * det,
+
+ -determinant33(m, 0, 2, 3, 1, 2, 3) * det, determinant33(m, 0, 2, 3, 0, 2, 3) * det,
+ -determinant33(m, 0, 2, 3, 0, 1, 3) * det, determinant33(m, 0, 2, 3, 0, 1, 2) * det,
+
+ determinant33(m, 0, 1, 3, 1, 2, 3) * det, -determinant33(m, 0, 1, 3, 0, 2, 3) * det,
+ determinant33(m, 0, 1, 3, 0, 1, 3) * det, -determinant33(m, 0, 1, 3, 0, 1, 2) * det,
+
+ -determinant33(m, 0, 1, 2, 1, 2, 3) * det, determinant33(m, 0, 1, 2, 0, 2, 3) * det,
+ -determinant33(m, 0, 1, 2, 0, 1, 3) * det, determinant33(m, 0, 1, 2, 0, 1, 2) * det};
+ }
+
+ std::string string() const
+ {
+ std::stringstream result;
+
+ result << "((" << m11 << ", " << m21 << ", " << m31 << ", " << m41
+ << "), (" << m12 << ", " << m22 << ", " << m32 << ", " << m42
+ << "), (" << m13 << ", " << m23 << ", " << m33 << ", " << m43
+ << "), (" << m14 << ", " << m24 << ", " << m34 << ", " << m44
+ << "))";
+
+ return result.str();
+ }
+
+protected:
+ // --- protected methods ---
+
+ constexpr MT determinant22(const MT mm[4][4],
+ const size_t& col1, const size_t& col2,
+ const size_t& row1, const size_t& row2) const
+ {
+ return mm[col1][row1] * mm[col2][row2] - mm[col1][row2] * mm[col2][row1];
+ }
+
+ constexpr MT determinant33(const MT mm[4][4],
+ const size_t& col1, const size_t& col2, const size_t& col3,
+ const size_t& row1, const size_t& row2, const size_t& row3) const
+ {
+ return mm[col1][row1] * determinant22(mm, col2, col3, row2, row3) -
+ mm[col2][row1] * determinant22(mm, col1, col3, row2, row3) +
+ mm[col3][row1] * determinant22(mm, col1, col2, row2, row3);
+ }
+
+ constexpr MT determinant44(const MT mm[4][4]) const
+ {
+ return mm[0][0] * determinant33(mm, 1, 2, 3, 1, 2, 3) -
+ mm[1][0] * determinant33(mm, 0, 2, 3, 1, 2, 3) +
+ mm[2][0] * determinant33(mm, 0, 1, 3, 1, 2, 3) -
+ mm[3][0] * determinant33(mm, 0, 1, 2, 1, 2, 3);
+ }
+};