dggbal.c

来自「InsightToolkit-1.4.0(有大量的优化算法程序)」· C语言 代码 · 共 515 行 · 第 1/2 页

            if (a[i + j * a_dim1] != 0. || b[i + j * b_dim1] != 0.) {
                goto L150;
            }
        }
        i = ip1 - 1;
L140:
        m = k;
        iflow = 2;
        goto L160;
L150:
        ;
    }
    goto L190;

/*     Permute rows M and I */

L160:
    lscale[m] = (doublereal) i;
    if (i == m) {
        goto L170;
    }
    i__1 = *n - k + 1;
    dswap_(&i__1, &a[i + k * a_dim1], lda, &a[m + k * a_dim1], lda);
    dswap_(&i__1, &b[i + k * b_dim1], ldb, &b[m + k * b_dim1], ldb);

/*     Permute columns M and J */

L170:
    rscale[m] = (doublereal) j;
    if (j == m) {
        goto L180;
    }
    dswap_(&l, &a[j * a_dim1 + 1], &c__1, &a[m * a_dim1 + 1], &c__1);
    dswap_(&l, &b[j * b_dim1 + 1], &c__1, &b[m * b_dim1 + 1], &c__1);

L180:
    switch ((int)iflow) {
        case 1:  goto L20;
        case 2:  goto L90;
    }

L190:
    *ilo = k;
    *ihi = l;

    if (*ilo == *ihi) {
        return;
    }

    if (lsame_(job, "P")) {
        return;
    }

/*     Balance the submatrix in rows ILO to IHI. */

    nr = *ihi - *ilo + 1;
    for (i = *ilo; i <= *ihi; ++i) {
        rscale[i] = 0.;
        lscale[i] = 0.;

        work[i] = 0.;
        work[i + *n] = 0.;
        work[i + *n * 2] = 0.;
        work[i + *n * 3] = 0.;
        work[i + *n * 4] = 0.;
        work[i + *n * 5] = 0.;
    }

/*     Compute right side vector in resulting linear equations */

    basl = d_lg10(&c_b34);
    for (i = *ilo; i <= *ihi; ++i) {
        for (j = *ilo; j <= *ihi; ++j) {
            tb = b[i + j * b_dim1];
            ta = a[i + j * a_dim1];
            if (ta == 0.) {
                goto L210;
            }
            d__1 = abs(ta);
            ta = d_lg10(&d__1) / basl;
L210:
            if (tb == 0.) {
                goto L220;
            }
            d__1 = abs(tb);
            tb = d_lg10(&d__1) / basl;
L220:
            work[i + *n * 4] -= ta + tb;
            work[j + *n * 5] -= ta + tb;
        }
    }

    coef = 1. / (doublereal) (nr << 1);
    coef2 = coef * coef;
    coef5 = coef2 * .5;
    nrp2 = nr + 2;
    beta = 0.;
    it = 1;

/*     Start generalized conjugate gradient iteration */

L250:

    gamma = ddot_(&nr, &work[*ilo + *n * 4], &c__1, &work[*ilo + *n * 4], &c__1)
          + ddot_(&nr, &work[*ilo + *n * 5], &c__1, &work[*ilo + *n * 5], &c__1);

    ew = 0.;
    ewc = 0.;
    for (i = *ilo; i <= *ihi; ++i) {
        ew +=  work[i + *n * 4];
        ewc += work[i + *n * 5];
    }

    gamma = coef * gamma - coef2 * (ew*ew + ewc*ewc) - coef5 * ((ew-ewc)*(ew-ewc));
    if (gamma == 0.) {
        goto L350;
    }
    if (it != 1) {
        beta = gamma / pgamma;
    }
    t = coef5 * (ewc - ew * 3.);
    tc = coef5 * (ew - ewc * 3.);

    dscal_(&nr, &beta, &work[*ilo], &c__1);
    dscal_(&nr, &beta, &work[*ilo + *n], &c__1);

    daxpy_(&nr, &coef, &work[*ilo + *n * 4], &c__1, &work[*ilo + *n], &c__1);
    daxpy_(&nr, &coef, &work[*ilo + *n * 5], &c__1, &work[*ilo], &c__1);

    for (i = *ilo; i <= *ihi; ++i) {
        work[i] += tc;
        work[i + *n] += t;
    }

