paradiso/src/lib/matrix.hpp

#ifndef PW_CORE_MATRIX_HPP
#define PW_CORE_MATRIX_HPP

#include "matrixbase.hpp"

#include <numeric>

namespace paradiso {

template <std::size_t R, std::size_t C, typename Scalar, bool RowMajor = false>
struct Matrix : MatrixBase<Scalar, Matrix<R, C, Scalar>> {

    Scalar data[R * C]{}; 

    static constexpr std::size_t rows{R};
    static constexpr std::size_t cols{C};
    static constexpr std::size_t coefficients{R * C};

    using col_type = Matrix<R, 1, Scalar>;
    using row_type = Matrix<1, C, Scalar>;
    using transpose_type = Matrix<C, R, Scalar>;

    template <typename... Arguments>
    static constexpr Matrix<R, C, Scalar> make(Arguments... values) noexcept {
        static_assert(sizeof...(Arguments) == R * C,
                      "Incorrect number of arguments");
        return {.data = {values...}};
    }

    constexpr size_t offset(size_t r, size_t c) const {
        return (RowMajor) ? r * C + c : c * R + r;
    }

    constexpr Scalar& operator()(std::size_t r, std::size_t c) {
        return data[offset(r, c)];
    }

    constexpr const Scalar& operator()(std::size_t r, std::size_t c) const {
        return data[offset(r, c)];
    }

    constexpr const Scalar* ptr() const { return &data[0]; }

    //! set identity
    constexpr Matrix& set_identity() {
        for (std::size_t r = 0; r < rows; r++)
            for (std::size_t c = 0; c < cols; c++)
                (*this)(r, c) = (c == r) ? Scalar(1) : Scalar(0);
        return *this;
    }

    constexpr Matrix& set_uniform(const Scalar& v) {
        std::fill(std::begin(data), std::end(data), v);
        return *this;
    }

    template <std::size_t Rs, std::size_t Cs, bool RowMajorSlice = RowMajor>
    auto slice(std::size_t r, std::size_t c) const {
        Matrix<Rs, Cs, Scalar, RowMajorSlice> s;
        for (std::size_t ri = 0; ri < Rs; ri++)
            for (std::size_t ci = 0; ci < Cs; ci++)
                s(ri, ci) = (*this)(ri + r, ci + c);
        return s;
    }

    template <std::size_t Rs, std::size_t Cs, bool RowMajorSlice = RowMajor>
    Matrix& set_slice(const Matrix<Rs, Cs, Scalar, RowMajorSlice>& s,
                      std::size_t r, std::size_t c) {
        for (std::size_t ri = 0; ri < Rs; ri++)
            for (std::size_t ci = 0; ci < Cs; ci++)
                (*this)(ri + r, ci + c) = s(ri, ci);
        return *this;
    }

    template <std::size_t Rs, std::size_t Cs, bool RowMajorSlice = RowMajor>
    auto minor(std::size_t r0, std::size_t c0) const {
        Matrix<Rs, Cs, Scalar, RowMajorSlice> m;
        size_t r = 0;
        for (size_t ri = 0; ri < R; ri++) {
            size_t c = 0;
            if (ri == r0)
                continue;
            for (size_t ci = 0; ci < C; ci++) {
                if (ci == c0)
                    continue;
                m(r, c) = (*this)(ri, ci);
                c++;
            }
            r++;
        }
        return m;
    }

    Scalar determinant() const {
        Scalar det(0);
        for (size_t c = 0; c < C; c++)
            det += ((c % 2 == 0) ? (*this)(0, c) : -(*this)(0, c)) *
                   this->minor<R - 1, C - 1, RowMajor>(0, c).determinant();
        return det;
    }

    auto transposed() const {
        transpose_type res;
        for (size_t r = rows; r-- > 0;)
            for (size_t c = cols; c-- > 0;)
                res(c, r) = (*this)(r, c);
        return res;
    }

    auto inverse() const {
        Scalar invDet = Scalar(1) / this->determinant();
        Matrix<R, C, Scalar, RowMajor> inv;
        for (int j = 0; j < C; j++)
            for (int i = 0; i < R; i++) {
                const Scalar minorDet =
                    this->minor<R - 1, C - 1, RowMajor>(j, i).determinant();
                const Scalar coFactor =
                    ((i + j) % 2 == 1) ? -minorDet : minorDet;
                inv(i, j) = invDet * coFactor;
            }
        return inv;
    }

    inline bool row_major() const { return RowMajor; }

    inline bool square() const { return R == C; }

    inline const Matrix operator+(const Matrix& other) const {
        Matrix res(*this);
        for (size_t r = 0; r < R; r++)
            for (size_t c = 0; c < C; c++)
                res(r, c) += other(r, c);
        return res;
    }

    inline const Matrix operator-(const Matrix& other) const {
        Matrix res(*this);
        for (size_t r = 0; r < R; r++)
            for (size_t c = 0; c < C; c++)
                res(r, c) -= other(r, c);
        return res;
    }

    auto row(size_t row_) const {
        row_type r;
        for (size_t i = 0; i < cols; i++)
            r[i] = (*this)(row_, i);
        return r;
    }

    auto column(size_t col_) const {
        col_type c;
        for (size_t i = 0; i < rows; i++)
            c[i] = (*this)(i, col_);
        return c;
    }

    static constexpr auto identity() {
        Matrix res;
        for (std::size_t r = 0; r < rows; r++)
            for (std::size_t c = 0; c < cols; c++)
                res(r, c) = (c == r) ? Scalar(1) : Scalar(0);
        return res;
    }
};

template <> inline float Matrix<1, 1, float>::determinant() const {
    return (*this)(0, 0);
}

template <> inline double Matrix<1, 1, double>::determinant() const {
    return (*this)(0, 0);
}

template <typename Scalar, std::size_t R, std::size_t Ca, std::size_t Cb>
auto operator*(const Matrix<R, Ca, Scalar>& A, const Matrix<R, Cb, Scalar>& B) {
    Matrix<R, Cb, Scalar> result;
    result.zero(); // zero the output
    for (size_t r = 0; r < R; r++)
        for (size_t c = 0; c < Cb; c++)
            for (size_t iI = 0; iI < R; iI++)
                result(r, c) += A(r, iI) * B(iI, c); // inner product
    return result;
}
} // namespace paradiso

#endif