GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-12 03:39:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

clean qrsolv

Browse Source
This commit is contained in:
Thomas Capricelli 2009-11-26 04:06:40 +01:00
parent e95f736402
commit e18caf7a01
1 changed files with 29 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
View File

@ -8,7 +8,7 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
int r_dim1, r_offset;
/* Local variables */
int i, j, k, l, jp1, kp1;
int i, j, k, l;
Scalar tan__, cos__, sin__, sum, temp, cotan;
int nsing;
Scalar qtbpj;
@ -30,13 +30,10 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
/* in particular, save the diagonal elements of r in x. */
for (j = 1; j <= n; ++j) {
for (i = j; i <= n; ++i) {
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. */
@ -47,13 +44,10 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
/* diagonal element using p from the qr factorization. */
l = ipvt[j];
if (diag[l] == 0.) {
if (diag[l] == 0.)
goto L90;
}
for (k = j; k <= n; ++k) {
for (k = j; k <= n; ++k)
sdiag[k] = 0.;
/* L30: */
}
sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */
@ -62,25 +56,21 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
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;
continue;
if ( ei_abs(r__[k + k * r_dim1]) < ei_abs(sdiag[k])) {
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:
} else {
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). */
@ -92,21 +82,11 @@ L50:
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) {
for (i = k+1; i <= n; ++i) {
temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i];
sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[
i];
sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[i];
r__[i + k * r_dim1] = temp;
/* L60: */
}
L70:
/* L80: */
;
}
L90:
@ -115,7 +95,6 @@ L90:
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 */
@ -123,44 +102,23 @@ L90:
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;
if (sdiag[j] == 0. && nsing == n) nsing = j - 1;
if (nsing < n) wa[j] = 0.;
}
if (nsing >= 1)
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) {
if (nsing>j)
for (i = j+1; 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_ */
}