eigenization of ei_r1updt()

2 files changed, 48 insertions, 47 deletions

diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
index 51cea50be..258297072 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
@@ -359,7 +359,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
         wa2 = (wa2-wa3)/pnorm;
 
         /* compute the qr factorization of the updated jacobian. */
-        ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
+        ei_r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
         ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
         ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
 
@@ -608,7 +608,7 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
         wa2 = (wa2-wa3)/pnorm;
 
         /* compute the qr factorization of the updated jacobian. */
-        ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
+        ei_r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
         ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
         ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
index 0c530f3aa..3d8978837 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
@@ -1,9 +1,13 @@
 
 template <typename Scalar>
-void ei_r1updt(Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u,
+void ei_r1updt(
+        Matrix< Scalar, Dynamic, Dynamic > &s,
+        const Matrix< Scalar, Dynamic, 1> &u,
         std::vector<PlanarRotation<Scalar> > &v_givens,
         std::vector<PlanarRotation<Scalar> > &w_givens,
-        Scalar *v, Scalar *w, bool *sing)
+        Matrix< Scalar, Dynamic, 1> &v,
+        Matrix< Scalar, Dynamic, 1> &w,
+        bool *sing)
 {
     /* Local variables */
     const int m = s.rows();
@@ -15,71 +19,68 @@ void ei_r1updt(Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u,
     // ei_r1updt had a broader usecase, but we dont use it here. And, more
     // importantly, we can not test it.
     assert(m==n);
+    assert(u.size()==m);
+    assert(v.size()==n);
+    assert(w.size()==n);
 
-    /* Parameter adjustments */
-    --w;
-    --u;
-    --v;
+    /* move the nontrivial part of the last column of s into w. */
+    w[n-1] = s(n-1,n-1);
 
-    /*     move the nontrivial part of the last column of s into w. */
-    w[n] = s(n-1,n-1);
-
-    /*     rotate the vector v into a multiple of the n-th unit vector */
-    /*     in such a way that a spike is introduced into w. */
-    for (j=n-1; j>=1; --j) {
+    /* rotate the vector v into a multiple of the n-th unit vector */
+    /* in such a way that a spike is introduced into w. */
+    for (j=n-2; j>=0; --j) {
         w[j] = 0.;
         if (v[j] != 0.) {
-            /*        determine a givens rotation which eliminates the */
-            /*        j-th element of v. */
-            givens.makeGivens(-v[n], v[j]);
+            /* determine a givens rotation which eliminates the */
+            /* j-th element of v. */
+            givens.makeGivens(-v[n-1], v[j]);
 
-            /*        apply the transformation to v and store the information */
-            /*        necessary to recover the givens rotation. */
-            v[n] = givens.s() * v[j] + givens.c() * v[n];
-            v_givens[j-1] = givens;
+            /* apply the transformation to v and store the information */
+            /* necessary to recover the givens rotation. */
+            v[n-1] = givens.s() * v[j] + givens.c() * v[n-1];
+            v_givens[j] = givens;
 
-            /*        apply the transformation to s and extend the spike in w. */
-            for (i = j; i <= m; ++i) {
-                temp = givens.c() * s(j-1,i-1) - givens.s() * w[i];
-                w[i] = givens.s() * s(j-1,i-1) + givens.c() * w[i];
-                s(j-1,i-1) = temp;
+            /* apply the transformation to s and extend the spike in w. */
+            for (i = j; i < m; ++i) {
+                temp = givens.c() * s(j,i) - givens.s() * w[i];
+                w[i] = givens.s() * s(j,i) + givens.c() * w[i];
+                s(j,i) = temp;
             }
         }
     }
 
-    /*     add the spike from the rank 1 update to w. */
-    for (i = 1; i <= m; ++i)
-        w[i] += v[n] * u[i];
+    /* add the spike from the rank 1 update to w. */
+    w += v[n-1] * u;
 
-    /*     eliminate the spike. */
+    /* eliminate the spike. */
     *sing = false;
-    for (j = 1; j <= n-1; ++j) {
+    for (j = 0; j < n-1; ++j) {
         if (w[j] != 0.) {
-            /*        determine a givens rotation which eliminates the */
-            /*        j-th element of the spike. */
-            givens.makeGivens(-s(j-1,j-1), w[j]);
+            /* determine a givens rotation which eliminates the */
+            /* j-th element of the spike. */
+            givens.makeGivens(-s(j,j), w[j]);
 
-            /*        apply the transformation to s and reduce the spike in w. */
-            for (i = j; i <= m; ++i) {
-                temp = givens.c() * s(j-1,i-1) + givens.s() * w[i];
-                w[i] = -givens.s() * s(j-1,i-1) + givens.c() * w[i];
-                s(j-1,i-1) = temp;
+            /* apply the transformation to s and reduce the spike in w. */
+            for (i = j; i < m; ++i) {
+                temp = givens.c() * s(j,i) + givens.s() * w[i];
+                w[i] = -givens.s() * s(j,i) + givens.c() * w[i];
+                s(j,i) = temp;
             }
 
-            /*        store the information necessary to recover the */
-            /*        givens rotation. */
-            w_givens[j-1] = givens;
+            /* store the information necessary to recover the */
+            /* givens rotation. */
+            w_givens[j] = givens;
         }
 
-        /*        test for zero diagonal elements in the output s. */
-        if (s(j-1,j-1) == 0.) {
+        /* test for zero diagonal elements in the output s. */
+        if (s(j,j) == 0.) {
             *sing = true;
         }
     }
-    /*     move w back into the last column of the output s. */
-    s(n-1,n-1) = w[n];
+    /* move w back into the last column of the output s. */
+    s(n-1,n-1) = w[n-1];
 
-    if (s(j-1,j-1) == 0.) {
+    if (s(j,j) == 0.) {
         *sing = true;
     }
     return;