Matrix.hpp

/*--------------------------------------------------------------------------------------------
Copyright (c) 2019, NDEVR LLC
tyler.parke@ndevr.org
  __    __   ____     _____ __     __  _______
 |  \  |  | |  __ \  |  ___|\ \   / / |   __  \
 |   \ |  | | |  \ \ | |___  \ \ / /  |  |__)  |
 |  . \|  | | |__/ / | |___   \ V /   |   _   /
 |  |\    |_|_____/__|_____|___\_/____|  | \  \
 |__| \__________________________________|  \__\
 
Subject to the terms of the Enterprise+ Agreement, NDEVR hereby grants 
Licensee a limited, non-exclusive, non-transferable, royalty-free license 
(without the right to sublicense) to use the API solely for the purpose of 
Licensee's internal development efforts to develop applications for which 
the API was provided.
 
The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
 
Library: Base
File: Matrix
Included in API: True
Author(s): Tyler Parke
 *-----------------------------------------------------------------------------------------**/
#pragma once
#include <NDEVR/BaseValues.h>
#include <NDEVR/Vector.h>
#include <NDEVR/Angle.h>
#include <NDEVR/AngleDefinitions.h>
namespace NDEVR
{
    template<uint01 t_cols, uint01 t_rows>
    class MatrixInverter;
 
    template<>
    class MatrixInverter<1, 1>
    {
    public:
        template<class t_type>
        static Vector<1, Vector<1, t_type>> invert(Vector<1, Vector<1, t_type>>& value)
        {
            return value;
        }
    };
 
    template<>
    class MatrixInverter<2, 2>
    {
    public:
        template<class t_type>
        static constexpr t_type determinant(const Vector<2, Vector<2, t_type>>& value)
        {
            return value[0][0] * value[1][1] - value[1][0] * value[0][1];
        }
        template<class t_type>
        static Vector<2, Vector<2, t_type>> invert(const Vector<2, Vector<2, t_type>>& value)
        {
            t_type OneOverDeterminant = static_cast<t_type>(1) / value.determinant();
            return Vector<3, Vector<3, t_type>>(
                +value[1][1] * OneOverDeterminant,
                -value[0][1] * OneOverDeterminant,
                -value[1][0] * OneOverDeterminant,
                +value[0][0] * OneOverDeterminant).template getAs<t_type, 2, 2>();
        }
    };
 
