matrix.h

Go to the documentation of this file.

#pragma once
#ifdef __LINUX
#include <string.h>
#endif
 
#include "symtensor.h"
#include <cassert>
 
template <class T,unsigned SX,unsigned SY=SX>
class Matrix {
private:
    class Column {
    public:
        constexpr Column(unsigned x,Matrix<T,SX,SY> &m) : x_(x), m_(m) { }
        constexpr Column(const Column &r) : x_(r.x_), m_(r.m_) { }
        constexpr const T &operator[](unsigned y) const { return m_.get(x_,y); }
        T &operator[](unsigned y) { return m_.get(x_,y); }
    private:
        void operator=(const Column &r)=delete;
        unsigned x_;
        Matrix<T,SX,SY> &m_;
    };
    // Using template-meta programming to unroll innermost loop of matrix multiply
    template <unsigned SZ,unsigned I,unsigned J,unsigned K>
    class innerLoop {
    public:
        static constexpr double execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m) {
            return l.t_[SX-I][SY-K]*m.t_[SY-K][SZ-J] + innerLoop<SZ,I,J,K-1>::execute(l,m);
        }
    };
    template <unsigned SZ,unsigned I,unsigned J> // Termination specialization
    class innerLoop<SZ,I,J,1> {
    public:
        static constexpr double execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m) {
            return l.t_[SX-I][SY-1]*m.t_[SY-1][SZ-J];
        }
    };
    template <unsigned I,unsigned K> // Specialization for column matrix (SZ=1)
    class innerLoop<1,I,1,K> {
    public:
        static constexpr double execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,1> &m) {
            return l.t_[SX-I][SY-K]*m.t_[SY-K] + innerLoop<1,I,1,K-1>::execute(l,m);
        }
    };
    template <unsigned I>  // termination specialization for column matrix specialization
    class innerLoop<1,I,1,1> {
    public:
        static constexpr double execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,1> &m) {
            return l.t_[SX-I][SY-1]*m.t_[SY-1];
        }
    };
    // Using template meta-programming to unroll second loop of matrix multiply.
    template <unsigned SZ,unsigned I,unsigned J>
    class loop2Multiply {
    public:
        static constexpr void execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m,Matrix<T,SX,SZ> &ret) {
            ret.t_[SX-I][SZ-J] = innerLoop<SZ,I,J,SY>::execute(l,m);
            loop2Multiply<SZ,I,J-1>::execute(l,m,ret);
        }
    };
    template <unsigned SZ,unsigned I>  // Termination specialization
    class loop2Multiply<SZ,I,1> {
    public:
        static constexpr void execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m,Matrix<T,SX,SZ> &ret) {
            ret.t_[SX-I][SZ-1] = innerLoop<SZ,I,1,SY>::execute(l,m);
        }
    };
    template <unsigned I>  // Column matrix specialization (SZ=1)
    class loop2Multiply<1,I,1> {
    public:
        static constexpr void execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,1> &m,Matrix<T,SX,1> &ret) {
            ret.t_[SX-I] = innerLoop<1,I,1,SY>::execute(l,m);
        }
    };
    // Using template meta-programming to unroll outermost loop of matrix multiply.
    template <unsigned SZ,unsigned I>
    class loopMultiply {
    public:
        static constexpr void execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m,Matrix<T,SX,SZ> &ret) {
            loop2Multiply<SZ,I,SZ>::execute(l,m,ret);
            loopMultiply<SZ,I-1>::execute(l,m,ret);
        }
    };
    template <unsigned SZ> // Termination specialization
    class loopMultiply<SZ,1> {
    public:
        static constexpr void execute(const Matrix<T,SX,SY> &l,const Matrix<T,SY,SZ> &m,Matrix<T,SX,SZ> &ret) {
            loop2Multiply<SZ,1,SZ>::execute(l,m,ret);
        }
    };
 
