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 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 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 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 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 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 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 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 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 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:
#ifdef _DEBUG
 Matrix() { set(initVal<T>()); }
#else
PUSHWARNING
VSWARNING(26495) // Initialization warning - on purpose
 Matrix() {}
POPWARNING
#endif
 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 Matrix(const Matrix<T,SX,SY> &m) { operator=(m); }
 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 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,*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>
 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!
 //for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // for (unsigned j=0;j<SZ;++j) { // COLUMNS OF SECOND
 // T &val = ret.get(i,j);
 // val = 0;
 // for (unsigned k=0;k<SY;++k) // Add up
 // val += get(i,k)*m.get(k,j);
 // }
 //}
 loopMultiply<SZ,SX>::execute(*this,m,ret);
 return ret;
 }
 //template <class T>
 //Matrix<T,SX,1> operator*(const Matrix<T,SY,1> &m) const {
 // Matrix<T,SX,1> ret;
 // // OK - we are using template expansion to do loop unrolling at compile time. Fun!
 // //for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // // T &val = ret.get(i,0);
 // // val = 0.0;
 // // for (unsigned k=0;k<SY;++k) // Add up
 // // val += get(i,k)*m.get(k,0);
 // //}
 // loopMultiply<1,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()) { for (unsigned i=0;i<SX;++i) for (unsigned j=0;j<SY;++j) t_[i][j] = 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:
#ifdef _DEBUG
 Matrix() { set(initVal<T>()); }
#else
 Matrix() { }
#endif
 constexpr explicit Matrix(const T &t) { set(t); }
 constexpr Matrix(const Matrix<T,SX,1> &m) { operator=(m); }
 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 Matrix<T,SX,1> &operator=(const Matrix<T,SX,1> &m) { for (uint32 i=0;i<SX;++i) t_[i] = m.t_[i]; return *this; }
 
 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 T & operator()(unsigned x,unsigned y) { return get(x,y); }
 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; }
 void set(const T &t=T()) { for (unsigned i=0;i<SX;++i) t_[i] = 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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:
#ifdef _DEBUG
 SymMatrix() { set(initVal<T>()); }
#else
 SymMatrix() { }
#endif
 explicit SymMatrix(const T &t) { set(t); }
 SymMatrix(const SymMatrix<T,SX> &m) { memcpy(t_,m.t_,sizeof(T)*len_); }
 const SymMatrix<T,SX> &operator=(const SymMatrix<T,SX> &m) { memcpy(t_,m.t_,sizeof(T)*len_); return *this; }
 
 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; }
 Matrix<T,SX,SX> operator*(const SymMatrix<T,SX> &m) const {
 Matrix<T,SX,SX> ret;
 loopMultiplyS<SX>::execute(*this,m,ret);
 //for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // for (unsigned j=0;j<SX;++j) { // COLUMNS OF SECOND
 // T &val = ret.get(i,j);
 // val = 0.0;
 // for (unsigned k=0;k<SX;++k) // Add up
 // val += get(i,k)*m.get(k,j);
 // }
 //}
 return ret;
 }
 template <unsigned SZ> Matrix<T,SX,SZ> operator*(const Matrix<T,SX,SZ> &m) const {
 Matrix<T,SX,SZ> ret;
 loopMultiply<SZ,SX>::execute(*this,m,ret);
 //for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // for (unsigned j=0;j<SY;++j) { // COLUMNS OF SECOND
 // T &val = ret.get(i,j);
 // val = 0.0;
 // for (unsigned k=0;k<SX;++k) // Add up
 // val += get(i,k)*m.get(k,j);
 // }
 //}
 return ret;
 }
 //template <> Matrix<T,SX,1> operator*(const Matrix<T,SX,1> &m) const {
 // Matrix<T,SX,1> ret;
 // for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // T &val = ret.get(i,0);
 // val = 0.0;
 // for (unsigned k=0;k<SX;++k) // Add up
 // val += get(i,k)*m.get(k,0);
 // }
 // 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; }
 void set(const T &t=T()) { for (unsigned i=0;i<len_;++i) t_[i] = 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);
 //for (unsigned i=0;i<SX;++i) { // ROWS OF FIRST
 // for (unsigned j=0;j<SY;++j) { // COLUMNS OF SECOND
 // T &val = ret.get(i,j);
 // val = 0.0;
 // for (unsigned k=0;k<SY;++k) // Add up
 // val += m.get(i,k)*s.get(k,j);
 // }
 //}
 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() : Matrix<T,S,1>() { }
 //explicit constexpr VMatrix(const T &t) : Matrix<T,S,1>(t) { }
 constexpr VMatrix(const VMatrix<T,S> &m) : Matrix<T,S,1>(m) { }
 constexpr VMatrix(const Matrix<T,S,1> &m) : Matrix<T,S,1>(m) { }
 constexpr const VMatrix<T,S> &operator=(const VMatrix<T,S> &m) { Matrix<T,S,1>::operator=(m); return *this; }
 constexpr const VMatrix<T,S> &operator=(const Matrix<T,S,1> &m) { Matrix<T,S,1>::operator=(m); return *this; }
 
 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; }
 
 //const T & get(unsigned x) const { return Matrix<T,S,1>::get(x,0); }
 //T & get(unsigned x) { return Matrix<T,S,1>::get(x,0); }
 constexpr const T &operator[](unsigned x) const { return this->get(x); }
 constexpr T & operator[](unsigned x) { return this->get(x); }
 //const T & operator()(unsigned x) const { return get(x); }
 //T & operator()(unsigned x) { return get(x); }
};
 
