fmat.h

#pragma once
 
#include "fvec.h"
#include <cassert>
#include <initializer_list>
 
namespace cgv {
namespace math {
 
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
class fmat : public fvec<T,N*M>
{
public:
    typedef fvec<T,N*M> base_type;
    typedef fmat<T,N,M> this_type;
    fmat() {}
    fmat(std::initializer_list<T> components) : fvec<T,N*M>((cgv::type::uint32_type)components.size(), components.begin()) {}
    fmat(std::initializer_list<fvec<T,N>> cols) : fvec<T,N*M>(N*(cgv::type::uint32_type)cols.size(), (T*)cols.begin()) {}
    fmat(const T& c) : base_type(c) {}
    fmat(cgv::type::uint32_type n, cgv::type::uint32_type m, const T* a, bool column_major = true) {
        cgv::type::uint32_type j;
        for (j = 0; j < std::min(m, M); ++j) {
            cgv::type::uint32_type i;
            for (i = 0; i < std::min(n, N); ++i)
                (*this)(i, j) = a[column_major ? j * n + i : i * m + j];
            for (; i < N; ++i)
                (*this)(i, j) = (i == j) ? T(1) : T(0);
        }
        for (; j < M; ++j)
            for (cgv::type::uint32_type i = 0; i < N; ++i)
                (*this)(i, j) = (i == j) ? T(1) : T(0);
    }
    template <typename S>
    fmat(cgv::type::uint32_type n, cgv::type::uint32_type m, const S* a, bool column_major = true) {
        for (cgv::type::uint32_type j = 0; j < std::min(m, M); ++j)
            for (cgv::type::uint32_type i = 0; i < std::min(n, N); ++i)
                (*this)(i, j) = (T)a[column_major ? j * n + i : i * m + j];
    }
    template <typename S>
    fmat(const fmat<S,N,M>& m) : base_type(m) {}
    template <typename T1, typename T2>
    fmat(const fvec<T1,N>& v, const fvec<T2,M>& w) { 
        for(unsigned i = 0; i < N; i++)
            for(unsigned j = 0; j < M; j++)
                (*this)(i,j) = (T)(v(i)*w(j)); 
    }
    constexpr static unsigned nrows() { return N; }
    constexpr static unsigned ncols() { return M; }
    template <typename S> 
    fmat<T,N,M>& operator = (const fmat<S,N,M>& m) {
        base_type::operator = (m);
        return *this;
    }
    this_type& operator  = (const T& s) { 
        fill (s);
        return *this; 
    }
    bool is_square() const { return N == M; }
    T& operator () (unsigned i, unsigned j) {
        assert(i < N && j < M);
        return base_type::v[j*N+i];
    }
    const T& operator () (unsigned i, unsigned j) const {
        assert(i < N && j < M);
        return base_type::v[j*N+i];
    }
    //in place scalar multiplication
    this_type& operator *= (const T& s) { base_type::operator *= (s); return *this; } 
    this_type operator * (const T& s) const { this_type r=(*this); r*=(T)s; return r; }
    fmat<T,N,M>& operator /= (const T& s) { base_type::operator /= (s); return *this; }
    const fmat<T,N,M> operator / (const T& s) const { this_type r=(*this); r/=(T)s; return r; }
    fmat<T,N,M>& operator += (const T& s) { base_type::operator += (s); return *this; }
    const fmat<T,N,M> operator + (const T& s) { this_type r=(*this); r+=(T)s; return r; }
    fmat<T,N,M>& operator -= (const T& s) { base_type::operator -= (s); return *this; }
    const fmat<T,N,M> operator - (const T& s) { this_type r=(*this); r-=(T)s; return r; }
    const fmat<T,N,M> operator-() const { return (*this)*(-1); }
    template <typename S> 
    fmat<T,N,M>& operator += (const fmat<S,N,M>& m) { base_type::operator += (m); return *this; } 
    template <typename S> 
    fmat<T,N,M>& operator -= (const fmat<S,N,M>& m) { base_type::operator -= (m); return *this; } 
    template <typename S>
    const fmat<T,N,M> operator+(const fmat<S,N,M> m2)const { fmat<T,N,M> r=(*this); r += m2; return r; }
    template <typename S>
    const fmat<T,N,M> operator-(const fmat<S,N,M> m2)const { fmat<T,N,M> r=(*this); r -= m2; return r; }
    template <typename S>
    const fmat<T,N,M> operator*=(const fmat<S,N,N>& m2) 
    {
        assert(N == M);
        fmat<T,N,N> r(T(0));    
        for(unsigned i = 0; i < N; i++)
            for(unsigned j = 0; j < N;j++)
                for(unsigned k = 0; k < N; k++)
                    r(i,j) += operator()(i,k) * (T)(m2(k,j)); 
        (*this)=r;
        return *this;
    }
    template <typename S, cgv::type::uint32_type L>
    const fmat<T,N,L> operator*(const fmat<S,M,L>& m2) const
    {
        fmat<T,N,L> r; r.zeros();   
        for(unsigned i = 0; i < N; i++)
            for(unsigned j = 0; j < L;j++)
                for(unsigned k = 0; k < M; k++)
                    r(i,j) += operator()(i,k) * (T)(m2(k,j)); 
        return r;
    }
    template <typename S>
    const fmat<T,N,N> mul_h (const fmat<S,N-1,N-1>& m2) const
    {
        static_assert(N == M, "Number of rows and columns must be equal");
        static const auto vzero = fvec<T, N-1>(0);
        fvec<T,N> rows[N]; // extracting a row takes linear time so we only want to do it once for each row
        for (unsigned i=0; i<N; i++)
            rows[i] = row(i);
        fmat<T,N,N> r;
        // (1) multiply with implied N x (N-1) matrix that is assumed to have an all-zero last row
        for (unsigned j=0; j<N-1; j++)
            for (unsigned i=0; i<N; i++)
                r(i,j) = dot_dir(rows[i], m2.col(j));
        // (2) assume homogeneous zero position vector in last column of 2nd operand for calculating last result column
        for (unsigned i=0; i<N; i++)
            r(i,N-1) = dot_pos(rows[i], vzero);
        return r;
    }
 