public:
    constexpr Matrix() { set(T{}); }
    constexpr Matrix(const Matrix<T,SX,SY> &m) = default;
    constexpr Matrix<T,SX,SY> &operator=(const Matrix<T,SX,SY> &)=default;
 
    explicit constexpr Matrix(const T &t) { set(t); }
    explicit constexpr Matrix(const std::array<std::array<T,SY>,SX> &a) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] = a[i][j]; }
    constexpr Matrix(const std::initializer_list<std::initializer_list<T>> l);
 
    constexpr const T & get(unsigned x,unsigned y) const { assert(x<SX);  assert(y<SY); return t_[x][y]; }
    constexpr T &       get(unsigned x,unsigned y) { assert(x<SX);  assert(y<SY); return t_[x][y]; }
    constexpr const Column operator[](unsigned y) const { return Column(y,const_cast<Matrix<T,SX,SY> &>(*this)); }
    constexpr Column       operator[](unsigned y) { return Column(y,*this); }
    constexpr const T & operator()(unsigned x,unsigned y) const { return get(x,y); }
    constexpr T &       operator()(unsigned x,unsigned y) { return get(x,y); }
    constexpr UVect2 size() const { return {SX,SY}; }
 
    constexpr const Matrix<T,SX,SY> &operator+=(const Matrix<T,SX,SY> &m) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] += m.t_[i][j];  return *this; }
    constexpr const Matrix<T,SX,SY> &operator-=(const Matrix<T,SX,SY> &m) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] -= m.t_[i][j];  return *this; }
    constexpr const Matrix<T,SX,SY> &operator*=(const T &t) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] *= t;  return *this; }
    constexpr const Matrix<T,SX,SY> &operator/=(const T &t) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] /= t;  return *this; }
 
    Matrix<T,SX,SY> operator+(const Matrix<T,SX,SY> &m) const { Matrix<T,SX,SY> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret.t_[i][j] = t_[i][j] + m.t_[i][j]; return ret; }
    Matrix<T,SX,SY> operator-(const Matrix<T,SX,SY> &m) const { Matrix<T,SX,SY> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret.t_[i][j] = t_[i][j] - m.t_[i][j]; return ret; }
    Matrix<T,SX,SY> operator*(const T &t) const { Matrix<T,SX,SY> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret.t_[i][j] = t_[i][j] * t;  return ret; }
    Matrix<T,SX,SY> operator/(const T &t) const { Matrix<T,SX,SY> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret.t_[i][j] = t_[i][j] / t;  return ret; }
    template <unsigned SZ>
    constexpr Matrix<T,SX,SZ> operator*(const Matrix<T,SY,SZ> &m) const {
        Matrix<T,SX,SZ> ret;
        // OK - we are using template expansion to do loop unrolling at compile time.   Fun!
        loopMultiply<SZ,SX>::execute(*this,m,ret);
        return ret;
    }
 
    // Block operations
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2>
    void addBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        addGenBlock<BX,BY,SX2,SY2>(m,iSrc*BX,jSrc*BY,iDst*BX,jDst*BY);
    }
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2>
    void addGenBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        for (unsigned i=iSrc,id=iDst;i<(iSrc+BX);i++,id++)
            for (unsigned j=jSrc,jd=jDst;j<(jSrc+BY);j++,jd++)
                get(id,jd) += m(i,j);
    }
 
    T maxNorm() const { T ret(0);  for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret = std::max(ret,std::abs(t_[i][j]));  return ret; }
    Matrix<T,SY,SX> transpose() const { Matrix<T,SY,SX> ret;  for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret(j,i) = t_[i][j];  return ret; }
    T trace() const { T tt = t_[0][0]; unsigned len = std::min(SX,SY); for (unsigned i=1; i<len; ++i) tt += t_[i][i]; return tt; }
    constexpr void set(const T &t=T{}) { std::fill(&t_[0][0], &t_[0][0]+(SX*SY),t); }
    constexpr T minEntry() const { double ret=limits<T>::max(); for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret=std::min(ret,t_[i][j]);  return ret; }
    constexpr T maxEntry() const { double ret=limits<T>::lowest(); for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) ret=std::max(ret,t_[i][j]);  return ret; }
 
    static constexpr Matrix<T,SX,SY> identity() { Matrix<T,SX,SY> ret(0.0);  for (unsigned i=0;i<std::min(SX,SY);++i) ret(i,i) = 1.0;  return ret; }
 
    T t_[SX][SY];
};
 
template <class T,unsigned SX,unsigned SY>
constexpr Matrix<T,SX,SY>::Matrix(const std::initializer_list<std::initializer_list<T>> l) {
    uint32 i=0;
    for (auto &y : l) {
        uint32 j = 0;
        for (auto &x : y) {
            t_[i][j] = x;
            ++j;
        }
        ++i;
    }
}
 
 
template <class T,unsigned SX>
class Matrix<T,SX,1> {
public:
    constexpr Matrix() { set(T{}); }
    constexpr Matrix(const Matrix<T,SX,1> &)=default;
    constexpr Matrix<T,SX,1> &operator=(const Matrix<T,SX,1> &)=default;
 
    constexpr explicit Matrix(const T &t) { set(t); }
    constexpr Matrix(const std::array<T,SX> &a) { for (uint32 i=0;i<SX;++i) t_[i] = a[i]; }
    constexpr Matrix(const std::initializer_list<T> l) { uint32 i=0; for (auto &x : l) { t_[i] = x; ++i; } }
 
    constexpr const T & get(unsigned x,[[maybe_unused]] unsigned y) const { assert(x<SX);  assert(!y); return t_[x]; }
    constexpr const T & get(unsigned x) const { assert(x<SX);  return t_[x]; }
    constexpr T &       get(unsigned x,[[maybe_unused]] unsigned y) { assert(x<SX);  assert(!y); return t_[x]; }
    constexpr T &       get(unsigned x) { assert(x<SX); return t_[x]; }
    constexpr const T & operator()(unsigned x,unsigned y) const { return get(x,y); }
    constexpr const T & operator()(unsigned x) const { return get(x); }
    constexpr const T & operator[](unsigned x) const { return get(x); }
    constexpr T &       operator()(unsigned x,unsigned y) { return get(x,y); }
    constexpr T &       operator()(unsigned x) { return get(x); }
    constexpr T &       operator[](unsigned x) { return get(x); }
    constexpr UVect2 size() const { return {SX,1}; }
 
    constexpr const Matrix<T,SX,1> &operator+=(const Matrix<T,SX,1> &m) { for (unsigned i=0;i<SX;++i) t_[i] += m.t_[i];  return *this; }
    constexpr const Matrix<T,SX,1> &operator-=(const Matrix<T,SX,1> &m) { for (unsigned i=0;i<SX;++i) t_[i] -= m.t_[i];  return *this; }
    constexpr const Matrix<T,SX,1> &operator*=(const T &t) { for (unsigned i=0;i<SX;++i) t_[i] *= t;  return *this; }
    constexpr const Matrix<T,SX,1> &operator/=(const T &t) { for (unsigned i=0;i<SX;++i) t_[i] /= t;  return *this; }
 