    template<>
    class MatrixInverter<3, 3>
    {
    public:
        template<class t_type>
        static constexpr t_type determinant(const Vector<3, Vector<3, t_type>>& value)
        {
                return + value[0][0] * (value[1][1] * value[2][2] - value[2][1] * value[1][2])
                       - value[1][0] * (value[0][1] * value[2][2] - value[2][1] * value[0][2])
                       + value[2][0] * (value[0][1] * value[1][2] - value[1][1] * value[0][2]);
        }
        template<class t_type>
        static Vector<3, Vector<3, t_type>> invert(const Vector<3, Vector<3, t_type>>& value)
        {
            t_type OneOverDeterminant = static_cast<t_type>(1) / determinant(value);
            Vector<3, Vector<3, t_type>> Inverse;
            Inverse[0][0] = +(value[1][1] * value[2][2] - value[2][1] * value[1][2]) * OneOverDeterminant;
            Inverse[1][0] = -(value[1][0] * value[2][2] - value[2][0] * value[1][2]) * OneOverDeterminant;
            Inverse[2][0] = +(value[1][0] * value[2][1] - value[2][0] * value[1][1]) * OneOverDeterminant;
            Inverse[0][1] = -(value[0][1] * value[2][2] - value[2][1] * value[0][2]) * OneOverDeterminant;
            Inverse[1][1] = +(value[0][0] * value[2][2] - value[2][0] * value[0][2]) * OneOverDeterminant;
            Inverse[2][1] = -(value[0][0] * value[2][1] - value[2][0] * value[0][1]) * OneOverDeterminant;
            Inverse[0][2] = +(value[0][1] * value[1][2] - value[1][1] * value[0][2]) * OneOverDeterminant;
            Inverse[1][2] = -(value[0][0] * value[1][2] - value[1][0] * value[0][2]) * OneOverDeterminant;
            Inverse[2][2] = +(value[0][0] * value[1][1] - value[1][0] * value[0][1]) * OneOverDeterminant;
 
            return Inverse;
        }
    };
    template<>
    class MatrixInverter<4, 4>
    {
    public:
        template<class t_type>
        static Vector<4, Vector<4, t_type>> invert(const Vector<4, Vector<4, t_type>>& value)
        {
            t_type Coef00 = value[2][2] * value[3][3] - value[3][2] * value[2][3];
            t_type Coef02 = value[1][2] * value[3][3] - value[3][2] * value[1][3];
            t_type Coef03 = value[1][2] * value[2][3] - value[2][2] * value[1][3];
 
            t_type Coef04 = value[2][1] * value[3][3] - value[3][1] * value[2][3];
            t_type Coef06 = value[1][1] * value[3][3] - value[3][1] * value[1][3];
            t_type Coef07 = value[1][1] * value[2][3] - value[2][1] * value[1][3];
 
            t_type Coef08 = value[2][1] * value[3][2] - value[3][1] * value[2][2];
            t_type Coef10 = value[1][1] * value[3][2] - value[3][1] * value[1][2];
            t_type Coef11 = value[1][1] * value[2][2] - value[2][1] * value[1][2];
 
            t_type Coef12 = value[2][0] * value[3][3] - value[3][0] * value[2][3];
            t_type Coef14 = value[1][0] * value[3][3] - value[3][0] * value[1][3];
            t_type Coef15 = value[1][0] * value[2][3] - value[2][0] * value[1][3];
 
            t_type Coef16 = value[2][0] * value[3][2] - value[3][0] * value[2][2];
            t_type Coef18 = value[1][0] * value[3][2] - value[3][0] * value[1][2];
            t_type Coef19 = value[1][0] * value[2][2] - value[2][0] * value[1][2];
 
            t_type Coef20 = value[2][0] * value[3][1] - value[3][0] * value[2][1];
            t_type Coef22 = value[1][0] * value[3][1] - value[3][0] * value[1][1];
            t_type Coef23 = value[1][0] * value[2][1] - value[2][0] * value[1][1];
 
            Vector<4, t_type> Fac0(Coef00, Coef00, Coef02, Coef03);
            Vector<4, t_type> Fac1(Coef04, Coef04, Coef06, Coef07);
            Vector<4, t_type> Fac2(Coef08, Coef08, Coef10, Coef11);
            Vector<4, t_type> Fac3(Coef12, Coef12, Coef14, Coef15);
            Vector<4, t_type> Fac4(Coef16, Coef16, Coef18, Coef19);
            Vector<4, t_type> Fac5(Coef20, Coef20, Coef22, Coef23);
 
            Vector<4, t_type> Vec0(value[1][0], value[0][0], value[0][0], value[0][0]);
            Vector<4, t_type> Vec1(value[1][1], value[0][1], value[0][1], value[0][1]);
            Vector<4, t_type> Vec2(value[1][2], value[0][2], value[0][2], value[0][2]);
            Vector<4, t_type> Vec3(value[1][3], value[0][3], value[0][3], value[0][3]);
 
            Vector<4, t_type> Inv0(Vec1 * Fac0 - Vec2 * Fac1 + Vec3 * Fac2);
            Vector<4, t_type> Inv1(Vec0 * Fac0 - Vec2 * Fac3 + Vec3 * Fac4);
            Vector<4, t_type> Inv2(Vec0 * Fac1 - Vec1 * Fac3 + Vec3 * Fac5);
            Vector<4, t_type> Inv3(Vec0 * Fac2 - Vec1 * Fac4 + Vec2 * Fac5);
 
            Vector<4, t_type> SignA(+1, -1, +1, -1);
            Vector<4, t_type> SignB(-1, +1, -1, +1);
 
            Vector<4, t_type> Row0(Inv0[0] * SignA[0], Inv1[0] * SignB[0], Inv2[0] * SignA[0], Inv3[0] * SignB[0]);
 
            Vector<4, t_type> Dot0(Vector<4, t_type>(value[0][0], value[0][1], value[0][2], value[0][3]) * Row0);
            t_type Dot1 = Dot0.sum();
 
            lib_assert(std::abs(Dot1) > 0.0, "bad matrix inverse");
            t_type OneOverDeterminant = static_cast<t_type>(1) / Dot1;
            Inv0 = Inv0 * SignA * OneOverDeterminant;
            Inv1 = Inv1 * SignB * OneOverDeterminant;
            Inv2 = Inv2 * SignA * OneOverDeterminant;
            Inv3 = Inv3 * SignB * OneOverDeterminant;
            return Vector<4, Vector<4, t_type>>(
                  { Inv0[0], Inv0[1], Inv0[2], Inv0[3] }
                , { Inv1[0], Inv1[1], Inv1[2], Inv1[3] }
                , { Inv2[0], Inv2[1], Inv2[2], Inv2[3]}
                , { Inv3[0], Inv3[1], Inv3[2], Inv3[3]});
        }
    };
    template<class t_type, uint01 t_row_dims = 4, uint01 t_col_dims = 4>
    class Matrix : public Vector<t_row_dims, Vector<t_col_dims, t_type>>
    {
    public:
        constexpr Matrix()
            : Vector<t_row_dims, Vector<t_col_dims, t_type>>()
        {}
        constexpr explicit Matrix(const t_type diagonal)
            : Vector<t_row_dims, Vector<t_col_dims, t_type>>(Vector<t_col_dims, t_type>(0))
        {
            for (uint01 i = 0; i < getMin(t_row_dims, t_col_dims); i++)
            {
                (*this)[i][i] = diagonal;
            }
        }
 
