| Brian Silverman | 72890c2 | 2015-09-19 14:37:37 -0400 | [diff] [blame^] | 1 | namespace Eigen { |
| 2 | |
| 3 | namespace internal { |
| 4 | |
| 5 | // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt |
| 6 | template <typename Scalar> |
| 7 | void 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 |