template <unsigned SX,unsigned SY=SX> class DMatrix : public Matrix<double,SX,SY> {
public:
 using Matrix<double,SX,SY>::Matrix;
 using Matrix<double,SX,SY>::operator=;
 constexpr DMatrix() : Matrix<double,SX,SY>() { }
 //explicit constexpr DMatrix(const double t[SX][SY]) : Matrix<double,SX,SY>(t) { }
 //explicit constexpr DMatrix(const double &t) : Matrix<double,SX,SY>(t) { }
 constexpr DMatrix(const DMatrix<SX,SY> &m) : Matrix<double,SX,SY>(m) { }
 constexpr DMatrix(const Matrix<double,SX,SY> &m) : Matrix<double,SX,SY>(m) { }
 constexpr const DMatrix<SX,SY> &operator=(const DMatrix<SX,SY> &m) { Matrix<double,SX,SY>::operator=(m); return *this; }
 //constexpr const DMatrix<SX,SY> &operator=(const Matrix<double,SX,SY> &m) { Matrix<double,SX,SY>::operator=(m); return *this; }
};
 
template <unsigned SX> class DSymMatrix : public SymMatrix<double,SX> {
public:
 DSymMatrix() : SymMatrix<double,SX>() { }
 explicit DSymMatrix(const double &t) : SymMatrix<double,SX>(t) { }
 DSymMatrix(const DSymMatrix<SX> &m) : SymMatrix<double,SX>(m) { }
 DSymMatrix(const SymMatrix<double,SX> &m) : SymMatrix<double,SX>(m) { }
 const DSymMatrix<SX> &operator=(const DSymMatrix<SX> &m) { SymMatrix<double,SX>::operator=(m); return *this; }
 const DSymMatrix<SX> &operator=(const SymMatrix<double,SX> &m) { SymMatrix<double,SX>::operator=(m); return *this; }
};
 
template <unsigned S> class DVMatrix : public VMatrix<double,S> {
public:
 using VMatrix<double,S>::VMatrix;
 using VMatrix<double,S>::operator=;
 
 //DVMatrix() : VMatrix<double,S>() { }
 //explicit DVMatrix(const double &d) : VMatrix<double,S>(d) { }
 constexpr DVMatrix(const DVMatrix<S> &m) : VMatrix<double,S>(m) { }
 constexpr DVMatrix(const Matrix<double,S,1> &m) : VMatrix<double,S>(m) { }
 const DVMatrix<S> &operator=(const DVMatrix<S> &m) { VMatrix<double,S>::operator=(m); return *this; }
 const DVMatrix<S> &operator=(const Matrix<double,S,1> &m) { VMatrix<double,S>::operator=(m); return *this; }
};
 
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}};
 //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
 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;
}
 
//template <unsigned SX>
//DSymMatrix<SX> toSymMatrix(const SymTensor &s) {
// static_assert(SX <= 3);
// static_assert(SX >= 2);
// DSymMatrix<SX> ret;
// ret.get(0,0) = s.s11();
// ret.get(0,1) = ret.get(1,0) = s.s12();
// ret.get(1,1) = s.s22();
// if constexpr (SX > 2) {
// ret.get(0,2) = ret.get(2,0) = s.s13();
// ret.get(1,2) = ret.get(2,1) = s.s23();
// ret.get(2,2) = s.s33();
// }
// 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