    template <typename S>
    const fvec<S,N> operator * (const fvec<S,M>& v) const {
        fvec<S,N> r;
        for(unsigned i = 0; i < N; i++)
            r(i) = dot(row(i),v);
        return r;
    }
    template <typename S>
    const fvec<S,N> mul_pos (const fvec<S,M-1>& v) const {
        fvec<S,N> r;
        for(unsigned i = 0; i < N; i++)
            r(i) = dot_pos(row(i), v);
        return r;
    }
    template <typename S>
    const fvec<S,N> mul_dir (const fvec<S,M-1>& v) const {
        fvec<S,N> r;
        for(unsigned i = 0; i < N; i++)
            r(i) = dot_dir(row(i), v);
        return r;
    }
 
    fvec<T,M> row(unsigned i) const {
        fvec<T,M> r;
        for(unsigned j = 0; j < M; j++) 
            r(j)=operator()(i,j);
        return r;
    }
    void set_row(unsigned i,const fvec<T,M>& v) {
        for(unsigned j = 0; j < M;j++) 
            operator()(i,j)=v(j);       
    }
    fvec<T,N>& col(unsigned j) {    
        return reinterpret_cast<fvec<T,N>*>(this)[j];
    }
    const fvec<T,N>& col(unsigned j) const {    
        return reinterpret_cast<const fvec<T,N>*>(this)[j];
    }
    void set_col(unsigned j,const fvec<T,N>& v) {
        reinterpret_cast<fvec<T,N>*>(this)[j] = v;
    }
 
    T trace() const {
        assert(N == M);
        T t = 0;
        for(unsigned i = 0; i < N;i++)
            t+=operator()(i,i);
        return t;
    }
 
