Subversion Repositories gelsvn

#include <iostream>
#include <vector>
#include "LapackFunc.h"

extern "C" {
        extern void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info);
        extern void dgetri_(const int *n, double *a, const int *lda, int *ipiv,double *work, int *lwork, int *info);
        extern int dgesvd_(const char *jobu, const char *jobvt, const int *m, const int *n, double *a, const int *lda, double *s,
                double *u, const int *ldu, double *vt, const int *ldvt, double *work, const int *lwork, int *info);
        extern void dgelss_(const int *, const int *, const int *, double *, const int *, double *, const int *, double *, double *, int *, double *, const int *, int *);
        extern void dgesv_(const int *N, const int *NRHS, double *A, const int *LDA, int *IPIV, double *B, const int *LDB, int *INFO);
        extern void dposv_(const char *UPLO, const int *N, const int *NRHS, double *A, const int *LDA, double *B, const int *LDB, int *INFO);
        extern void dgeqrf_(const int *m, const int *n, double* A,const int* lda,double*tau,double *work, const int* lwork, int* info);
        extern void dorgqr_(const int *m, const int *n, const int *k, double* A, const int *lda,const double *tau,double* work, const int *lwork, int *info);
        extern void dgerqf_(const int *m, const int *n, double* A,const int* lda,double*tau,double *work, const int* lwork, int* info);
        extern void dorgrq_(const int *m, const int *n, const int *k, double* A, const int *lda,const double *tau,double* work, const int *lwork, int *info);

        int dsysv_(char *uplo, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, double *work, int *lwork, int *info);
}

namespace{
        
        template <class T>
                inline T MIN(const T& a,const T& b)
        {
                return a<b?a:b;
        }
        
        template <class T>
                inline T MAX(const T& a,const T& b)
        {
                return a>b?a:b;
        }
        
}