    Matrix<T,SX,1> operator+(const Matrix<T,SX,1> &m) const { Matrix<T,SX,1> ret; for (unsigned i=0;i<SX;++i) ret.t_[i] = t_[i] + m.t_[i]; return ret; }
    Matrix<T,SX,1> operator-(const Matrix<T,SX,1> &m) const { Matrix<T,SX,1> ret; for (unsigned i=0;i<SX;++i) ret.t_[i] = t_[i] - m.t_[i]; return ret; }
    Matrix<T,SX,1> operator*(const T &t) const { Matrix<T,SX,1> ret; for (unsigned i=0;i<SX;++i) ret.t_[i] = t_[i] * t;  return ret; }
    Matrix<T,SX,1> operator/(const T &t) const { Matrix<T,SX,1> ret; for (unsigned i=0;i<SX;++i) ret.t_[i] = t_[i] / t;  return ret; }
    template <unsigned SZ> Matrix<T,SX,SZ> operator*(const Matrix<T,1,SZ> &m) const {
        Matrix<T,SX,SZ> ret;
        for (unsigned i=0;i<SX;++i) // ROWS OF FIRST
            for (unsigned j=0;j<SZ;++j) // COLUMNS OF SECOND
                ret.get(i,j) = get(i,0)*m.get(0,j);
        return ret;
    }
 
    // Block operations
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2> void addBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        addGenBlock<BX,BY,SX2,SY2>(m,iSrc*BX,jSrc*BY,iDst*BX,jDst*BY);
    }
    template <unsigned BX,unsigned SX2> void addBlock(const Matrix<T,SX2,1> &m,unsigned iSrc,unsigned iDst) {
        addGenBlock<BX,SX2>(m,iSrc*BX,iDst*BX);
    }
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2> void addGenBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        for (unsigned i=iSrc,id=iDst;i<(iSrc+BX);i++,id++)
            for (unsigned j=jSrc,jd=jDst;j<(jSrc+BY);j++,jd++)
                get(id,jd) += m(i,j);
    }
    template <unsigned BX,unsigned SX2> void addGenBlock(const Matrix<T,SX2,1> &m,unsigned iSrc,unsigned iDst) {
        for (unsigned i=iSrc,id=iDst;i<(iSrc+BX);i++,id++)
            get(id) += m(i);
    }
 
    T maxNorm() const { T ret(0);  for (unsigned i=0;i<SX;++i) ret = std::max(ret,std::abs(t_[i]));  return ret; }
    Matrix<T,1,SX> transpose() const { Matrix<T,1,SX> ret;  for (unsigned i=0;i<SX;++i) ret(0,i) = t_[i];  return ret; }
    constexpr void set(const T &t=T{}) { std::fill(&t_[0],&t_[0]+SX,t); }
    constexpr T minEntry() const { double ret=limits<T>::max(); for (unsigned i=0;i<SX;++i) ret=std::min(ret,t_[i]);  return ret; }
    constexpr T maxEntry() const { double ret=limits<T>::lowest(); for (unsigned i=0;i<SX;++i) ret=std::max(ret,t_[i]);  return ret; }
 
    static Matrix<T,SX,1> identity() { Matrix<T,SX,1> ret(0.0);  ret.t_[0] = 1.0; return ret; }
    T t_[SX];
};
 
