| // IWYU pragma: private |
| #include "./InternalHeaderCheck.h" |
| |
| namespace Eigen { |
| |
| namespace internal { |
| |
| // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt |
| template <typename Scalar> |
| void qrsolv(Matrix<Scalar, Dynamic, Dynamic> &s, |
| // TODO : use a PermutationMatrix once lmpar is no more: |
| const VectorXi &ipvt, const Matrix<Scalar, Dynamic, 1> &diag, const Matrix<Scalar, Dynamic, 1> &qtb, |
| Matrix<Scalar, Dynamic, 1> &x, Matrix<Scalar, Dynamic, 1> &sdiag) |
| |
| { |
| typedef DenseIndex Index; |
| |
| /* Local variables */ |
| Index i, j, k, l; |
| Scalar temp; |
| Index n = s.cols(); |
| Matrix<Scalar, Dynamic, 1> wa(n); |
| JacobiRotation<Scalar> givens; |
| |
| /* Function Body */ |
| // the following will only change the lower triangular part of s, including |
| // the diagonal, though the diagonal is restored afterward |
| |
| /* copy r and (q transpose)*b to preserve input and initialize s. */ |
| /* in particular, save the diagonal elements of r in x. */ |
| x = s.diagonal(); |
| wa = qtb; |
| |
| s.topLeftCorner(n, n).template triangularView<StrictlyLower>() = s.topLeftCorner(n, n).transpose(); |
| |
| /* eliminate the diagonal matrix d using a givens rotation. */ |
| for (j = 0; 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.) break; |
| sdiag.tail(n - j).setZero(); |
| 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. */ |
| Scalar qtbpj = 0.; |
| for (k = j; k < n; ++k) { |
| /* determine a givens rotation which eliminates the */ |
| /* appropriate element in the current row of d. */ |
| givens.makeGivens(-s(k, k), sdiag[k]); |
| |
| /* compute the modified diagonal element of r and */ |
| /* the modified element of ((q transpose)*b,0). */ |
| s(k, k) = givens.c() * s(k, k) + givens.s() * sdiag[k]; |
| temp = givens.c() * wa[k] + givens.s() * qtbpj; |
| qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; |
| wa[k] = temp; |
| |
| /* accumulate the transformation in the row of s. */ |
| for (i = k + 1; i < n; ++i) { |
| temp = givens.c() * s(i, k) + givens.s() * sdiag[i]; |
| sdiag[i] = -givens.s() * s(i, k) + givens.c() * sdiag[i]; |
| s(i, k) = temp; |
| } |
| } |
| } |
| |
| /* solve the triangular system for z. if the system is */ |
| /* singular, then obtain a least squares solution. */ |
| Index nsing; |
| for (nsing = 0; nsing < n && sdiag[nsing] != 0; nsing++) { |
| } |
| |
| wa.tail(n - nsing).setZero(); |
| s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); |
| |
| // restore |
| sdiag = s.diagonal(); |
| s.diagonal() = x; |
| |
| /* permute the components of z back to components of x. */ |
| for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; |
| } |
| |
| } // end namespace internal |
| |
| } // end namespace Eigen |