use PlanarRotation<> instead of handmade givens rotation in cminpack code

+ cleaning.
This results in some more memory being used, but not much.

5 files changed, 78 insertions, 147 deletions

diff --git a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
index e1d10bdcc..539c58d72 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
@@ -218,6 +218,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
         )
 {
     int j;
+    std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n);
+
     jeval = true;
 
     /* calculate the jacobian matrix. */
@@ -359,9 +361,9 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(
         wa2 = (wa2-wa3)/pnorm;
 
         /* compute the qr factorization of the updated jacobian. */
-        ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
-        ei_r1mpyq<Scalar>(n, n, fjac.data(), wa2.data(), wa3.data());
-        ei_r1mpyq<Scalar>(1, n, qtf.data(), wa2.data(), wa3.data());
+        ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
+        ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
+        ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
 
         jeval = false;
     }
@@ -465,6 +467,8 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
         )
 {
     int j;
+    std::vector<PlanarRotation<Scalar> > v_givens(n), w_givens(n);
+
     jeval = true;
     if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
     if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
@@ -608,9 +612,9 @@ HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(
         wa2 = (wa2-wa3)/pnorm;
 
         /* compute the qr factorization of the updated jacobian. */
-        ei_r1updt<Scalar>(n, n, R, wa1.data(), wa2.data(), wa3.data(), &sing);
-        ei_r1mpyq<Scalar>(n, n, fjac.data(), wa2.data(), wa3.data());
-        ei_r1mpyq<Scalar>(1, n, qtf.data(), wa2.data(), wa3.data());
+        ei_r1updt<Scalar>(R, wa1.data(), v_givens, w_givens, wa2.data(), wa3.data(), &sing);
+        ei_r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
+        ei_r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
 
         jeval = false;
     }
diff --git a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
index 1a3ca12ae..adbb3c835 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
@@ -468,8 +468,7 @@ LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(
     int rownb = 2;
     for (i = 0; i < m; ++i) {
         if (functor.df(x, wa3, rownb) < 0) return UserAsked;
-        temp = fvec[i];
-        ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), &temp, wa1.data(), wa2.data());
+        ei_rwupdt<Scalar>(n, fjac.data(), fjac.rows(), wa3.data(), qtf.data(), fvec[i]);
         ++rownb;
     }
     ++njev;
diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
index ac1feeffc..855cb7a1b 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
@@ -2,46 +2,21 @@
 // TODO : move this to GivensQR once there's such a thing in Eigen
 
 template <typename Scalar>