template <class T,unsigned SX>
class SymMatrix {
private:
    class Column {
    public:
        Column(unsigned x,SymMatrix<T,SX> &m) : x_(x), m_(m) { }
        Column(const Column &r) : x_(r.x_), m_(r.m_) { }
        const T &operator[](unsigned y) const { return m_.get(x_,y); }
        T &operator[](unsigned y) { return m_.get(x_,y); }
    private:
        void operator=(const Column &r); // DISALLOWED
        unsigned x_;
        SymMatrix<T,SX> &m_;
    };
    // Using template-meta programming to unroll innermost loop of matrix multiply
    template <unsigned SZ,unsigned I,unsigned J,unsigned K>
    class innerLoop {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m) {
            return l.t_[index(SX-I,SX-K)]*m.t_[SX-K][SZ-J] + innerLoop<SZ,I,J,K-1>::execute(l,m);
        }
    };
    template <unsigned SZ,unsigned I,unsigned J> // Termination specialization
    class innerLoop<SZ,I,J,1> {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m) {
            return l.t_[index(SX-I,SX-1)]*m.t_[SX-1][SZ-J];
        }
    };
    template <unsigned I,unsigned K> // Specialization for column matrix (SZ=1)
    class innerLoop<1,I,1,K> {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,1> &m) {
            return l.t_[index(SX-I,SX-K)]*m.t_[SX-K] + innerLoop<1,I,1,K-1>::execute(l,m);
        }
    };
    template <unsigned I>  // termination specialization for column matrix specialization
    class innerLoop<1,I,1,1> {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,1> &m) {
            return l.t_[index(SX-I,SX-1)]*m.t_[SX-1];
        }
    };
    // Using template meta-programming to unroll second loop of matrix multiply.
    template <unsigned SZ,unsigned I,unsigned J>
    class loop2Multiply {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m,Matrix<T,SX,SZ> &ret) {
            ret.t_[SX-I][SZ-J] = innerLoop<SZ,I,J,SX>::execute(l,m);
            loop2Multiply<SZ,I,J-1>::execute(l,m,ret);
        }
    };
    template <unsigned SZ,unsigned I>  // Termination specialization
    class loop2Multiply<SZ,I,1> {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m,Matrix<T,SX,SZ> &ret) {
            ret.t_[SX-I][SZ-1] = innerLoop<SZ,I,1,SX>::execute(l,m);
        }
    };
    template <unsigned I>  // Column matrix specialization (SZ=1)
    class loop2Multiply<1,I,1> {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,1> &m,Matrix<T,SX,1> &ret) {
            ret.t_[SX-I] = innerLoop<1,I,1,SX>::execute(l,m);
        }
    };
    // Using template meta-programming to unroll outermost loop of matrix multiply.
    template <unsigned SZ,unsigned I>
    class loopMultiply {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m,Matrix<T,SX,SZ> &ret) {
            loop2Multiply<SZ,I,SZ>::execute(l,m,ret);
            loopMultiply<SZ,I-1>::execute(l,m,ret);
        }
    };
    template <unsigned SZ> // Termination specialization
    class loopMultiply<SZ,1> {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const Matrix<T,SX,SZ> &m,Matrix<T,SX,SZ> &ret) {
            loop2Multiply<SZ,1,SZ>::execute(l,m,ret);
        }
    };
 
 
    // Using template-meta programming to unroll innermost loop of matrix multiply
    template <unsigned I,unsigned J,unsigned K>
    class innerLoopS {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m) {
            return l.t_[index(SX-I,SX-K)]*m.t_[index(SX-K,SX-J)] + innerLoopS<I,J,K-1>::execute(l,m);
        }
    };
 
    template <unsigned I,unsigned J> // Termination specialization
    class innerLoopS<I,J,1> {
    public:
        static constexpr double execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m) {
            return l.t_[index(SX-I,SX-1)]*m.t_[index(SX-1,SX-J)];
            // what is X here? seems undefined??
        }
    };
 
    // Using template meta-programming to unroll second loop of matrix multiply.
    template <unsigned I,unsigned J>
    class loop2MultiplyS {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m,Matrix<T,SX,SX> &ret) {
            ret.t_[SX-I][SX-J] = innerLoopS<I,J,SX>::execute(l,m);
            loop2MultiplyS<I,J-1>::execute(l,m,ret);
        }
    };
    template <unsigned I>  // Termination specialization
    class loop2MultiplyS<I,1> {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m,Matrix<T,SX,SX> &ret) {
            ret.t_[SX-I][SX-1] = innerLoopS<I,1,SX>::execute(l,m);
        }
    };
    // Using template meta-programming to unroll outermost loop of matrix multiply.
    template <unsigned I>
    class loopMultiplyS {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m,Matrix<T,SX,SX> &ret) {
            loop2MultiplyS<I,SX>::execute(l,m,ret);
            loopMultiplyS<I-1>::execute(l,m,ret);
        }
    };
#ifndef __LINUX
    template <> // Termination specialization
    class loopMultiplyS<1> {
    public:
        static constexpr void execute(const SymMatrix<T,SX> &l,const SymMatrix<T,SX> &m,Matrix<T,SX,SX> &ret) {
            loop2MultiplyS<1,SX>::execute(l,m,ret);
        }
    };
#endif
 
