예제 #1
0
파일: Solver.cpp 프로젝트: 93i/godot
/** Bi-conjugate gradient stabilized method. */
int BiCGSTABPrecondSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, const IPreconditioner &M, float epsilon ) {
    piDebugCheck( A.IsSquare() );
    piDebugCheck( A.Width() == b.Dim() );
    piDebugCheck( A.Width() == x.Dim() );

    int i = 0;
    const int D = A.Width();
    const int i_max = D;
    //	const int i_max = 1000;


    float resid;
    float rho_1 = 0;
    float rho_2 = 0;
    float alpha = 0;
    float beta = 0;
    float omega = 0;

    DenseVector p(D);
    DenseVector phat(D);
    DenseVector s(D);
    DenseVector shat(D);
    DenseVector t(D);
    DenseVector v(D);

    DenseVector r(D);
    DenseVector rtilde(D);

    DenseVector tmp(D);

    // r = b - A·x;
    A.Product( x, tmp );
    r.Sub( b, tmp );

    // rtilde = r
    rtilde.Set( r );


    float normb = b.Norm();
    if( normb == 0.0 ) normb = 1;

    // test convergence
    resid = r.Norm() / normb;
    if( resid < epsilon ) {
        // method converges?
        return 0;
    }


    while( i<i_max ) {

        i++;

        rho_1 = DenseVectorDotProduct( rtilde, r );
        if( rho_1 == 0 ) {
            // method fails
            return -i;
        }


        if( i == 1 ) {
            p.Set( r );
        }
        else {
            beta = (rho_1 / rho_2) * (alpha / omega);

            // p = r + beta * (p - omega * v);
            p.Mad( p, v, -omega );
            p.Mad( r, p, beta );
        }

        //phat = M.solve(p);
        //phat.Set( p );
        M.Precond( &phat, p );

        //v = A * phat;
        A.Product( phat, v );

        alpha = rho_1 / DenseVectorDotProduct( rtilde, v );

        // s = r - alpha * v;
        s.Mad( r, v, -alpha );


        resid = s.Norm() / normb;

        //printf( "--- Iteration %d: residual = %f\n", i, resid );

        if( resid < epsilon ) {
            // x += alpha * phat;
            x.Mad( x, phat, alpha );
            return i;
        }

        //shat = M.solve(s);
        //shat.Set( s );
        M.Precond( &shat, s );

        //t = A * shat;
        A.Product( shat, t );

        omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t );

        // x += alpha * phat + omega * shat;
        x.Mad( x, shat, omega );
        x.Mad( x, phat, alpha );

        //r = s - omega * t;
        r.Mad( s, t, -omega );

        rho_2 = rho_1;

        resid = r.Norm() / normb;
        if( resid < epsilon ) {
            return i;
        }

        if( omega == 0 ) {
            return -i;	// ???
        }
    }

    return i;
}
예제 #2
0
파일: Solver.cpp 프로젝트: 93i/godot
/** Bi-conjugate gradient method.  */
MATHLIB_API int BiConjugateGradientSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) {
    piDebugCheck( A.IsSquare() );
    piDebugCheck( A.Width() == b.Dim() );
    piDebugCheck( A.Width() == x.Dim() );

    int i = 0;
    const int D = A.Width();
    const int i_max = 4 * D;

    float resid;
    float rho_1 = 0;
    float rho_2 = 0;
    float alpha;
    float beta;

    DenseVector r(D);
    DenseVector rtilde(D);
    DenseVector p(D);
    DenseVector ptilde(D);
    DenseVector q(D);
    DenseVector qtilde(D);
    DenseVector tmp(D);	// temporal vector.

    // r = b - A·x;
    A.Product( x, tmp );
    r.Sub( b, tmp );

    // rtilde = r
    rtilde.Set( r );

    // p = r;
    p.Set( r );

    // ptilde = rtilde
    ptilde.Set( rtilde );



    float normb = b.Norm();
    if( normb == 0.0 ) normb = 1;

    // test convergence
    resid = r.Norm() / normb;
    if( resid < epsilon ) {
        // method converges?
        return 0;
    }


    while( i < i_max ) {

        i++;

        rho_1 = DenseVectorDotProduct( r, rtilde );

        if( rho_1 == 0 ) {
            // method fails.
            return -i;
        }

        if (i == 1) {
            p.Set( r );
            ptilde.Set( rtilde );
        } 
        else {
            beta = rho_1 / rho_2;

            // p = r + beta * p;
            p.Mad( r, p, beta );

            // ptilde = ztilde + beta * ptilde;
            ptilde.Mad( rtilde, ptilde, beta );
        }

        // q = A * p;
        A.Product( p, q );

        // qtilde = A^t * ptilde;
        A.TransProduct( ptilde, qtilde );

        alpha = rho_1 / DenseVectorDotProduct( ptilde, q );

        // x += alpha * p;
        x.Mad( x, p, alpha );

        // r -= alpha * q;
        r.Mad( r, q, -alpha );

        // rtilde -= alpha * qtilde;
        rtilde.Mad( rtilde, qtilde, -alpha );

        rho_2 = rho_1;

        // test convergence
        resid = r.Norm() / normb;
        if( resid < epsilon ) {
            // method converges
            return i;
        }
    }

    return i;
}