merging ei_hybrj() and hybrj_template()

unsupported/Eigen/src/NonLinear/MathFunctions.h

@@ -25,49 +25,6 @@
 #ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H
 #define EIGEN_NONLINEAR_MATHFUNCTIONS_H
 
-
-template<typename Functor, typename Scalar>
-int ei_hybrj(
-        Matrix< Scalar, Dynamic, 1 >  &x,
-        Matrix< Scalar, Dynamic, 1 >  &fvec,
-        int &nfev,
-        int &njev,
-        Matrix< Scalar, Dynamic, Dynamic > &fjac,
-        Matrix< Scalar, Dynamic, 1 >  &R,
-        Matrix< Scalar, Dynamic, 1 >  &qtf,
-        Matrix< Scalar, Dynamic, 1 >  &diag,
-        int mode=1,
-        int maxfev = 1000,
-        Scalar factor = Scalar(100.),
-        Scalar xtol = ei_sqrt(epsilon<Scalar>()),
-        int nprint=0
-        )
-{
-    int n = x.size();
-    int lr = (n*(n+1))/2;
-    Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(n);
-
-    fvec.resize(n);
-    qtf.resize(n);
-    R.resize(lr);
-    int ldfjac = n;
-    fjac.resize(ldfjac, n);
-    return hybrj_template<Scalar> (
-            Functor::f, 0,
-            n, x.data(), fvec.data(),
-            fjac.data(), ldfjac,
-            xtol, maxfev,
-            diag.data(), mode, 
-            factor,
-            nprint, 
-            nfev,
-            njev,
-            R.data(), lr,
-            qtf.data(),
-            wa1.data(), wa2.data(), wa3.data(), wa4.data()
-    );
-}
-
 template<typename Functor, typename Scalar>
 int ei_lmstr(
         Matrix< Scalar, Dynamic, 1 >  &x,

unsupported/Eigen/src/NonLinear/hybrd.h

@@ -245,7 +245,7 @@ L190:
     /*           evaluate the function at x + p and calculate its norm. */
 
     iflag = Functor::f(0, n, wa2.data(), wa4.data(), 1);
-    ++(nfev);
+    ++nfev;
     if (iflag < 0) {
         goto L300;
     }

unsupported/Eigen/src/NonLinear/hybrj.h

@@ -1,19 +1,33 @@
 
-template<typename Scalar>
+template<typename Functor, typename Scalar>
-int hybrj_template(minpack_funcder_nn fcn, void *p, int n, Scalar *x, Scalar *
+int ei_hybrj(
-        fvec, Scalar *fjac, int ldfjac, Scalar xtol, int
+        Matrix< Scalar, Dynamic, 1 >  &x,
-        maxfev, Scalar *diag, int mode, Scalar factor, int
+        Matrix< Scalar, Dynamic, 1 >  &fvec,
-        nprint, int &nfev, int &njev, Scalar *r__, 
+        int &nfev,
-        int lr, Scalar *qtf, Scalar *wa1, Scalar *wa2, 
+        int &njev,
-        Scalar *wa3, Scalar *wa4)
+        Matrix< Scalar, Dynamic, Dynamic > &fjac,
+        Matrix< Scalar, Dynamic, 1 >  &R,
+        Matrix< Scalar, Dynamic, 1 >  &qtf,
+        Matrix< Scalar, Dynamic, 1 >  &diag,
+        int mode=1,
+        int maxfev = 1000,
+        Scalar factor = Scalar(100.),
+        Scalar xtol = ei_sqrt(epsilon<Scalar>()),
+        int nprint=0
+        )
 {
-    /* Initialized data */
+    const int n = x.size();
+    const int lr = (n*(n+1))/2;
+    Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n), wa3(n), wa4(n);
 
-    /* System generated locals */
+    fvec.resize(n);
-    int fjac_offset;
+    qtf.resize(n);
+    R.resize(lr);
+    int ldfjac = n;
+    fjac.resize(ldfjac, n);
 
     /* Local variables */
-    int i, j, l, jm1, iwa[1];
+    int i, j, l, iwa[1];
     Scalar sum;
     int sing;
     int iter;
@@ -30,21 +44,7 @@ int hybrj_template(minpack_funcder_nn fcn, void *p, int n, Scalar *x, Scalar *
     Scalar actred, prered;
     int info;
 
-    /* Parameter adjustments */
-    --wa4;
-    --wa3;
-    --wa2;
-    --wa1;
-    --qtf;
-    --diag;
-    --fvec;
-    --x;
-    fjac_offset = 1 + ldfjac;
-    fjac -= fjac_offset;
-    --r__;
-
     /* Function Body */
-
     info = 0;
     iflag = 0;
     nfev = 0;
@@ -59,7 +59,7 @@ int hybrj_template(minpack_funcder_nn fcn, void *p, int n, Scalar *x, Scalar *
     if (mode != 2) {
         goto L20;
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         if (diag[j] <= 0.) {
             goto L300;
         }
@@ -70,12 +70,12 @@ L20:
     /*     evaluate the function at the starting point */
     /*     and calculate its norm. */
 
-    iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 1);
+    iflag = Functor::f(0, n, x.data(), fvec.data(), fjac.data(), ldfjac, 1);
     nfev = 1;
     if (iflag < 0) {
         goto L300;
     }
-    fnorm = ei_enorm<Scalar>(n, &fvec[1]);
+    fnorm = fvec.stableNorm();;
 
     /*     initialize iteration counter and monitors. */
 
@@ -92,7 +92,7 @@ L30:
 
     /*        calculate the jacobian matrix. */
 
-    iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 2);
+    iflag = Functor::f(0, n, x.data(), fvec.data(), fjac.data(), ldfjac, 2);
     ++njev;
     if (iflag < 0) {
         goto L300;
@@ -100,8 +100,7 @@ L30:
 
     /*        compute the qr factorization of the jacobian. */
 
-    qrfac(n, n, &fjac[fjac_offset], ldfjac, FALSE_, iwa, 1, &wa1[1], &
+    qrfac(n, n,fjac.data(), ldfjac, FALSE_, iwa, 1, wa1.data(), wa2.data(), wa3.data());
-            wa2[1], &wa3[1]);
 
     /*        on the first iteration and if mode is 1, scale according */
     /*        to the norms of the columns of the initial jacobian. */
@@ -112,7 +111,7 @@ L30:
     if (mode == 2) {
         goto L50;
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         diag[j] = wa2[j];
         if (wa2[j] == 0.) {
             diag[j] = 1.;
@@ -124,11 +123,11 @@ L50:
     /*        on the first iteration, calculate the norm of the scaled x */
     /*        and initialize the step bound delta. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         wa3[j] = diag[j] * x[j];
         /* L60: */
     }
-    xnorm = ei_enorm<Scalar>(n, &wa3[1]);
+    xnorm = wa3.stableNorm();;
     delta = factor * xnorm;
     if (delta == 0.) {
         delta = factor;
@@ -137,22 +136,22 @@ L70:
 
     /*        form (q transpose)*fvec and store in qtf. */
 
-    for (i = 1; i <= n; ++i) {
+    for (i = 0; i < n; ++i) {
         qtf[i] = fvec[i];
         /* L80: */
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
-        if (fjac[j + j * ldfjac] == 0.) {
+        if (fjac(j,j) == 0.) {
             goto L110;
         }
         sum = 0.;
-        for (i = j; i <= n; ++i) {
+        for (i = j; i < n; ++i) {
-            sum += fjac[i + j * ldfjac] * qtf[i];
+            sum += fjac(i,j) * qtf[i];
             /* L90: */
         }
-        temp = -sum / fjac[j + j * ldfjac];
+        temp = -sum / fjac(j,j);
-        for (i = j; i <= n; ++i) {
+        for (i = j; i < n; ++i) {
-            qtf[i] += fjac[i + j * ldfjac] * temp;
+            qtf[i] += fjac(i,j) * temp;
             /* L100: */
         }
 L110:
@@ -163,19 +162,16 @@ L110:
     /*        copy the triangular factor of the qr factorization into r. */
 
     sing = FALSE_;
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         l = j;
-        jm1 = j - 1;
+        if (j) {
-        if (jm1 < 1) {
+            for (i = 0; i < j; ++i) {
-            goto L140;
+                R[l] = fjac(i,j);
+                l = l + n - i -1;
+                /* L130: */
+            }
         }
-        for (i = 1; i <= jm1; ++i) {
+        R[l] = wa1[j];
-            r__[l] = fjac[i + j * ldfjac];
-            l = l + n - i;
-            /* L130: */
-        }
-L140:
-        r__[l] = wa1[j];
         if (wa1[j] == 0.) {
             sing = TRUE_;
         }
@@ -184,14 +180,15 @@ L140:
 
     /*        accumulate the orthogonal factor in fjac. */
 
-    qform(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]);
+    qform(n, n, fjac.data(), ldfjac, wa1.data());
 
     /*        rescale if necessary. */
 
     if (mode == 2) {
         goto L170;
     }
-    for (j = 1; j <= n; ++j) /* Computing MAX */
+    /* Computing MAX */
+    for (j = 0; j < n; ++j)
         diag[j] = max(diag[j], wa2[j]);
 L170:
 
@@ -199,14 +196,14 @@ L170:
 
 L180:
 
-    /*           if requested, call fcn to enable printing of iterates. */
+    /*           if requested, call Functor::f to enable printing of iterates. */
 
     if (nprint <= 0) {
         goto L190;
     }
     iflag = 0;
     if ((iter - 1) % nprint == 0) {
-        iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
+        iflag = Functor::f(0, n, x.data(), fvec.data(), fjac.data(), ldfjac, 0);
     }
     if (iflag < 0) {
         goto L300;
@@ -215,18 +212,17 @@ L190:
 
     /*           determine the direction p. */
 
-    dogleg(n, &r__[1], lr, &diag[1], &qtf[1], delta, &wa1[1], &wa2[1], &wa3[
+    dogleg(n, R.data(), lr, diag.data(), qtf.data(), delta, wa1.data(), wa2.data(), wa3.data());
-            1]);
 
     /*           store the direction p and x + p. calculate the norm of p. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         wa1[j] = -wa1[j];
         wa2[j] = x[j] + wa1[j];
         wa3[j] = diag[j] * wa1[j];
         /* L200: */
     }
-    pnorm = ei_enorm<Scalar>(n, &wa3[1]);
+    pnorm = wa3.stableNorm();
 
     /*           on the first iteration, adjust the initial step bound. */
 
@@ -236,12 +232,12 @@ L190:
 
     /*           evaluate the function at x + p and calculate its norm. */
 
-    iflag = (*fcn)(p, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, 1);
+    iflag = Functor::f(0, n, wa2.data(), wa4.data(), fjac.data(), ldfjac, 1);
     ++nfev;
     if (iflag < 0) {
         goto L300;
     }
-    fnorm1 = ei_enorm<Scalar>(n, &wa4[1]);
+    fnorm1 = wa4.stableNorm();
 
     /*           compute the scaled actual reduction. */
 
@@ -251,18 +247,18 @@ L190:
 
     /*           compute the scaled predicted reduction. */
 
-    l = 1;
+    l = 0;
-    for (i = 1; i <= n; ++i) {
+    for (i = 0; i < n; ++i) {
         sum = 0.;
-        for (j = i; j <= n; ++j) {
+        for (j = i; j < n; ++j) {
-            sum += r__[l] * wa1[j];
+            sum += R[l] * wa1[j];
             ++l;
             /* L210: */
         }
         wa3[i] = qtf[i] + sum;
         /* L220: */
     }
-    temp = ei_enorm<Scalar>(n, &wa3[1]);
+    temp = wa3.stableNorm();
     prered = 0.;
     if (temp < fnorm) /* Computing 2nd power */
         prered = 1. - ei_abs2(temp / fnorm);
@@ -302,13 +298,13 @@ L240:
 
     /*           successful iteration. update x, fvec, and their norms. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         x[j] = wa2[j];
         wa2[j] = diag[j] * x[j];
         fvec[j] = wa4[j];
         /* L250: */
     }
-    xnorm = ei_enorm<Scalar>(n, &wa2[1]);
+    xnorm = wa2.stableNorm();
     fnorm = fnorm1;
     ++iter;
 L260:
@@ -363,10 +359,10 @@ L260:
     /*           calculate the rank one modification to the jacobian */
     /*           and update qtf if necessary. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         sum = 0.;
-        for (i = 1; i <= n; ++i) {
+        for (i = 0; i < n; ++i) {
-            sum += fjac[i + j * ldfjac] * wa4[i];
+            sum += fjac(i,j) * wa4[i];
             /* L270: */
         }
         wa2[j] = (sum - wa3[j]) / pnorm;
@@ -379,9 +375,9 @@ L260:
 
     /*           compute the qr factorization of the updated jacobian. */
 
-    r1updt(n, n, &r__[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing);
+    r1updt(n, n, R.data(), lr, wa1.data(), wa2.data(), wa3.data(), &sing);
-    r1mpyq(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]);
+    r1mpyq(n, n, fjac.data(), ldfjac, wa2.data(), wa3.data());
-    r1mpyq(1, n, &qtf[1], 1, &wa2[1], &wa3[1]);
+    r1mpyq(1, n, qtf.data(), 1, wa2.data(), wa3.data());
 
     /*           end of the inner loop. */
 
@@ -400,7 +396,7 @@ L300:
         info = iflag;
     }
     if (nprint > 0) {
-        iflag = (*fcn)(p, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, 0);
+        iflag = Functor::f(0, n, x.data(), fvec.data(), fjac.data(), ldfjac, 0);
     }
     return info;