public:
    constexpr SymMatrix() { set(T{}); }
    constexpr SymMatrix(const SymMatrix<T,SX> &) = default;
    constexpr SymMatrix<T,SX> &operator=(const SymMatrix<T,SX> &) = default;
 
    explicit SymMatrix(const T &t) { set(t); }
 
    const T & get(unsigned x,unsigned y) const { return t_[index(x,y)]; }
    T &       get(unsigned x,unsigned y) { return t_[index(x,y)]; }
    const Column operator[](unsigned y) const { return Column(y,*this); }
    Column       operator[](unsigned y) { return Column(y,*this); }
    const T & operator()(unsigned x,unsigned y) const { return get(x,y); }
    T &       operator()(unsigned x,unsigned y) { return get(x,y); }
    constexpr UVect2 size() const { return {SX,SX}; }
 
    const SymMatrix<T,SX> &operator+=(const SymMatrix<T,SX> &m) { for (unsigned i=0;i<len_;++i) t_[i] += m.t_[i]; return *this; }
    const SymMatrix<T,SX> &operator-=(const SymMatrix<T,SX> &m) { for (unsigned i=0;i<len_;++i) t_[i] -= m.t_[i]; return *this; }
    const SymMatrix<T,SX> &operator*=(const T &t) { for (unsigned i=0;i<len_;++i) t_[i] *= t;  return *this; }
    const SymMatrix<T,SX> &operator/=(const T &t) { for (unsigned i=0;i<len_;++i) t_[i] /= t;  return *this; }
 
    SymMatrix<T,SX> operator+(const SymMatrix<T,SX> &m) const { SymMatrix<T,SX> ret; for (unsigned i=0;i<len_;++i) ret.t_[i] = t_[i] + m.t_[i]; return ret; }
    Matrix<T,SX,SX> operator+(const Matrix<T,SX,SX> &m) const { Matrix<T,SX,SX> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SX;++j) ret(i,j) = get(i,j) + m.get(i,j); return ret; }
    SymMatrix<T,SX> operator-(const SymMatrix<T,SX> &m) const { SymMatrix<T,SX> ret; for (unsigned i=0;i<len_;++i) ret.t_[i] = t_[i] - m.t_[i]; return ret; }
    Matrix<T, SX, SX> operator-(const Matrix<T, SX, SX>& m) const { Matrix<T, SX, SX> ret; for (unsigned i = 0; i < SX; ++i) for (unsigned j = 0; j < SX; ++j) ret(i, j) = get(i, j) - m.get(i, j); return ret; }
 
    SymMatrix<T,SX> operator*(const T &t) const { SymMatrix<T,SX> ret; for (unsigned i=0;i<len_;++i) ret.t_[i] = t_[i] * t;  return ret; }
    SymMatrix<T,SX> operator/(const T &t) const { SymMatrix<T,SX> ret; for (unsigned i=0;i<len_;++i) ret.t_[i] = t_[i] / t;  return ret; }
    constexpr Matrix<T,SX,SX> operator*(const SymMatrix<T,SX> &m) const {
        Matrix<T,SX,SX> ret;
        loopMultiplyS<SX>::execute(*this,m,ret);
        return ret;
    }
    template <unsigned SZ> constexpr Matrix<T,SX,SZ> operator*(const Matrix<T,SX,SZ> &m) const {
        Matrix<T,SX,SZ> ret;
        loopMultiply<SZ,SX>::execute(*this,m,ret);
        return ret;
    }
 
    // Block operations
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2> void addBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        addGenBlock<BX,BY,SX2,SY2>(m,iSrc*BX,jSrc*BY,iDst*BX,jDst*BY);
    }
    template <unsigned BX,unsigned BY,unsigned SX2> void addBlock(const SymMatrix<T,SX2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        addGenBlock<BX,BY,SX2>(m,iSrc*BX,jSrc*BY,iDst*BX,jDst*BY);
    }
    template <unsigned BX,unsigned BY,unsigned SX2,unsigned SY2> void addGenBlock(const Matrix<T,SX2,SY2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        for (unsigned i=iSrc,id=iDst;i<(iSrc+BX);i++,id++) {
            for (unsigned j=jSrc,jd=jDst;j<(jSrc+BY);j++,jd++) {
                if (id<=jd)
                    get(id,jd) += m(i,j);
            }
        }
    }
    template <unsigned BX,unsigned BY,unsigned SX2> void addGenBlock(const SymMatrix<T,SX2> &m,unsigned iSrc,unsigned jSrc,unsigned iDst,unsigned jDst) {
        for (unsigned i=iSrc,id=iDst;i<(iSrc+BX);i++,id++) {
            for (unsigned j=jSrc,jd=jDst;j<(jSrc+BY);j++,jd++) {
                if (id<=jd)
                    get(id,jd) += m(i,j);
            }
        }
    }
 
    T maxNorm() const { T ret(0);  for (unsigned i=0;i<len_;++i) ret = std::max(std::abs(t_[i]),ret);  return ret; }
    SymMatrix<T,SX> transpose() const { return *this; }
    T trace() const { T tt = t_[0]; for (unsigned i=1; i<SX; ++i) tt += get(i,i); return tt; }
    constexpr void set(const T &t=T{}) { std::fill(&t_[0],&t_[0]+len_,t); }
    constexpr T minEntry() const { double ret=limits<T>::max(); for (unsigned i=0;i<len_;++i) ret=std::min(ret,t_[i]);  return ret; }
    constexpr T maxEntry() const { double ret=limits<T>::lowest(); for (unsigned i=0;i<len_;++i) ret=std::max(ret,t_[i]);  return ret; }
    Matrix<T,SX,SX> toMatrix() const { Matrix<T,SX,SX> ret; for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SX;++j) ret(i,j)=get(i,j);  return ret; }
 
    static SymMatrix<T,SX> identity() { SymMatrix<T,SX> ret(0.0);  for (unsigned i=0;i<SX;++i) ret(i,i) = 1.0;  return ret; }
    static SymMatrix<T,SX> fromMatrix(const Matrix<T,SX,SX> &m) {
        SymMatrix<T,SX> ret;
#ifdef _DEBUG
        double maxdif = 0.0;
#endif
        for (unsigned i=0;i<SX;++i) {
            ret(i,i) = m(i,i);
            for (unsigned j=i+1;j<SX;++j) {
                ret(i,j) = (m(i,j) + m(j,i)) * 0.5;
#ifdef _DEBUG
                maxdif = std::max(std::abs(ret(i,j) - m(i,j)),maxdif);
#endif
            }
        }
#ifdef _DEBUG
        assert(maxdif <= ret.maxNorm()*limits<float>::epsilon());
#endif
        return ret;
    }
 
    static constexpr unsigned len_ = (SX*SX + SX) / 2;
    static constexpr unsigned indexConst_ = 1 + 2*SX;
    static constexpr unsigned index(unsigned x,unsigned y) {
        unsigned x2 = x > y ? y : x;
        unsigned y2 = x > y ? x : y;
        unsigned i = (((indexConst_-x2)*x2)/2) + (y2-x2);
        return i;
    }
 
    T t_[len_];
};
 