/*     Apply matrix to vector */

    for (i = *ilo; i <= *ihi; ++i) {
        kount = 0;
        sum = 0.;
        for (j = *ilo; j <= *ihi; ++j) {
            if (a[i + j * a_dim1] == 0.) {
                goto L280;
            }
            ++kount;
            sum += work[j];
L280:
            if (b[i + j * b_dim1] == 0.) {
                goto L290;
            }
            ++kount;
            sum += work[j];
L290:
            ;
        }
        work[i + *n * 2] = (doublereal) kount * work[i + *n] + sum;
    }

    for (j = *ilo; j <= *ihi; ++j) {
        kount = 0;
        sum = 0.;
        for (i = *ilo; i <= *ihi; ++i) {
            if (a[i + j * a_dim1] == 0.) {
                goto L310;
            }
            ++kount;
            sum += work[i + *n];
L310:
            if (b[i + j * b_dim1] == 0.) {
                goto L320;
            }
            ++kount;
            sum += work[i + *n];
L320:
            ;
        }
        work[j + *n * 3] = (doublereal) kount * work[j] + sum;
    }

    sum = ddot_(&nr, &work[*ilo + *n], &c__1, &work[*ilo + *n * 2], &c__1)
        + ddot_(&nr, &work[*ilo     ], &c__1, &work[*ilo + *n * 3], &c__1);
    alpha = gamma / sum;

/*     Determine correction to current iteration */

    cmax = 0.;
    for (i = *ilo; i <= *ihi; ++i) {
        cor = alpha * work[i + *n];
        if (abs(cor) > cmax) {
            cmax = abs(cor);
        }
        lscale[i] += cor;
        cor = alpha * work[i];
        if (abs(cor) > cmax) {
            cmax = abs(cor);
        }
        rscale[i] += cor;
    }
    if (cmax < .5) {
        goto L350;
    }

    d__1 = -alpha;
    daxpy_(&nr, &d__1, &work[*ilo + *n * 2], &c__1, &work[*ilo + *n * 4], &c__1);
    d__1 = -alpha;
    daxpy_(&nr, &d__1, &work[*ilo + *n * 3], &c__1, &work[*ilo + *n * 5], &c__1);

    pgamma = gamma;
    ++it;
    if (it <= nrp2) {
        goto L250;
    }

/*     End generalized conjugate gradient iteration */

L350:
    sfmin = dlamch_("S");
    sfmax = 1. / sfmin;
    lsfmin = (integer) (d_lg10(&sfmin) / basl + 1.);
    lsfmax = (integer) (d_lg10(&sfmax) / basl);
    for (i = *ilo; i <= *ihi; ++i) {
        i__1 = *n - *ilo + 1;
        irab = idamax_(&i__1, &a[i + *ilo * a_dim1], lda);
        rab = abs(a[i + (irab + *ilo - 1) * a_dim1]);
        irab = idamax_(&i__1, &b[i + *ilo * b_dim1], lda);
        rab = max(rab, abs(b[i + (irab + *ilo - 1) * b_dim1]));
        d__1 = rab + sfmin;
        lrab = (integer) (d_lg10(&d__1) / basl + 1.);
        ir = (integer) (lscale[i] + d_sign(&c_b70, &lscale[i]));
        ir = min(min(max(ir,lsfmin),lsfmax),lsfmax-lrab);
        lscale[i] = pow_di(&c_b34, &ir);
        icab = idamax_(ihi, &a[i * a_dim1 + 1], &c__1);
        cab = abs(a[icab + i * a_dim1]);
        icab = idamax_(ihi, &b[i * b_dim1 + 1], &c__1);
        cab = max(cab, abs(b[icab + i * b_dim1]));
        d__1 = cab + sfmin;
        lcab = (integer) (d_lg10(&d__1) / basl + 1.);
        jc = (integer) (rscale[i] + d_sign(&c_b70, &rscale[i]));
        jc = min(min(max(jc,lsfmin),lsfmax),lsfmax-lcab);
        rscale[i] = pow_di(&c_b34, &jc);
    }

/*     Row scaling of matrices A and B */

    for (i = *ilo; i <= *ihi; ++i) {
        i__1 = *n - *ilo + 1;
        dscal_(&i__1, &lscale[i], &a[i + *ilo * a_dim1], lda);
        dscal_(&i__1, &lscale[i], &b[i + *ilo * b_dim1], ldb);
    }

/*     Column scaling of matrices A and B */

    for (j = *ilo; j <= *ihi; ++j) {
        dscal_(ihi, &rscale[j], &a[j * a_dim1 + 1], &c__1);
        dscal_(ihi, &rscale[j], &b[j * b_dim1 + 1], &c__1);
    }
} /* dggbal_ */