BiconjugateGradient.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __solve_BiconjugateGradient_h__
#define __solve_BiconjugateGradient_h__

#define BCG_DEBUG       FALSE
#define BCG_TRACE       FALSE
#define BCG_VERIFY      (FE_CODEGEN<=FE_DEBUG)

namespace fe
{
namespace ext
{

/**************************************************************************//**
    @brief solve Ax=b for x

    @ingroup solve

    Uses Biconjugate-Gradient.  The matrix must be positive-definite,
    but not necessarily symmetric.  For symmetric matrices, a regular
    Conjugate-Gradient can be about twice as fast.

    The arguments are templated, so any argument types should work,
    given that they have the appropriate methods and operators.

    TODO try seeding with previous x instead of clearing to zero.
*//***************************************************************************/
template <typename MATRIX, typename VECTOR>
class BiconjugateGradient
{
    public:
                /// @brief Choice to alter matrix during solve
        enum    Preconditioning
                {
                    e_none,
                    e_diagonal_A
                };

                BiconjugateGradient(void):
                        m_threshold(1e-6f),
                        m_preconditioning(e_none)                           {}

        void    solve(VECTOR& x, const MATRIX& A, const VECTOR& b);

        void    setThreshold(F64 threshold) { m_threshold=threshold; }

        void    setPreconditioning(Preconditioning preconditioning)
                {   m_preconditioning=preconditioning; }

    private:
        VECTOR          r,r_1,r_2;          //* residual    (at k, k-1, k-2)
        VECTOR          rbar,rbar_1,rbar_2; //* second residual (r bar)
        VECTOR          p,p_1;              //* direction
        VECTOR          pbar,pbar_1;        //* second direction
        VECTOR          temp;               //* persistent temporary
        VECTOR          Ap;
        VECTOR          m_preconditionedVector;
        MATRIX          m_preconditionedMatrix;
        F64             m_threshold;
        Preconditioning m_preconditioning;
};

template <typename MATRIX, typename VECTOR>
inline void BiconjugateGradient<MATRIX,VECTOR>::solve(VECTOR& x,
        const MATRIX& A, const VECTOR& b)
{
    U32 N=size(b);
    if(size(x)!=N)
    {
        x=b;        // adopt size
    }
    if(size(Ap)!=N)
    {
        Ap=b;       // adopt size
    }
    set(x);

    F64 dot_r_1;

#if BCG_DEBUG
    feLog("\nA\n%s\nb=<%s>\n",fe::print(A).c_str(),fe::print(b).c_str());
#endif

    if(magnitudeSquared(b)<m_threshold)
    {
#if BCG_DEBUG
        feLog("BiconjugateGradient::solve has trivial solution\n");
#endif
        return;
    }

    const MATRIX* pA=&A;
    const VECTOR* pb=&b;
    if(m_preconditioning==e_diagonal_A)
    {
        const MATRIX& preconditioner=A;
        premultiplyInverseDiagonal(m_preconditionedMatrix,preconditioner,A);
        pA=&m_preconditionedMatrix;

        premultiplyInverseDiagonal(m_preconditionedVector,preconditioner,b);
        pb=&m_preconditionedVector;

#if BCG_DEBUG
        feLog("A'\n%s\nb`=<%s>\n",fe::print(*pA).c_str(),fe::print(*pb).c_str());
#endif
    }

    for(U32 k=1;k<=N;k++)
    {
        if(k==1)
        {
            p= *pb;
            r_1=p;

            pbar= *pb;
            rbar_1=pbar;

            dot_r_1=dot(rbar_1,r_1);
        }
        else
        {
            r_2=r_1;
            r_1=r;
            p_1=p;

            rbar_2=rbar_1;
            rbar_1=rbar;
            pbar_1=pbar;

            dot_r_1=dot(rbar_1,r_1);

            F64 beta=dot_r_1/dot(rbar_2,r_2);

//          p=r_1+beta*p_1;
            temp=p_1;
            temp*=beta;
            p=r_1;
            p+=temp;

//          pbar=rbar_1+beta*pbar_1;
            temp=pbar_1;
            temp*=beta;
            pbar=rbar_1;
            pbar+=temp;
        }

        transformVector(*pA,p,Ap);
        F64 alpha=dot_r_1/dot(pbar,Ap);

#if BCG_TRACE==FALSE
        if(magnitudeSquared(p)==0.0f)
#endif
        {
            feLog("\n%d alpha=%.6G x<%s>\n",k,alpha,fe::print(x).c_str());
            feLog("r<%s>\nr_1<%s>\nr_2<%s>\n",
                    fe::print(r).c_str(),fe::print(r_1).c_str(),fe::print(r_2).c_str());
            feLog("rbar<%s>\nrbar_1<%s>\nrbar_2<%s>\n",
                    fe::print(rbar).c_str(),fe::print(rbar_1).c_str(),
                    fe::print(rbar_2).c_str());
            feLog("p<%s>\np_1<%s>\nA*p<%s>\n",
                    fe::print(p).c_str(),fe::print(p_1).c_str(),fe::print(*pA*p).c_str());
            feLog("pbar<%s>\npbar_1<%s>\n",
                    fe::print(pbar).c_str(),fe::print(pbar_1).c_str());
        }

        if(magnitudeSquared(p)==0.0f)
        {
            feX("BiconjugateGradient::solve","direction lost its magnitude");
        }

//      x+=alpha*p;
        temp=p;
        temp*=alpha;
        x+=temp;

//      r=r_1-alpha*(Ap);
        temp=Ap;
        temp*=alpha;
        r=r_1;
        r-=temp;

//      rbar=rbar_1-alpha*transposeMultiply(*pA,pbar);
        transposeTransformVector(*pA,pbar,temp);
        temp*=alpha;
        rbar=rbar_1;
        rbar-=temp;

        if(magnitudeSquared(r)<m_threshold)
        {
#if BCG_DEBUG
            feLog("BiconjugateGradient::solve early solve %d/%d\n",k,N);
#endif
            break;
        }

        if(k==N)
        {
            feLog("BiconjugateGradient::solve ran %d/%d\n",k,N);
        }
    }


#if BCG_DEBUG
    feLog("\nx=<%s>\nA*x=<%s>\n",fe::print(x).c_str(),fe::print(A*x).c_str());
#endif

#if BCG_VERIFY
    BWORD invalid=FALSE;
    for(U32 k=0;k<N;k++)
    {
        if(FE_INVALID_SCALAR(x[k]))
        {
            invalid=TRUE;
        }
    }
    VECTOR Ax=A*x;
    F64 distance=magnitude(Ax-b);
    if(invalid || distance>1.0f)
    {
        feLog("BiconjugateGradient::solve failed to converge (dist=%.6G)\n",
                distance);
        if(size(x)<100)
        {
            feLog("  collecting state ...\n");
            feLog("A=\n%s\nx=<%s>\nA*x=<%s>\nb=<%s>\n",
                    fe::print(A).c_str(),fe::print(x).c_str(),
                    fe::print(Ax).c_str(),fe::print(b).c_str());
        }
//      feX("BiconjugateGradient::solve","failed to converge");
    }
#endif
}

/** @brief solve Ax=b (4x4 version)

    @relates Matrix
    */
template <typename T>
void solve(Vector<4,T> x, const Matrix<4,4,T>& A, const Vector<4,T> &b)
{
    Matrix<4,4,T> iA;
    invert(iA,A);

    x=iA*b;
}

} /* namespace ext */
} /* namespace fe */

#endif /* __solve_BiconjugateGradient_h__ */