    void transpose() {
        assert(N == M);
        for(unsigned i = 1; i < N; i++)
            for(unsigned j = 0; j < i; j++)
                std::swap(operator()(i,j), operator()(j,i));
    }
    T frobenius_norm() const { return base_type::length(); }
    void identity()
    {
        base_type::zeros();
        for(unsigned i = 0; i < M && i < N; ++i)
            operator()(i,i)=1;
    }
};
 
#define CGV_MATH_FMAT_DECLARED
 
// For N > 4, convert to mat first and use inv function from inv.h
// Example:
//   mat<T> M(N, N, &m(0, 0));
//   return fmat<T, N, N>(N, N, &(inv(M)(0, 0)));
template <typename T, cgv::type::uint32_type N>
fmat<T, N, N> inverse(const fmat<T, N, N>& m) = delete;
 
template <typename T>
fmat<T, 2, 2> inverse(const fmat<T, 2, 2>& m)
{
    fmat<T, 2, 2> im;
    T t4 = T(1) / (-m(0, 0) * m(1, 1) + m(0, 1) * m(1, 0));
    im(0, 0) = -m(1, 1) * t4;
    im(1, 0) = +m(1, 0) * t4;
    im(0, 1) = +m(0, 1) * t4;
    im(1, 1) = -m(0, 0) * t4;
 
    return im;
}
 
template <typename T>
fmat<T, 3, 3> inverse(const fmat<T, 3, 3>& m)
{
    T inv_det = (T)1 / (
        +m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1))
        - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0))
        + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)));
 
    fmat<T, 3, 3> im;
    im(0, 0) = +(m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) * inv_det;
    im(0, 1) = -(m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1)) * inv_det;
    im(0, 2) = +(m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1)) * inv_det;
    im(1, 0) = -(m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) * inv_det;
    im(1, 1) = +(m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0)) * inv_det;
    im(1, 2) = -(m(0, 0) * m(1, 2) - m(0, 2) * m(1, 0)) * inv_det;
    im(2, 0) = +(m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0)) * inv_det;
    im(2, 1) = -(m(0, 0) * m(2, 1) - m(0, 1) * m(2, 0)) * inv_det;
    im(2, 2) = +(m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0)) * inv_det;
    return im;
}
 
template <typename T>
fmat<T, 4, 4> inverse(const fmat<T, 4, 4>& m)
{
    T coef00 = m(2, 2) * m(3, 3) - m(2, 3) * m(3, 2);
    T coef02 = m(2, 1) * m(3, 3) - m(2, 3) * m(3, 1);
    T coef03 = m(2, 1) * m(3, 2) - m(2, 2) * m(3, 1);
 
    T coef04 = m(1, 2) * m(3, 3) - m(1, 3) * m(3, 2);
    T coef06 = m(1, 1) * m(3, 3) - m(1, 3) * m(3, 1);
    T coef07 = m(1, 1) * m(3, 2) - m(1, 2) * m(3, 1);
 
    T coef08 = m(1, 2) * m(2, 3) - m(1, 3) * m(2, 2);
    T coef10 = m(1, 1) * m(2, 3) - m(1, 3) * m(2, 1);
    T coef11 = m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1);
 
    T coef12 = m(0, 2) * m(3, 3) - m(0, 3) * m(3, 2);
    T coef14 = m(0, 1) * m(3, 3) - m(0, 3) * m(3, 1);
    T coef15 = m(0, 1) * m(3, 2) - m(0, 2) * m(3, 1);
 
    T coef16 = m(0, 2) * m(2, 3) - m(0, 3) * m(2, 2);
    T coef18 = m(0, 1) * m(2, 3) - m(0, 3) * m(2, 1);
    T coef19 = m(0, 1) * m(2, 2) - m(0, 2) * m(2, 1);
 
    T coef20 = m(0, 2) * m(1, 3) - m(0, 3) * m(1, 2);
    T coef22 = m(0, 1) * m(1, 3) - m(0, 3) * m(1, 1);
    T coef23 = m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1);
 
    fvec<T, 4> fac0(coef00, coef00, coef02, coef03);
    fvec<T, 4> fac1(coef04, coef04, coef06, coef07);
    fvec<T, 4> fac2(coef08, coef08, coef10, coef11);
    fvec<T, 4> fac3(coef12, coef12, coef14, coef15);
    fvec<T, 4> fac4(coef16, coef16, coef18, coef19);
    fvec<T, 4> fac5(coef20, coef20, coef22, coef23);
 
    fvec<T, 4> vec0(m(0, 1), m(0, 0), m(0, 0), m(0, 0));
    fvec<T, 4> vec1(m(1, 1), m(1, 0), m(1, 0), m(1, 0));
    fvec<T, 4> vec2(m(2, 1), m(2, 0), m(2, 0), m(2, 0));
    fvec<T, 4> vec3(m(3, 1), m(3, 0), m(3, 0), m(3, 0));
 
    fvec<T, 4> inv0(vec1 * fac0 - vec2 * fac1 + vec3 * fac2);
    fvec<T, 4> inv1(vec0 * fac0 - vec2 * fac3 + vec3 * fac4);
    fvec<T, 4> inv2(vec0 * fac1 - vec1 * fac3 + vec3 * fac5);
    fvec<T, 4> inv3(vec0 * fac2 - vec1 * fac4 + vec2 * fac5);
 
    fvec<T, 4> sign_a(+(T)1, -(T)1, +(T)1, -(T)1);
    fvec<T, 4> sign_b(-(T)1, +(T)1, -(T)1, +(T)1);
 
    fmat<T, 4, 4> im;
    im.set_col(0, inv0 * sign_a);
    im.set_col(1, inv1 * sign_b);
    im.set_col(2, inv2 * sign_a);
    im.set_col(3, inv3 * sign_b);
    fvec<T, 4> row0(im(0, 0), im(1, 0), im(2, 0), im(3, 0));
    fvec<T, 4> dot0(m.row(0) * row0);
    T dot1 = (dot0[0] + dot0[1]) + (dot0[2] + dot0[3]);
    T inv_det = (T)1 / dot1;
    return im * inv_det;
}
 
