FMatrix.h

// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

namespace Imagine {


    template <typename T,int M,int N> class FMatrix {
    protected:
        T _data[M*N];
    public:
        FMatrix(){}
        explicit FMatrix(const T& v) {
            fill(v);
        }
        FMatrix(const T t[M*N]){
            for (int k=0;k<M*N;k++)
                _data[k] = t[k];
        }
        FMatrix(const T t[M][N]){
            for (int j=0;j<N;j++) for (int i=0;i<M;i++)
                (*this)(i,j) = t[i][j];
        }
        template <typename T2> FMatrix(const FMatrix<T2,M,N>& A) {
            for (int k=0;k<M*N;k++)
                _data[k] = T(A[k]);
        }
        FMatrix& fill(const T& v) {
            for (int k=0;k<M*N;k++)
                _data[k] = v;
            return *this;
        }
        static FMatrix Zero() {
            FMatrix Z;
            std::memset(Z._data,0,M*N*sizeof(T));
            return Z;
        }
        template <typename T2> 
        FMatrix& operator = (const FMatrix<T2,M,N>& B) {
            for (int k=0;k<M*N;k++)
                _data[k] = T(B[k]);
            return *this;
        }
        int size() const { return M*N; }
        int nrow() const { return M; }
        int ncol() const { return N; }
        const T& operator () (int i, int j) const {
            assert(i>=0 && i<M && j>=0 && j<N);
            return (*this)[i+M*j];
        }
        T& operator () (int i, int j) {
            assert(i>=0 && i<M && j>=0 && j<N);
            return (*this)[i+M*j];
        }
        const T& operator [] (int k) const {
            assert(k>=0 && k<M*N);
            return _data[k];
        }
        T& operator [] (int k) {
            assert(k>=0 && k<M*N);
            return _data[k];
        }
        const T* data() const { return _data; }
        T* data() { return _data; }
        static FMatrix Identity() {
            assert(M==N);
            FMatrix I(T(0));
            for (int i=0;i<M;i++)
                I(i,i) = T(1);
            return I;
        }
        static FMatrix CrossProd(const FVector<T,3>& v) {
            assert(M==N && M==3);
            FMatrix P(T(0));
            P(1,0) = v.z();
            P(0,1) = - v.z();
            P(2,0) = - v.y();
            P(0,2) = v.y();
            P(2,1) = v.x();
            P(1,2) = - v.x();
            return P;
        }       
        bool operator == (const FMatrix& B) const {
            for (int k=0;k<M*N;k++)
                if ((*this)[k] != B[k]) return false;
            return true;
        }
        bool operator != (const FMatrix& B) const {
            return !(*this == B);
        }
        FMatrix operator + (const FMatrix& B) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] + B[k];
            return C;
        }
        FMatrix operator - (const FMatrix& B) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] - B[k];
            return C;
        }
        FMatrix& operator += (const FMatrix& B) {
            for (int k=0;k<M*N;k++) _data[k] += B[k];
            return *this;
        }
        FMatrix& operator -= (const FMatrix& B) {
            for (int k=0;k<M*N;k++) _data[k] -= B[k];
            return *this;
        }
        FMatrix operator + (T s) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] + s;
            return C;
        }
        FMatrix operator - (T s) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] - s;
            return C;
        }
        FMatrix& operator += (T s) {
            for (int k=0;k<M*N;k++) _data[k] += s;
            return *this;
        }
        FMatrix& operator -= (T s) {
            for (int k=0;k<M*N;k++) _data[k] -= s;
            return *this;
        }
        FMatrix operator - () const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = -_data[k];
            return C;
        }
        FMatrix operator * (T s) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] * s;
            return C;
        }
        FMatrix operator / (T s) const {
            FMatrix C;
            for (int k=0;k<M*N;k++) C[k] = _data[k] / s;
            return C;
        }
        FMatrix& operator *= (T s) {
            for (int k=0;k<M*N;k++) _data[k] *= s;
            return *this;
        }
        FMatrix& operator /= (T s) {
            for (int k=0;k<M*N;k++) _data[k] /= s;
            return *this;
        }
        template <int O>
        FMatrix<T,M,O> operator * (const FMatrix<T,N,O>& B) const {
            FMatrix<T,M,O> C(T(0));
            for (int j=0;j<O;j++) for (int i=0;i<M;i++) {
                for (int k=0;k<N;k++) C(i,j) += (*this)(i,k) * B(k,j);
            }
            return C;
        }
        FVector<T,M> operator * (const FVector<T,N>& v) const {
            FVector<T,M> w(T(0));
            for (int j=0;j<N;j++)
                for (int i=0;i<M;i++) w[i] += (*this)(i,j) * v[j];
            return w;
        }
        FVector<T,M> getCol(int j) const {
            assert(j>=0 && j<N);
            FVector<T,M> v;
            for (int i=0;i<M;i++) v[i] = (*this)(i,j);
            return v;
        }
        FMatrix& setCol(int j, const FVector<T,M>& v) {
            assert(j>=0 && j<N);
            for (int i=0;i<M;i++) (*this)(i,j) = v[i];
            return *this;
        }
        FVector<T,N> getRow(int i) const {
            assert(i>=0 && i<M);
            FVector<T,N> v;
            for (int j=0;j<N;j++) v[j] = (*this)(i,j);
            return v;
        }
        FMatrix& setRow(int i, const FVector<T,N>& v) {
            assert(i>=0 && i<M);
            for (int j=0;j<N;j++) (*this)(i,j) = v[j];
            return *this;
        }
        friend T norm2(const FMatrix& A) {
            T n = 0;
            for (int k=0;k<M*N;k++) n += A._data[k] * A._data[k];
            return n;
        }
        friend inline T norm(const FMatrix& A) {
            assert( !std::numeric_limits<T>::is_integer );
            return ::sqrt(norm2(A));
        }
        friend void write(std::ostream& out, const FMatrix& A) {
            out.write((const char*)A.data(),std::streamsize(M*N*sizeof(T)));
        }
        friend void read(std::istream& in, FMatrix& A) {
            in.read((char*)A.data(),std::streamsize(M*N*sizeof(T)));
        }
        friend std::ostream& operator<<(std::ostream& out, const FMatrix& A) {
            for (int i=0;i<A.nrow();i++) {
                for (int j=0;j<A.ncol();j++) {
                    out<<A(i,j);
                    if (j<A.ncol()-1) out<<" ";
                }
                out<<std::endl;
            }
            return out;
        }
        friend std::istream& operator>>(std::istream& in, FMatrix& A) {
            for (int i=0;i<M;i++)
                for (int j=0;j<N;j++)
                    in>>A(i,j);
            return in;
        }

    };

    template <typename T,int M,int N>
    FMatrix<T,N,M> transpose(const FMatrix<T,M,N>&A) {
        FMatrix<T,N,M> C;
        for (int j=0;j<N;j++) for (int i=0;i<M;i++) C(j,i) = A(i,j);
        return C;
    }

    template <typename T,int M,int N>
    FMatrix<T,M,N> operator*(T s,const FMatrix<T,M,N>& A) {
        return A*s;
    }
    template <typename T,int M,int N>
    FMatrix<T,M,N> operator+(T s,const FMatrix<T,M,N>& A) {
        return A+s;
    }
    template <typename T,int M,int N>
    FMatrix<T,M,N> operator-(T s,const FMatrix<T,M,N>& A) {
        return (-A)+s;
    }
    template <typename T,int M,int N,int O>
    FMatrix<T,N,O> tmult(const FMatrix<T,M,N>&A,const FMatrix<T,M,O>& B)  {
        FMatrix<T,N,O> C(T(0));
        for (int j=0;j<O;j++) for (int i=0;i<N;i++)
            for (int k=0;k<M;k++) C(i,j) += A(k,i) * B(k,j);
        return C;
    }
    template <typename T,int M,int N,int O>
    FMatrix<T,M,O> multt(const FMatrix<T,M,N>&A,const FMatrix<T,O,N>& B)  {
        FMatrix<T,M,O> C(T(0));
        for (int j=0;j<O;j++) for (int i=0;i<M;i++)
            for (int k=0;k<N;k++) C(i,j) += A(i,k) * B(j,k); 
        return C;
    }
    template <typename T,int M,int N,int O>
    FMatrix<T,N,O> tmultt(const FMatrix<T,M,N>&A,const FMatrix<T,O,M>& B)  {
        FMatrix<T,N,O> C(T(0));
        for (int j=0;j<O;j++) for (int i=0;i<N;i++)
            for (int k=0;k<M;k++) C(i,j) += A(k,i) * B(j,k);
        return C;
    }
    template <typename T,int M,int N>
    FVector<T,N> tmult(const FMatrix<T,M,N>& A,const FVector<T,M>& v) {
        FVector<T,N> w(T(0));
        for (int j=0;j<N;j++)
            for (int i=0;i<M;i++) w[j] += A(i,j) * v[i];
        return w;
    }
    template <typename T,int M>
    FMatrix<T,M,M> inverse(const FMatrix<T,M,M>& A) {
        FMatrix<T,M,M> I(0.);
        if (M<=3){
            const T _det=det(A);
            if (_det==0) {
                std::cerr << "Non invertible matrix" << std::endl;
            } else {
                if (M==1) {
                    I(0,0)=1/_det;
                } else if (M==2) {
                    I(0,0)=A(1,1)/_det;
                    I(1,1)=A(0,0)/_det;
                    I(0,1)=-A(0,1)/_det;
                    I(1,0)=-A(1,0)/_det;
                } else if (M==3) {
                    // could be made faster with direct formula 
                    for (int i=0;i<3;i++) for (int j=0;j<3;j++) {
                        const int ia = (i+1)%3;
                        const int ib = (i+2)%3;
                        const int ja = (j+1)%3;
                        const int jb = (j+2)%3;
                        I(j,i) = ( A(ia,ja) * A(ib,jb) - A(ib,ja) * A(ia,jb) ) / _det;
                    }
                }
            }
        } else {
            std::cerr << "Code for inverting matrices of size " << M << "*" << M << " is not available" << std::endl;
            std::cerr << "Consider using inverseFMatrix" << std::endl;
        }
        return I;
    }
    template <typename T,int M>
    T det(const FMatrix<T,M,M>& A) {
        if (M==1)
            return A(0,0);
        if(M==2){
            return A(0,0)*A(1,1)-A(0,1)*A(1,0);
        }
        if (M==3) {
            return
                A(0,0) * (A(1,1) * A(2,2) - A(1,2) * A(2,1))
                - A(1,0) * (A(0,1) * A(2,2) - A(2,1) * A(0,2))
                + A(2,0) * (A(0,1) * A(1,2) - A(1,1) * A(0,2));
        }
        std::cerr << "Code for determinant of matrices of size " << M << "*" << M << " is not available" << std::endl;
        std::cerr << "Consider using Matrix class instead of FMatrix" << std::endl;
        return T(0);
    }

    template <typename T,int M>
    FMatrix<T,M,M> Diagonal(const FVector<T,M>& d){
        FMatrix<T,M,M> D=FMatrix<T,M,M>::Zero();
        for (int i=0;i<M;i++)
            D(i,i)=d[i];
        return D;
    }

}