// MOO NOTE!:: There are optimizations that can be made here!
// Using template-meta programming to unroll innermost loop of matrix multiply
template <class T,unsigned SX,unsigned SY,unsigned I,unsigned J,unsigned K>
class innerLoopMS {
public:
    static double execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m) {
        return l.t_[SX-I][SY-K]*m.t_[m.index(SY-K,SY-J)] + innerLoopMS<T,SX,SY,I,J,K-1>::execute(l,m);
    }
};
template <class T,unsigned SX,unsigned SY,unsigned I,unsigned J> // Termination specialization
class innerLoopMS<T,SX,SY,I,J,1> {
public:
    static double execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m) {
        return l.t_[SX-I][SY-1]*m.t_[m.index(SY-1,SY-J)];
    }
};
// Using template meta-programming to unroll second loop of matrix multiply.
template <class T,unsigned SX,unsigned SY,unsigned I,unsigned J>
class loop2MultiplyMS {
public:
    static void execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m,Matrix<T,SX,SY> &ret) {
        ret.t_[SX-I][SY-J] = innerLoopMS<T,SX,SY,I,J,SY>::execute(l,m);
        loop2MultiplyMS<T,SX,SY,I,J-1>::execute(l,m,ret);
    }
};
template <class T,unsigned SX,unsigned SY,unsigned I>  // Termination specialization
class loop2MultiplyMS<T,SX,SY,I,1> {
public:
    static void execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m,Matrix<T,SX,SY> &ret) {
        ret.t_[SX-I][SY-1] = innerLoopMS<T,SX,SY,I,1,SY>::execute(l,m);
    }
};
// Using template meta-programming to unroll outermost loop of matrix multiply.
template <class T,unsigned SX,unsigned SY,unsigned I>
class loopMultiplyMS {
public:
    static void execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m,Matrix<T,SX,SY> &ret) {
        loop2MultiplyMS<T,SX,SY,I,SY>::execute(l,m,ret);
        loopMultiplyMS<T,SX,SY,I-1>::execute(l,m,ret);
    }
};
template <class T,unsigned SX,unsigned SY> // Termination specialization
class loopMultiplyMS<T,SX,SY,1> {
public:
    static void execute(const Matrix<T,SX,SY> &l,const SymMatrix<T,SY> &m,Matrix<T,SX,SY> &ret) {
        loop2MultiplyMS<T,SX,SY,1,SY>::execute(l,m,ret);
    }
};
 
template <class T,unsigned SX,unsigned SY>
Matrix<T,SX,SY> operator*(const Matrix<T,SX,SY> &m,const SymMatrix<T,SY> &s) {
    Matrix<T,SX,SY> ret;
    loopMultiplyMS<T,SX,SY,SX>::execute(m,s,ret);
    return ret;
}
 
template <class T,unsigned S> 
class VMatrix : public Matrix<T,S,1> {
public:
    using Matrix<T,S,1>::Matrix;
    using Matrix<T,S,1>::operator=;
    using iterator = T *;
    using const_iterator = const T *;
 
    //constexpr VMatrix()=default;
    constexpr VMatrix(const VMatrix<T,S> &)=default;
    constexpr VMatrix(const Matrix<T,S,1> &m) : Matrix<T,S,1>(m) { }
    constexpr VMatrix<T,S> &operator=(const VMatrix<T,S> &m)=default;
 
    iterator begin() { return this->t_; }
    iterator end() { return this->t_+S; }
    const_iterator begin() const { return this->t_; }
    const_iterator end() const { return this->t_+S; }
 
    constexpr const T &operator[](unsigned x) const { return this->get(x); }
    constexpr T &      operator[](unsigned x) { return this->get(x); }
};
 
template <unsigned SX,unsigned SY=SX>  
using DMatrix = Matrix<double,SX,SY>;
 
template <unsigned SX> 
using DSymMatrix = SymMatrix<double,SX>;
 
template <unsigned S> 
using DVMatrix = VMatrix<double,S>;
 
template <class T,unsigned S>
double innerProduct(const VMatrix<T,S> &v1,const VMatrix<T,S> &v2) {
    double d=0.0;
    for (uint32 i=0;i<S;++i)
        d += v1(i)*v2(i);
    return d;
}
 
template <class T>
double innerProduct(const VMatrix<T,3> &v1,const DVect3 &v2) {
    double d=0.0;
    for (unsigned i=0;i<3;++i)
        d += v1(i)*v2[i];
    return d;
}
 
template <class T>
double innerProduct(const VMatrix<T,2> &v1,const DVect2 &v2) {
    double d=0.0;
    for (unsigned i=0;i<2;++i)
        d += v1(i)*v2[i];
    return d;
}
 
template <class T,unsigned S>
Matrix<T,S,S> outerProduct(const VMatrix<T,S> &v1,const VMatrix<T,S> &v2) {
    Matrix<T,S,S> ret;
    for (uint32 i=0;i<S;++i) {
        for (uint32 j=0;j<S;++j)
            ret(i,j) = v1(i) * v2(j);
    }
    return ret;
}
 
template <class T>
Matrix<T,3,3> outerProduct(const Vector3<T> &v1,const Vector3<T> &v2) {
    Matrix<T,3,3> ret;
    ret.get(0,0) = v1.x() * v2.x();
    ret.get(0,1) = v1.x() * v2.y();
    ret.get(0,2) = v1.x() * v2.z();
    ret.get(1,0) = v1.y() * v2.x();
    ret.get(1,1) = v1.y() * v2.y();
    ret.get(1,2) = v1.y() * v2.z();
    ret.get(2,0) = v1.z() * v2.x();
    ret.get(2,1) = v1.z() * v2.y();
    ret.get(2,2) = v1.z() * v2.z();
    return ret;
}
 
template <class T>
Matrix<T,2,2> outerProduct(const Vector2<T> &v1,const Vector2<T> &v2) {
    Matrix<T,2,2> ret;
    ret.get(0,0) = v1.x() * v2.x();
    ret.get(0,1) = v1.x() * v2.y();
    ret.get(1,0) = v1.y() * v2.x();
    ret.get(1,1) = v1.y() * v2.y();
    return ret;
}
 
template <class T>
constexpr T determinant(const Matrix<T,3,3> &mat) {
    T t = mat.get(0,0) * (mat.get(1,1) * mat.get(2,2) - mat.get(2,1) * mat.get(1,2));
    t -= mat.get(1,0) * (mat.get(0,1) * mat.get(2,2) - mat.get(2,1) * mat.get(0,2));
    t += mat.get(2,0) * (mat.get(0,1) * mat.get(1,2) - mat.get(1,1) * mat.get(0,2));
    return t;
}
 
