Matrix.h

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


namespace Imagine {

    template<typename T> class SymMatrix;

    template <typename T>
    class Matrix : public MultiArray<T,2> {
        typedef MultiArray<T,2> Base;
    public:
        typedef typename Base::iterator iterator;
        typedef typename Base::const_iterator const_iterator;

        Matrix() : Base() {}
        Matrix(int M,int N) : Base(M,N) {}
        Matrix(T* t,int M,int N,bool handleDelete=false) : Base(t,M,N,handleDelete) {}
        Matrix(const Base& A) : Base(A) {}
        Matrix(const SymMatrix<T>& A);
        ~Matrix() {}
        Matrix& operator=(const Matrix& A) { Base::operator=(A); return *this; }
        Matrix clone() const
        {
            Matrix A(nrow(),ncol());
            equalize((int)this->totalSize(),this->data(),1,A.data(),1);
            return A;
        }
        int nrow() const { return this->width(); }
        int ncol() const { return this->height(); }
        Matrix& fill(T x) { Base::fill(x); return *this; }
        static Matrix Zero(int M,int N){
            Matrix Z(M,N);
            memset(Z.data(),0,Z.totalSize()*sizeof(T));
            return Z;
        }
        static Matrix Identity(int N){
            Matrix I=Matrix::Zero(N,N);
            for (int i=0;i<N;i++)
                I(i,i)=T(1);
            return I;
        }
        Vector<T> getCol(int j) const {
            assert(j>=0 && j<ncol());
            Vector<T> v(nrow());
            equalize(nrow(),this->data()+nrow()*j,1,v.data(),1);
            return v;
        }
        Vector<T> getRow(int i) const {
            assert(i>=0 && i<nrow());
            Vector<T> v(ncol());
            equalize(ncol(),this->data()+i,nrow(),v.data(),1);
            return v;
        }
        Vector<T> getDiagonal() const {
            int n=std::min(nrow(),ncol());
            Vector<T> v(n);
            for (int i=0;i<n;i++)
                v[i]=(*this)(i,i);
            return v;
        }
        Matrix& setCol(int j, const Vector<T>& v) {
            assert(v.size()==(size_t)nrow() && j>=0 && j<ncol());
            equalize(nrow(),v.data(),1,this->data()+nrow()*j,1);
            return *this;
        }
        Matrix& setRow(int i, const Vector<T>& v) {
            assert(v.size()==(size_t)ncol() && i>=0 && i<nrow());
            equalize(ncol(),v.data(),1,this->data()+i,nrow());
            return *this;
        }
        Matrix& setDiagonal(const Vector<T>& v) {
            int n=int(v.size());
            assert(n==std::min(nrow(),ncol()));
            for (int i=0;i<n;i++)
                (*this)(i,i)=v[i];
            return *this;
        }
        Vector<T> getColRef(int j) {
            assert (j>=0 && j<ncol());
            return Vector<T>(this->data()+(size_t)nrow()*j,nrow());
        }
        Vector<T> getSubColRef(int j, int i0, int m) {
            assert (j>=0 && j<ncol() && i0>=0 && m>=0 && i0+m<=nrow());
            return Vector<T>(this->data()+i0+(size_t)+nrow()*j,m);
        }
        Matrix getSubMat(int i0, int m, int j0, int n) const { 
            assert (i0>=0 && i0+m<=nrow() && j0>=0 && j0+n<=ncol());
            Matrix retMat(m,n);
            for(int j=0;j<n;j++)
                equalize(m,this->data()+(j+j0)*nrow()+i0, 1, retMat.data()+j*m, 1);
            return retMat;
        }
        Matrix getColsRef(int j0,int n) {
            assert (j0>=0 && n>=0 && j0+n<=ncol());
            return Matrix(this->data()+(size_t)nrow()*j0, nrow(), n);
        }
        Vector<T> operator *(const Vector<T>& v) const {
            assert((size_t)ncol()==v.size());
            Vector<T> y(nrow());
            y.fill(T(0));
            affinity(nrow(), ncol(), T(1), this->data(), v.data(), T(0), y.data(), '0');
            return y;
        }
        friend Vector<T> tmult(const Matrix& A,const Vector<T>& v) {
            assert((size_t)A.nrow()==v.size());
            Vector<T> y(A.ncol());
            y.fill(T(0));
            affinity(A.nrow(), A.ncol(), T(1), A.data(), v.data(), T(0), y.data(), 'T');
            return y;
        }
        friend Matrix transpose(const Matrix& A) {
            Matrix r(A.ncol(),A.nrow());
            for(int i=0;i<A.nrow();i++) for(int j=0;j<A.ncol();j++) r(j,i)=A(i,j);
            return r;
        }
        friend Matrix inverse(const Matrix& A) {
            assert(A.nrow()==A.ncol());
            Matrix invA=A.clone();
            int *pivots=new int[A.nrow()];
            int info,n=A.ncol();
            // LU Inverse
            matrixInverse(&n,invA.data(),&n,pivots,&info);
            if (info) {
                std::cerr<<"Cannot invert matrix"<<std::endl;
                invA.fill(T(0));
                delete [] pivots;
                return invA;
            }
            delete[] pivots;
            return invA;
        }
        Matrix operator *(const Matrix& B) const {
            assert(ncol()==B.nrow());
            int p=ncol();
            Matrix C(nrow(),B.ncol());
            C.fill(T(0));
            matrixProduct(C.nrow(),C.ncol(),p,T(1),this->data(),nrow(),B.data(),B.nrow(),T(0),C.data(), 'N', 'N');
            return C;
        }
        friend Matrix tmult(const Matrix& A,const Matrix& B)  {
            assert(A.nrow()==B.nrow());
            int p=A.nrow();
            Matrix C(A.ncol(),B.ncol());
            C.fill(T(0));
            matrixProduct(C.nrow(),C.ncol(),p,T(1),A.data(),A.nrow(),B.data(),B.nrow(),T(0),C.data(),'T','N');
            return C;
        }
        friend Matrix multt(const Matrix& A,const Matrix& B)  {
            assert(A.ncol()==B.ncol());
            int p=A.ncol();
            Matrix C(A.nrow(),B.nrow());
            C.fill(T(0));
            matrixProduct(C.nrow(),C.ncol(),p,T(1),A.data(),A.nrow(),B.data(),B.nrow(),T(0),C.data(),'N','T');
            return C;
        }
        friend Matrix tmultt(const Matrix& A,const Matrix& B)  {
            assert(A.nrow()==B.ncol());
            int p=A.nrow();
            Matrix C(A.ncol(),B.nrow());
            C.fill(T(0));
            matrixProduct(C.nrow(),C.ncol(),p,T(1),A.data(),A.nrow(),B.data(),B.nrow(),T(0),C.data(),'T','T');
            return C;
        }
        Matrix operator *(const SymMatrix<T>& B) const;
        Matrix operator +(const Matrix& B) const {
            Matrix C=clone();
            return (C+=B);
        }
        Matrix operator -(const Matrix& B) const {
            Matrix C=clone();
            return (C-=B);
        }
        Matrix& operator +=(const Matrix& B) {
            assert(ncol()==B.ncol());
            assert(nrow()==B.nrow());
            combine((int)this->totalSize(), 1, B.data(), this->data());
            return *this;
        }
        Matrix& operator -=(const Matrix& B) {
            assert(ncol()==B.ncol());
            assert(nrow()==B.nrow());
            combine((int)this->totalSize(), -1, B.data(), this->data());
            return *this;
        }
        Matrix operator - () const {
            return (*this)*T(-1);
        }
        Matrix operator *(T x) const {
            Matrix C=clone();
            return (C*=x);
        }
        Matrix operator /(T x) const {
            Matrix C=clone();
            return (C/=x);
        }
        Matrix& operator *=(T x) {
            multiply((int)this->totalSize(), x, this->data());
            return *this;
        }
        Matrix& operator /=(T x) {
            multiply((int)this->totalSize(), 1 / x, this->data());
            return *this;
        }
        friend inline Matrix operator *(T x, const Matrix& B) {return B*x;}
        friend T dot(const Matrix& a,const Matrix& b) {
            assert(a.nrow()==b.nrow() && a.ncol()==b.ncol());
            return scalarProduct((int)a.totalSize(), a.data(), b.data());
        }
        friend T norm(const Matrix& A,char type='F') {
            int nr=A.nrow(),nc=A.ncol();
            T n = matrixNorm(&type,&nr,&nc,(T*)A.data(),&nr);
            return n;
        }
        friend T rcond(const Matrix& A) 
        {   
            if (A.nrow()<A.ncol())
                return rcond(transpose(A));
            // QR decomposition
            Matrix tmpA=A.clone();
            int info,m=A.nrow(),n=A.ncol();
            // Estimation of condition number
            T rcond;
            conditionNumber(&n,tmpA.data(),&m,&rcond,&info);
            if (info!=0) {
                std::cerr << "Bad cond number " << std::endl;
                rcond = T(-1);
            }
            return rcond;
        }
        friend void svd(const Matrix& A,Matrix &U,Vector<T> &S, Matrix &Vt,bool all=false) {
            Matrix cpy=A.clone();
            U=Matrix(A.nrow(),A.nrow());
            Vt=Matrix(A.ncol(),A.ncol());
            S=Vector<T>(std::min(A.nrow(),A.ncol()));
            int nr=A.nrow(),nc=A.ncol();
            int info;
            singularValuesDecomposition(all?"A":"S",&nr,&nc,cpy.data(),S.data(),U.data(),Vt.data(),&info);
            if (info) {
                std::cerr<<"Cannot compute SVD"<<std::endl;
                return;
            }
        }
        friend Vector<T> linSolve(const Matrix& A,const Vector<T>& b)
        {
            assert(b.size() == (size_t)A.nrow());
            if(A.nrow()==A.ncol())
            {
                // Linear system is not over- or under- determined
                Matrix tmpA=A.clone();
                Vector<T> tmpb=b.clone();
                // LU factorization
                int *pivots=new int[A.nrow()];
                int info,n=A.nrow();
                LUSystemSolve(&n,tmpA.data(),pivots,tmpb.data(),&info);
                if (info) {
          std::cerr<<"Cannot solve linear system"<<std::endl;
                    tmpb.fill(T(0));
                    delete [] pivots;
                    return tmpb;
                    tmpb.fill(T(0));
                    std::cerr<<"Cannot solve linear system"<<std::endl;
                }
                delete[] pivots;
                return tmpb;
            } else {
                // Least-squares solution in the over-determined case, minimum norm solution in the under-determined one
                int info,m=A.nrow(),n=A.ncol(),o=std::max(m,n);
                Matrix tmpA=A.clone();
                Vector<T> tmpb(o);
                for (int i=0;i<m;i++) tmpb[i] = b[i];
                QRSystemSolve(&m,&n,tmpA.data(),tmpb.data(),&info);
                if (info) {
                    tmpb.fill(T(0));
                    std::cerr<<"Cannot solve linear system"<<std::endl;
                }
                return tmpb.getSubVect(0,A.ncol());
            }
        }
        friend Matrix pseudoInverse(const Matrix& A,T tolrel=0) {
            int m=A.nrow(),n=A.ncol();
            if (n > m) 
                return transpose(pseudoInverse(transpose(A)));
            Matrix U,Vt;
            Vector<T> S;
            svd(A,U,S,Vt);
            if (tolrel==0) 
                tolrel=std::numeric_limits<T>::epsilon();
            T tol = S[0] * tolrel;
            for (int i=0;i<n;i++) {
                T s=(S[i]>tol)?T(1)/S[i]:T(0);
                for (int j=0;j<n;j++)
                    Vt(i,j)*=s;
            }
            return transpose(U.getColsRef(0,n)*Vt);
        }
        friend bool QR(const Matrix& A,Matrix& Q, Matrix& R, bool all=false){
            assert(A.nrow()>=A.ncol());
            int info,m=A.nrow(),n=A.ncol(),qn=(all?m:n);
            if(all) {
                // Copy A in beginning of Q
                Q = Matrix(A.nrow(),A.nrow());
                equalize((int)A.totalSize(),A.data(),1,Q.data(),1);
        R=Matrix(A.nrow(),A.ncol());
        QRFactorization(&m,&qn,&n,Q.data(),R.data(),&info);
            } else
      {
                Q = A.clone();
        R=Matrix(A.ncol(),A.ncol());
        QRFactorization(&m,&qn,&n,Q.data(),R.data(),&info);
      }
            if(info!=1) {
                std::cerr << "Failed QR" << std::endl;
                return false;
            }
            return true;
        }
        friend Matrix cholesky(const Matrix& A,bool low=true)  {
            assert(A.nrow()==A.ncol());
            Matrix b=A.clone();
            int n=A.nrow(); 
            int info;
            choleskyDecomposition(low?"L":"U",&n,b.data(),&info);
            if (info) {
                std::cerr << "Bad cholesky" << std::endl;
                b.fill(T(0));
                return b;
            }
            if (low) {
                for (int j=1; j<n; j++) 
                    for (int i=0; i<j; i++) 
                        b(i,j)=0;
            } else {
                for (int j=1; j<n; j++) 
                    for (int i=0; i<j; i++) 
                        b(j,i)=0;
            }
            return b;
        }