-void ei_r1mpyq(int m, int n, Scalar *a, const Scalar *v, const Scalar *w)
+void ei_r1mpyq(int m, int n, Scalar *a, const std::vector<PlanarRotation<Scalar> > &v_givens, const std::vector<PlanarRotation<Scalar> > &w_givens)
 {
-    /* Local variables */
-    int i, j;
-    Scalar cos__=0., sin__=0., temp;
-
-    /* Function Body */
-    if (n<=1)
-        return;
-
     /*     apply the first set of givens rotations to a. */
-    for (j = n-2; j>=0; --j) {
-        if (ei_abs(v[j]) > 1.) {
-            cos__ = 1. / v[j];
-            sin__ = ei_sqrt(1. - ei_abs2(cos__));
-        } else  {
-            sin__ = v[j];
-            cos__ = ei_sqrt(1. - ei_abs2(sin__));
-        }
-        for (i = 0; i<m; ++i) {
-            temp = cos__ * a[i+m*j] - sin__ * a[i+m*(n-1)];
-            a[i+m*(n-1)] = sin__ * a[i+m*j] + cos__ * a[i+m*(n-1)];
+    for (int j = n-2; j>=0; --j)
+        for (int i = 0; i<m; ++i) {
+            Scalar temp = v_givens[j].c() * a[i+m*j] - v_givens[j].s() * a[i+m*(n-1)];
+            a[i+m*(n-1)] = v_givens[j].s() * a[i+m*j] + v_givens[j].c() * a[i+m*(n-1)];
             a[i+m*j] = temp;
         }
-    }
     /*     apply the second set of givens rotations to a. */
-    for (j = 0; j<n-1; ++j) {
-        if (ei_abs(w[j]) > 1.) {
-            cos__ = 1. / w[j];
-            sin__ = ei_sqrt(1. - ei_abs2(cos__));
-        } else  {
-            sin__ = w[j];
-            cos__ = ei_sqrt(1. - ei_abs2(sin__));
-        }
-        for (i = 0; i<m; ++i) {
-            temp = cos__ * a[i+m*j] + sin__ * a[i+m*(n-1)];
-            a[i+m*(n-1)] = -sin__ * a[i+m*j] + cos__ * a[i+m*(n-1)];
+    for (int j = 0; j<n-1; ++j)
+        for (int i = 0; i<m; ++i) {
+            Scalar temp = w_givens[j].c() * a[i+m*j] + w_givens[j].s() * a[i+m*(n-1)];
+            a[i+m*(n-1)] = -w_givens[j].s() * a[i+m*j] + w_givens[j].c() * a[i+m*(n-1)];
             a[i+m*j] = temp;
         }
-    }
-    return;
 }
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
index 08744ce39..0c530f3aa 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
@@ -1,12 +1,16 @@
 
 template <typename Scalar>
-void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u, Scalar *v, Scalar *w, bool *sing)
+void ei_r1updt(Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar *u,
+        std::vector<PlanarRotation<Scalar> > &v_givens,
+        std::vector<PlanarRotation<Scalar> > &w_givens,
+        Scalar *v, Scalar *w, bool *sing)
 {
     /* Local variables */
-    int i, j=1, nm1;
-    Scalar tan__;
-    int nmj;
-    Scalar cos__, sin__, tau, temp, cotan;
+    const int m = s.rows();
+    const int n = s.cols();
+    int i, j=1;
+    Scalar temp;
+    PlanarRotation<Scalar> givens;
 
     // ei_r1updt had a broader usecase, but we dont use it here. And, more
     // importantly, we can not test it.
@@ -17,52 +21,31 @@ void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar
     --u;
     --v;
 
-    /* Function Body */
-    const Scalar giant = std::numeric_limits<Scalar>::max();
-
     /*     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. */
-    nm1 = n - 1;
-    if (nm1 >= 1)
-        for (nmj = 1; nmj <= nm1; ++nmj) {
-            j = n - nmj;
-            w[j] = 0.;
-            if (v[j] != 0.) {
-                /*        determine a givens rotation which eliminates the */
-                /*        j-th element of v. */
-                if (ei_abs(v[n]) < ei_abs(v[j])) {
-                    cotan = v[n] / v[j];
-                    /* Computing 2nd power */
-                    sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
-                    cos__ = sin__ * cotan;
-                    tau = 1.;
-                    if (ei_abs(cos__) * giant > 1.) {
-                        tau = 1. / cos__;
-                    }
-                } else {
-                    tan__ = v[j] / v[n];
-                    /* Computing 2nd power */
-                    cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
-                    sin__ = cos__ * tan__;
-                    tau = sin__;
-                }
+    for (j=n-1; j>=1; --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]);
 
-                /*        apply the transformation to v and store the information */
-                /*        necessary to recover the givens rotation. */
-                v[n] = sin__ * v[j] + cos__ * v[n];
-                v[j] = tau;
+            /*        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 s and extend the spike in w. */
-                for (i = j; i <= m; ++i) {
-                    temp = cos__ * s(j-1,i-1) - sin__ * w[i];
-                    w[i] = sin__ * s(j-1,i-1) + cos__ * 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-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;
             }
         }
+    }
 
     /*     add the spike from the rank 1 update to w. */
     for (i = 1; i <= m; ++i)
@@ -70,45 +53,29 @@ void ei_r1updt(int m, int n, Matrix< Scalar, Dynamic, Dynamic > &s, const Scalar
 
     /*     eliminate the spike. */
     *sing = false;
-    if (nm1 >=  1)
-        for (j = 1; j <= nm1; ++j) {
-            if (w[j] != 0.) {
-                /*        determine a givens rotation which eliminates the */
-                /*        j-th element of the spike. */
-                if (ei_abs(s(j-1,j-1)) < ei_abs(w[j])) {
-                    cotan = s(j-1,j-1) / w[j];
-                    /* Computing 2nd power */
-                    sin__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
-                    cos__ = sin__ * cotan;
-                    tau = 1.;
-                    if (ei_abs(cos__) * giant > 1.) {
-                        tau = 1. / cos__;
-                    }
-                } else {
-                    tan__ = w[j] / s(j-1,j-1);
-                    /* Computing 2nd power */
-                    cos__ = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
-                    sin__ = cos__ * tan__;
-                    tau = sin__;
-                }
+    for (j = 1; 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]);
 
-                /*        apply the transformation to s and reduce the spike in w. */
-                for (i = j; i <= m; ++i) {
-                    temp = cos__ * s(j-1,i-1) + sin__ * w[i];
-                    w[i] = -sin__ * s(j-1,i-1) + cos__ * w[i];
-                    s(j-1,i-1) = temp;
-                }
-
-                /*        store the information necessary to recover the */
-                /*        givens rotation. */
-                w[j] = tau;
+            /*        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;
             }
 
-            /*        test for zero diagonal elements in the output s. */
-            if (s(j-1,j-1) == 0.) {
-                *sing = true;
-            }
+            /*        store the information necessary to recover the */
+            /*        givens rotation. */
+            w_givens[j-1] = givens;
         }
+
+        /*        test for zero diagonal elements in the output s. */
+        if (s(j-1,j-1) == 0.) {
+            *sing = true;
+        }
+    }
     /*     move w back into the last column of the output s. */
     s(n-1,n-1) = w[n];
 
diff --git a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
index 38d614ae0..38676e720 100644
--- a/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
+++ b/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
@@ -1,18 +1,15 @@
 
-template <typename Scalar>
-void ei_rwupdt(int n, Scalar *r__, int ldr, 
-	const Scalar *w, Scalar *b, Scalar *alpha, Scalar *cos__, 
-	Scalar *sin__)
+    template <typename Scalar>
+void ei_rwupdt(int n, Scalar *r__, int ldr, const Scalar *w, Scalar *b, Scalar alpha)
 {
+    std::vector<PlanarRotation<Scalar> > givens(n);
     /* System generated locals */
     int r_dim1, r_offset;
 
     /* Local variables */
-    Scalar tan__, temp, rowj, cotan;
+    Scalar temp, rowj;
 
     /* Parameter adjustments */
-    --sin__;
-    --cos__;
     --b;
     --w;
     r_dim1 = ldr;
@@ -23,34 +20,23 @@ void ei_rwupdt(int n, Scalar *r__, int ldr,
     for (int j = 1; j <= n; ++j) {
         rowj = w[j];
 
-        /*        apply the previous transformations to */
-        /*        r(i,j), i=1,2,...,j-1, and to w(j). */
+        /* apply the previous transformations to */
+        /* r(i,j), i=1,2,...,j-1, and to w(j). */
         if (j-1>=1)
             for (int i = 1; i <= j-1; ++i) {
-                temp = cos__[i] * r__[i + j * r_dim1] + sin__[i] * rowj;
-                rowj = -sin__[i] * r__[i + j * r_dim1] + cos__[i] * rowj;
+                temp = givens[i-1].c() * r__[i + j * r_dim1] + givens[i-1].s() * rowj;
+                rowj = -givens[i-1].s() * r__[i + j * r_dim1] + givens[i-1].c() * rowj;
                 r__[i + j * r_dim1] = temp;
             }
 
-        /*        determine a givens rotation which eliminates w(j). */
-        cos__[j] = 1.;
-        sin__[j] = 0.;
+        /* determine a givens rotation which eliminates w(j). */
         if (rowj != 0.) {
-            if (ei_abs(r__[j + j * r_dim1]) < ei_abs(rowj)) {
-                cotan = r__[j + j * r_dim1] / rowj;
-                sin__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(cotan));
-                cos__[j] = sin__[j] * cotan;
-            }
-            else {
-                tan__ = rowj / r__[j + j * r_dim1];
-                cos__[j] = Scalar(.5) / ei_sqrt(Scalar(0.25) + Scalar(0.25) * ei_abs2(tan__));
-                sin__[j] = cos__[j] * tan__;
-            }
+            givens[j-1].makeGivens(-r__[j + j * r_dim1], rowj);
 
-            /*        apply the current transformation to r(j,j), b(j), and alpha. */
-            r__[j + j * r_dim1] = cos__[j] * r__[j + j * r_dim1] + sin__[j] * rowj;
-            temp = cos__[j] * b[j] + sin__[j] * *alpha;
-            *alpha = -sin__[j] * b[j] + cos__[j] * *alpha;
+            /* apply the current transformation to r(j,j), b(j), and alpha. */
+            r__[j + j * r_dim1] = givens[j-1].c() * r__[j + j * r_dim1] + givens[j-1].s() * rowj;
+            temp = givens[j-1].c() * b[j] + givens[j-1].s() * alpha;
+            alpha = -givens[j-1].s() * b[j] + givens[j-1].c() * alpha;
             b[j] = temp;
         }
     }