template <class T>
constexpr T determinant(const Matrix<T,2,2> &mat) {
    return mat.get(0,0) * mat.get(1,1) - mat.get(0,1) * mat.get(1,0);
}
 
template <class T>
constexpr Matrix<T,3,3> inverse(const Matrix<T,3,3> &mat) {
    // First calculate the adjoint
    const T a11 = mat(0,0), a21 = mat(1,0), a31 = mat(2,0);
    const T a12 = mat(0,1), a22 = mat(1,1), a32 = mat(2,1);
    const T a13 = mat(0,2), a23 = mat(1,2), a33 = mat(2,2);
 
    Matrix<T,3,3> rm = {{ a22*a33 - a32*a23,-a12*a33 + a32*a13, a12*a23 - a22*a13},
                        {-a21*a33 + a31*a23, a11*a33 - a31*a13,-a11*a23 + a21*a13},
                        { a21*a32 - a31*a22,-a11*a32 + a31*a12, a11*a22 - a21*a12}};
    return rm / determinant(mat);
}
 
template <class T>
constexpr Matrix<T,2,2> inverse(const Matrix<T,2,2> &mat) {
    Matrix<T,2,2> rm;
    rm.get(0,0) = mat.get(1,1);
    rm.get(1,1) = mat.get(0,0);
    rm.get(0,1) = mat.get(0,1)*(-1);
    rm.get(1,0) = mat.get(1,0)*(-1);
    //Divide by the determinant
    return rm / determinant(mat);
}
 
// Returns both  inverse and determinant (Before inverse).
template <class T>
constexpr std::tuple<Matrix<T,3,3>,double> inverseDet(const Matrix<T,3,3> &mat) {
    // First calculate the adjoint
    const T &a11 = mat(0,0), a21 = mat(1,0), a31 = mat(2,0);
    const T &a12 = mat(0,1), a22 = mat(1,1), a32 = mat(2,1);
    const T &a13 = mat(0,2), a23 = mat(1,2), a33 = mat(2,2);
 
    Matrix<T,3,3> rm;
    rm.get(0,0) = a22*a33 - a32*a23;
    rm.get(1,0) = -a21*a33 + a31*a23;
    rm.get(2,0) = a21*a32 - a31*a22;
    rm.get(0,1) = -a12*a33 + a32*a13;
    rm.get(1,1) = a11*a33 - a31*a13;
    rm.get(2,1) = -a11*a32 + a31*a12;
    rm.get(0,2) = a12*a23 - a22*a13;
    rm.get(1,2) = -a11*a23 + a21*a13;
    rm.get(2,2) = a11*a22 - a21*a12;
    // Divide by the determinant
    double det = determinant(mat);
    return {rm / det,det};
}
 
template <class T>
constexpr VMatrix<T,2> toMatrix(const Vector2<T> &v) {
    VMatrix<T,2> ret;
    ret.get(0) = v.x();
    ret.get(1) = v.y();
    return ret;
}
 
template <class T>
constexpr VMatrix<T,3> toMatrix(const Vector3<T> &v) {
    VMatrix<T,3> ret;
    ret.get(0) = v.x();
    ret.get(1) = v.y();
    ret.get(2) = v.z();
    return ret;
}
 
template <class T,unsigned SX>
constexpr VMatrix<T,SX> toVMatrix(const Vector2<T> &v,unsigned start=0) {
    VMatrix<T,SX> ret(0.0);
    ret.get(start) = v.x();
    ret.get(start+1) = v.y();
    return ret;
}
 
template <unsigned SX> constexpr DVMatrix<SX> toDVMatrix(const DVect2 &v,unsigned start=0) { return toVMatrix<double,SX>(v,start); }
 
template <class T,unsigned SX>
constexpr VMatrix<T,SX> toVMatrix(const Vector3<T> &v,unsigned start=0) {
    VMatrix<T,SX> ret(0.0);
    ret.get(start) = v.x();
    ret.get(start+1) = v.y();
    ret.get(start+2) = v.z();
    return ret;
}
 
template <unsigned SX>
DVMatrix<SX> constexpr toDVMatrix(const DVect3 &v,unsigned start=0) { return toVMatrix<double,SX>(v,start); }
 
template <class T>
constexpr Vector2<T> operator*(const Matrix<T,2,2> &m,const Vector2<T> &v) {
    Vector2<T> ret(v.x()*m.get(0,0) + v.y()*m.get(0,1),
        v.x()*m.get(1,0) + v.y()*m.get(1,1));
    return ret;
}
 
template <class T>
constexpr Vector2<T> operator*(const SymMatrix<T,2> &m,const Vector2<T> &v) {
    Vector2<T> ret(v.x()*m.get(0,0) + v.y()*m.get(0,1),
                   v.x()*m.get(1,0) + v.y()*m.get(1,1));
    return ret;
}
 
template <class T>
constexpr Vector3<T> operator*(const Matrix<T,3,3> &m,const Vector3<T> &v) {
    Vector3<T> ret(v.x()*m.get(0,0) + v.y()*m.get(0,1) + v.z()*m.get(0,2),
                   v.x()*m.get(1,0) + v.y()*m.get(1,1) + v.z()*m.get(1,2),
                   v.x()*m.get(2,0) + v.y()*m.get(2,1) + v.z()*m.get(2,2));
    return ret;
}
 
template <class T>
Vector3<T> operator*(const SymMatrix<T,3> &m,const Vector3<T> &v) {
    Vector3<T> ret(v.x()*m.get(0,0) + v.y()*m.get(0,1) + v.z()*m.get(0,2),
                   v.x()*m.get(1,0) + v.y()*m.get(1,1) + v.z()*m.get(1,2),
                   v.x()*m.get(2,0) + v.y()*m.get(2,1) + v.z()*m.get(2,2));
    return ret;
}
 