        friend T det(const Matrix& A) {
            assert(A.nrow()==A.ncol());
            Matrix b=A.clone();
            // LU decomposition
            int info,m=A.nrow(),n=A.ncol();
            T d = determinant(&m,&n,b.data(),&info);
            if (info!=0) {
                std::cerr<<"Cannot compute LU decomposition"<<std::endl;
                return 0;
            }
            return d;
        }
        friend inline std::ostream& operator<<(std::ostream& out,const Matrix& A) {
            out<<A.nrow()<<" "<<A.ncol()<< std::endl;
            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 inline std::istream& operator>>(std::istream& in,Matrix& A) {
            int m,n;
            in>>m>>n;
            A.setSize(m,n);
            for (int i=0;i<A.nrow();i++)
                for (int j=0;j<A.ncol();j++)
                    in>>A(i,j);
            return in;
        }
    };

    template <typename T>
    inline Matrix<T> Diagonal(const Vector<T>& d){
        int n=int(d.size());
        Matrix<T> D=Matrix<T>::Zero(n,n);
        for (int i=0;i<n;i++)
            D(i,i)=d[i];
        return D;
    }

    template <typename T>
    Matrix<T> outerProduct(const Vector<T>& v1,const Vector<T>& v2) {
        assert(v1.size()==v2.size());
        int n=(int)v1.size();
        Matrix<T> A(n,n);
        A.fill(T(0));
        matrixProduct(n,n,1,T(1),v1.data(),n,v2.data(),1,T(0),A.data(),'N','N');
        return A;
    }

