make qrsolv use eigen types

2 files changed, 46 insertions, 52 deletions

diff --git a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
index ef1b64083..c1ab2626c 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
@@ -122,9 +122,7 @@ void ei_lmpar(
         temp = ei_sqrt(par);
         wa1 = temp * diag;
 
-        ipvt.cwise()+=1; // qrsolv() expects the fortran convention (as qrfac provides)
-        ei_qrsolv<Scalar>(n, r.data(), r.rows(), ipvt.data(), wa1.data(), qtb.data(), x.data(), sdiag.data());
-        ipvt.cwise()-=1;
+        ei_qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
 
         wa2 = diag.cwise() * x;
         dxnorm = wa2.blueNorm();
diff --git a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
index 77786636c..adfa2be50 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
@@ -1,44 +1,43 @@
 
-template <typename Scalar>
-void ei_qrsolv(int n, Scalar *r__, int ldr, 
+#if 0
+        int n, Scalar *r__, int ldr, 
         const int *ipvt, const Scalar *diag, const Scalar *qtb, Scalar *x, 
         Scalar *sdiag)
-{
-    /* System generated locals */
-    int r_dim1, r_offset;
+#endif
+
+
+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;
     Scalar tan__, cos__, sin__, sum, temp, cotan;
     int nsing;
     Scalar qtbpj;
-    Matrix< Scalar, Dynamic, 1 >  wa(n+1);
-
-    /* Parameter adjustments */
-    --sdiag;
-    --x;
-    --qtb;
-    --diag;
-    --ipvt;
-    r_dim1 = ldr;
-    r_offset = 1 + r_dim1 * 1;
-    r__ -= r_offset;
+    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];
-        x[j] = r__[j + j * r_dim1];
+    for (j = 0; j < n; ++j) {
+        for (i = j; i < n; ++i)
+            r(i,j) = r(j,i);
+        x[j] = r(j,j);
         wa[j] = qtb[j];
     }
 
     /*     eliminate the diagonal matrix d using a givens rotation. */
-
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
 
         /*        prepare the row of d to be eliminated, locating the */
         /*        diagonal element using p from the qr factorization. */
@@ -46,7 +45,7 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
         l = ipvt[j];
         if (diag[l] == 0.)
             goto L90;
-        for (k = j; k <= n; ++k)
+        for (k = j; k < n; ++k)
             sdiag[k] = 0.;
         sdiag[j] = diag[l];
 
@@ -55,18 +54,18 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
         /*        beyond the first n, which is initially zero. */
 
         qtbpj = 0.;
-        for (k = j; k <= n; ++k) {
+        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.)
                 continue;
-            if ( ei_abs(r__[k + k * r_dim1]) < ei_abs(sdiag[k])) {
-                cotan = r__[k + k * r_dim1] / sdiag[k];
+            if ( ei_abs(r(k,k)) < ei_abs(sdiag[k])) {
+                cotan = r(k,k) / 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];
+                tan__ = sdiag[k] / r(k,k);
                 /* Computing 2nd power */
                 cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
                 sin__ = cos__ * tan__;
@@ -75,17 +74,16 @@ void ei_qrsolv(int n, Scalar *r__, int ldr,
             /*           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];
+            r(k,k) = cos__ * r(k,k) + sin__ * sdiag[k];
             temp = cos__ * wa[k] + sin__ * qtbpj;
             qtbpj = -sin__ * wa[k] + cos__ * qtbpj;
             wa[k] = temp;
 
             /*           accumulate the tranformation in the row of s. */
-            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];
-                r__[i + k * r_dim1] = temp;
+            for (i = k+1; i<n; ++i) {
+                temp = cos__ * r(i,k) + sin__ * sdiag[i];
+                sdiag[i] = -sin__ * r(i,k) + cos__ * sdiag[i];
+                r(i,k) = temp;
             }
         }
 L90:
@@ -93,30 +91,28 @@ 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];
+        sdiag[j] = r(j,j);
+        r(j,j) = x[j];
     }
 
     /*     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.;
+    nsing = n-1;
+    for (j = 0; j < n; ++j) {
+        if (sdiag[j] == 0. && nsing == n-1) nsing = j - 1;
+        if (nsing < n-1) wa[j] = 0.;
+    }
+    for (k = 0; k <= nsing; ++k) {
+        j = nsing - k;
+        sum = 0.;
+        for (i = j+1; i <= nsing; ++i)
+            sum += r(i,j) * wa[i];
+        wa[j] = (wa[j] - sum) / sdiag[j];
     }
-    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];
-        }
 
     /*     permute the components of z back to components of x. */
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         l = ipvt[j];
         x[l] = wa[j];
     }