mat.h

/********************************************************************
 * Copyright (C) 2015 Liangliang Nan <liangliang.nan@gmail.com>
 * https://3d.bk.tudelft.nl/liangliang/
 *
 * This file is part of Easy3D. If it is useful in your research/work,
 * I would be grateful if you show your appreciation by citing it:
 * ------------------------------------------------------------------
 *      Liangliang Nan.
 *      Easy3D: a lightweight, easy-to-use, and efficient C++ library
 *      for processing and rendering 3D data.
 *      Journal of Open Source Software, 6(64), 3255, 2021.
 * ------------------------------------------------------------------
 *
 * Easy3D is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License Version 3
 * as published by the Free Software Foundation.
 *
 * Easy3D is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 ********************************************************************/
 
#ifndef EASY3D_CORE_MAT_H
#define EASY3D_CORE_MAT_H
 
#include <iostream>
#include <iomanip>
 
#include <easy3d/core/vec.h>
#include <easy3d/core/constant.h>
#include <easy3d/util/logging.h>
 
 
namespace easy3d {
 
 
    //#define MATRIX_ROW_MAJOR
 
    template <typename T>   class Mat2;
    template <typename T>   class Mat3;
    template <typename T>   class Mat4;
 
 
    template <size_t N, size_t M, typename T>
    class Mat
    {
    public:
        //  ----------------- constructor and destructor -------------------
 
        Mat() = default;
 
        explicit Mat(T s);
 
        template <size_t rN, size_t rM>
        explicit Mat(const Mat<rN, rM, T> &rhs);
 
        explicit Mat(const T *m);
 
        static Mat<N, M, T> identity();
 
        //  ------------------- size query ---------------------
 
        size_t num_rows() const { return N; }
 
        size_t num_columns() const { return M; }
 
        //  --------------------- access -----------------------
 
        Vec<M, T> row(size_t r) const;
 
        Vec<N, T> col(size_t c) const;
 
        const T& operator()(size_t r, size_t c) const;
 
        T& operator()(size_t r, size_t c);
 
        operator const T*() const;
 
        operator T*();
 
        //  --------------------- modification  ------------------
 
        void load_zero();
 
        void load_identity(T s = T(1));
 
        template <size_t vN>
        void set_row(size_t r, const Vec<vN, T> &v);
 
        template <size_t vN>
        void set_col(size_t c, const Vec<vN, T> &v);
 
        void swap_rows(size_t a, size_t b);
 
        void swap_cols(size_t a, size_t b);
 
        //  ------- matrix-matrix arithmetic operators --------
 
        bool operator==(const Mat<N, M, T> &rhs) const;
 
        bool operator!=(const Mat<N, M, T> &rhs) const;
 
        template <size_t rM>
        Mat<N, rM, T> operator*(const Mat<M, rM, T> &rhs) const;
 
        Mat<N, M, T> operator+(const Mat<N, M, T> &rhs) const;
 
        Mat<N, M, T> operator-(const Mat<N, M, T> &rhs) const;
 
        Mat<N, M, T> operator-() const;
 
        //  ------- matrix-vector arithmetic operators --------
 
        Vec<N, T> operator*(const Vec<M, T> &rhs) const;
 
        //  ------- matrix-scalar arithmetic operators --------
 
        Mat<N, M, T> operator*(T rhs) const;
        Mat<N, M, T> operator/(T rhs) const;
 
        //  ------- matrix-matrix assignment operators --------
 
        Mat<N, M, T>& operator*=(const Mat<N, M, T> &rhs);
 
        Mat<N, M, T>& operator+=(const Mat<N, M, T> &rhs);
 
        Mat<N, M, T>& operator-=(const Mat<N, M, T> &rhs);
 
        //  ------- matrix-scalar assignment operators --------
 
        Mat<N, M, T>& operator*=(T rhs);
 
        Mat<N, M, T>& operator/=(T rhs);
 
        Mat<N, M, T>& operator+=(T rhs);
 
        Mat<N, M, T>& operator-=(T rhs);
 
    protected:
        T m_[N * M];
    };
 
 
    //  ------- global scalar-matrix arithmetic operators --------
 
    template <size_t N, size_t M, typename T>
    Mat<N, M, T> operator*(T lhs, const Mat<N, M, T> &rhs);
 
    //  ------- global matrix-matrix multiplication operators --------
 
    template <typename T>
    Mat2<T> operator*(const Mat2<T> &lhs, const Mat2<T> &rhs);
 
    template <typename T>
    Mat3<T> operator*(const Mat3<T> &lhs, const Mat3<T> &rhs);
 
    template <typename T>
    Mat4<T> operator*(const Mat4<T> &lhs, const Mat4<T> &rhs);
 
    //  ------- global matrix-vector arithmetic operators --------
 
    template <typename T>
    Vec<3, T> operator*(const Mat4<T> &lhs, const Vec<3, T> &rhs);
 
    template <typename T>
    Vec<2, T> operator*(const Mat3<T> &lhs, const Vec<2, T> &rhs);
 
    template <typename T>
    Vec<2, T> operator*(const Mat2<T> &lhs, const Vec<2, T> &rhs);
    template <typename T>
    Vec<3, T> operator*(const Mat3<T> &lhs, const Vec<3, T> &rhs);
    template <typename T>
    Vec<4, T> operator*(const Mat4<T> &lhs, const Vec<4, T> &rhs);
 
    //  -------------- global matrix related function ---------------
 
    template <size_t N, typename T>
    T trace(const Mat<N, N, T> &m);
 
    template <size_t N, typename T, size_t A>
    T determinant(const Mat<N, N, T> &m);
 
    template <size_t N, size_t M, typename T>
    Mat<M, N, T> transpose(const Mat<N, M, T> &m);
 
    template <size_t N, typename T>
    Mat<N, N, T> inverse(const Mat<N, N, T> &m);
 
    template <size_t M, size_t N, typename T>
    Mat<N, M, T> tensor(const Vec<M, T> &u, const Vec<N, T> &v);
 
    template<size_t N, size_t M, typename T>
    bool gauss_jordan_elimination(const Mat<N, N, T> &a, const Mat<N, M, T> &b, Mat<N, N, T> *ainv, Mat<N, M, T> *x);
 
    template<size_t N, typename T>
    bool lu_decomposition(const Mat<N, N, T> &a, Mat<N, N, T> *alu, Vec<N, T> *rowp, T *d);
 
    template <size_t N, typename T>
    void lu_back_substitution(const Mat<N, N, T> &alu, const Vec<N, T> &rowp, const Vec<N, T> &b, Vec<N, T> *x);
 
    template<size_t N, typename FT>
    bool cholesky_decompose(const Mat<N, N, FT> &A, Mat<N, N, FT> &L);
 
    template <size_t N, typename FT>
    void cholesky_solve(const Mat<N, N, FT> &L, const Vec<N, FT> &b, Vec<N, FT>& x);
 
    template<size_t N, size_t M, typename T>
    void cholesky_solve(const Mat<N, N, T> &L, const Mat<N, M, T> &B, Mat<N, M, T>& X);
 
 
    template <size_t N, size_t M, typename T>
    std::ostream& operator<< (std::ostream& output, const Mat<N, M, T>& m);
 
    template <size_t N, size_t M, typename T>
    std::istream& operator>> (std::istream& input, Mat<N, M, T>& m);
 
 
 
