eigen/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h

    template <typename Scalar>
void ei_qrsolv(int n, Scalar *r__, int ldr, 
        const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x, 
        Scalar *sdiag, Scalar *wa)
{
    /* System generated locals */
    int r_dim1, r_offset;

    /* Local variables */
    int i, j, k, l, jp1, kp1;
    Scalar tan__, cos__, sin__, sum, temp, cotan;
    int nsing;
    Scalar qtbpj;

    /* Parameter adjustments */
    --wa;
    --sdiag;
    --x;
    --qtb;
    --diag;
    --ipvt;
    r_dim1 = ldr;
    r_offset = 1 + r_dim1 * 1;
    r__ -= r_offset;

    /* Function Body */

    /*     copy r and (q transpose)*b to preserve input and initialize s. */
    /*     in particular, save the diagonal elements of r in x. */

    for (j = 1; j <= n; ++j) {
        for (i = j; i <= n; ++i) {
            r__[i + j * r_dim1] = r__[j + i * r_dim1];
            /* L10: */
        }
        x[j] = r__[j + j * r_dim1];
        wa[j] = qtb[j];
        /* L20: */
    }

    /*     eliminate the diagonal matrix d using a givens rotation. */

    for (j = 1; j <= n; ++j) {

        /*        prepare the row of d to be eliminated, locating the */
        /*        diagonal element using p from the qr factorization. */

        l = ipvt[j];
        if (diag[l] == 0.) {
            goto L90;
        }
        for (k = j; k <= n; ++k) {
            sdiag[k] = 0.;
            /* L30: */
        }
        sdiag[j] = diag[l];

        /*        the transformations to eliminate the row of d */
        /*        modify only a single element of (q transpose)*b */
        /*        beyond the first n, which is initially zero. */

        qtbpj = 0.;
        for (k = j; k <= n; ++k) {

            /*           determine a givens rotation which eliminates the */
            /*           appropriate element in the current row of d. */

            if (sdiag[k] == 0.)
                goto L70;
            if ( ei_abs(r__[k + k * r_dim1]) >= ei_abs(sdiag[k]))
                goto L40;
            cotan = r__[k + k * r_dim1] / sdiag[k];
            /* Computing 2nd power */
            sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
            cos__ = sin__ * cotan;
            goto L50;
L40:
            tan__ = sdiag[k] / r__[k + k * r_dim1];
            /* Computing 2nd power */
            cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
            sin__ = cos__ * tan__;
L50:

            /*           compute the modified diagonal element of r and */
            /*           the modified element of ((q transpose)*b,0). */

            r__[k + k * r_dim1] = cos__ * r__[k + k * r_dim1] + sin__ * sdiag[
                k];
            temp = cos__ * wa[k] + sin__ * qtbpj;
            qtbpj = -sin__ * wa[k] + cos__ * qtbpj;
            wa[k] = temp;

            /*           accumulate the tranformation in the row of s. */

            kp1 = k + 1;
            if (n < kp1) {
                goto L70;
            }
            for (i = kp1; i <= n; ++i) {
                temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i];
                sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[
                    i];
                r__[i + k * r_dim1] = temp;
                /* L60: */
            }
L70:
            /* L80: */
            ;
        }
L90:

        /*        store the diagonal element of s and restore */
        /*        the corresponding diagonal element of r. */

        sdiag[j] = r__[j + j * r_dim1];
        r__[j + j * r_dim1] = x[j];
        /* L100: */
    }

    /*     solve the triangular system for z. if the system is */
    /*     singular, then obtain a least squares solution. */

    nsing = n;
    for (j = 1; j <= n; ++j) {
        if (sdiag[j] == 0. && nsing == n) {
            nsing = j - 1;
        }
        if (nsing < n) {
            wa[j] = 0.;
        }
        /* L110: */
    }
    if (nsing < 1) {
        goto L150;
    }
    for (k = 1; k <= nsing; ++k) {
        j = nsing - k + 1;
        sum = 0.;
        jp1 = j + 1;
        if (nsing < jp1) {
            goto L130;
        }
        for (i = jp1; i <= nsing; ++i) {
            sum += r__[i + j * r_dim1] * wa[i];
            /* L120: */
        }
L130:
        wa[j] = (wa[j] - sum) / sdiag[j];
        /* L140: */
    }
L150:

    /*     permute the components of z back to components of x. */

    for (j = 1; j <= n; ++j) {
        l = ipvt[j];
        x[l] = wa[j];
        /* L160: */
    }
    return;

    /*     last card of subroutine qrsolv. */

} /* qrsolv_ */