    template <typename T,int N>
    FMatrix<T,N,N> inverseFMatrix(const FMatrix<T,N,N>& A) {
        if (N<=3)
            return inverse(A); // Direct inversion
        FMatrix<T,N,N> invA(A);
        // LU
        int *pivots=new int[N];
        int info,n=N;
        // Inverse
        matrixInverse(&n,invA.data(),&n,pivots,&info);
        if (info) {
            std::cerr<<"Cannot invert matrix"<<std::endl;
            invA.fill(T(0));
        }
        delete[] pivots;
        return invA;
    }

    template <typename T,int M,int N> 
    void svd(const FMatrix<T,M,N>& A, FMatrix<T,M,M> &U, FVector<T,M> &S, FMatrix<T,N,N> &Vt, bool all=false) {
        FMatrix<T,M,N> cpy(A);
        int nr=M,nc=N;
        int info;
        singularValuesDecomposition(all?"A":"S",&nr,&nc,cpy.data(),S.data(),U.data(),Vt.data(),&info);
        if (info) {
            std::cerr<<"Cannot compute SVD"<<std::endl;
            return;
        }
    }

    template <typename T>
    void eigenvalues(const Matrix<T>& A, Vector<T>& wr, Vector<T>& wi)
    {
        assert(A.ncol()==A.nrow());
        Matrix<T> cpy = A.clone();
        wr = Vector<T>(A.nrow());
        wi = Vector<T>(A.nrow());

        int n = A.nrow();
        int info;
        eigenValues(&n, cpy.data(), wr.data(), wi.data(), 0, &info);
        if (info)
            std::cerr << "Cannot compute eigenvalues" << std::endl;
    }