        constexpr Matrix(
            const t_type n00, const t_type n10,
            const t_type n01, const t_type n11)
            : Vector<2, Vector<2, t_type>>(
                Vector<2, t_type>(n00, n10)
                , Vector<2, t_type>(n01, n11))
        {
            static_assert(t_row_dims == 2 && t_col_dims == 2, "Bad Transform Constructor, should be 2x2");
        }
        constexpr Matrix(
            const t_type n00, const t_type n10, const t_type n20,
            const t_type n01, const t_type n11, const t_type n21,
            const t_type n02, const t_type n12, const t_type n22)
            : Vector<3, Vector<3, t_type>>(
                Vector<3, t_type>(n00, n10, n20)
                , Vector<3, t_type>(n01, n11, n21)
                , Vector<3, t_type>(n02, n12, n22))
        {
            static_assert(t_row_dims == 3 && t_col_dims == 3, "Bad Transform Constructor, should be 3x3");
        }
        constexpr Matrix(
            const t_type n00, const t_type n10, const t_type n20, const t_type n30,
            const t_type n01, const t_type n11, const t_type n21, const t_type n31,
            const t_type n02, const t_type n12, const t_type n22, const t_type n32,
            const t_type n03, const t_type n13, const t_type n23, const t_type n33)
            : Vector<4, Vector<4, t_type>>(
                Vector<4, t_type>(n00, n10, n20, n30)
                , Vector<4, t_type>(n01, n11, n21, n31)
                , Vector<4, t_type>(n02, n12, n22, n32)
                , Vector<4, t_type>(n03, n13, n23, n33))
        {
            static_assert(t_row_dims == 4 && t_col_dims == 4, "Bad Transform Constructor, should be 4x4");
        }
        template<class t_angle_type>
        constexpr Matrix(const Vector<t_row_dims - 1, t_type>& offset, const Vector<3, Angle<t_angle_type>>& orientation, const Vector<t_row_dims - 1, t_type>& scale)
            : Vector<t_row_dims, Vector<t_col_dims, t_type>>(Vector<t_col_dims, t_type>(0))
        {
            //rotate the matrix
            *this = RotationMatrix(orientation);
            //Offset the matrix (Note will not overlap with rotation, so order independent)
            (*this)[t_row_dims - 1][0] = offset[X];
            if constexpr (t_col_dims >= 2)
                (*this)[t_row_dims - 1][1] = offset[Y];
            if constexpr (t_col_dims >= 3)
                (*this)[t_row_dims - 1][2] = offset[Z];
            //scale the matrix
            *this = this->scale(scale);
        }
        constexpr Matrix(const Vector<t_row_dims* t_col_dims, t_type>& vector)
        {
            memcpy(this, &vector, sizeof(Vector<t_row_dims* t_col_dims, t_type>));
        }
        constexpr Matrix(const Vector<t_row_dims, Vector<t_col_dims, t_type>>& vector)
            : Vector<t_row_dims, Vector<t_col_dims, t_type>>(vector)
        {}
        constexpr Matrix(const Matrix& mat) = default;
 
