qr.h

高效的c++科学算法库

/*                                                           -*- c++ -*-
 ***********************************************************************
 * Scientific Library (GNU Public Licence)
 *
 * module sparse
 *
 * Author: Pierre Aubert [e-mail]
 *
 * $Id: qr.h,v 1.1 1998/07/24 10:52:22 paubert Exp $
 *
 * Suggestions: [e-mail]
 * Bugs:   [e-mail] 
 *
 * For more information, please see the sl++ Home Page:
 * http://wwwinfo.cern.ch/~ldeniau/sl.html 
 *
 ***********************************************************************
 */

#ifndef SL_ALGEBRA_LU_ROWMAJOR_H
#define SL_ALGEBRA_LU_ROWMAJOR_H

#ifdef HAVE_NAMESPACE
namespace sl 
{
#endif

    template <typename T_value, typename T_storage>
    void lu( Matrix<T_value,RowMajor,T_storage> & m, Matrix<Index,TVector> & indx)
    {
        typedef T_value value_t;
        
          // declarations
        Index i,j,k,jp;
        Index n=m.n();
        Index p=m.p();
        value_t tmp,t;
        
          // check for size
        if( n != p )
            {
                cerr << "sl/algebra/rowmajor/lu.h: lu() matrix must be square" << endl;
                throw eMatrixMustBeSquare;
            }
        
          // resize indx if needed
        indx.resize(n);
        
        for( j=1; j<= n; ++j )
            {
                  // find pivot in column j and  test for singularity.
                jp = j;
                tmp = abs( m(j,j) );
                
                for( i=j+1; i<=p; ++i )
                    if ( abs( m(i,j)) > t)
                        {
                            jp = i;
                            t = fabs( m(i,j));
                        }
                
                indx(j) = jp;
                
                  // jp now has the index of maximum element 
                  // of column j, below the diagonal
                
                if( m(jp,j) == 0.0 )                 
                    {
                        cerr << "sl/algebra/rowmajor/lu.h: lu() matrix is not positive definite, m(" << jp << "," << j << ")=0.0" << endl;
                        throw eMatrixIsNotPositiveDefinite;
                    }

                if( jp != j )            // swap rows j and jp
                    for (k=1; k<=n; k++)
                        {
                            t =  m(j,k);
                            m(j,k) =  m(jp,k);
                            m(jp,k) =t;
                        }
                
                if( j<p )                // compute elements j+1:M of jth column
                    {
                          // note  m(j,j), was  m(jp,p) previously which was
                          // guarranteed not to be zero (Label #1)
                          //
                        value_t recp =  1.0 /  m(j,j);
                        
                        for (k=j+1; k<=p; k++)
                            m(k,j) *= recp;
                    }
                
                
                if( j<n )
                    {
                          // rank-1 update to trailing submatrix:   E = E - x*y;
                          //
                          // E is the region  m(j+1:M, j+1:N)
                          // x is the column vector  m(j+1:M,j)
                          // y is row vector  m(j,j+1:N)
                        
                        Index ii,jj;
                        
                        for (ii=j+1; ii<=p; ii++)
                            for (jj=j+1; jj<=n; jj++)
                                m(ii,jj) -=  m(ii,j)* m(j,jj);
                    }
            }

        
    } // end lu()
        

    template <typename T_value, typename T_storage>
    void luLowerSolve( Matrix<T_value,RowMajor,T_storage> const& m,
                       Matrix<Index,TVector>              const& indx,
                       Matrix<T_value,TVector>            const& B,
                       Matrix<T_value,TVector>                 & X )
    {
        typedef T_value value_t;
        value_t tmp;
        Index i,j,ip,ii;
        
        X = B;
        for( i=1; i<=m.n() ; i++) 
            {
                ip=indx(i);
                tmp=X(ip);
                X(ip)=X(i);
                if( ii )
                    for( j=ii; j<=i-1 ; j++ ) 
                        tmp -= m(i,j)*X(j);
                else
                    if( tmp )
                        ii=i;
                X(i)=tmp;
            }
    }

    template <typename T_value, typename T_storage>
    void luUpperSolve( Matrix<T_value,RowMajor,T_storage> const& A ,
                       Matrix<Index,TVector>              const& indx,
                       Matrix<T_value,TVector>                 & X )
    {

        typedef T_value value_t;
        Index i,j,n=A.n();
        value_t tmp;

        for( i=n ; i>=1 ; i-- ) 
            {
                tmp=X(i);
                for( j=i+1 ; j<=n ; j++ ) 
                    tmp -= A(i,j)*X(j);
                X(i)=tmp/A(i,i);
            }
    }


    template <typename T_value, typename T_storage>
    void luSolve( Matrix<T_value,RowMajor,T_storage> & A ,
                  Matrix<Index,TVector>              & indx,
                  Matrix<T_value,TVector>       const& B,
                  Matrix<T_value,TVector>            & X )
    {
        try {
            lu(A,indx);
        }
        catch( ErrorCode error ) 
            {
                if( error == eMatrixIsNotPositiveDefinite )
                    cerr << "sl/algebra/rowmajor/lu.h: luSolve() matrix is not positive definite" << endl;
                else if( error == eMatrixMustBeSquare )
                    cerr << "sl/algebra/rowmajor/lu.h: luSolve() matrix must be square" << endl;
                else
                    cerr << "sl/algebra/rowmajor/lu.h: luSolve() unknown exception" << endl;
                throw;
            }
        
        luLowerSolve(A,indx,B,X);
        luUpperSolve(A,indx,X);
    }

#ifdef HAVE_NAMESPACE
}
#endif

#endif
// end of SL_ALGEBRA_LU_ROWMAJOR_H definition(s)
// do not write anything after this line!