Matrix3.h

//
//  Copyright (C) 2017, Alexander Stukowski and Paul Erhart
//
//  This file is part of atomicrex.
//
//  Atomicrex is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 3 of the License, or
//  (at your option) any later version.
//
//  Atomicrex 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/>.
//

#pragma once

#include <array>
#include <ostream>
#include <cassert>

#include "LinAlg.h"
#include "Point3.h"
#include "Vector3.h"

namespace atomicrex {

template<typename T>
class Matrix_3 : public std::array<Vector_3<T>,3>
{
public:

    struct Zero {};

    struct Identity {};

    typedef T element_type;

    typedef Vector_3<T> column_type;

    using typename std::array<Vector_3<T>, 3>::size_type;
    using typename std::array<Vector_3<T>, 3>::difference_type;
    using typename std::array<Vector_3<T>, 3>::value_type;
    using typename std::array<Vector_3<T>, 3>::iterator;
    using typename std::array<Vector_3<T>, 3>::const_iterator;

public:

    Matrix_3() {}

    constexpr Matrix_3(T m11, T m12, T m13,
                       T m21, T m22, T m23,
                       T m31, T m32, T m33)
        : std::array<Vector_3<T>,3>{{Vector_3<T>(m11,m21,m31),
                                     Vector_3<T>(m12,m22,m32),
                                     Vector_3<T>(m13,m23,m33)}} {}

    constexpr Matrix_3(const column_type& c1, const column_type& c2, const column_type& c3)
        : std::array<Vector_3<T>,3>{{c1, c2, c3}} {}

    constexpr Matrix_3(Zero)
        : std::array<Vector_3<T>,3>{{typename Vector_3<T>::Zero(), typename Vector_3<T>::Zero(), typename Vector_3<T>::Zero()}} {}

    constexpr Matrix_3(Identity)
        : std::array<Vector_3<T>,3>{{Vector_3<T>(T(1),T(0),T(0)),
                                     Vector_3<T>(T(0),T(1),T(0)),
                                     Vector_3<T>(T(0),T(0),T(1))}} {}

    template<typename U>
    constexpr explicit operator Matrix_3<U>() const {
        return Matrix_3<U>(
                static_cast<U>((*this)(0,0)), static_cast<U>((*this)(0,1)), static_cast<U>((*this)(0,2)),
                static_cast<U>((*this)(1,0)), static_cast<U>((*this)(1,1)), static_cast<U>((*this)(1,2)),
                static_cast<U>((*this)(2,0)), static_cast<U>((*this)(2,1)), static_cast<U>((*this)(2,2)));
    }

    static constexpr size_type row_count() { return 3; }

    static constexpr size_type col_count() { return 3; }

    constexpr inline T operator()(size_type row, size_type col) const {
        return (*this)[col][row];
    }

    inline T& operator()(size_type row, size_type col) {
        return (*this)[col][row];
    }

    constexpr const column_type& column(size_type col) const {
        return (*this)[col];
    }

    column_type& column(size_type col) {
        return (*this)[col];
    }

    constexpr Vector_3<T> row(size_type row) const {
        return { (*this)[0][row], (*this)[1][row], (*this)[2][row] };
    }

    const element_type* elements() const {
        return column(0).data();
    }

    element_type* elements() {
        return column(0).data();
    }

    void setZero() {
        (*this)[0].setZero();
        (*this)[1].setZero();
        (*this)[2].setZero();
    }

    Matrix_3& operator=(Zero) {
        setZero();
        return *this;
    }

    void setIdentity() {
        (*this)[0] = Vector_3<T>(1,0,0);
        (*this)[1] = Vector_3<T>(0,1,0);
        (*this)[2] = Vector_3<T>(0,0,1);
    }

    Matrix_3& operator=(Identity) {
        setIdentity();
        return *this;
    }


    constexpr bool operator==(const Matrix_3& b) const {
        return (b[0] == (*this)[0]) && (b[1] == (*this)[1]) && (b[2] == (*this)[2]);
    }

    constexpr bool operator!=(const Matrix_3& b) const {
        return !(*this == b);
    }

    inline bool equals(const Matrix_3& m, T tolerance = T(FLOATTYPE_EPSILON)) const {
        for(size_type i = 0; i < col_count(); i++)
            if(!column(i).equals(m.column(i), tolerance)) return false;
        return true;
    }

    inline bool isZero(T tolerance = T(FLOATTYPE_EPSILON)) const {
        for(size_type i = 0; i < col_count(); i++)
            if(!column(i).isZero(tolerance)) return false;
        return true;
    }