        template<class t_new_type>
        [[nodiscard]] constexpr decltype(auto) as() const
        {
            if constexpr (std::is_same_v<t_new_type, t_type>)
            {
                return static_cast<const Matrix<t_type>&>(*this);
            }
            else
            {
                Matrix<t_new_type> mat;
                for (uint01 row = 0; row < t_row_dims; ++row)
                {
                    for (uint01 col = 0; col < t_col_dims; ++col)
                    {
                        mat[row][col] = cast<t_new_type>((*this)[row][col]);
                    }
                }
                return mat;
            }
        }
        template<class t_new_type, uint01 t_new_row_dims, uint01 t_new_col_dims>
        [[nodiscard]] constexpr decltype(auto) as() const
        {
            if constexpr (t_new_row_dims == t_row_dims && t_new_col_dims == t_col_dims && std::is_same_v<t_new_type, t_type>)
            {
                return static_cast<const Matrix<t_type>&>(*this);
            }
            else
            {
                Matrix<t_new_type, t_new_row_dims, t_new_col_dims> mat;
                for (uint01 row = 0; row < getMin(t_row_dims, t_new_row_dims); ++row)
                {
                    for (uint01 col = 0; col < getMin(t_col_dims, t_new_col_dims); ++col)
                    {
                        mat[row][col] = cast<t_new_type>((*this)[row][col]);
                    }
                }
                return mat;
            }
        }
        template<uint01 t_i0, uint01 t_i1, uint01 t_j0, uint01 t_j1>
        [[nodiscard]] constexpr Matrix<t_type, t_i1 - t_i0 + 1, t_j1 - t_j0 + 1> subMatrix() const
        {
            Matrix<t_type, t_i1 - t_i0 + 1, t_j1 - t_j0 + 1> mat;
            for (uint01 i = t_i0; i <= t_i1; i++)
            {
                for (uint01 j = t_j0; j <= t_j1; j++)
                    mat[i - t_i0][j - t_j0] = (*this)[i][j];
            }
            return mat;
        }
        template<uint01 t_j0, uint01 t_j1, uint01 t_i_size>
        [[nodiscard]] constexpr Matrix<t_type, t_i_size, t_j1 - t_j0 + 1> subMatrix(const Vector<t_i_size, uint01>& r) const
        {
            Matrix<t_type, t_i_size, t_j1 - t_j0 + 1> mat;
            for (uint01 i = 0; i < t_i_size; i++)
            {
                for (uint01 j = t_j0; j <= t_j1; j++)
                {
                    mat[i][j - t_j0] = (*this)[r[i]][j];
                }
            }
            return mat;
        }
        
