GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-12 11:49:02 +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

100
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; int r_dim1, r_offset;
/* Local variables */ /* Local variables */
int i, j, k, l, jp1, kp1; int i, j, k, l;
Scalar tan__, cos__, sin__, sum, temp, cotan; Scalar tan__, cos__, sin__, sum, temp, cotan;
int nsing; int nsing;
Scalar qtbpj; 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. */ /* in particular, save the diagonal elements of r in x. */
for (j = 1; j <= n; ++j) { 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]; r__[i + j * r_dim1] = r__[j + i * r_dim1];
/* L10: */
}
x[j] = r__[j + j * r_dim1]; x[j] = r__[j + j * r_dim1];
wa[j] = qtb[j]; wa[j] = qtb[j];
/* L20: */
} }
/* eliminate the diagonal matrix d using a givens rotation. */ /* 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. */ /* diagonal element using p from the qr factorization. */
l = ipvt[j]; l = ipvt[j];
if (diag[l] == 0.) { if (diag[l] == 0.)
goto L90; goto L90;
} for (k = j; k <= n; ++k)
for (k = j; k <= n; ++k) {
sdiag[k] = 0.; sdiag[k] = 0.;
/* L30: */
}
sdiag[j] = diag[l]; sdiag[j] = diag[l];
/* the transformations to eliminate the row of d */ /* the transformations to eliminate the row of d */
@ -62,25 +56,21 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
qtbpj = 0.; qtbpj = 0.;
for (k = j; k <= n; ++k) { for (k = j; k <= n; ++k) {
/* determine a givens rotation which eliminates the */ /* determine a givens rotation which eliminates the */
/* appropriate element in the current row of d. */ /* appropriate element in the current row of d. */
if (sdiag[k] == 0.) if (sdiag[k] == 0.)
goto L70; continue;
if ( ei_abs(r__[k + k * r_dim1]) >= ei_abs(sdiag[k])) if ( ei_abs(r__[k + k * r_dim1]) < ei_abs(sdiag[k])) {
goto L40; cotan = r__[k + k * r_dim1] / sdiag[k];
cotan = r__[k + k * r_dim1] / sdiag[k]; /* Computing 2nd power */
/* Computing 2nd power */ sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan)); cos__ = sin__ * cotan;
cos__ = sin__ * cotan; } else {
goto L50; tan__ = sdiag[k] / r__[k + k * r_dim1];
L40: /* Computing 2nd power */
tan__ = sdiag[k] / r__[k + k * r_dim1]; cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
/* Computing 2nd power */ sin__ = cos__ * tan__;
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 */ /* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */ /* the modified element of ((q transpose)*b,0). */
@ -92,21 +82,11 @@ L50:
wa[k] = temp; wa[k] = temp;
/* accumulate the tranformation in the row of s. */ /* accumulate the tranformation in the row of s. */
for (i = k+1; i <= n; ++i) {
kp1 = k + 1;
if (n < kp1) {
goto L70;
}
for (i = kp1; i <= n; ++i) {
temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i]; temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i];
sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[ sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[i];
i];
r__[i + k * r_dim1] = temp; r__[i + k * r_dim1] = temp;
/* L60: */
} }
L70:
/* L80: */
;
} }
L90: L90:
@ -115,7 +95,6 @@ L90:
sdiag[j] = r__[j + j * r_dim1]; sdiag[j] = r__[j + j * r_dim1];
r__[j + j * r_dim1] = x[j]; r__[j + j * r_dim1] = x[j];
/* L100: */
} }
/* solve the triangular system for z. if the system is */ /* solve the triangular system for z. if the system is */
@ -123,44 +102,23 @@ L90:
nsing = n; nsing = n;
for (j = 1; j <= n; ++j) { for (j = 1; j <= n; ++j) {
if (sdiag[j] == 0. && nsing == n) { if (sdiag[j] == 0. && nsing == n) nsing = j - 1;
nsing = j - 1; if (nsing < n) wa[j] = 0.;
}
if (nsing < n) {
wa[j] = 0.;
}
/* L110: */
} }
if (nsing < 1) { if (nsing >= 1)
goto L150; for (k = 1; k <= nsing; ++k) {
} j = nsing - k + 1;
for (k = 1; k <= nsing; ++k) { sum = 0.;
j = nsing - k + 1; if (nsing>j)
sum = 0.; for (i = j+1; i <= nsing; ++i)
jp1 = j + 1; sum += r__[i + j * r_dim1] * wa[i];
if (nsing < jp1) { wa[j] = (wa[j] - sum) / sdiag[j];
goto L130;
} }
for (i = jp1; 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. */ /* permute the components of z back to components of x. */
for (j = 1; j <= n; ++j) { for (j = 1; j <= n; ++j) {
l = ipvt[j]; l = ipvt[j];
x[l] = wa[j]; x[l] = wa[j];
/* L160: */
} }
return; }
/* last card of subroutine qrsolv. */
} /* qrsolv_ */