template <typename T, cgv::type::uint32_type N>
fmat<T,N,N> transpose(const fmat<T,N,N>& m)
{
    fmat<T,N,N> m_t(m);
    m_t.transpose();
    return m_t; 
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
fmat<T,N,M> operator * (const T& s, const fmat<T,N,M>& m)
{ 
    return m*s; 
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
fvec<T, M> operator * (const fvec<T, N>& v_row, const fmat<T, N, M>& m)
{
    fvec<T, M> r_row;
    for (unsigned i = 0; i < M; i++)
        r_row(i) = dot(m.col(i), v_row);
    return r_row;
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
std::ostream& operator<<(std::ostream& out, const fmat<T,N,M>& m)
{
    for (unsigned i=0;i<N;++i) {
        for(unsigned j =0;j < M-1;++j)
            out << m(i,j) << " ";
        out << m(i,M-1);
        if(i != N-1)
            out <<"\n";
    }
    return out;
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
std::istream& operator>>(std::istream& in, fmat<T,N,M>& m)
{
    for (unsigned i=0;i<m.nrows();++i)
        for(unsigned j =0;j < m.ncols();++j)
            in >> m(i,j);
    return in;
}
 
template <typename T, cgv::type::uint32_type N, typename S, cgv::type::uint32_type M>
fmat<T, N, M> dyad(const fvec<T,N>& v, const fvec<S,M>& w)
{
    fmat<T, N, M> m;
    for (unsigned i = 0; i < N; i++)
        for (unsigned j = 0; j < M; j++)
            m(i, j) = v(i)*(T)w(j);
    return m;
}
 
template <typename T>
T det(const fmat<T, 2, 2>& m) {
    return m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
}
 
template <typename T>
T det(const fmat<T, 3, 3>& m) {
    T a = m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1);
    T b = -m(2, 0) * m(1, 1) * m(0, 2) - m(2, 1) * m(1, 2) * m(0, 0) - m(2, 2) * m(1, 0) * m(0, 1);
    return a + b;
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
const fmat<T, N, M> lerp(const fmat<T, N, M>& m1, const fmat<T, N, M>& m2, T t)
{
    fmat<T, N, M> m;
    for(unsigned i = 0; i < N; i++)
        for(unsigned j = 0; j < M; j++)
            m(i, j) = ((T)1 - t)*m1(i, j) + t * m2(i, j);
    return m;
}
 
template <typename T, cgv::type::uint32_type N, cgv::type::uint32_type M>
const fmat<T, N, M> lerp(const fmat<T, N, M>& m1, const fmat<T, N, M>& m2, fmat<T, N, M> t)
{
    fmat<T, N, M> m;
    for(unsigned i = 0; i < N; i++)
        for(unsigned j = 0; j < M; j++)
            m(i, j) = ((T)1 - t(i, j))*m1(i, j) + t(i, j)*m2(i, j);
    return m;
}
 
} // namespace math
 
 
typedef cgv::math::fmat<float, 2, 2> mat2;
typedef cgv::math::fmat<float, 3, 3> mat3;
typedef cgv::math::fmat<float, 4, 4> mat4;
typedef cgv::math::fmat<float, 3, 4> mat3x4;
 
typedef cgv::math::fmat<double, 2, 2> dmat2;
typedef cgv::math::fmat<double, 3, 3> dmat3;
typedef cgv::math::fmat<double, 4, 4> dmat4;
typedef cgv::math::fmat<double, 3, 4> dmat3x4;
 
 
}// namespace cgv