    Matrix_3 inverse() const {
        T det = determinant();
        assert(det != T(0));
        return Matrix_3(((*this)[1][1]*(*this)[2][2] - (*this)[1][2]*(*this)[2][1])/det,
                        ((*this)[2][0]*(*this)[1][2] - (*this)[1][0]*(*this)[2][2])/det,
                        ((*this)[1][0]*(*this)[2][1] - (*this)[1][1]*(*this)[2][0])/det,
                        ((*this)[2][1]*(*this)[0][2] - (*this)[0][1]*(*this)[2][2])/det,
                        ((*this)[0][0]*(*this)[2][2] - (*this)[2][0]*(*this)[0][2])/det,
                        ((*this)[0][1]*(*this)[2][0] - (*this)[0][0]*(*this)[2][1])/det,
                        ((*this)[0][1]*(*this)[1][2] - (*this)[1][1]*(*this)[0][2])/det,
                        ((*this)[0][2]*(*this)[1][0] - (*this)[0][0]*(*this)[1][2])/det,
                        ((*this)[0][0]*(*this)[1][1] - (*this)[1][0]*(*this)[0][1])/det);
    }

    bool inverse(Matrix_3& result, T epsilon = T(FLOATTYPE_EPSILON)) const {
        T det = determinant();
        if(std::abs(det) <= epsilon) return false;
        result = Matrix_3(((*this)[1][1]*(*this)[2][2] - (*this)[1][2]*(*this)[2][1])/det,
                        ((*this)[2][0]*(*this)[1][2] - (*this)[1][0]*(*this)[2][2])/det,
                        ((*this)[1][0]*(*this)[2][1] - (*this)[1][1]*(*this)[2][0])/det,
                        ((*this)[2][1]*(*this)[0][2] - (*this)[0][1]*(*this)[2][2])/det,
                        ((*this)[0][0]*(*this)[2][2] - (*this)[2][0]*(*this)[0][2])/det,
                        ((*this)[0][1]*(*this)[2][0] - (*this)[0][0]*(*this)[2][1])/det,
                        ((*this)[0][1]*(*this)[1][2] - (*this)[1][1]*(*this)[0][2])/det,
                        ((*this)[0][2]*(*this)[1][0] - (*this)[0][0]*(*this)[1][2])/det,
                        ((*this)[0][0]*(*this)[1][1] - (*this)[1][0]*(*this)[0][1])/det);
        return true;
    }

    constexpr inline T determinant() const {
        return(((*this)[0][0]*(*this)[1][1] - (*this)[0][1]*(*this)[1][0])*((*this)[2][2])
              -((*this)[0][0]*(*this)[1][2] - (*this)[0][2]*(*this)[1][0])*((*this)[2][1])
              +((*this)[0][1]*(*this)[1][2] - (*this)[0][2]*(*this)[1][1])*((*this)[2][0]));
    }

    constexpr Matrix_3 transposed() const {
        return Matrix_3((*this)[0][0], (*this)[0][1], (*this)[0][2],
                        (*this)[1][0], (*this)[1][1], (*this)[1][2],
                        (*this)[2][0], (*this)[2][1], (*this)[2][2]);
    }

    inline constexpr T prodrow(const Point_3<T>& p, typename Point_3<T>::size_type index) const {
        return (*this)[0][index] * p[0] + (*this)[1][index] * p[1] + (*this)[2][index] * p[2];
    }

    inline constexpr T prodrow(const Vector_3<T>& v, typename Vector_3<T>::size_type index) const {
        return (*this)[0][index] * v[0] + (*this)[1][index] * v[1] + (*this)[2][index] * v[2];
    }

    constexpr bool isOrthogonalMatrix(T epsilon = T(FLOATTYPE_EPSILON)) const {
        return
            (std::abs((*this)[0][0]*(*this)[1][0] + (*this)[0][1]*(*this)[1][1] + (*this)[0][2]*(*this)[1][2]) <= epsilon) &&
            (std::abs((*this)[0][0]*(*this)[2][0] + (*this)[0][1]*(*this)[2][1] + (*this)[0][2]*(*this)[2][2]) <= epsilon) &&
            (std::abs((*this)[1][0]*(*this)[2][0] + (*this)[1][1]*(*this)[2][1] + (*this)[1][2]*(*this)[2][2]) <= epsilon) &&
            (std::abs((*this)[0][0]*(*this)[0][0] + (*this)[0][1]*(*this)[0][1] + (*this)[0][2]*(*this)[0][2] - T(1)) <= epsilon) &&
            (std::abs((*this)[1][0]*(*this)[1][0] + (*this)[1][1]*(*this)[1][1] + (*this)[1][2]*(*this)[1][2] - T(1)) <= epsilon) &&
            (std::abs((*this)[2][0]*(*this)[2][0] + (*this)[2][1]*(*this)[2][1] + (*this)[2][2]*(*this)[2][2] - T(1)) <= epsilon);
    }

    constexpr bool isRotationMatrix(T epsilon = T(FLOATTYPE_EPSILON)) const {
        return isOrthogonalMatrix(epsilon) && (std::abs(determinant() - T(1)) <= epsilon);
    }

