eigenize dogleg()

unsupported/Eigen/src/NonLinear/dogleg.h

@@ -1,8 +1,11 @@
 
-    template <typename Scalar>
-void ei_dogleg(int n, const Scalar *r__, int /* lr*/ , 
-        const Scalar *diag, const Scalar *qtb, Scalar delta, Scalar *x, 
-        Scalar *wa1, Scalar *wa2)
+template <typename Scalar>
+void ei_dogleg(
+        Matrix< Scalar, Dynamic, 1 >  &r__,
+        const Matrix< Scalar, Dynamic, 1 >  &diag,
+        const Matrix< Scalar, Dynamic, 1 >  &qtb,
+        Scalar delta,
+        Matrix< Scalar, Dynamic, 1 >  &x)
 {
     /* Local variables */
     int i, j, k, l, jj, jp1;
@@ -10,30 +13,26 @@ void ei_dogleg(int n, const Scalar *r__, int /* lr*/ ,
     Scalar gnorm, qnorm;
     Scalar sgnorm;
 
-    /* Parameter adjustments */
-    --wa2;
-    --wa1;
-    --x;
-    --qtb;
-    --diag;
-    --r__;
-
     /* Function Body */
     const Scalar epsmch = epsilon<Scalar>();
+    const int n = diag.size();
+    Matrix< Scalar, Dynamic, 1 >  wa1(n), wa2(n);
+    assert(n==qtb.size());
+    assert(n==x.size());
 
     /*     first, calculate the gauss-newton direction. */
 
-    jj = n * (n + 1) / 2 + 1;
-    for (k = 1; k <= n; ++k) {
-        j = n - k + 1;
+    jj = n * (n + 1) / 2;
+    for (k = 0; k < n; ++k) {
+        j = n - k - 1;
         jp1 = j + 1;
-        jj -= k;
+        jj -= k+1;
         l = jj + 1;
         sum = 0.;
         if (n < jp1) {
             goto L20;
         }
-        for (i = jp1; i <= n; ++i) {
+        for (i = jp1; i < n; ++i) {
             sum += r__[l] * x[i];
             ++l;
             /* L10: */
@@ -44,7 +43,7 @@ L20:
             goto L40;
         }
         l = j;
-        for (i = 1; i <= j; ++i) {
+        for (i = 0; i <= j; ++i) {
             /* Computing MAX */
             temp = std::max(temp,ei_abs(r__[l]));
             l = l + n - i;
@@ -61,12 +60,12 @@ L40:
 
     /*     test whether the gauss-newton direction is acceptable. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         wa1[j] = 0.;
         wa2[j] = diag[j] * x[j];
         /* L60: */
     }
-    qnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
+    qnorm = wa2.stableNorm();
     if (qnorm <= delta) {
         /* goto L140; */
         return;
@@ -75,10 +74,10 @@ L40:
     /*     the gauss-newton direction is not acceptable. */
     /*     next, calculate the scaled gradient direction. */
 
-    l = 1;
-    for (j = 1; j <= n; ++j) {
+    l = 0;
+    for (j = 0; j < n; ++j) {
         temp = qtb[j];
-        for (i = j; i <= n; ++i) {
+        for (i = j; i < n; ++i) {
             wa1[i] += r__[l] * temp;
             ++l;
             /* L70: */
@@ -90,7 +89,7 @@ L40:
     /*     calculate the norm of the scaled gradient and test for */
     /*     the special case in which the scaled gradient is zero. */
 
-    gnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&wa1[1],n).stableNorm();
+    gnorm = wa1.stableNorm();
     sgnorm = 0.;
     alpha = delta / qnorm;
     if (gnorm == 0.) {
@@ -100,14 +99,14 @@ L40:
     /*     calculate the point along the scaled gradient */
     /*     at which the quadratic is minimized. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         wa1[j] = wa1[j] / gnorm / diag[j];
         /* L90: */
     }
-    l = 1;
-    for (j = 1; j <= n; ++j) {
+    l = 0;
+    for (j = 0; j < n; ++j) {
         sum = 0.;
-        for (i = j; i <= n; ++i) {
+        for (i = j; i < n; ++i) {
             sum += r__[l] * wa1[i];
             ++l;
             /* L100: */
@@ -115,7 +114,7 @@ L40:
         wa2[j] = sum;
         /* L110: */
     }
-    temp = Map< Matrix< Scalar, Dynamic, 1 > >(&wa2[1],n).stableNorm();
+    temp = wa2.stableNorm();
     sgnorm = gnorm / temp / temp;
 
     /*     test whether the scaled gradient direction is acceptable. */
@@ -129,7 +128,7 @@ L40:
     /*     finally, calculate the point along the dogleg */
     /*     at which the quadratic is minimized. */
 
-    bnorm = Map< Matrix< Scalar, Dynamic, 1 > >(&qtb[1],n).stableNorm();
+    bnorm = qtb.stableNorm();
     temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
     /* Computing 2nd power */
     temp = temp - delta / qnorm * ei_abs2(sgnorm / delta) + ei_sqrt(ei_abs2(temp - delta / qnorm) + (1.-ei_abs2(delta / qnorm)) * (1.-ei_abs2(sgnorm / delta)));
@@ -141,14 +140,10 @@ L120:
     /*     direction and the scaled gradient direction. */
 
     temp = (1. - alpha) * std::min(sgnorm,delta);
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         x[j] = temp * wa1[j] + alpha * x[j];
-        /* L130: */
     }
-    /* L140: */
     return;
 
-    /*     last card of subroutine dogleg. */
-
-} /* dogleg_ */
+}
 

unsupported/Eigen/src/NonLinear/hybrd.h

@@ -209,7 +209,7 @@ L190:
 
     /*           determine the direction p. */
 
-    ei_dogleg<Scalar>(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data());
+    ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
 
     /*           store the direction p and x + p. calculate the norm of p. */
 

unsupported/Eigen/src/NonLinear/hybrj.h

@@ -198,7 +198,7 @@ L190:
 
     /*           determine the direction p. */
 
-    ei_dogleg<Scalar>(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data());
+    ei_dogleg<Scalar>(R, diag, qtf, delta, wa1);
 
     /*           store the direction p and x + p. calculate the norm of p. */