mirror of
https://gitlab.com/libeigen/eigen.git
synced 2026-04-10 11:34:33 +08:00
90 lines
3.0 KiB
C++
90 lines
3.0 KiB
C++
// 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
|