        [[nodiscard]] static constexpr Matrix<t_type> OffsetMatrix(const Vector<3, t_type>& translation)
        {
            Matrix<t_type> mat(1);
            mat[t_row_dims - 1][0] = translation[X];
            if constexpr (t_col_dims >= 2)
                mat[t_row_dims - 1][1] = translation[Y];
            if constexpr (t_col_dims >= 3)
                mat[t_row_dims - 1][2] = translation[Z];
            return mat;
        }
        [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(t_type scale)
        {
            Matrix<t_type> mat(1);
            mat[0][0] = scale;
            if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
                mat[1][1] = scale;
            if constexpr (t_col_dims >= 3 && t_row_dims >= 3)
                mat[2][2] = scale;
            return mat;
        }
        [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(const Vector<2, t_type>& scale)
        {
            Matrix mat(1);
            mat[0][0] = scale[X];
            if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
                mat[1][1] = scale[Y];
            return mat;
        }
        [[nodiscard]] static constexpr Matrix<t_type> ScalerMatrix(const Vector<3, t_type>& scale)
        {
            Matrix<t_type> mat(1);
            mat[0][0] = scale[X];
            if constexpr (t_col_dims >= 2 && t_row_dims >= 2)
                mat[1][1] = scale[Y];
            if constexpr (t_col_dims >= 3 && t_row_dims >= 3)
                mat[2][2] = scale[Z];
            return mat;
        }
 
        static constexpr Matrix<t_type> SwapAxes(uint01 a, uint01 b)
        {
            Matrix<t_type> m = Matrix<t_type>(cast<t_type>(1));
            static_assert(t_row_dims == t_col_dims, "needs to be same size");
            lib_assert(a < t_row_dims && b < t_col_dims, "dims too high for matrix");
            if (a == b)
                return m;
            m[a][a] = cast<t_type>(0);
            m[b][b] = cast<t_type>(0);
            m[a][b] = cast<t_type>(1);
            m[b][a] = cast<t_type>(1);
            return m;
        }
        template<class t_angle_type>
        static constexpr Matrix<t_type> RotationMatrix(const Vector<3, Angle<t_angle_type>>& orientation)
        {
            t_type ch = t_type(1);
            t_type sh = t_type(0);
            t_type ca = t_type(1);
            t_type sa = t_type(0);
            t_type cb = t_type(1);
            t_type sb = t_type(0);
            if (IsValid(orientation[YAW]))
            {
                ch = cos(orientation[YAW]);
                sh = sin(orientation[YAW]);
            }
            if (IsValid(orientation[PITCH]))
            {
                ca = cos(orientation[PITCH]);
                sa = sin(orientation[PITCH]);
            }
            if (IsValid(orientation[ROLL]))
            {
                cb = cos(orientation[ROLL]);
                sb = sin(orientation[ROLL]);
            }
 
            t_type m00 = ch * ca;
            t_type m01 = ch * sa * sb - sh * cb;
            t_type m02 = ch * sa * cb + sh * sb;
            t_type m10 = sh * ca;
            t_type m11 = sh * sa * sb + ch * cb;
            t_type m12 = sh * sa * cb - ch * sb;
            t_type m20 = -sa;
            t_type m21 = ca * sb;
            t_type m22 = ca * cb;
            return Matrix<t_type>(
                m00, m10, m20, 0
                , m01, m11, m21, 0
                , m02, m12, m22, 0
                , 0, 0, 0, 1);
        }
        template<class t_angle_type>
        static constexpr Matrix<t_type> RotationMatrix(const Angle<t_angle_type>& phi, const Vector<3, t_type>& axis)
        {
            const t_type cos_val = cast<t_type>(cos(phi));
            const t_type sin_val = cast<t_type>(sin(phi));
            const t_type x = axis[X];
            const t_type y = axis[Y];
            const t_type z = axis[Z];
            return Matrix<t_type>(
                (cos_val + x * x * (1 - cos_val)), (z * sin_val + y * x * (1 - cos_val)), (-y * sin_val + z * x * (1 - cos_val)), 0
                , (-z * sin_val + x * y * (1 - cos_val)), (cos_val + y * y * (1 - cos_val)), (x * sin_val + z * y * (1 - cos_val)), 0
                , (y * sin_val + x * z * (1 - cos_val)), (-x * sin_val + y * z * (1 - cos_val)), (cos_val + z * z * (1 - cos_val)), 0
                , 0, 0, 0, 1);
        }
        [[nodiscard]] constexpr t_type determinant() const
        {
            static_assert(t_col_dims == t_row_dims, "Rows and columns must be equal for determinate");
            switch (t_col_dims)
            {
            case 1:
                return (*this)[0][0];
            case 2:
                return (*this)[0][0] * (*this)[1][1] - (*this)[1][0] * (*this)[0][1];
            case 3:
                return
                    +(*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[2][1] * (*this)[1][2])
                    - (*this)[1][0] * ((*this)[0][1] * (*this)[2][2] - (*this)[2][1] * (*this)[0][2])
                    + (*this)[2][0] * ((*this)[0][1] * (*this)[1][2] - (*this)[1][1] * (*this)[0][2]);
            case 4:
            {
                t_type SubFactor00 = (*this)[2][2] * (*this)[3][3] - (*this)[3][2] * (*this)[2][3];
                t_type SubFactor01 = (*this)[2][1] * (*this)[3][3] - (*this)[3][1] * (*this)[2][3];
                t_type SubFactor02 = (*this)[2][1] * (*this)[3][2] - (*this)[3][1] * (*this)[2][2];
                t_type SubFactor03 = (*this)[2][0] * (*this)[3][3] - (*this)[3][0] * (*this)[2][3];
                t_type SubFactor04 = (*this)[2][0] * (*this)[3][2] - (*this)[3][0] * (*this)[2][2];
                t_type SubFactor05 = (*this)[2][0] * (*this)[3][1] - (*this)[3][0] * (*this)[2][1];
                Vector<4, t_type> DetCof(
                    +((*this)[1][1] * SubFactor00 - (*this)[1][2] * SubFactor01 + (*this)[1][3] * SubFactor02),
                    -((*this)[1][0] * SubFactor00 - (*this)[1][2] * SubFactor03 + (*this)[1][3] * SubFactor04),
                    +((*this)[1][0] * SubFactor01 - (*this)[1][1] * SubFactor03 + (*this)[1][3] * SubFactor05),
                    -((*this)[1][0] * SubFactor02 - (*this)[1][1] * SubFactor04 + (*this)[1][2] * SubFactor05));
                return
                    (*this)[0][0] * DetCof[0] + (*this)[0][1] * DetCof[1] +
                    (*this)[0][2] * DetCof[2] + (*this)[0][3] * DetCof[3];
            }
            default:
                return Constant<t_type>::Invalid;
            }
        }
        [[nodiscard]] constexpr Vector<3, t_type> decomposeScale() const
        {
            static_assert(t_col_dims >= 4 && t_row_dims >= 4, "Cannot decompose lower dimension matrix");
            Vector<3, t_type> scaling;
            Vector<3, t_type> rows[3];
            for (uint01 i = 0; i < 3; i++)
            {
                rows[i] = (*this)[i].template as<3, t_type>();
                scaling[i] = rows[i].template magnitude<t_type>();
            }
            scaling = quantize(scaling, Vector<3, t_type>(.000001));
            const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
            if (dot(temp_z, rows[2]) < 0)
            {
                scaling[X] = -scaling[X];
                rows[0] = -rows[0];
            }
            return scaling;
        }
        [[nodiscard]] constexpr Vector<3, t_type> decomposeOffset() const
        {
            static_assert(t_col_dims >= 3 && t_row_dims >= 3, "Cannot decompose lower dimension matrix");
            Vector<3, t_type> offset;
            offset[X] = (*this)[3][0];
            offset[Y] = (*this)[3][1];
            offset[Z] = (*this)[3][2];
            return offset;
        }
        /* [[nodiscard]] constexpr Vector<3, Angle> decomposeRotation() const
        {
            return decomposeRotation<sint04>();
        }*/
        template<class t_angle_type>
        [[nodiscard]] constexpr Vector<3, Angle<t_angle_type>> decomposeRotation() const
        {
            static_assert(t_col_dims >= 3 && t_row_dims >= 3, "Cannot decompose lower dimension matrix");
            Vector<3, Angle<t_angle_type>> rotation;
            Vector<3, t_type> rows[3];
            for (uint01 i = 0; i < 3; i++)
            {
                rows[i] = (*this)[i].template as<3, t_type>();
            }
            const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
            if (dot(temp_z, rows[2]) < 0)
            {
                rows[0] = -rows[0];
            }
            for (uint04 i = 0; i < 3; i++)
            {
                rows[i] = rows[i].template normalized<t_type>();
            }
            t_type sy = sqrt(rows[0][0] * rows[0][0] + rows[0][1] * rows[0][1]);
 
            rotation[PITCH] = Angle<t_angle_type>::atan2(-rows[0][2], sy);
            if (sy > cast<t_type>(1E-6))
            {
                rotation[ROLL] = Angle<t_angle_type>::atan2(rows[1][2], rows[2][2]);
                rotation[YAW] = Angle<t_angle_type>::atan2(rows[0][1], rows[0][0]);
            }
            else
            {
                rotation[ROLL] = Angle<t_angle_type>::atan2(-rows[2][1], rows[1][1]);
                rotation[YAW] = Angle<t_angle_type>(RADIANS, 0.0f);
            }
            return rotation;
        }
        template<class t_angle_type>
        constexpr Angle<t_angle_type> angleAbout(const Vector<3, t_type>& axis_unit) const
        {
            auto& R = *this;
            const t_type sin_phi =
                ((R[2][1] - R[1][2]) * axis_unit[0] +
                (R[0][2] - R[2][0]) * axis_unit[1] +
                (R[1][0] - R[0][1]) * axis_unit[2]) * static_cast<t_type>(0.5);
 
            // Project R*a onto a to extract cos component:
            Vector<3, t_type> Ra{
                R[0][0] * axis_unit[0] + R[0][1] * axis_unit[1] + R[0][2] * axis_unit[2],
                R[1][0] * axis_unit[0] + R[1][1] * axis_unit[1] + R[1][2] * axis_unit[2],
                R[2][0] * axis_unit[0] + R[2][1] * axis_unit[1] + R[2][2] * axis_unit[2]
            };
 
            const t_type cos_phi = dot(axis_unit, Ra);
 
            return Angle<t_angle_type>::atan2(sin_phi, cos_phi);
        }
        [[nodiscard]] constexpr Vector<4, t_type> decomposeRotationQuaternion() const
        {
            t_type trace = (*this)[0][0] + (*this)[1][1] + (*this)[2][2];
            Vector<4, t_type> temp;
 
            if (trace > t_type(0.0))
            {
                t_type s = sqrt(trace + t_type(1.0));
                temp[3] = (s * t_type(0.5));
                s = t_type(0.5) / s;
 
                temp[0] = (((*this)[2][1] - (*this)[1][2]) * s);
                temp[1] = (((*this)[0][2] - (*this)[2][0]) * s);
                temp[2] = (((*this)[1][0] - (*this)[0][1]) * s);
            }
            else
            {
                uint01 i = (*this)[0][0] < (*this)[1][1] ?
                    ((*this)[1][1] < (*this)[2][2] ? 2U : 1U) :
                    ((*this)[0][0] < (*this)[2][2] ? 2U : 0U);
                uint01 j = (i + 1) % 3;
                uint01 k = (i + 2) % 3;
 
                t_type s = t_type((*this)[i][i] - (*this)[j][j] - (*this)[k][k] + t_type(1.0));
                temp[i] = s * t_type(0.5);
                s = t_type(0.5) / s;
 
                temp[3] = ((*this)[k][j] - (*this)[j][k]) * s;
                temp[j] = ((*this)[j][i] + (*this)[i][j]) * s;
                temp[k] = ((*this)[k][i] + (*this)[i][k]) * s;
            }
            return temp;
        }
        template<class t_angle_type>
        constexpr void decompose(Vector<3, t_type>& scaling, Vector<3, Angle<t_angle_type>>& rotation, Vector<3, t_type>& position) const
        {
            static_assert(t_col_dims >= 4 && t_row_dims >= 4, "Cannot decompose lower dimension matrix");
 
            position[X] = (*this)[3][0];
            position[Y] = (*this)[3][1];
            position[Z] = (*this)[3][2];
 
            Vector<3, t_type> rows[3];
            for (uint01 i = 0; i < 3; i++)
            {
                rows[i] = (*this)[i].template as<3, t_type>();
                scaling[i] = rows[i].template magnitude<t_type>();
            }
            scaling = quantize(scaling, Vector<3, t_type>(.000001));
            const Vector<3, t_type> temp_z = cross(rows[0], rows[1]);
            if (dot(temp_z, rows[2]) < 0)
            {
                scaling[X] = -scaling[X];
                rows[0] = -rows[0];
            }
            for (uint04 i = 0; i < 3; i++)
            {
                rows[i] = rows[i].template normalized<t_type>();
            }
            t_type sy = sqrt(rows[0][0] * rows[0][0] + rows[0][1] * rows[0][1]);
 
            rotation[PITCH] = Angle<t_angle_type>::atan2(-rows[0][2], sy);
            if (sy > cast<t_type>(1E-6))
            {
                rotation[ROLL] = Angle<t_angle_type>::atan2(rows[1][2], rows[2][2]);
                rotation[YAW]  = Angle<t_angle_type>::atan2(rows[0][1], rows[0][0]);
            }
            else 
            {
                rotation[ROLL] = Angle<t_angle_type>::atan2(-rows[2][1], rows[1][1]);
                rotation[YAW]  = Angle<t_angle_type>(RADIANS, 0.0f);
            }
        }
        [[nodiscard]] constexpr Matrix offset(const Vector<2, t_type>& translation) const
        {
            Matrix mat(1);
            mat[t_row_dims - 1][0] = translation[X];
            if constexpr (t_col_dims >= 2)
                mat[t_row_dims - 1][1] = translation[Y];
            if constexpr (t_col_dims >= 3)
                mat[t_row_dims - 1][2] = 0;
            return (*this) * mat;
 
        }
 
        
        [[nodiscard]] constexpr Matrix offset(const Vector<3, t_type>& translation) const
        {
            return (*this) * Matrix::OffsetMatrix(translation);
        }
        
        [[nodiscard]] constexpr inline Matrix scale(t_type scale) const
        {
            return (*this) * Matrix::ScalerMatrix(scale);
        }
        
 
        [[nodiscard]] constexpr Matrix scale(const Vector<2, t_type>& scale) const
        {
            return (*this) * Matrix::ScalerMatrix(scale);
        }
 
        
        [[nodiscard]] constexpr Matrix scale(const Vector<3, t_type>& scale) const
        {
            return (*this) * Matrix::ScalerMatrix(scale);
        }
 
        Matrix& correctZeroScale(t_type new_value = 1e-6, t_type epsilon = 1e-9)
        {
            for (uint01 i = 0; i < getMin(t_row_dims, t_col_dims); i++)
            {
                if (std::abs((*this)[i][i]) < epsilon)
                    (*this)[i][i] = new_value;
            }
            return *this;
        }
        
        [[nodiscard]] constexpr Matrix scale(const Vector<3, t_type>& direction, t_type scale) const
        {
            Matrix mat(1);
            mat[0][0] += (scale - 1) * direction[X] * direction[X];
            mat[0][1] = (scale - 1) * direction[Y] * direction[X];
            mat[0][2] = (scale - 1) * direction[Z] * direction[X];
            
            mat[1][0] = (scale - 1) * direction[X] * direction[Y];
            mat[1][1] += (scale - 1) * direction[Y] * direction[Y];
            mat[1][2] = (scale - 1) * direction[Z] * direction[Y];
 
            mat[2][0] = (scale - 1) * direction[X] * direction[Z];
            mat[2][1] = (scale - 1) * direction[Y] * direction[Z];
            mat[2][2] += (scale - 1) * direction[Z] * direction[Z];
 
            return (*this) * mat;
        }
        template<class t_angle_type>
        [[nodiscard]] constexpr Matrix rotate(const Vector<3, Angle<t_angle_type>>& orientation) const
        {
            return *this * Matrix<t_type>::RotationMatrix(orientation);
        }
        template<class t_angle_type>
        [[nodiscard]] constexpr Matrix<t_type> rotate(const Angle<t_angle_type>& phi, const Vector<3, t_type>& axis) const
        {
            return *this * RotationMatrix(phi, axis);
        }
        [[nodiscard]] constexpr Matrix shear(t_type dx, t_type dy) const
        {
            Matrix mat(1);
            mat[0][1] = dx;
            mat[1][0] = dy;
            return (*this) * mat;
        }
        [[nodiscard]] constexpr Matrix<t_type, t_col_dims, t_row_dims> transpose() const
        {
            Matrix<t_type, t_col_dims, t_row_dims> mat;
            for (uint01 row = 0; row < t_row_dims; row++)
                for (uint01 col = 0; col < t_col_dims; col++)
                    mat[col][row] = (*this)[row][col];
            return mat;
        }
        
        template<uint01 t_cols = t_col_dims, uint01 t_rows = t_row_dims>
        [[nodiscard]] inline Matrix<t_type, t_cols, t_rows> invert() const
        {
            return Matrix<t_type, t_cols, t_rows>(MatrixInverter<t_cols, t_rows>::template invert<t_type>(*this));
        }
        
        [[nodiscard]] inline t_type* begin()
        {
            return &(*this)[0][0];
        }
 
        [[nodiscard]] inline const t_type* begin() const
        {
            return &(*this)[0][0];
        }
        [[nodiscard]] inline const t_type& inlineGet(uint01 val) const
        {
            return (&(*this)[0][0])[val];
        }
        [[nodiscard]] inline t_type& inlineGet(uint01 val)
        {
            return (&(*this)[0][0])[val];
        }
    public:
        [[nodiscard]] Matrix operator*(const Matrix& right) const
        {
            Matrix<t_type, t_row_dims, t_col_dims> product;
            for(uint01 col = 0; col < t_col_dims; ++col)
            {
                for(uint01 row = 0; row < t_row_dims; ++row)
                {
                    for(uint01 i = 0; i < t_row_dims; ++i)
                    {
                        product[row][col] += ((*this)[i][col] * right[row][i]);
                    }
                }
            }
            return product;
        }
        inline Matrix& operator=(const Matrix& right)
        {
            for(uint01 col = 0; col < t_col_dims; ++col)
            {
                for(uint01 row = 0; row < t_row_dims; ++row)
                {
                    (*this)[row][col] = right[row][col];
                }
            }
            return *this;
        }
        inline Matrix& operator*=(const Matrix& right) 
        {
            *this = *this * right;
            return *this;
        }
    };

    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    struct Constant<Matrix<t_type, t_row_dims, t_col_dims>>
    {
        constexpr const static Matrix<t_type, t_row_dims, t_col_dims> Invalid = Matrix<t_type, t_row_dims, t_col_dims>(Constant<Vector<t_row_dims, Vector<t_col_dims, t_type>>>::Invalid);
        constexpr const static Matrix<t_type, t_row_dims, t_col_dims> Min = Matrix<t_type, t_row_dims, t_col_dims>(Constant<Vector<t_row_dims, Vector<t_col_dims, t_type>>>::Min);
        constexpr const static Matrix<t_type, t_row_dims, t_col_dims> Max = Matrix<t_type, t_row_dims, t_col_dims>(Constant<Vector<t_row_dims, Vector<t_col_dims, t_type>>>::Max);
    };

 
    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    static constexpr bool IsInvalid(const Matrix<t_type, t_row_dims, t_col_dims>& value)
    {
        for (uint01 dim_a = 0; dim_a < t_col_dims; ++dim_a)
        {
            for (uint01 dim_b = 0; dim_b < t_row_dims; ++dim_b)
            {
                if (IsInvalid(value[dim_b][dim_a]))
                    return true;
            }
        }
        return false;
    }
    template<class t_type, uint01 t_row_dims, uint01 t_col_dims>
    static constexpr bool IsValid(const Matrix<t_type, t_row_dims, t_col_dims>& value)
    {
        for (uint01 dim_a = 0; dim_a < t_col_dims; ++dim_a)
        {
            for (uint01 dim_b = 0; dim_b < t_row_dims; ++dim_b)
            {
                if (IsInvalid(value[dim_b][dim_a]))
                    return false;
            }
        }
        return true;
    }
}