merging ei_lmdif() and lmdif_template()

4 files changed, 63 insertions, 198 deletions

diff --git a/unsupported/Eigen/NonLinear b/unsupported/Eigen/NonLinear
index 9c617b04d..cc3e2d147 100644
--- a/unsupported/Eigen/NonLinear
+++ b/unsupported/Eigen/NonLinear
@@ -59,80 +59,15 @@ typedef int (*minpack_func_mn)(void *p, int m, int n, const double *x, double *f
 typedef int (*minpack_funcder_mn)(void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag );
 typedef int (*minpack_funcderstr_mn)(void *p, int m, int n, const double *x, double *fvec, double *fjrow, int iflag );
 
-/* MINPACK functions: */
-int hybrd1 ( minpack_func_nn fcn, void *p, int n, double *x, double *fvec, double tol,
-	       double *wa, int lwa );
-int hybrd ( minpack_func_nn fcn,
-	      void *p, int n, double *x, double *fvec, double xtol, int maxfev,
-	      int ml, int mu, double epsfcn, double *diag, int mode,
-	      double factor, int nprint, int *nfev,
-	      double *fjac, int ldfjac, double *r, int lr, double *qtf,
-	      double *wa1, double *wa2, double *wa3, double *wa4);
-  
-int hybrj1 ( minpack_funcder_nn fcn, void *p, int n, double *x,
-	       double *fvec, double *fjac, int ldfjac, double tol,
-	       double *wa, int lwa );
-int hybrj ( minpack_funcder_nn fcn, void *p, int n, double *x,
-	      double *fvec, double *fjac, int ldfjac, double xtol,
-	      int maxfev, double *diag, int mode, double factor,
-	      int nprint, int *nfev, int *njev, double *r,
-	      int lr, double *qtf, double *wa1, double *wa2,
-	      double *wa3, double *wa4 );
-
-int lmdif1 ( minpack_func_mn fcn,
-	       void *p, int m, int n, double *x, double *fvec, double tol,
-	       int *iwa, double *wa, int lwa );
-int lmdif ( minpack_func_mn fcn,
-	      void *p, int m, int n, double *x, double *fvec, double ftol,
-	      double xtol, double gtol, int maxfev, double epsfcn,
-	      double *diag, int mode, double factor, int nprint,
-	      int *nfev, double *fjac, int ldfjac, int *ipvt,
-	      double *qtf, double *wa1, double *wa2, double *wa3,
-	      double *wa4 );
-
-int lmder1 ( minpack_funcder_mn fcn,
-	       void *p, int m, int n, double *x, double *fvec, double *fjac,
-	       int ldfjac, double tol, int *ipvt,
-	       double *wa, int lwa );
-int lmder ( minpack_funcder_mn fcn,
-	      void *p, int m, int n, double *x, double *fvec, double *fjac,
-	      int ldfjac, double ftol, double xtol, double gtol,
-	      int maxfev, double *diag, int mode, double factor,
-	      int nprint, int *nfev, int *njev, int *ipvt,
-	      double *qtf, double *wa1, double *wa2, double *wa3,
-	      double *wa4 );
-
-int lmstr1 ( minpack_funcderstr_mn fcn, void *p, int m, int n,
-	       double *x, double *fvec, double *fjac, int ldfjac,
-	       double tol, int *ipvt, double *wa, int lwa );
-int lmstr (  minpack_funcderstr_mn fcn, void *p, int m,
-	      int n, double *x, double *fvec, double *fjac,
-	      int ldfjac, double ftol, double xtol, double gtol,
-	      int maxfev, double *diag, int mode, double factor,
-	      int nprint, int *nfev, int *njev, int *ipvt,
-	      double *qtf, double *wa1, double *wa2, double *wa3,
-	      double *wa4 );
- 
-void chkder ( int m, int n, const double *x, double *fvec, double *fjac,
-	       int ldfjac, double *xp, double *fvecp, int mode,
-	       double *err  );
-
 void covar(int n, double *r__, int ldr, const int *ipvt, double tol, double *wa);
 #endif
 
 
-template<typename Scalar>
-Scalar ei_enorm ( int n, const Scalar *x ){
-    return Map< Matrix< Scalar, Dynamic, 1 > >(x,n).stableNorm();
-//    return Map< Matrix< Scalar, Dynamic, 1 > >(x,n).blueNorm();
-}
-
 #include "src/NonLinear/lmder.h"
 #include "src/NonLinear/hybrd.h"
 #include "src/NonLinear/lmstr.h"
 #include "src/NonLinear/lmdif.h"
 #include "src/NonLinear/hybrj.h"
-#include "src/NonLinear/MathFunctions.h"
 #include "src/NonLinear/lmder1.h"
 #include "src/NonLinear/lmstr1.h"
 #include "src/NonLinear/hybrd1.h"
diff --git a/unsupported/Eigen/src/NonLinear/MathFunctions.h b/unsupported/Eigen/src/NonLinear/MathFunctions.h
deleted file mode 100644
index 7e5ed944b..000000000
--- a/unsupported/Eigen/src/NonLinear/MathFunctions.h
+++ /dev/null
@@ -1,74 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
-//
-// Eigen is free software; you can redistribute it and/or
-// modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation; either
-// version 3 of the License, or (at your option) any later version.
-//
-// Alternatively, you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of
-// the License, or (at your option) any later version.
-//
-// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
-// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License and a copy of the GNU General Public License along with
-// Eigen. If not, see <http://www.gnu.org/licenses/>.
-
-#ifndef EIGEN_NONLINEAR_MATHFUNCTIONS_H
-#define EIGEN_NONLINEAR_MATHFUNCTIONS_H
-
-template<typename Functor, typename Scalar>
-int ei_lmdif(
-        Matrix< Scalar, Dynamic, 1 >  &x,
-        Matrix< Scalar, Dynamic, 1 >  &fvec,
-        int &nfev,
-        Matrix< Scalar, Dynamic, Dynamic > &fjac,
-        VectorXi &ipvt,
-        Matrix< Scalar, Dynamic, 1 >  &qtf,
-        Matrix< Scalar, Dynamic, 1 >  &diag,
-        int mode=1,
-        Scalar factor = 100.,
-        int maxfev = 400,
-        Scalar ftol = ei_sqrt(epsilon<Scalar>()),
-        Scalar xtol = ei_sqrt(epsilon<Scalar>()),
-        Scalar gtol = Scalar(0.),
-        Scalar epsfcn = Scalar(0.),
-        int nprint=0
-        )
-{
-    Matrix< Scalar, Dynamic, 1 >
-        wa1(x.size()), wa2(x.size()), wa3(x.size()),
-        wa4(fvec.size());
-    int ldfjac = fvec.size();
-
-    ipvt.resize(x.size());
-    fjac.resize(ldfjac, x.size());
-    diag.resize(x.size());
-    qtf.resize(x.size());
-    return lmdif_template<Scalar> (
-            Functor::f, 0,
-            fvec.size(), x.size(), x.data(), fvec.data(),
-            ftol, xtol, gtol, 
-            maxfev,
-            epsfcn,
-            diag.data(), mode,
-            factor,
-            nprint,
-            nfev,
-            fjac.data() , ldfjac,
-            ipvt.data(),
-            qtf.data(),
-            wa1.data(), wa2.data(), wa3.data(), wa4.data()
-    );
-}
-
-#endif // EIGEN_NONLINEAR_MATHFUNCTIONS_H
-
diff --git a/unsupported/Eigen/src/NonLinear/lmdif.h b/unsupported/Eigen/src/NonLinear/lmdif.h
index 5f06a9b82..ab97794ca 100644
--- a/unsupported/Eigen/src/NonLinear/lmdif.h
+++ b/unsupported/Eigen/src/NonLinear/lmdif.h
@@ -1,17 +1,33 @@
 
-template<typename Scalar>
-int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x, 
-        Scalar *fvec, Scalar ftol, Scalar xtol, Scalar gtol,
-        int maxfev, Scalar epsfcn, Scalar *diag, int
-        mode, Scalar factor, int nprint, int &nfev,
-        Scalar *fjac, int ldfjac, int *ipvt, Scalar *
-        qtf, Scalar *wa1, Scalar *wa2, Scalar *wa3, Scalar *
-        wa4)
+template<typename Functor, typename Scalar>
+int ei_lmdif(
+        Matrix< Scalar, Dynamic, 1 >  &x,
+        Matrix< Scalar, Dynamic, 1 >  &fvec,
+        int &nfev,
+        Matrix< Scalar, Dynamic, Dynamic > &fjac,
+        VectorXi &ipvt,
+        Matrix< Scalar, Dynamic, 1 >  &qtf,
+        Matrix< Scalar, Dynamic, 1 >  &diag,
+        int mode=1,
+        Scalar factor = 100.,
+        int maxfev = 400,
+        Scalar ftol = ei_sqrt(epsilon<Scalar>()),
+        Scalar xtol = ei_sqrt(epsilon<Scalar>()),
+        Scalar gtol = Scalar(0.),
+        Scalar epsfcn = Scalar(0.),
+        int nprint=0
+        )
 {
-    /* Initialized data */
+    const int m = fvec.size(), n = x.size();
+    Matrix< Scalar, Dynamic, 1 >
+        wa1(n), wa2(n), wa3(n),
+        wa4(m);
+    int ldfjac = m;
 
-    /* System generated locals */
-    int fjac_offset;
+    ipvt.resize(n);
+    fjac.resize(ldfjac, n);
+    diag.resize(n);
+    qtf.resize(n);
 
     /* Local variables */
     int i, j, l;
@@ -25,21 +41,7 @@ int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x,
     Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
     int info;
 
-    /* Parameter adjustments */
-    --wa4;
-    --fvec;
-    --wa3;
-    --wa2;
-    --wa1;
-    --qtf;
-    --ipvt;
-    --diag;
-    --x;
-    fjac_offset = 1 + ldfjac;
-    fjac -= fjac_offset;
-
     /* Function Body */
-
     info = 0;
     iflag = 0;
     nfev = 0;
@@ -53,7 +55,7 @@ int lmdif_template(minpack_func_mn fcn, void *p, int m, int n, Scalar *x,
     if (mode != 2) {
         goto L20;
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         if (diag[j] <= 0.) {
             goto L300;
         }
@@ -64,12 +66,12 @@ L20:
     /*     evaluate the function at the starting point */
     /*     and calculate its norm. */
 
-    iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 1);
+    iflag = Functor::f(0, m, n, x.data(), fvec.data(), 1);
     nfev = 1;
     if (iflag < 0) {
         goto L300;
     }
-    fnorm = ei_enorm<Scalar>(m, &fvec[1]);
+    fnorm = fvec.stableNorm();
 
     /*     initialize levenberg-marquardt parameter and iteration counter. */
 
@@ -82,21 +84,21 @@ L30:
 
     /*        calculate the jacobian matrix. */
 
-    iflag = fdjac2(fcn, p, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac,
-            epsfcn, &wa4[1]);
+    iflag = fdjac2(Functor::f, 0, m, n, x.data(), fvec.data(), fjac.data(), ldfjac,
+            epsfcn, wa4.data());
     nfev += n;
     if (iflag < 0) {
         goto L300;
     }
 
-    /*        if requested, call fcn to enable printing of iterates. */
+    /*        if requested, call Functor::f to enable printing of iterates. */
 
     if (nprint <= 0) {
         goto L40;
     }
     iflag = 0;
     if ((iter - 1) % nprint == 0) {
-        iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 0);
+        iflag = Functor::f(0, m, n, x.data(), fvec.data(), 0);
     }
     if (iflag < 0) {
         goto L300;
@@ -105,8 +107,8 @@ L40:
 
     /*        compute the qr factorization of the jacobian. */
 
-    qrfac(m, n, &fjac[fjac_offset], ldfjac, TRUE_, &ipvt[1], n, &wa1[1], &
-            wa2[1], &wa3[1]);
+    qrfac(m, n, fjac.data(), ldfjac, TRUE_, ipvt.data(), n, wa1.data(), wa2.data(), wa3.data());
+    ipvt.cwise()-=1; // qrfac() creates ipvt with fortran convetion (1->n), convert it to c (0->n-1)
 
     /*        on the first iteration and if mode is 1, scale according */
     /*        to the norms of the columns of the initial jacobian. */
@@ -117,7 +119,7 @@ L40:
     if (mode == 2) {
         goto L60;
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         diag[j] = wa2[j];
         if (wa2[j] == 0.) {
             diag[j] = 1.;
@@ -129,11 +131,11 @@ L60:
     /*        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];
         /* L70: */
     }
-    xnorm = ei_enorm<Scalar>(n, &wa3[1]);
+    xnorm = wa3.stableNorm();;
     delta = factor * xnorm;
     if (delta == 0.) {
         delta = factor;
@@ -143,21 +145,21 @@ L80:
     /*        form (q transpose)*fvec and store the first n components in */
     /*        qtf. */
 
-    for (i = 1; i <= m; ++i) {
+    for (i = 0; i < m; ++i) {
         wa4[i] = fvec[i];
         /* L90: */
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         if (fjac[j + j * ldfjac] == 0.) {
             goto L120;
         }
         sum = 0.;
-        for (i = j; i <= m; ++i) {
+        for (i = j; i < m; ++i) {
             sum += fjac[i + j * ldfjac] * wa4[i];
             /* L100: */
         }
         temp = -sum / fjac[j + j * ldfjac];
-        for (i = j; i <= m; ++i) {
+        for (i = j; i < m; ++i) {
             wa4[i] += fjac[i + j * ldfjac] * temp;
             /* L110: */
         }
@@ -173,13 +175,13 @@ L120:
     if (fnorm == 0.) {
         goto L170;
     }
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         l = ipvt[j];
         if (wa2[l] == 0.) {
             goto L150;
         }
         sum = 0.;
-        for (i = 1; i <= j; ++i) {
+        for (i = 0; i <= j; ++i) {
             sum += fjac[i + j * ldfjac] * (qtf[i] / fnorm);
             /* L140: */
         }
@@ -205,7 +207,7 @@ L170:
     if (mode == 2) {
         goto L190;
     }
-    for (j = 1; j <= n; ++j) /* Computing MAX */
+    for (j = 0; j < n; ++j) /* Computing MAX */
         diag[j] = max(diag[j], wa2[j]);
 L190:
 
@@ -215,18 +217,20 @@ L200:
 
     /*           determine the levenberg-marquardt parameter. */
 
-    lmpar(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], delta,
-            &par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]);
+    ipvt.cwise()+=1; // lmpar() expects the fortran convention (as qrfac provides)
+    lmpar(n, fjac.data(), ldfjac, ipvt.data(), diag.data(), qtf.data(), delta,
+            &par, wa1.data(), wa2.data(), wa3.data(), wa4.data());
+    ipvt.cwise()-=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];
         /* L210: */
     }
-    pnorm = ei_enorm<Scalar>(n, &wa3[1]);
+    pnorm = wa3.stableNorm();
 
     /*           on the first iteration, adjust the initial step bound. */
 
@@ -236,12 +240,12 @@ L200:
 
     /*           evaluate the function at x + p and calculate its norm. */
 
-    iflag = (*fcn)(p, m, n, &wa2[1], &wa4[1], 1);
+    iflag = Functor::f(0, m, n, wa2.data(), wa4.data(), 1);
     ++nfev;
     if (iflag < 0) {
         goto L300;
     }
-    fnorm1 = ei_enorm<Scalar>(m, &wa4[1]);
+    fnorm1 = wa4.stableNorm();
 
     /*           compute the scaled actual reduction. */
 
@@ -252,18 +256,18 @@ L200:
     /*           compute the scaled predicted reduction and */
     /*           the scaled directional derivative. */
 
-    for (j = 1; j <= n; ++j) {
+    for (j = 0; j < n; ++j) {
         wa3[j] = 0.;
         l = ipvt[j];
         temp = wa1[l];
-        for (i = 1; i <= j; ++i) {
+        for (i = 0; i <= j; ++i) {
             wa3[i] += fjac[i + j * ldfjac] * temp;
             /* L220: */
         }
         /* L230: */
     }
-    temp1 = ei_abs2(ei_enorm<Scalar>(n, &wa3[1]) / fnorm);
-    temp2 = ei_abs2( ei_sqrt(par) * pnorm / fnorm);
+    temp1 = ei_abs2(wa3.stableNorm() / fnorm);
+    temp2 = ei_abs2(ei_sqrt(par) * pnorm / fnorm);
     /* Computing 2nd power */
     prered = temp1 + temp2 / p5;
     dirder = -(temp1 + temp2);
@@ -311,16 +315,16 @@ L260:
 
     /*           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];
         /* L270: */
     }
-    for (i = 1; i <= m; ++i) {
+    for (i = 0; i < m; ++i) {
         fvec[i] = wa4[i];
         /* L280: */
     }
-    xnorm = ei_enorm<Scalar>(n, &wa2[1]);
+    xnorm = wa2.stableNorm();
     fnorm = fnorm1;
     ++iter;
 L290:
@@ -377,7 +381,7 @@ L300:
     }
     iflag = 0;
     if (nprint > 0) {
-        iflag = (*fcn)(p, m, n, &x[1], &fvec[1], 0);
+        iflag = Functor::f(0, m, n, x.data(), fvec.data(), 0);
     }
     return info;
 
diff --git a/unsupported/test/NonLinear.cpp b/unsupported/test/NonLinear.cpp
index e7eeff58c..11abd7b47 100644
--- a/unsupported/test/NonLinear.cpp
+++ b/unsupported/test/NonLinear.cpp
@@ -762,7 +762,7 @@ void testLmdif()
   covar_ftol = epsilon<double>();
   covfac = fnorm*fnorm/(m-n);
   VectorXd wa(n);
-//  ipvt.cwise()+=1; // covar() expects the fortran convention (as qrfac provides)
+  ipvt.cwise()+=1; // covar() expects the fortran convention (as qrfac provides)
   covar(n, fjac.data(), m, ipvt.data(), covar_ftol, wa.data());
 
   MatrixXd cov_ref(n,n);