diff options
author | Thomas Capricelli <orzel@freehackers.org> | 2009-11-26 04:06:40 +0100 |
---|---|---|
committer | Thomas Capricelli <orzel@freehackers.org> | 2009-11-26 04:06:40 +0100 |
commit | e18caf7a01db6b856f8ad4a798fb886bfcbefc32 (patch) | |
tree | 85d59f89d83101b3177b97e93a7f580a194baa7e /unsupported/Eigen | |
parent | e95f736402958e9fef27977d35389ef134f7e73d (diff) |
clean qrsolv
Diffstat (limited to 'unsupported/Eigen')
-rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/qrsolv.h | 100 |
1 files changed, 29 insertions, 71 deletions
diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h index cbb45107a..77786636c 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h +++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h @@ -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; - 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: - 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: + 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; + } 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__; + } /* 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 (sdiag[j] == 0. && nsing == n) nsing = j - 1; + if (nsing < n) wa[j] = 0.; } - if (nsing < 1) { - goto L150; - } - 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) { - sum += r__[i + j * r_dim1] * wa[i]; - /* L120: */ + if (nsing >= 1) + for (k = 1; k <= nsing; ++k) { + j = nsing - k + 1; + sum = 0.; + if (nsing>j) + for (i = j+1; i <= nsing; ++i) + sum += r__[i + j * r_dim1] * wa[i]; + wa[j] = (wa[j] - sum) / sdiag[j]; } -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_ */ +} |