    /*******************************************************************************
 
    IMPLEMENTATION:
 
    *******************************************************************************/
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>::Mat(T s) {
        for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < M; ++j)
                if (i == j)
                    (*this)(i, j) = T(s);
                else
                    (*this)(i, j) = T(0);
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    template <size_t rN, size_t rM>
    inline Mat<N, M, T>::Mat(const Mat<rN, rM, T> &rhs) {
        assert(rN >= N);
        assert(rM >= M);
 
        for (size_t i = 0; i < N; ++i)
            for (size_t j = 0; j < M; ++j)
                (*this)(i, j) = rhs(i, j);
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>::Mat(const T *m) {
        assert(m != 0);
 
        for (size_t i = 0; i < N * M; ++i)
            m_[i] = m[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::identity() {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < M; ++j)
                if (i == j)
                    result(i, j) = T(1);
                else
                    result(i, j) = T(0);
        }
        return result;
    }
 
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Vec<M, T> Mat<N, M, T>::row(size_t r) const {
        assert(r < N);
 
        Vec<M, T> result;
        for (size_t i = 0; i < M; ++i)
            result[i] = (*this)(r, i);
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Vec<N, T> Mat<N, M, T>::col(size_t c) const {
        assert(c < M);
 
        Vec<N, T> result;
        for (size_t i = 0; i < N; ++i)
            result[i] = (*this)(i, c);
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    template <size_t vN>
    inline void Mat<N, M, T>::set_row(size_t r, const Vec<vN, T> &v) {
        assert(r < N);
        assert(vN >= M);
 
        for (size_t i = 0; i < M; ++i)
            (*this)(r, i) = v[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    template <size_t vN>
    inline void Mat<N, M, T>::set_col(size_t c, const Vec<vN, T> &v) {
        assert(c < M);
        assert(vN >= N);
 
        for (size_t i = 0; i < N; ++i)
            (*this)(i, c) = v[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline void Mat<N, M, T>::swap_rows(size_t a, size_t b) {
        assert(a < N);
        assert(b < N);
 
        for (size_t i = 0; i < M; ++i) {
            T tmp = (*this)(a, i);
            (*this)(a, i) = (*this)(b, i);
            (*this)(b, i) = tmp;
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline void Mat<N, M, T>::load_zero() {
        size_t size = N * M;
        for (size_t i = 0; i < size; ++i)
            m_[i] = T(0);
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline void Mat<N, M, T>::load_identity(T s /* = T(1)*/) {
        for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < M; ++j)
                if (i == j)
                    (*this)(i, j) = T(s);
                else
                    (*this)(i, j) = T(0);
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline void Mat<N, M, T>::swap_cols(size_t a, size_t b) {
        assert(a < M);
        assert(b < M);
 
        for (size_t i = 0; i < N; ++i) {
            T tmp = (*this)(i, a);
            (*this)(i, a) = (*this)(i, b);
            (*this)(i, b) = tmp;
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>::operator const T*() const {
        return m_;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>::operator T*() {
        return m_;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline const T& Mat<N, M, T>::operator()(size_t row, size_t col) const {
        assert(row < N);
        assert(col < M);
 
    #ifdef MATRIX_ROW_MAJOR
        return m_[row * M + col]; // row-major
    #else
        return m_[col * N + row]; // column-major
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline T& Mat<N, M, T>::operator()(size_t row, size_t col) {
        assert(row < N);
        assert(col < M);
 
    #ifdef MATRIX_ROW_MAJOR
        return m_[row * M + col]; // row-major
    #else
        return m_[col * N + row]; // column-major
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline bool Mat<N, M, T>::operator==(const Mat<N, M, T> &rhs) const {
        bool result = true;
        for (size_t i = 0; i < N * M; ++i)
            result &= equal(m_[i], rhs[i]);
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline bool Mat<N, M, T>::operator!=(const Mat<N, M, T> &rhs) const {
        return !(*this == rhs);
    }
 
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    template <size_t rM>
    inline Mat<N, rM, T> Mat<N, M, T>::operator*(const Mat<M, rM, T> &rhs) const {
        Mat<N, rM, T> result;
        for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < rM; ++j) {
                result(i, j) = T(0);
                for (size_t k = 0; k < M; ++k) {
                    result(i, j) += (*this)(i, k) * rhs(k, j);
                }
            }
        }
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::operator+(const Mat<N, M, T> &rhs) const {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = m_[i] + rhs[i];
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::operator-(const Mat<N, M, T> &rhs) const {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = m_[i] - rhs[i];
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::operator-() const {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = -m_[i];
        return result;
    }
 
 
    //  MATRIX-VECTOR ARITHMETIC OPERATORS:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Vec<N, T> Mat<N, M, T>::operator*(const Vec<M, T> &rhs) const {
        Vec<N, T> result;
        for (size_t i = 0; i < N; ++i) {
            result[i] = 0;
            for (size_t j = 0; j < M; ++j) {
                result[i] += (*this)(i, j) * rhs[j];
            }
        }
        return result;
    }
 
 
    //  MATRIX-SCALAR ARITHMETIC OPERATORS:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::operator*(T rhs) const {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = m_[i] * rhs;
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> Mat<N, M, T>::operator/(T rhs) const {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = m_[i] / rhs;
        return result;
    }
 
    //  MATRIX-MATRIX ASSIGNMENT OPERATORS:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator*=(const Mat<N, M, T> &rhs) {
        Mat<N, M, T> tmp;
        tmp = *this * rhs;
        *this = tmp;
        return *this;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator+=(const Mat<N, M, T> &rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] += rhs[i];
        return *this;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator-=(const Mat<N, M, T> &rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] -= rhs[i];
        return *this;
    }
 
 
    //  MATRIX-SCALAR ASSIGNMENT OPERATORS:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator*=(T rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] *= rhs;
        return *this;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator/=(T rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] /= rhs;
        return *this;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator+=(T rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] += rhs;
        return *this;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T>& Mat<N, M, T>::operator-=(T rhs) {
        for (size_t i = 0; i < N * M; ++i)
            m_[i] -= rhs;
        return *this;
    }
 
 
    //  GLOBAL SCALAR-MATRIX ARITHMETIC OPERATORS:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<N, M, T> operator*(T lhs, const Mat<N, M, T> &rhs) {
        Mat<N, M, T> result;
        for (size_t i = 0; i < N * M; ++i)
            result[i] = lhs * rhs[i];
        return result;
    }
 
 
    //  ------- global matrix-matrix multiplication operators --------
 
    template <typename T>
    inline Mat2<T> operator*(const Mat2<T> &lhs, const Mat2<T> &rhs) {
        Mat2<T> result;
        for (size_t i = 0; i < 2; ++i) {
            for (size_t j = 0; j < 2; ++j) {
                result(i, j) = T(0);
                for (size_t k = 0; k < 2; ++k) {
                    result(i, j) += lhs(i, k) * rhs(k, j);
                }
            }
        }
        return result;
    }
 
    template <typename T>
    inline Mat3<T> operator*(const Mat3<T> &lhs, const Mat3<T> &rhs) {
        Mat3<T> result;
        for (size_t i = 0; i < 3; ++i) {
            for (size_t j = 0; j < 3; ++j) {
                result(i, j) = T(0);
                for (size_t k = 0; k < 3; ++k) {
                    result(i, j) += lhs(i, k) * rhs(k, j);
                }
            }
        }
        return result;
    }
 
    template <typename T>
    inline Mat4<T> operator*(const Mat4<T> &lhs, const Mat4<T> &rhs) {
        Mat4<T> result;
        for (size_t i = 0; i < 4; ++i) {
            for (size_t j = 0; j < 4; ++j) {
                result(i, j) = T(0);
                for (size_t k = 0; k < 4; ++k) {
                    result(i, j) += lhs(i, k) * rhs(k, j);
                }
            }
        }
        return result;
    }
 
    //  GLOBAL MATRIX-VECTOR ARITHMETIC OPERATORS:
 
    /*----------------------------- homogeneous version --------------------------*/
    template <typename T>
    inline Vec<3, T> operator*(const Mat4<T> &lhs, const Vec<3, T> &rhs) {
        Vec<4, T> tmp(rhs, T(1));
        Vec<4, T> result = lhs * tmp;
        result /= result[3];
        return Vec<3, T>((T*)result);
    }
 
    /*----------------------------- homogeneous version --------------------------*/
    template <typename T>
    inline Vec<2, T> operator*(const Mat3<T> &lhs, const Vec<2, T> &rhs) {
        Vec<3, T> tmp(rhs, T(1));
        Vec<3, T> result = lhs * tmp;
        result /= result[2];
        return Vec<2, T>((T*)result);
    }
 
 
    /*--------------------------- non-homogeneous version ------------------------*/
    template <typename T>
    inline Vec<2, T> operator*(const Mat2<T> &lhs, const Vec<2, T> &rhs) {
        Vec<2, T> result;
        for (size_t i = 0; i < 2; ++i) {
            result[i] = 0;
            for (size_t j = 0; j < 2; ++j) {
                result[i] += lhs(i, j) * rhs[j];
            }
        }
        return result;
    }
 
    /*--------------------------- non-homogeneous version ------------------------*/
    template <typename T>
    inline Vec<3, T> operator*(const Mat3<T> &lhs, const Vec<3, T> &rhs) {
        Vec<3, T> result;
        for (size_t i = 0; i < 3; ++i) {
            result[i] = 0;
            for (size_t j = 0; j < 3; ++j) {
                result[i] += lhs(i, j) * rhs[j];
            }
        }
        return result;
    }
 
    /*--------------------------- non-homogeneous version ------------------------*/
    template <typename T>
    inline Vec<4, T> operator*(const Mat4<T> &lhs, const Vec<4, T> &rhs) {
        Vec<4, T> result;
        for (size_t i = 0; i < 4; ++i) {
            result[i] = 0;
            for (size_t j = 0; j < 4; ++j) {
                result[i] += lhs(i, j) * rhs[j];
            }
        }
        return result;
    }
 
 
 
    //  GLOBAL SERVICES:
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    inline Mat<M, N, T> transpose(const Mat<N, M, T> &m) {
        Mat<M, N, T> result;
        for (size_t i = 0; i < N; ++i) {
            for (size_t j = 0; j < M; ++j) {
                result(j, i) = m(i, j);
            }
        }
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t D, typename T>
    inline T trace(const Mat<D, D, T> &m) {
        T result = m(0, 0);
        for (size_t i = 1; i < D; ++i)
            result += m(i, i);
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, typename T>
    inline T determinant(const Mat<N, N, T> &m) {
        /*  Use LU decomposition to find the determinant. */
        Mat<N, N, T> tmp;
        Vec<N, size_t> rowp;
        T result;
        lu_decomposition(m, &tmp, &rowp, &result);
        for (size_t i = 0; i < N; ++i)
            result *= tmp(i, i);
        return result;
    }
 
    template <typename T>
    inline T determinant(const Mat2<T> &m) {
        return m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
    }
 
    template <typename T>
    inline T determinant(const Mat3<T> &m) {
        return
            m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) +
            m(0, 1) * (m(2, 0) * m(1, 2) - m(1, 0) * m(2, 2)) +
            m(0, 2) * (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1));
    }
 
    template <typename T>
    inline T determinant(const Mat4<T> &m) {
        return
            m(0, 3) * m(1, 2) * m(2, 1) * m(3, 0) - m(0, 2) * m(1, 3) * m(2, 1) * m(3, 0) -
            m(0, 3) * m(1, 1) * m(2, 2) * m(3, 0) + m(0, 1) * m(1, 3) * m(2, 2) * m(3, 0) +
            m(0, 2) * m(1, 1) * m(2, 3) * m(3, 0) - m(0, 1) * m(1, 2) * m(2, 3) * m(3, 0) -
            m(0, 3) * m(1, 2) * m(2, 0) * m(3, 1) + m(0, 2) * m(1, 3) * m(2, 0) * m(3, 1) +
            m(0, 3) * m(1, 0) * m(2, 2) * m(3, 1) - m(0, 0) * m(1, 3) * m(2, 2) * m(3, 1) -
            m(0, 2) * m(1, 0) * m(2, 3) * m(3, 1) + m(0, 0) * m(1, 2) * m(2, 3) * m(3, 1) +
            m(0, 3) * m(1, 1) * m(2, 0) * m(3, 2) - m(0, 1) * m(1, 3) * m(2, 0) * m(3, 2) -
            m(0, 3) * m(1, 0) * m(2, 1) * m(3, 2) + m(0, 0) * m(1, 3) * m(2, 1) * m(3, 2) +
            m(0, 1) * m(1, 0) * m(2, 3) * m(3, 2) - m(0, 0) * m(1, 1) * m(2, 3) * m(3, 2) -
            m(0, 2) * m(1, 1) * m(2, 0) * m(3, 3) + m(0, 1) * m(1, 2) * m(2, 0) * m(3, 3) +
            m(0, 2) * m(1, 0) * m(2, 1) * m(3, 3) - m(0, 0) * m(1, 2) * m(2, 1) * m(3, 3) -
            m(0, 1) * m(1, 0) * m(2, 2) * m(3, 3) + m(0, 0) * m(1, 1) * m(2, 2) * m(3, 3);
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, typename T>
    inline Mat<N, N, T> inverse(const Mat<N, N, T> &m) {
        // Use Gauss-Jordan elimination to find inverse. This uses less memory than the LU decomposition method.
        // See lu_back_substitution() for an example of computing inverse using LU decomposition.
        size_t indxc[N], indxr[N], ipiv[N];
 
        Mat<N, N, T> result = m;
 
        for (size_t i = 0; i < N; ++i)
            ipiv[i] = 0;
 
        for (size_t i = 0; i < N; ++i) { // for each column
            T max = T(0);
            size_t maxc, maxr;
            for (size_t j = 0; j < N; ++j) { // search for pivot
                if (ipiv[j] != 1) {
                    for (size_t k = 0; k < N; ++k) { // for each column
                        if (ipiv[k] == 0) {
                            T element = std::abs(result(j, k));
                            if (element > max) {
                                max = element;
                                maxr = j;
                                maxc = k;
                            }
                        }
                    }
                }
            }
            ++ipiv[maxc];
 
            if (maxr != maxc)
                result.swap_rows(maxr, maxc);
 
            indxr[i] = maxr;
            indxc[i] = maxc;
 
            //  check for singular matrix:
            if (std::abs(result(maxc, maxc)) < epsilon<T>()) {
                LOG(ERROR) << "input matrix is singular";
                return result; // return partial result
            }
 
            //  multiply row by 1/pivot:
            T rpivot = T(1) / result(maxc, maxc);
            result(maxc, maxc) = T(1);
            result.set_row(maxc, result.row(maxc) * rpivot);
 
            //  reduce rows (except pivot):
            for (size_t j = 0; j < N; ++j) {
                if (j != maxc) {
                    T dum = result(j, maxc);
                    result(j, maxc) = T(0);
                    for (size_t k = 0; k < N; ++k)
                        result(j, k) -= result(maxc, k) * dum;
                }
            }
        }
 
        //  undo column swap:
        size_t i = N;
        do {
            --i;
            if (indxr[i] != indxc[i]) // if swap occurred
                result.swap_cols(indxr[i], indxc[i]);
        } while (i != 0);
 
        return result;
    }
 
    template <typename T>
    inline Mat2<T> inverse(const Mat2<T> &m) {
        Mat2<T> result;
        result(0, 0) = m(1, 1);
        result(0, 1) = -m(0, 1);
        result(1, 0) = -m(1, 0);
        result(1, 1) = m(0, 0);
 
        T det = determinant(m);
        det = T(1) / det;
        result *= det;
 
        return result;
    }
 
    template <typename T>
    inline Mat3<T> inverse(const Mat3<T> &m) {
        Mat3<T> result;
        result(0, 0) = (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2));
        result(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1));
        result(0, 2) = (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1));
        result(1, 0) = -(m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0));
        result(1, 1) = (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0));
        result(1, 2) = -(m(0, 0) * m(1, 2) - m(1, 0) * m(0, 2));
        result(2, 0) = (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1));
        result(2, 1) = -(m(0, 0) * m(2, 1) - m(2, 0) * m(0, 1));
        result(2, 2) = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1));
 
        T det = determinant(m);
        det = T(1) / det;
        result *= det;
 
        return result;
    }
 
    template <typename T>
    inline Mat4<T> inverse(const Mat4<T> &m) {
        Mat4<T> result;
        result(0, 0) = m(1, 2) * m(2, 3) * m(3, 1) - m(1, 3) * m(2, 2) * m(3, 1) + m(1, 3) * m(2, 1) * m(3, 2) - m(1, 1) * m(2, 3) * m(3, 2) - m(1, 2) * m(2, 1) * m(3, 3) + m(1, 1) * m(2, 2) * m(3, 3);
        result(0, 1) = m(0, 3) * m(2, 2) * m(3, 1) - m(0, 2) * m(2, 3) * m(3, 1) - m(0, 3) * m(2, 1) * m(3, 2) + m(0, 1) * m(2, 3) * m(3, 2) + m(0, 2) * m(2, 1) * m(3, 3) - m(0, 1) * m(2, 2) * m(3, 3);
        result(0, 2) = m(0, 2) * m(1, 3) * m(3, 1) - m(0, 3) * m(1, 2) * m(3, 1) + m(0, 3) * m(1, 1) * m(3, 2) - m(0, 1) * m(1, 3) * m(3, 2) - m(0, 2) * m(1, 1) * m(3, 3) + m(0, 1) * m(1, 2) * m(3, 3);
        result(0, 3) = m(0, 3) * m(1, 2) * m(2, 1) - m(0, 2) * m(1, 3) * m(2, 1) - m(0, 3) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 3) * m(2, 2) + m(0, 2) * m(1, 1) * m(2, 3) - m(0, 1) * m(1, 2) * m(2, 3);
        result(1, 0) = m(1, 3) * m(2, 2) * m(3, 0) - m(1, 2) * m(2, 3) * m(3, 0) - m(1, 3) * m(2, 0) * m(3, 2) + m(1, 0) * m(2, 3) * m(3, 2) + m(1, 2) * m(2, 0) * m(3, 3) - m(1, 0) * m(2, 2) * m(3, 3);
        result(1, 1) = m(0, 2) * m(2, 3) * m(3, 0) - m(0, 3) * m(2, 2) * m(3, 0) + m(0, 3) * m(2, 0) * m(3, 2) - m(0, 0) * m(2, 3) * m(3, 2) - m(0, 2) * m(2, 0) * m(3, 3) + m(0, 0) * m(2, 2) * m(3, 3);
        result(1, 2) = m(0, 3) * m(1, 2) * m(3, 0) - m(0, 2) * m(1, 3) * m(3, 0) - m(0, 3) * m(1, 0) * m(3, 2) + m(0, 0) * m(1, 3) * m(3, 2) + m(0, 2) * m(1, 0) * m(3, 3) - m(0, 0) * m(1, 2) * m(3, 3);
        result(1, 3) = m(0, 2) * m(1, 3) * m(2, 0) - m(0, 3) * m(1, 2) * m(2, 0) + m(0, 3) * m(1, 0) * m(2, 2) - m(0, 0) * m(1, 3) * m(2, 2) - m(0, 2) * m(1, 0) * m(2, 3) + m(0, 0) * m(1, 2) * m(2, 3);
        result(2, 0) = m(1, 1) * m(2, 3) * m(3, 0) - m(1, 3) * m(2, 1) * m(3, 0) + m(1, 3) * m(2, 0) * m(3, 1) - m(1, 0) * m(2, 3) * m(3, 1) - m(1, 1) * m(2, 0) * m(3, 3) + m(1, 0) * m(2, 1) * m(3, 3);
        result(2, 1) = m(0, 3) * m(2, 1) * m(3, 0) - m(0, 1) * m(2, 3) * m(3, 0) - m(0, 3) * m(2, 0) * m(3, 1) + m(0, 0) * m(2, 3) * m(3, 1) + m(0, 1) * m(2, 0) * m(3, 3) - m(0, 0) * m(2, 1) * m(3, 3);
        result(2, 2) = m(0, 1) * m(1, 3) * m(3, 0) - m(0, 3) * m(1, 1) * m(3, 0) + m(0, 3) * m(1, 0) * m(3, 1) - m(0, 0) * m(1, 3) * m(3, 1) - m(0, 1) * m(1, 0) * m(3, 3) + m(0, 0) * m(1, 1) * m(3, 3);
        result(2, 3) = m(0, 3) * m(1, 1) * m(2, 0) - m(0, 1) * m(1, 3) * m(2, 0) - m(0, 3) * m(1, 0) * m(2, 1) + m(0, 0) * m(1, 3) * m(2, 1) + m(0, 1) * m(1, 0) * m(2, 3) - m(0, 0) * m(1, 1) * m(2, 3);
        result(3, 0) = m(1, 2) * m(2, 1) * m(3, 0) - m(1, 1) * m(2, 2) * m(3, 0) - m(1, 2) * m(2, 0) * m(3, 1) + m(1, 0) * m(2, 2) * m(3, 1) + m(1, 1) * m(2, 0) * m(3, 2) - m(1, 0) * m(2, 1) * m(3, 2);
        result(3, 1) = m(0, 1) * m(2, 2) * m(3, 0) - m(0, 2) * m(2, 1) * m(3, 0) + m(0, 2) * m(2, 0) * m(3, 1) - m(0, 0) * m(2, 2) * m(3, 1) - m(0, 1) * m(2, 0) * m(3, 2) + m(0, 0) * m(2, 1) * m(3, 2);
        result(3, 2) = m(0, 2) * m(1, 1) * m(3, 0) - m(0, 1) * m(1, 2) * m(3, 0) - m(0, 2) * m(1, 0) * m(3, 1) + m(0, 0) * m(1, 2) * m(3, 1) + m(0, 1) * m(1, 0) * m(3, 2) - m(0, 0) * m(1, 1) * m(3, 2);
        result(3, 3) = m(0, 1) * m(1, 2) * m(2, 0) - m(0, 2) * m(1, 1) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2) + m(0, 0) * m(1, 1) * m(2, 2);
 
        T det = determinant(m);
        det = T(1) / det;
        result *= det;
 
        return result;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t M, size_t N, typename T>
    inline Mat<N, M, T> tensor(const Vec<M, T> &u, const Vec<N, T> &v) {
        Mat<N, 1, T> mu(u); // column vector u
        Mat<1, M, T> mv(v); // row vector v
        return mu * mv; // use matrix multiplication
    }
 
    /*----------------------------------------------------------------------------*/
    /*  Adapted from "Numerical Recipes in C" (Press et al.) */
    template <size_t N, size_t M, typename T>
    inline bool gauss_jordan_elimination(
        const Mat<N, N, T> &a,
        const Mat<N, M, T> &b,
        Mat<N, N, T> *ainv,
        Mat<N, M, T> *x) {
 
        size_t indxc[N], indxr[N], ipiv[N];
 
        Mat<N, N, T> &amat = *ainv;
        amat = a; // copy A
        Mat<N, M, T> &bmat = *x;
        bmat = b; // copy B
 
        for (size_t i = 0; i < N; ++i)
            ipiv[i] = 0;
 
        for (size_t i = 0; i < N; ++i) { // for each column
            T max = T(0);
            size_t maxc, maxr;
            for (size_t j = 0; j < N; ++j) { // search for pivot
                if (ipiv[j] != 1) {
                    for (size_t k = 0; k < N; ++k) { // for each column
                        if (ipiv[k] == 0) {
                            T element = std::abs(amat(j, k));
                            if (element > max) {
                                max = element;
                                maxr = j;
                                maxc = k;
                            }
                        }
                    }
                }
            }
            ++ipiv[maxc];
 
            if (maxr != maxc) {
                amat.swap_rows(maxr, maxc);
                bmat.swap_rows(maxr, maxc);
            }
 
            indxr[i] = maxr;
            indxc[i] = maxc;
 
            //  check for singular matrix:
            if (std::abs(amat(maxc, maxc)) < epsilon<T>())
                return false;
 
            //  multiply row by 1/pivot:
            T rpivot = T(1) / amat(maxc, maxc);
            amat(maxc, maxc) = T(1);
            amat.set_row(maxc, amat.row(maxc) * rpivot);
            bmat.set_row(maxc, bmat.row(maxc) * rpivot);
 
            //  reduce rows (except pivot):
            for (size_t j = 0; j < N; ++j) {
                if (j != maxc) {
                    T dum = amat(j, maxc);
                    amat(j, maxc) = T(0);
                    for (size_t k = 0; k < N; ++k)
                        amat(j, k) -= amat(maxc, k) * dum;
                    for (size_t k = 0; k < M; ++k)
                        bmat(j, k) -= bmat(maxc, k) * dum;
                }
            }
        }
 
        //  unscramble a by interchanging columns:
        size_t i = N;
        do {
            --i;
            if (indxr[i] != indxc[i]) // if swap occurred
                amat.swap_cols(indxr[i], indxc[i]);
        } while (i != 0);
 
        return true;
    }
 
    /*----------------------------------------------------------------------------*/
    /*  Adapted from "Numerical Recipes in C" (Press et al.) */
    template <size_t N, typename T>
    inline bool lu_decomposition(
        const Mat<N, N, T> &a,
        Mat<N, N, T> *alu,
        Vec<N, T> *rowp,
        T *d) {
 
        Vec<N, T> scalev; // stores implicit scaling of each row
        *d = T(1); // no rows changed
        Mat<N, N, T> &amat = *alu;
        amat = a; // copy a
 
        for (size_t i = 0; i < N; ++i) { // for each row
            //  get implicit scaling:
            T max = T(0);
            for (size_t j = 0; j < N; ++j) {
                T element = std::abs(amat(i, j));
                if (element > max)
                    max = element;
            }
 
            //  check for singular matrix:
            if (std::abs(max) < min<T>())
                return false;
 
            scalev[i] = T(1) / max; // save scaling factor
        }
 
        for (size_t j = 0; j < N; ++j) { // for each column
            for (size_t i = 0; i < j; ++i) {
                T sum = amat(i, j);
                for (size_t k = 0; k < i; ++k)
                    sum -= amat(i, k) * amat(k, j);
                amat(i, j) = sum;
            }
 
            T max = T(0);
            size_t imax;
            for (size_t i = j; i < N; ++i) {
                T sum = amat(i, j);
                for (size_t k = 0; k < j; ++k)
                    sum -= amat(i, k) * amat(k, j);
                amat(i, j) = sum;
 
                T dum = scalev[i] * std::abs(sum);
                if (dum >= max) {
                    max = dum;
                    imax = i;
                }
            }
 
            if (j != imax) { // if we need to swap rows
                amat.swap_rows(imax, j);
                scalev[imax] = scalev[j]; // also swap scale factor
                *d = -(*d); // change parity of d
            }
            (*rowp)[j] = (T)imax;
 
            //  check for singular matrix:
            if (std::abs(amat(j, j)) < epsilon<T>())
                return false;
 
            //  divide by the pivot element:
            if (j != N) {
                T dum = T(1) / amat(j, j);
                for (size_t i = j + 1; i < N; ++i)
                    amat(i, j) *= dum;
            }
 
        }
 
        return true;
    }
 
    /*----------------------------------------------------------------------------*/
    /*  Adapted from "Numerical Recipes in C" (Press et al.) */
    template <size_t N, typename T>
    inline void lu_back_substitution(
        const Mat<N, N, T> &alu,
        const Vec<N, T> &rowp,
        const Vec<N, T> &b,
        Vec<N, T> *x
        )
    {
        Vec<N, T> &result = *x;
        result = b; // copy b to result
 
        size_t ii = 0;
        for (size_t i = 0; i < N; ++i) {
            size_t ip = (size_t)rowp[i];
            assert(ip < N);
 
            T sum = result[ip];
            result[ip] = result[i];
            if (ii != 0) {
                for (size_t j = ii - 1; j < i; ++j)
                    sum -= alu(i, j) * result[j];
            }
            else if (std::abs(sum) > epsilon<T>()) {
                ii = i + 1;
            }
            result[i] = sum;
        }
 
        size_t i = N;
        do {
            --i;
            T sum = result[i];
            for (size_t j = i + 1; j < N; ++j)
                sum -= alu(i, j) * result[j];
            result[i] = sum / alu(i, i);
        } while (i != 0);
    }
 
 
    /*----------------------------------------------------------------------------*/
    // Adapted from Template Numerical Toolkit.
    template<size_t N, typename FT>
    bool cholesky_decompose(const Mat<N, N, FT> &A, Mat<N, N, FT> &L) {
        bool spd = true;
        for (size_t j = 0; j < N; ++j) {
            FT d = 0;
            for (size_t k = 0; k < j; ++k) {
                FT s = 0;
                for (size_t i = 0; i < k; ++i)
                    s += L(k, i) * L(j, i);
 
                L(j, k) = s = (A(j, k) - s) / L(k, k);
                d = d + s * s;
                spd = spd && (A(k, j) == A(j, k));
            }
 
            d = A(j, j) - d;
            spd = spd && (d > 0);
 
            L(j, j) = std::sqrt(d > 0 ? d : 0);
            for (size_t k = j + 1; k < N; ++k)
                L(j, k) = 0;
        }
        return spd;
    }
 
    /*----------------------------------------------------------------------------*/
    // Adapted from Template Numerical Toolkit.
    template <size_t N, typename FT>
    void cholesky_solve(const Mat<N, N, FT> &L, const Vec<N, FT> &b, Vec<N, FT>& x) {
        x = b;
        // solve L*y = b
        for (size_t k = 0; k < N; ++k) {
            for (size_t i = 0; i < k; ++i)
                x[k] -= x[i] * L(k, i);
            x[k] /= L(k, k);
        }
 
        // solve L'*x = y
        for (int k = N - 1; k >= 0; --k) {  // attention: size_t will not work
            for (size_t i = k + 1; i < N; ++i)
                x[k] -= x[i] * L(i, k);
            x[k] /= L(k, k);
        }
    }
 
    template<size_t N, size_t M, typename T>
    void cholesky_solve(const Mat<N, N, T> &L, const Mat<N, M, T> &B, Mat<N, M, T> &X) {
        X = B;
        // solve L_*Y = B
        for (size_t j = 0; j < M; ++j) {
            for (size_t k = 0; k < N; ++k) {
                for (size_t i = 0; i < k; ++i)
                    X(k, j) -= X(i, j) * L(k, i);
 
                X(k, j) /= L(k, k);
            }
        }
 
        // solve L'*X = Y
        for (size_t j = 0; j < M; ++j) {
            for (int k = N - 1; k >= 0; --k) {  // attention: size_t will not work
                for (size_t i = k + 1; i < N; ++i)
                    X(k, j) -= X(i, j) * L(i, k);
 
                X(k, j) /= L(k, k);
            }
        }
    }
 
    /*----------------------------------------------------------------------------*/
 
    template <size_t N, size_t M, typename T> inline
    std::ostream& operator<< (std::ostream& output, const Mat<N, M, T>& m) {
        output << std::fixed << std::setprecision(8);
        const char sep = ' ';
        for (int i = 0; i < N; i++) {
            for (int j = 0; j < M; j++) {
                output << sep  << std::setw(7) << m(i, j);
            }
            output << std::endl;
        }
        output << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
        return output;
    }
 
    /*----------------------------------------------------------------------------*/
    template <size_t N, size_t M, typename T>
    std::istream& operator>> (std::istream& input, Mat<N, M, T>& m) {
        for (int i = 0; i < N; i++) {
            for (int j = 0; j < M; j++) {
                input >> m(i, j);
            }
        }
        return input;
    }
 
 
    /*----------------------------------------------------------------------------*/
 
    template<size_t N, typename FT>
    Mat<N, 1, FT> to_matrix(const Vec<N, FT>& v) {
        return Mat<N, 1, FT>(v.data());
    }
 
 
    template<size_t N, typename FT>
    Mat<1, N, FT> transpose(const Vec<N, FT>& v) {
        return Mat<1, N, FT>(v.data());
    }
 
    /*----------------------------------------------------------------------------*/
 
    template <size_t N, size_t M, typename T>
    inline bool has_nan(const Mat<N, M, T>& m) {
        for (int i = 0; i < N; i++) {
            for (int j = 0; j < M; j++) {
                if (std::isnan(m(i, j)) || std::isinf(m(i, j)))
                    return true;
            }
        }
        return false;
    }
 
 
    /*******************************************************************************
 
    --------------------------------  2x2 matrix  ----------------------------------
 
    *******************************************************************************/
 
    template <typename T>
    class Mat2 : public Mat<2, 2, T>
    {
    public:
        Mat2() = default;
 
        explicit Mat2(T s);
 
        Mat2(const Mat<2, 2, T> &rhs);
 
        explicit Mat2(const Mat<3, 3, T> &rhs);
 
        Mat2(
            T s00, T s01,
            T s10, T s11
            );
 
        explicit Mat2(const T *m);
 
        Mat2(
            const Vec<2, T> &x,
            const Vec<2, T> &y
            );
 
        static Mat2<T> rotation(T angle);
 
        static Mat2<T> scale(T s);
 
        static Mat2<T> scale(T x, T y);
 
    }; // class Mat2
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(T s) {
        for (size_t i = 0; i < 2; ++i) {
            for (size_t j = 0; j < 2; ++j)
                if (i == j)
                    (*this)(i, j) = T(s);
                else
                    (*this)(i, j) = T(0);
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(const Mat<2, 2, T> &rhs) {
        for (size_t i = 0; i < 4; ++i)
            (*this).m_[i] = rhs[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(const Mat<3, 3, T> &rhs) {
        (*this)(0, 0) = rhs(0, 0); (*this)(0, 1) = rhs(0, 1);
        (*this)(1, 0) = rhs(1, 0); (*this)(1, 1) = rhs(1, 1);
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(
        T s00, T s01,
        T s10, T s11
        ) {
        (*this)(0, 0) = s00; (*this)(0, 1) = s01;
        (*this)(1, 0) = s10; (*this)(1, 1) = s11;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(const T *m) {
        assert(m != 0);
 
    #ifdef MATRIX_ROW_MAJOR
        (*this)(0, 0) = m[0]; (*this)(0, 1) = m[1];
        (*this)(1, 0) = m[2]; (*this)(1, 1) = m[3];
    #else
        (*this)(0, 0) = m[0]; (*this)(0, 1) = m[2];
        (*this)(1, 0) = m[1]; (*this)(1, 1) = m[3];
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T>::Mat2(const Vec<2, T> &x, const Vec<2, T> &y) {
    #ifdef MATRIX_ROW_MAJOR
        (*this).set_row(0, x);
        (*this).set_row(1, y);
    #else
        (*this).set_col(0, x);
        (*this).set_col(1, y);
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T> Mat2<T>::rotation(T angle) {
        return Mat2<T>(
            std::cos(angle), -std::sin(angle),
            std::sin(angle),  std::cos(angle)
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T> Mat2<T>::scale(T s) {
        return Mat2<T>(
            s, T(0),
            T(0), s
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T> Mat2<T>::scale(T x, T y) {
        return Mat2<T>(
            x, T(0),
            T(0), y
            );
    }
 
    /*******************************************************************************
 
    --------------------------------  3x3 matrix  ----------------------------------
 
    *******************************************************************************/
 
    template <typename T>   class Quat;
 
    template <typename T>
    class Mat3 : public Mat<3, 3, T> {
    public:
        Mat3() = default;
 
        explicit Mat3(T s);
 
        Mat3(const Mat<3, 3, T> &rhs);
 
        explicit Mat3(const Mat<4, 4, T> &rhs);
 
        Mat3(
            T s00, T s01, T s02,
            T s10, T s11, T s12,
            T s20, T s21, T s22
            );
 
        explicit Mat3(const T *m);
 
        Mat3(
            const Vec<3, T> &x,
            const Vec<3, T> &y,
            const Vec<3, T> &z
            );
 
        explicit Mat3(const Mat<2, 2, T> &rhs);
 
        Mat2<T> sub() const;
 
        static Mat3<T> scale(T s);
 
        static Mat3<T> scale(T x, T y, T z);
 
        static Mat3<T> rotation(const Vec<3, T> &axis, T angle);
        
        static Mat3<T> rotation(const Vec<3, T> &axis_angle);
        
        static Mat3<T> rotation(const Quat<T> &q);
        
        static Mat3<T> rotation(T x, T y, T z, int order = 312);
 
    }; // class Mat3
 
 
    /*******************************************************************************
 
    IMPLEMENTATION:
 
    *******************************************************************************/
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(T s) {
        for (size_t i = 0; i < 3; ++i) {
            for (size_t j = 0; j < 3; ++j)
                if (i == j)
                    (*this)(i, j) = T(s);
                else
                    (*this)(i, j) = T(0);
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(const Mat<3, 3, T> &rhs) {
        for (size_t i = 0; i < 9; ++i)
            (*this).m_[i] = rhs[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(const Mat<4, 4, T> &rhs) {
        (*this)(0, 0) = rhs(0, 0); (*this)(0, 1) = rhs(0, 1); (*this)(0, 2) = rhs(0, 2);
        (*this)(1, 0) = rhs(1, 0); (*this)(1, 1) = rhs(1, 1); (*this)(1, 2) = rhs(1, 2);
        (*this)(2, 0) = rhs(2, 0); (*this)(2, 1) = rhs(2, 1); (*this)(2, 2) = rhs(2, 2);
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(
        T s00, T s01, T s02,
        T s10, T s11, T s12,
        T s20, T s21, T s22
        ) {
        (*this)(0, 0) = s00; (*this)(0, 1) = s01; (*this)(0, 2) = s02;
        (*this)(1, 0) = s10; (*this)(1, 1) = s11; (*this)(1, 2) = s12;
        (*this)(2, 0) = s20; (*this)(2, 1) = s21; (*this)(2, 2) = s22;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(const T *m) {
        assert(m != 0);
 
    #ifdef MATRIX_ROW_MAJOR
        (*this)(0, 0) = m[0]; (*this)(0, 1) = m[1]; (*this)(0, 2) = m[2];
        (*this)(1, 0) = m[3]; (*this)(1, 1) = m[4]; (*this)(1, 2) = m[5];
        (*this)(2, 0) = m[6]; (*this)(2, 1) = m[7]; (*this)(2, 2) = m[8];
    #else
        (*this)(0, 0) = m[0]; (*this)(0, 1) = m[3]; (*this)(0, 2) = m[6];
        (*this)(1, 0) = m[1]; (*this)(1, 1) = m[4]; (*this)(1, 2) = m[7];
        (*this)(2, 0) = m[2]; (*this)(2, 1) = m[5]; (*this)(2, 2) = m[8];
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(const Vec<3, T> &x, const Vec<3, T> &y, const Vec<3, T> &z) {
    #ifdef MATRIX_ROW_MAJOR
        (*this).set_row(0, x);
        (*this).set_row(1, y);
        (*this).set_row(2, z);
    #else
        (*this).set_col(0, x);
        (*this).set_col(1, y);
        (*this).set_col(2, z);
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T>::Mat3(const Mat<2, 2, T> &rhs) {
        (*this)(0, 0) = rhs(0, 1);  (*this)(0, 0) = rhs(0, 1);  (*this)(0, 2) = T(0);
        (*this)(1, 0) = rhs(1, 1);  (*this)(1, 0) = rhs(1, 1);  (*this)(1, 2) = T(0);
        (*this)(2, 0) = T(0);       (*this)(2, 0) = T(0);       (*this)(2, 2) = T(1);
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat2<T> Mat3<T>::sub() const {
        Mat2<T> mat;
        for (size_t i = 0; i < 2; i++) {
            for (size_t j = 0; j < 2; j++) {
                mat(i, j) = (*this)(i, j);
            }
        }
        return mat;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::scale(T s) {
        return Mat3<T>(
            s,    T(0), T(0),
            T(0), s,    T(0),
            T(0), T(0), s
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::scale(T x, T y, T z) {
        return Mat3<T>(
            x,    T(0), T(0),
            T(0), y,    T(0),
            T(0), T(0), z
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::rotation(const Vec<3, T> &axis, T angle) {
        assert(std::abs(axis.length() - 1) < epsilon<T>());
 
        //  cross-product matrix of axis:
        const Mat3<T> cpm(
             T(0),     -axis[2],   axis[1],
             axis[2],   T(0),     -axis[0],
            -axis[1],   axis[0],   T(0)
            );
 
        //  axis-axis tensor product:
        const Mat3<T> tpm = tensor(axis, axis);
 
        //  trig coefficients:
        const T c = std::cos(angle);
        const T rc = T(1) - c;
        const T s = std::sin(angle);
 
        return Mat3<T>::identity() * c + cpm * s + tpm * rc;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::rotation(const Vec<3, T> &axis_angle) {
        const T len = axis_angle.length();
        return rotation(axis_angle/len, len);
    }
    
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::rotation(const Quat<T> &q) {
        // input must be unit quaternion
        assert(std::abs(q.length() - 1) < epsilon<T>());
        const T x = q.x;
        const T y = q.y;
        const T z = q.z;
        const T w = q.w;
        Mat3<T> m;
        m(0, 0) = 1.0 - 2.0 * (y*y + z*z);    m(0, 1) = 2.0 * (x*y - w*z);          m(0, 2) = 2.0 * (x*z + w*y);
        m(1, 0) = 2.0 * (x*y + w*z);          m(1, 1) = 1.0 - 2.0 * (x*x + z*z);    m(1, 2) = 2.0 * (y*z - w*x);
        m(2, 0) = 2.0 * (x*z - w*y);          m(2, 1) = 2.0 * (y*z + w*x);          m(2, 2) = 1.0 - 2.0 * (x*x + y*y);
        return m;
    }
    
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat3<T>::rotation(T x, T y, T z, int order) {
        // The code is not optimized. The final matrix can be directly derived.
        // http://www.songho.ca/opengl/gl_anglestoaxes.html
        // http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/
        Mat3<T> rx; // Rotation about X-axis (Pitch)
        rx(0, 0) = T(1);         rx(0, 1) = T(0);           rx(0, 2) = T(0);
        rx(1, 0) = T(0);         rx(1, 1) = std::cos(x);    rx(1, 2) = -std::sin(x);
        rx(2, 0) = T(0);         rx(2, 1) = std::sin(x);    rx(2, 2) = std::cos(x);
 
        Mat3<T> ry; // Rotation about Y-axis (Yaw, Heading)
        ry(0, 0) = std::cos(y);  ry(0, 1) = T(0);           ry(0, 2) = std::sin(y);
        ry(1, 0) = T(0);         ry(1, 1) = T(1);           ry(1, 2) = T(0);
        ry(2, 0) = -std::sin(y); ry(2, 1) = T(0);           ry(2, 2) = std::cos(y);
        
        Mat3<T> rz; // Rotation about Z-axis (Roll)
        rz(0, 0) = std::cos(z);  rz(0, 1) = -std::sin(z);   rz(0, 2) = T(0);
        rz(1, 0) = std::sin(z);  rz(1, 1) = std::cos(z);    rz(1, 2) = T(0);
        rz(2, 0) = T(0);         rz(2, 1) = T(0);           rz(2, 2) = T(1);
        
        switch (order) {
            case 123:   return rz * ry * rx;
            case 132:   return ry * rz * rx;
            case 213:   return rz * rx * ry;
            case 231:   return rx * rz * ry;
            case 312:   return ry * rx * rz;
            case 321:   return rx * ry * rz;
            default:
                LOG(ERROR) << "invalid rotation order";
                return rx * rz * ry;
        }
    }
 
    /*******************************************************************************
 
    --------------------------------  4x4 matrix  ----------------------------------
 
    *******************************************************************************/
 
    template <typename T>
    class Mat4 : public Mat<4, 4, T> {
    public:
        Mat4() = default;
 
        explicit Mat4(T s);
 
        Mat4(const Mat<4, 4, T> &rhs);
 
        Mat4(
            T s00, T s01, T s02, T s03,
            T s10, T s11, T s12, T s13,
            T s20, T s21, T s22, T s23,
            T s30, T s31, T s32, T s33
            );
 
        explicit Mat4(const T *m);
 
        Mat4(
            const Vec<4, T> &x,
            const Vec<4, T> &y,
            const Vec<4, T> &z,
            const Vec<4, T> &w
            );
 
        explicit Mat4(const Mat<3, 3, T> &rhs);
 
        Mat4(const Vec<3, T> &s, const Quat<T> &r, const Vec<3, T> &t);
 
        // the upper-left 3x3 sub-matrix,
        Mat3<T> sub() const;
 
        static Mat4<T> scale(T s);
 
        static Mat4<T> scale(const Vec<4, T>& s);  // set w to 1 for 3D scaling
        static Mat4<T> scale(T x, T y, T z, T w);  // set w to 1 for 3D scaling
        
        static Mat4<T> rotation(const Vec<3, T> &axis, T angle);
        
        static Mat4<T> rotation(const Vec<3, T> &axis_angle);
 
        static Mat4<T> rotation(const Quat<T> &q);
 
        static Mat4<T> rotation(T x, T y, T z, int order = 312);
 
        static Mat4<T> translation(const Vec<3, T> &t);
        static Mat4<T> translation(T x, T y, T z);
 
    }; // class Mat4
 
 
    /*******************************************************************************
 
    IMPLEMENTATION:
 
    *******************************************************************************/
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(T s) {
        for (size_t i = 0; i < 4; ++i) {
            for (size_t j = 0; j < 4; ++j)
                if (i == j)
                    (*this)(i, j) = T(s);
                else
                    (*this)(i, j) = T(0);
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(const Mat<4, 4, T> &rhs) {
        for (size_t i = 0; i < 16; ++i)
            (*this).m_[i] = rhs[i];
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(
        T s00, T s01, T s02, T s03,
        T s10, T s11, T s12, T s13,
        T s20, T s21, T s22, T s23,
        T s30, T s31, T s32, T s33
        ) {
        (*this)(0, 0) = s00; (*this)(0, 1) = s01; (*this)(0, 2) = s02; (*this)(0, 3) = s03;
        (*this)(1, 0) = s10; (*this)(1, 1) = s11; (*this)(1, 2) = s12; (*this)(1, 3) = s13;
        (*this)(2, 0) = s20; (*this)(2, 1) = s21; (*this)(2, 2) = s22; (*this)(2, 3) = s23;
        (*this)(3, 0) = s30; (*this)(3, 1) = s31; (*this)(3, 2) = s32; (*this)(3, 3) = s33;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(const T *m) {
        assert(m != 0);
 
    #ifdef MATRIX_ROW_MAJOR
        (*this)(0, 0) = m[0];  (*this)(0, 1) = m[1];  (*this)(0, 2) = m[2];  (*this)(0, 3) = m[3];
        (*this)(1, 0) = m[4];  (*this)(1, 1) = m[5];  (*this)(1, 2) = m[6];  (*this)(1, 3) = m[7];
        (*this)(2, 0) = m[8];  (*this)(2, 1) = m[9];  (*this)(2, 2) = m[10]; (*this)(2, 3) = m[11];
        (*this)(3, 0) = m[12]; (*this)(3, 1) = m[13]; (*this)(3, 2) = m[14]; (*this)(3, 3) = m[15];
    #else
        (*this)(0, 0) = m[0]; (*this)(0, 1) = m[4]; (*this)(0, 2) = m[8];  (*this)(0, 3) = m[12];
        (*this)(1, 0) = m[1]; (*this)(1, 1) = m[5]; (*this)(1, 2) = m[9];  (*this)(1, 3) = m[13];
        (*this)(2, 0) = m[2]; (*this)(2, 1) = m[6]; (*this)(2, 2) = m[10]; (*this)(2, 3) = m[14];
        (*this)(3, 0) = m[3]; (*this)(3, 1) = m[7]; (*this)(3, 2) = m[11]; (*this)(3, 3) = m[15];
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(
        const Vec<4, T> &x,
        const Vec<4, T> &y,
        const Vec<4, T> &z,
        const Vec<4, T> &w
        ) {
 
    #ifdef MATRIX_ROW_MAJOR
        (*this).set_row(0, x);
        (*this).set_row(1, y);
        (*this).set_row(2, z);
        (*this).set_row(3, w);
    #else
        (*this).set_col(0, x);
        (*this).set_col(1, y);
        (*this).set_col(2, z);
        (*this).set_col(3, w);
    #endif
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(const Mat<3, 3, T> &rhs) {
        (*this)(0, 0) = rhs(0, 0);  (*this)(0, 1) = rhs(0, 1);  (*this)(0, 2) = rhs(0, 2);  (*this)(0, 3) = T(0);
        (*this)(1, 0) = rhs(1, 0);  (*this)(1, 1) = rhs(1, 1);  (*this)(1, 2) = rhs(1, 2);  (*this)(1, 3) = T(0);
        (*this)(2, 0) = rhs(2, 0);  (*this)(2, 1) = rhs(2, 1);  (*this)(2, 2) = rhs(2, 2);  (*this)(2, 3) = T(0);
        (*this)(3, 0) = T(0);       (*this)(3, 1) = T(0);       (*this)(3, 2) = T(0);       (*this)(3, 3) = T(1);
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat3<T> Mat4<T>::sub() const {
        Mat3<T> mat;
        for (size_t i = 0; i < 3; i++) {
            for (size_t j = 0; j < 3; j++) {
                mat(i, j) = (*this)(i, j);
            }
        }
        return mat;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T>::Mat4(const Vec<3, T> &s, const Quat<T> &rot, const Vec<3, T> &t) {
        assert(std::abs(rot.length() - 1) < epsilon<T>());
 
        //  get rotation matrix from quaternion:
        Mat3<T> r(rot);
 
        //  incorporate scale (cheaper than a matrix multiply):
        r(0, 0) = r(0, 0) * s[0]; r(0, 1) = r(0, 1) * s[1]; r(0, 2) = r(0, 2) * s[2];
        r(1, 0) = r(1, 0) * s[0]; r(1, 1) = r(1, 1) * s[1]; r(1, 2) = r(1, 2) * s[2];
        r(2, 0) = r(2, 0) * s[0]; r(2, 1) = r(2, 1) * s[1]; r(2, 2) = r(2, 2) * s[2];
 
        //  combine rotation matrix, scale and translation:
        (*this)(0, 0) = r(0, 0);    (*this)(0, 1) = r(0, 1);    (*this)(0, 2) = r(0, 2);    (*this)(0, 3) = t[0];
        (*this)(1, 0) = r(1, 0);    (*this)(1, 1) = r(1, 1);    (*this)(1, 2) = r(1, 2);    (*this)(1, 3) = t[1];
        (*this)(2, 0) = r(2, 0);    (*this)(2, 1) = r(2, 1);    (*this)(2, 2) = r(2, 2);    (*this)(2, 3) = t[2];
        (*this)(3, 0) = T(0);       (*this)(3, 1) = T(0);       (*this)(3, 2) = T(0);       (*this)(3, 3) = T(1);
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::scale(T s) {
        return Mat4<T>(
            s, T(0), T(0), T(0),
            T(0), s, T(0), T(0),
            T(0), T(0), s, T(0),
            T(0), T(0), T(0), T(1)
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::scale(T x, T y, T z, T w) {
        return Mat4<T>(
            x,    T(0), T(0), T(0),
            T(0), y,    T(0), T(0),
            T(0), T(0), z,    T(0),
            T(0), T(0), T(0), w
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::scale(const Vec<4, T>& s) {
        return Mat4<T>(
            s.x,  T(0), T(0), T(0),
            T(0), s.y,  T(0), T(0),
            T(0), T(0), s.z,  T(0),
            T(0), T(0), T(0), s.w
            );
    }
 
    /*----------------------------------------------------------------------------*/
    
    template <typename T>
    inline Mat4<T> Mat4<T>::rotation(const Vec<3, T> &axis, T angle) {
        assert(std::abs(axis.length() - 1) < epsilon<T>());
        return Mat4<T>(Mat3<T>::rotation(axis, angle)); // gen 3x3 rotation matrix as arg to Mat4 constructor
    }
    
    /*----------------------------------------------------------------------------*/
    
    template <typename T>
    inline Mat4<T> Mat4<T>::rotation(const Vec<3, T> &axis_angle) {
        const T len = axis_angle.length();
        return Mat4<T>(Mat3<T>::rotation(axis_angle/len, len));
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::rotation(const Quat<T> &q) {
        // input must be unit quaternion
        assert(std::abs(q.length() - 1) < epsilon<T>());
        const T x = q.x;
        const T y = q.y;
        const T z = q.z;
        const T w = q.w;
        Mat4<T> m;
        m(0, 0) = 1.0 - 2.0 * (y*y + z*z);    m(0, 1) = 2.0 * (x*y - w*z);          m(0, 2) = 2.0 * (x*z + w*y);          m(0, 3) = 0.0;
        m(1, 0) = 2.0 * (x*y + w*z);          m(1, 1) = 1.0 - 2.0 * (x*x + z*z);    m(1, 2) = 2.0 * (y*z - w*x);          m(1, 3) = 0.0;
        m(2, 0) = 2.0 * (x*z - w*y);          m(2, 1) = 2.0 * (y*z + w*x);          m(2, 2) = 1.0 - 2.0 * (x*x + y*y);    m(2, 3) = 0.0;
        m(3, 0) = 0.0;                        m(3, 1) = 0.0;                        m(3, 2) = 0.0;                        m(3, 3) = 1.0;
        return m;
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::rotation(T x, T y, T z, int order) {
        // The code is not optimized. The final matrix can be directly derived.
        // http://www.songho.ca/opengl/gl_anglestoaxes.html
        // http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/
        Mat4<T> rx; // Rotation about X-axis (Pitch)
        rx(0, 0) = T(1);         rx(0, 1) = T(0);           rx(0, 2) = T(0);            rx(0, 3) = T(0);
        rx(1, 0) = T(0);         rx(1, 1) = std::cos(x);    rx(1, 2) = -std::sin(x);    rx(1, 3) = T(0);
        rx(2, 0) = T(0);         rx(2, 1) = std::sin(x);    rx(2, 2) = std::cos(x);     rx(2, 3) = T(0);
        rx(3, 0) = T(0);         rx(3, 1) = T(0);           rx(3, 2) = T(0);            rx(3, 3) = T(1);
 
        Mat4<T> ry; // Rotation about Y-axis (Yaw, Heading)
        ry(0, 0) = std::cos(y);  ry(0, 1) = T(0);           ry(0, 2) = std::sin(y);     ry(0, 3) = T(0);
        ry(1, 0) = T(0);         ry(1, 1) = T(1);           ry(1, 2) = T(0);            ry(1, 3) = T(0);
        ry(2, 0) = -std::sin(y); ry(2, 1) = T(0);           ry(2, 2) = std::cos(y);     ry(2, 3) = T(0);
        ry(3, 0) = T(0);         ry(3, 1) = T(0);           ry(3, 2) = T(0);            ry(3, 3) = T(1);
 
        Mat4<T> rz; // Rotation about Z-axis (Roll)
        rz(0, 0) = std::cos(z);  rz(0, 1) = -std::sin(z);   rz(0, 2) = T(0);            rz(0, 3) = T(0);
        rz(1, 0) = std::sin(z);  rz(1, 1) = std::cos(z);    rz(1, 2) = T(0);            rz(1, 3) = T(0);
        rz(2, 0) = T(0);         rz(2, 1) = T(0);           rz(2, 2) = T(1);            rz(2, 3) = T(0);
        rz(3, 0) = T(0);         rz(3, 1) = T(0);           rz(3, 2) = T(0);            rz(3, 3) = T(1);
 
        switch (order) {
            case 123:   return rz * ry * rx;
            case 132:   return ry * rz * rx;
            case 213:   return rz * rx * ry;
            case 231:   return rx * rz * ry;
            case 312:   return ry * rx * rz;
            case 321:   return rx * ry * rz;
            default:
                LOG(ERROR) << "invalid rotation order";
                return rx * rz * ry;
        }
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::translation(const Vec<3, T> &t) {
        return Mat4<T>(
            T(1), T(0), T(0), t[0],
            T(0), T(1), T(0), t[1],
            T(0), T(0), T(1), t[2],
            T(0), T(0), T(0), T(1)
            );
    }
 
    /*----------------------------------------------------------------------------*/
    template <typename T>
    inline Mat4<T> Mat4<T>::translation(T x, T y, T z) {
        return Mat4<T>(
            T(1), T(0), T(0), x,
            T(0), T(1), T(0), y,
            T(0), T(0), T(1), z,
            T(0), T(0), T(0), T(1)
            );
    }
 
}
 
#endif // EASY3D_CORE_MAT_H