BASE_EXPORT DMatrix<3,3> toMatrix3(const SymTensor &s);
BASE_EXPORT DMatrix<2,2> toMatrix2(const SymTensor &s);
 
BASE_EXPORT DMatrix<2,2> toMatrix2(const SymTensor &s);
 
BASE_EXPORT DSymMatrix<3> toSymMatrix3(const SymTensor &s);
BASE_EXPORT DSymMatrix<2> toSymMatrix2(const SymTensor &s);
 
inline SymTensor toSymTensor(const DMatrix<3,3> &m) {
    SymTensor ret(m.get(0,0),
                  m.get(1,1),
                  m.get(2,2),
                  (m.get(0,1)+m.get(1,0))*0.5,
                  (m.get(0,2)+m.get(2,0))*0.5,
                  (m.get(1,2)+m.get(2,1))*0.5);
    return ret;
}
 
inline SymTensor toSymTensor(const DMatrix<2,2> &m) {
    SymTensor ret(m.get(0,0),
                  m.get(1,1),
                  0.0,
                  (m.get(0,1)+m.get(1,0))*0.5,
                  0.0,
                  0.0);
    return ret;
}
 
inline SymTensor toSymTensor(const DSymMatrix<3> &m) {
    SymTensor ret(m.get(0,0),m.get(1,1),m.get(1,2),
                  m.get(0,1),m.get(0,2),m.get(1,2));
    return ret;
}
 
template <class T,unsigned SX>
Vector2<T> toVector2(const VMatrix<T,SX> &m,unsigned start=0) { Vector2<T> ret(m(start),m(start+1));  return ret; }
 
template <class T,unsigned SX>
Vector3<T> toVector3(const VMatrix<T,SX> &m,unsigned start=0) { Vector3<T> ret(m(start),m(start+1),m(start+2));  return ret; }
 
template <class T>
Vector2<T> toVector(const VMatrix<T,2> &m) { Vector2<T> ret(m(0),m(1));  return ret; }
 
template <class T>
Vector3<T> toVector(const VMatrix<T,3> &m) { Vector3<T> ret(m(0),m(1),m(2));  return ret; }
 
template <class T,unsigned SY>
Vector2<T> columnToVector(const Matrix<T,2,SY> &m,unsigned col) { Vector2<T> ret(m(0,col),m(1,col));  return ret; }
template <class T,unsigned SY>
Vector3<T> columnToVector(const Matrix<T,3,SY> &m,unsigned col) { Vector3<T> ret(m(0,col),m(1,col),m(2,col));  return ret; }
template <class T,unsigned SX>
Vector2<T> rowToVector(const Matrix<T,SX,2> &m,unsigned row) { Vector2<T> ret(m(row,0),m(row,1));  return ret; }
template <class T,unsigned SX>
Vector3<T> rowToVector(const Matrix<T,SX,3> &m,unsigned row) { Vector3<T> ret(m(row,0),m(row,1),m(row,2));  return ret; }
 
template <class T,unsigned SY>
constexpr void vectorToColumn(Matrix<T,2,SY> &m,const DVect2 &v,unsigned col) { m(0,col) = v.x();  m(1,col) = v.y(); }
 
template <class T,unsigned SY>
constexpr void vectorToColumn(Matrix<T,3,SY> &m,const DVect3 &v,unsigned col) { m(0,col) = v.x();  m(1,col) = v.y();  m(2,col) = v.z(); }
 
template <class T,unsigned SX,unsigned SY>
constexpr void vectorToColumn(Matrix<T,SX,SY> &m,const VMatrix<T,SX> &v,unsigned col) { for (uint32 i=0;i<SX;++i) m(i,col) = v(i); }
 
template <class T,unsigned SX>
void vectorToRow(Matrix<T,SX,2> &m,const DVect2 &v,unsigned row) { m(row,0) = v.x();  m(row,1) = v.y(); }
 
template <class T,unsigned SX>
void vectorToRow(Matrix<T,SX,3> &m,const DVect3 &v,unsigned row) { m(row,0) = v.x();  m(row,1) = v.y();  m(row,2) = v.z(); }
 
template <class T,unsigned SX,unsigned SY>
constexpr void vectorToRow(Matrix<T,SX,SY> &m,const VMatrix<T,SY> &v,unsigned row) { for (uint32 j=0;j<SY;++j) m(row,j) = v(j); }
 
template <class T,unsigned SX,unsigned SY>
VMatrix<T,SX> column(const Matrix<T,SX,SY> &m,unsigned c) {
    VMatrix<T,SX> ret;
    for (unsigned i=0;i<SX;++i)
        ret(i) = m(i,c);
    return ret;
}
 
inline DVMatrix<3> operator *(const SymTensor &s,const DVMatrix<3> &v) {
    DVMatrix<3> ret;
    ret[0] = s.s11()*v[0] + s.s12()*v[1] + s.s13()*v[2];
    ret[1] = s.s12()*v[0] + s.s22()*v[1] + s.s23()*v[2];
    ret[2] = s.s13()*v[0] + s.s23()*v[1] + s.s33()*v[2];
    return ret;
}
 
inline DVMatrix<2> operator *(const SymTensor &s,const DVMatrix<2> &v) {
    DVMatrix<2> ret;
    ret[0] = s.s11()*v[0] + s.s12()*v[1];
    ret[1] = s.s12()*v[0] + s.s22()*v[1];
    return ret;
}
 
// EoF