namespace LinAlg
{

void SVD(const CMatrix& A,
                 CMatrix& U,
                 CVector& s,
                 CMatrix& V)
{
        //Notice, that the Lapack/Fortran CMatrix representation is the transposed version of the C/C++
        CMatrix a;
        A.Transposed(a);
        
        int nRows=A.Rows();
        int nCols=A.Cols();
        int info;
        int lda = MAX(nRows,1);
        int lwork= 6*MIN(nRows,nCols) + nRows + nCols;
        
        s.Resize(MIN(nRows,nCols));
        double *work = new double[lwork];
        
        char jobu='A';
        int ldu=nRows;
        U.Resize(nRows,nRows);
        
        char jobvt='A';
        int ldvt=nCols;
        V.Resize(nCols,nCols);
        
        
        dgesvd_ (&jobu, &jobvt, &nRows, &nCols, a[0], &lda, &(s[0]), U[0], &ldu, V[0], &ldvt,work, &lwork, &info);
        
        assert(info==0);
        
        delete work;
        U.Transpose();
}

void SVD(const CMatrix& A,
                 CMatrix& U,
                 CMatrix& S,
                 CMatrix& V)
{
        CVector s;
        SVD(A,U,s,V);
        S.Resize(A.Rows(),A.Cols());
        S=0;
        for(int cS=MIN(A.Rows(),A.Cols())-1;cS>=0;cS--)
        {
                S[cS][cS]=s[cS];
        }
}

CVector SVD(const CMatrix& A)
{
        CMatrix a;
        A.Transposed(a);
        
        int nRows=A.Rows();
        int nCols=A.Cols();
        int info;
        int lda = MAX(nRows,1);
        int lwork= 6*MIN(nRows,nCols) + nRows + nCols;
        
        CVector s(MIN(nRows,nCols));
        double *work = new double[lwork];
        
        char jobu='N';
        int ldu=1;
        
        char jobvt='N';
        int ldvt=1;
        
        dgesvd_ (&jobu, &jobvt, &nRows, &nCols, a[0], &lda, &(s[0]), NULL, &ldu, NULL, &ldvt,work, &lwork, &info);
        
        assert(info==0);
        
        delete work;
        
        return s;
}


void LinearSolve(const CMatrix& A,
                                 const CVector&b,
                                 CVector& x)
{
        assert(A.Rows()==b.Length());
        assert(A.Rows()==A.Cols());
        
        CMatrix a;
        A.Transposed(a);
        x=b;
        int nRows=A.Rows();
        int nrhs=1;     //only one CVector, change here to make b a CMatrix.
        int info;
        int *ipiv =new int[nRows];
        
        dgesv_(&nRows, &nrhs, a[0], &nRows, ipiv, &(x[0]), &nRows, &info);
        
        assert(info==0);
        delete [] ipiv;
}

CVector LinearSolve(const CMatrix& A,
                                   const CVector&b)
{
        CVector x;
        LinearSolve(A,b,x);
        return x;
}


void LinearSolveSPD(const CMatrix& A,
                                        const CVector&b,
                                        CVector& x)
{
        assert(A.Rows()==b.Length());
        assert(A.Rows()==A.Cols());
        
        CMatrix a(A);
        x=b;
        
        char uplo='U';
        int nRows=A.Rows();
        int info;
        int nrhs=1;
        
        dposv_(&uplo, &nRows, &nrhs, a[0], &nRows, &(x[0]), &nRows, &info);
        
        assert(info==0);
}

void LinearSolveSym(const CMatrix& A,
                                                                                const CVector&b,
                                                                                CVector& x)
{
        assert(A.Rows()==b.Length());
        assert(A.Rows()==A.Cols());
        
        CMatrix a(A);
        x=b;
        
        char uplo='U';
        int nRows=A.Rows();
        int nCols=A.Cols();
        int info;
        int nrhs=1;
        
        int lwork= 10*6*MIN(nRows,nCols) + nRows + nCols;
        double *work = new double[lwork];

        std::vector<int> ipiv(A.Rows());
        dsysv_(&uplo, &nRows, &nrhs, a[0], &nRows, &ipiv[0], &(x[0]), &nRows, 
                                 work, &lwork, &info);
        delete work;
        assert(info==0);
}


CVector LinearSolveSPD(const CMatrix& A,const CVector&b)
{
        CVector x;
        LinearSolveSPD(A,b,x);
        return x;
}

void LinearLSSolve(const CMatrix& A,
                                   const CVector& b,
                                   CVector& x)
{
        assert(A.Rows()==b.Length());
        
        int nRows=A.Rows();
        int nCols=A.Cols();
        CMatrix a;
        A.Transposed(a);
        
        int ldb=MAX(nRows,nCols);
        double* BX=new double[ldb];
        memcpy(BX,&(b[0]),nRows*sizeof(double));
        
        int nrhs=1;
        double* s=new double[MIN(nRows,nCols)];
        double rcond = -1.0; // using machine precision
        int info,rank;
        int lwork = 5*nRows*nCols + 1; // larger than necessary
        double *work = new double[lwork];
        
        dgelss_(&nRows, &nCols, &nrhs, a[0], &nRows, BX, &ldb, s, &rcond, &rank, work, &lwork, &info);
        
        x.Resize(nCols);
        memcpy(&(x[0]),BX,nCols*sizeof(double));
        
        
        assert(info==0);
        
        delete s;
        delete work;
}

CVector LinearLSSolve(const CMatrix& A,
                                         const CVector& b)
{
        CVector x;
        LinearLSSolve(A,b,x);
        return x;
}

void Invert(CMatrix& A)
{
        assert(A.Rows()==A.Cols());
        int nRows=A.Rows();
        int info;
        int *ipiv = new int[nRows];
        //Perform th LU factorization of A
        dgetrf_ (&nRows, &nRows, A[0], &nRows, ipiv, &info);
        if (info != 0) { //An error occured
                delete [] ipiv;
                assert(info==0);        //info will be < 0 if A is rank deficient.
                return;
        }

        //Calculate the inverse
        int lwork = nRows * 5; 
        double *work = new double[lwork];
        dgetri_ (&nRows, A[0], &nRows, ipiv, work, &lwork, &info);
        delete [] ipiv;
        delete [] work;
        assert(info==0);
}

void Inverted(const CMatrix& A,
                          CMatrix& InvA)
{
        InvA=A;
        Invert(InvA);
}

CMatrix Inverted(const CMatrix& A)
{
        CMatrix InvA(A);
        Invert(InvA);
        return InvA;
}

void QRfact(const CMatrix& A,CMatrix& Q, CMatrix& R)
{

        
        int nRows=A.Rows();
        int nCols=A.Cols();
        assert(A.Rows()>0 && A.Cols()>0); //herby lda=nRows.
        int lwork= 6*nCols;
        int info;
        int k=MIN(nRows,nCols);


        A.Transposed(R);

        double *work = new double[lwork];


        CVector TAU(k);

        dgeqrf_(&nRows,&nCols,R[0],&nRows,&TAU[0],work,&lwork,&info);
        assert(info==0);


        Q=R;

        nCols=MIN(nCols,nRows);

        dorgqr_(&nRows,&nCols,&k,Q[0],&nRows,&TAU[0],work,&lwork,&info);
        assert(info==0);


        delete work;

        R.Transpose();
        Q.Transpose();

        //Set the zeros of R
        double * pRow;
        int ColStop;
        for(int cRow=1;cRow<R.Rows();cRow++)
        {
                pRow=R[cRow];
                ColStop=MIN(cRow,R.Cols());
                for(int cCol=0;cCol<ColStop;cCol++)
                {
                        pRow[cCol]=0;
                }
        }

}


void RQfact(const CMatrix& A,CMatrix& R, CMatrix& Q)
{
        int nRows=A.Rows();
        int nCols=A.Cols();
        assert(A.Rows()>0 && A.Cols()>0); //herby lda=nRows.
        int lwork= 6*nCols;
        int info;
        int k=MIN(nRows,nCols);


        A.Transposed(R);

        double *work = new double[lwork];


        CVector TAU(k);

        dgerqf_(&nRows,&nCols,R[0],&nRows,&TAU[0],work,&lwork,&info);
        assert(info==0);

        Q=R;

        nCols=MAX(nCols,nRows);

        dorgrq_(&nRows,&nCols,&k,Q[0],&nRows,&TAU[0],work,&lwork,&info);
        assert(info==0);

        
        delete work;

        R.Transpose();
        Q.Transpose();

        //Set the zeros of R
        double * pRow;
        int ColStop;
        for(int cRow=1;cRow<R.Rows();cRow++)
        {
                pRow=R[cRow];
                ColStop=MIN(cRow,R.Cols());
                for(int cCol=0;cCol<ColStop;cCol++)
                {
                        pRow[cCol]=0;
                }
        }

}


}