1namespace Eigen {
2
3namespace internal {
4
5// TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
6template <typename Scalar>
7void qrsolv(
8        Matrix< Scalar, Dynamic, Dynamic > &s,
9        // TODO : use a PermutationMatrix once lmpar is no more:
10        const VectorXi &ipvt,
11        const Matrix< Scalar, Dynamic, 1 >  &diag,
12        const Matrix< Scalar, Dynamic, 1 >  &qtb,
13        Matrix< Scalar, Dynamic, 1 >  &x,
14        Matrix< Scalar, Dynamic, 1 >  &sdiag)
15
16{
17    typedef DenseIndex Index;
18
19    /* Local variables */
20    Index i, j, k, l;
21    Scalar temp;
22    Index n = s.cols();
23    Matrix< Scalar, Dynamic, 1 >  wa(n);
24    JacobiRotation<Scalar> givens;
25
26    /* Function Body */
27    // the following will only change the lower triangular part of s, including
28    // the diagonal, though the diagonal is restored afterward
29
30    /*     copy r and (q transpose)*b to preserve input and initialize s. */
31    /*     in particular, save the diagonal elements of r in x. */
32    x = s.diagonal();
33    wa = qtb;
34
35    s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
36
37    /*     eliminate the diagonal matrix d using a givens rotation. */
38    for (j = 0; j < n; ++j) {
39
40        /*        prepare the row of d to be eliminated, locating the */
41        /*        diagonal element using p from the qr factorization. */
42        l = ipvt[j];
43        if (diag[l] == 0.)
44            break;
45        sdiag.tail(n-j).setZero();
46        sdiag[j] = diag[l];
47
48        /*        the transformations to eliminate the row of d */
49        /*        modify only a single element of (q transpose)*b */
50        /*        beyond the first n, which is initially zero. */
51        Scalar qtbpj = 0.;
52        for (k = j; k < n; ++k) {
53            /*           determine a givens rotation which eliminates the */
54            /*           appropriate element in the current row of d. */
55            givens.makeGivens(-s(k,k), sdiag[k]);
56
57            /*           compute the modified diagonal element of r and */
58            /*           the modified element of ((q transpose)*b,0). */
59            s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
60            temp = givens.c() * wa[k] + givens.s() * qtbpj;
61            qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
62            wa[k] = temp;
63
64            /*           accumulate the tranformation in the row of s. */
65            for (i = k+1; i<n; ++i) {
66                temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
67                sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
68                s(i,k) = temp;
69            }
70        }
71    }
72
73    /*     solve the triangular system for z. if the system is */
74    /*     singular, then obtain a least squares solution. */
75    Index nsing;
76    for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
77
78    wa.tail(n-nsing).setZero();
79    s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
80
81    // restore
82    sdiag = s.diagonal();
83    s.diagonal() = x;
84
85    /*     permute the components of z back to components of x. */
86    for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
87}
88
89} // end namespace internal
90
91} // end namespace Eigen
92