    template <typename T,int M,int N>
    FVector<T,N> linSolve(const FMatrix<T,M,N>& A,const FVector<T,M>& b)
    {
        if(M==N)
            {
                // Linear system is not over- or under- determined
                FMatrix<T,M,N> tmpA(A);
                FVector<T,M> tmpb(b);
                // LU factorization
                int *pivots=new int[M];
                int info,n=M;
                // Solve linear system using factorization
                LUSystemSolve(&n,tmpA.data(),pivots,tmpb.data(),&info);
                if (info) {
                    std::cerr<<"Cannot solve linear system"<<std::endl;
                    delete [] pivots;
                    return FVector<T,N>(T(0));
                    tmpb.fill(T(0));
                    std::cerr<<"Cannot solve linear system"<<std::endl;
                }
                delete[] pivots;
                return FVector<T,N>(tmpb.data()); //Safe since M==N but necessary for instantiation if M!=N
            }
        // Least-squares solution in the over-determined case, minimum norm solution in the under-determined one
        int info,m=M,n=N;
        FMatrix<T,M,N> tmpA(A);
        if(M > N) { // Over-determined system: least squares solution
            FVector<T,M> tmpb;
            for (int i=0;i<m;i++) tmpb[i] = b[i];
            QRSystemSolve(&m,&n,tmpA.data(),tmpb.data(),&info);
            if (info) {
                tmpb.fill(T(0));
                std::cerr<<"Cannot solve linear system"<<std::endl;
            }
            return FVector<T,N>( tmpb.data() );
        }
        // Under-determined system (M<N): minimum norm solution
        FVector<T,N> tmpb;
        for (int i=0;i<m;i++) tmpb[i] = b[i];
        QRSystemSolve(&m,&n,tmpA.data(),tmpb.data(),&info);
        if (info) {
            tmpb.fill(T(0));
            std::cerr<<"Cannot solve linear system"<<std::endl;
        }
        return tmpb;
    }
    template <typename T,int M,int N>
    FMatrix<T,N,M> pseudoInverse(const FMatrix<T,M,N>& A,T tolrel=0) {
        if (N > M) 
            return transpose(pseudoInverse(transpose(A)));
        FMatrix<T,M,M> U; FMatrix<T,N,N> Vt;
        FVector<T,M> S;
        svd(A,U,S,Vt);
        if (tolrel==0) 
            tolrel=std::numeric_limits<T>::epsilon();
        T tol = S[0] * tolrel;
        for (int i=0;i<N;i++) {
            T s=(S[i]>tol)?T(1)/S[i]:T(0);
            for (int j=0;j<N;j++)
                Vt(i,j)*=s;
        }
        return transpose(FMatrix<T,M,N>(U.data())*Vt);
    }
    template <typename T,int M,int N>
    bool QR(const FMatrix<T,M,N>& A,FMatrix<T,M,N>& Q, FMatrix<T,N,N>& R){
        assert(M>=N);
        int info,m=M,n=N;
        Q = A;
        QRFactorization(&m,&n,&n,Q.data(),R.data(),&info);
        if(info!=1) {
            std::cerr << "Failed QR" << std::endl;
            return false;
        }
        return true;
    }
    template <typename T,int M,int N>
    bool QRAll(const FMatrix<T,M,N>& A,FMatrix<T,M,M>& Q, FMatrix<T,M,N>& R){
        assert(M>=N);
        int info,m=M,n=N;
        equalize(M*N,A.data(),1,Q.data(),1);
        QRFactorization(&m,&m,&n,Q.data(),R.data(),&info);
        if(info!=1) {
            std::cerr << "Failed QR" << std::endl;
            return false;
        }
        return true;
    }
    template <typename T,int N>
    FMatrix<T,N,N> cholesky(const FMatrix<T,N,N>& A,bool low=true) {
        FMatrix<T,N,N> b(A);
        int n=N;
        int info;
        choleskyDecomposition(low?"L":"U",&n,b.data(),&info);
        if (info) {
            std::cerr << "Bad cholesky" << std::endl;
            b.fill(T(0));
            return b;
        }
        if (low) {
            for (int j=1; j<n; j++) 
                for (int i=0; i<j; i++) 
                    b(i,j)=0;
        } else {
            for (int j=1; j<n; j++) 
                for (int i=0; i<j; i++) 
                    b(j,i)=0;
        }
        return b;
    }

    template <typename T,int N>
    T detFMatrix(const FMatrix<T,N,N>& A) {
        int n=N; 
        FMatrix<T,N,N> b(A);
        int info;
        // Try cholesky factorization -> only for positive matrices!
        choleskyDecomposition((char*)"L",&n,b.data(),&info);
        if (!info) {
            T d=T(1);
            for (int i=0; i<n; i++) d*=b(i,i); 
            return d*d;
        }
        // Try SVD
        FMatrix<T,N,N> U,V;
        FVector<T,N> s;
        svd(A,U,s,V);
        T d=T(1);
        for (int i=0; i<N; i++) d*=s[i]; 
        return d;
    }

}