diff options
Diffstat (limited to 'unsupported/Eigen/src/NonLinearOptimization/qrsolv.h')
| -rw-r--r-- | unsupported/Eigen/src/NonLinearOptimization/qrsolv.h | 170 |
1 files changed, 48 insertions, 122 deletions
diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h index 57884870c..1ee21d5ed 100644 --- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h +++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h @@ -1,166 +1,92 @@ - template <typename Scalar> -void ei_qrsolv(int n, Scalar *r__, int ldr, - const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x, - Scalar *sdiag, Scalar *wa) -{ - /* System generated locals */ - int r_dim1, r_offset; +template <typename Scalar> +void ei_qrsolv( + Matrix< Scalar, Dynamic, Dynamic > &r, + VectorXi &ipvt, // TODO : const once ipvt mess fixed + const Matrix< Scalar, Dynamic, 1 > &diag, + const Matrix< Scalar, Dynamic, 1 > &qtb, + Matrix< Scalar, Dynamic, 1 > &x, + Matrix< Scalar, Dynamic, 1 > &sdiag) +{ /* Local variables */ - int i, j, k, l, jp1, kp1; - Scalar tan__, cos__, sin__, sum, temp, cotan; - int nsing; - Scalar qtbpj; - - /* Parameter adjustments */ - --wa; - --sdiag; - --x; - --qtb; - --diag; - --ipvt; - r_dim1 = ldr; - r_offset = 1 + r_dim1 * 1; - r__ -= r_offset; + int i, j, k, l; + Scalar sum, temp; + int n = r.cols(); + Matrix< Scalar, Dynamic, 1 > wa(n); /* Function Body */ /* copy r and (q transpose)*b to preserve input and initialize s. */ /* in particular, save the diagonal elements of r in x. */ - for (j = 1; j <= n; ++j) { - 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: */ - } + x = r.diagonal(); + wa = qtb; - /* eliminate the diagonal matrix d using a givens rotation. */ + for (j = 0; j < n; ++j) + for (i = j+1; i < n; ++i) + r(i,j) = r(j,i); - for (j = 1; j <= n; ++j) { + /* 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.) { - goto L90; - } - for (k = j; k <= n; ++k) { - sdiag[k] = 0.; - /* L30: */ - } + if (diag[l] == 0.) + break; + sdiag.segment(j,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. */ - qtbpj = 0.; - for (k = j; k <= n; ++k) { - + Scalar 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: + PlanarRotation<Scalar> givens; + givens.makeGivens(-r(k,k), sdiag[k]); /* compute the modified diagonal element of r and */ /* the modified element of ((q transpose)*b,0). */ - r__[k + k * r_dim1] = cos__ * r__[k + k * r_dim1] + sin__ * sdiag[ - k]; - temp = cos__ * wa[k] + sin__ * qtbpj; - qtbpj = -sin__ * wa[k] + cos__ * qtbpj; + r(k,k) = givens.c() * r(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 tranformation in the row of s. */ - - kp1 = k + 1; - if (n < kp1) { - goto L70; + for (i = k+1; i<n; ++i) { + temp = givens.c() * r(i,k) + givens.s() * sdiag[i]; + sdiag[i] = -givens.s() * r(i,k) + givens.c() * sdiag[i]; + r(i,k) = temp; } - for (i = kp1; i <= n; ++i) { - temp = cos__ * r__[i + k * r_dim1] + sin__ * sdiag[i]; - sdiag[i] = -sin__ * r__[i + k * r_dim1] + cos__ * sdiag[ - i]; - r__[i + k * r_dim1] = temp; - /* L60: */ - } -L70: - /* L80: */ - ; } -L90: - - /* store the diagonal element of s and restore */ - /* the corresponding diagonal element of r. */ - - sdiag[j] = r__[j + j * r_dim1]; - r__[j + j * r_dim1] = x[j]; - /* L100: */ } + // restore + sdiag = r.diagonal(); + r.diagonal() = x; + /* solve the triangular system for z. if the system is */ /* singular, then obtain a least squares solution. */ - 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; - } - for (k = 1; k <= nsing; ++k) { - j = nsing - k + 1; + int nsing; + for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++); + wa.segment(nsing,n-nsing).setZero(); + nsing--; // nsing is the last nonsingular index + + for (j = nsing; j>=0; j--) { 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: */ - } -L130: + for (i = j+1; i <= nsing; ++i) + sum += r(i,j) * wa[i]; 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_ */ + for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; +} |