    void orthonormalize() {

        // Compute q0.
        (*this)[0].normalize();

        // Compute q1.
        T dot0 = (*this)[0].dot((*this)[1]);
        (*this)[1][0] -= dot0 * (*this)[0][0];
        (*this)[1][1] -= dot0 * (*this)[0][1];
        (*this)[1][2] -= dot0 * (*this)[0][2];
        (*this)[1].normalize();

        // compute q2
        dot0 = (*this)[0].dot((*this)[2]);
        T dot1 = (*this)[1].dot((*this)[2]);
        (*this)[2][0] -= dot0*(*this)[0][0] + dot1*(*this)[1][0];
        (*this)[2][1] -= dot0*(*this)[0][1] + dot1*(*this)[1][1];
        (*this)[2][2] -= dot0*(*this)[0][2] + dot1*(*this)[1][2];
        (*this)[2].normalize();
    }

};

template<typename T>
constexpr inline Vector_3<T> operator*(const Matrix_3<T>& m, const Vector_3<T>& v)
{
    return { m(0,0)*v[0] + m(0,1)*v[1] + m(0,2)*v[2],
             m(1,0)*v[0] + m(1,1)*v[1] + m(1,2)*v[2],
             m(2,0)*v[0] + m(2,1)*v[1] + m(2,2)*v[2] };
}

template<typename T>
constexpr inline Point_3<T> operator*(const Matrix_3<T>& m, const Point_3<T>& p)
{
    return { m(0,0)*p[0] + m(0,1)*p[1] + m(0,2)*p[2],
             m(1,0)*p[0] + m(1,1)*p[1] + m(1,2)*p[2],
             m(2,0)*p[0] + m(2,1)*p[1] + m(2,2)*p[2] };
}

template<typename T>
constexpr inline Matrix_3<T> operator*(const Matrix_3<T>& a, const Matrix_3<T>& b)
{
#if 1
    return Matrix_3<T>(
            a(0,0)*b(0,0) + a(0,1)*b(1,0) + a(0,2)*b(2,0),
            a(0,0)*b(0,1) + a(0,1)*b(1,1) + a(0,2)*b(2,1),
            a(0,0)*b(0,2) + a(0,1)*b(1,2) + a(0,2)*b(2,2),

            a(1,0)*b(0,0) + a(1,1)*b(1,0) + a(1,2)*b(2,0),
            a(1,0)*b(0,1) + a(1,1)*b(1,1) + a(1,2)*b(2,1),
            a(1,0)*b(0,2) + a(1,1)*b(1,2) + a(1,2)*b(2,2),

            a(2,0)*b(0,0) + a(2,1)*b(1,0) + a(2,2)*b(2,0),
            a(2,0)*b(0,1) + a(2,1)*b(1,1) + a(2,2)*b(2,1),
            a(2,0)*b(0,2) + a(2,1)*b(1,2) + a(2,2)*b(2,2)
    );
#else
    Matrix_3<T> m;
    for(typename Matrix_3<T>::size_type col = 0; col < 3; col++) {
        for(typename Matrix_3<T>::size_type row = 0; row < 3; row++) {
            T v{0};
            for(typename Matrix_3<T>::size_type k = 0; k < 3; k++)
                v += a(row, k) * b(k, col);
            m(row, col) = v;
        }
    }
    return m;
#endif
}


template<typename T>
inline Matrix_3<T> operator-(const Matrix_3<T>& a, const Matrix_3<T>& b)
{
    Matrix_3<T> m;
    for(typename Matrix_3<T>::size_type col = 0; col < 3; col++) {
        for(typename Matrix_3<T>::size_type row = 0; row < 3; row++) {
            m(row, col) = a(row, col) - b(row, col);
        }
    }
    return m;
}



template<typename T>
constexpr inline Matrix_3<T> operator*(const Matrix_3<T>& a, T s)
{
#if 1
    return Matrix_3<T>(
            a(0,0)*s, a(0,1)*s, a(0,2)*s,
            a(1,0)*s, a(1,1)*s, a(1,2)*s,
            a(2,0)*s, a(2,1)*s, a(2,2)*s
    );
#else
    Matrix_3<T> m;
    for(typename Matrix_3<T>::size_type i = 0; i < 3; i++) {
        for(typename Matrix_3<T>::size_type j = 0; j < 3; j++) {
            m(i, j) = a(i, j) * s;
        }
    }
    return m;
#endif
}

template<typename T>
constexpr inline Matrix_3<T> operator*(T s, const Matrix_3<T>& a) {
    return a * s;
}

template<typename T>
inline std::ostream& operator<<(std::ostream &os, const Matrix_3<T>& m) {
    for(typename Matrix_3<T>::size_type row = 0; row < m.row_count(); row++)
        os << m.row(row) << std::endl;
    return os;
}

using Matrix3 = Matrix_3<FloatType>;

} // End of namespace