summaryrefslogtreecommitdiff
path: root/numlib/varmet.c
diff options
context:
space:
mode:
Diffstat (limited to 'numlib/varmet.c')
-rwxr-xr-xnumlib/varmet.c290
1 files changed, 290 insertions, 0 deletions
diff --git a/numlib/varmet.c b/numlib/varmet.c
new file mode 100755
index 0000000..3c4b61a
--- /dev/null
+++ b/numlib/varmet.c
@@ -0,0 +1,290 @@
+
+/* Multi-dimentional minizer using Variable Metric method */
+/* This is good for smoother, well behaved functions. */
+
+/* Code is an original expression of the algorithms decsribed in */
+/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */
+/* S.A.Teukolsky & W.T.Vetterling. */
+
+/*
+ * Copyright 2000, 2006, 2007, 2017 Graeme W. Gill
+ * All rights reserved.
+ *
+ * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
+ * see the License.txt file for licencing details.
+ */
+
+/* TTBD:
+ Fix error handling to return status (malloc, excessive itters)
+ Create to "safe" library ?
+ Make standalone - ie remove numsup ?
+ */
+
+/* Note that all arrays are indexed from 0 */
+
+#include "numsup.h"
+#include "powell.h"
+#include "varmet.h"
+
+#undef DEBUG /* Debug */
+
+#if defined(PDEBUG) || defined(CDEBUG)
+# undef DBG
+# define DBG(xxx) printf xxx ;
+#else
+# undef DBG
+# define DBG(xxx)
+#endif
+
+#define FMAX(A,B) ((A) > (B) ? (A) : (B))
+#define EPS 1.0e-10 /* Machine precision. */
+#define TOLX (4 * EPS) /* X value stop value */
+#define MAXLEN 100.0 /* Maximum step length */
+
+void linesearch(int di, double cpold[], double fpold, double g[], double p[], double cpnew[],
+ double *pfp, double maxstep, double (*func)(void *fdata, double tp[]), void *fdata);
+
+/* return 0 on sucess, 1 on failure due to excessive itterions */
+/* Result will be in cp */
+/* Note that we could use gradient in line minimiser, */
+/* but haven't bothered yet. */
+int varmet(
+double *rv, /* If not NULL, return the residual error */
+int di, /* Dimentionality */
+double cp[], /* Initial starting point */
+double s[], /* Size of initial search area */
+double ftol, /* Tollerance of error change to stop on */
+int maxit, /* Maximum iterations allowed */
+double (*func)(void *fdata, double tp[]), /* Error function to evaluate */
+double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient function to evaluate */
+void *fdata /* Opaque data needed by function */
+) {
+ int iter;
+ double fp, sumsq, maxstep;
+ double *sdir, sumsdir; /* Search direction */
+ double *dp, *lastdp;
+ double **hessian; /* Hessian matrix */
+ double *hlastdp; /* Hessian times lastdp */
+ double *cpnew; /* new cp value from linemin */
+ double test;
+
+ double den, fac, fad, fae;
+ double sumdg;
+ int i, j;
+
+ sdir = dvector(0, di-1);
+ dp = dvector(0, di-1);
+ lastdp = dvector(0, di-1);
+ hessian = dmatrix(0, di-1, 0, di-1);
+ hlastdp = dvector(0, di-1);
+ cpnew = dvector(0, di-1);
+
+ fp = (*dfunc)(fdata, dp, cp);
+
+ /* Initial line direction and pde squared */
+ sumsq = 0.0;
+ for (i = 0; i < di ;i++) {
+ sdir[i] = -dp[i];
+ sumsq += cp[i] * cp[i];
+ }
+
+ DBG((" initial fp %f dp %s\n", fp, debPdv(di, dp)));
+
+ /* Initialize inverse Hessian to unity */
+ for (i = 0; i < di ;i++) {
+ for (j = 0; j < di ; j++) {
+ if (i == j)
+ hessian[i][j] = 1.0;
+ else
+ hessian[i][j] = 0.0;
+ }
+ }
+
+ /* Maximum line search step size */
+ maxstep = MAXLEN * FMAX(sqrt(sumsq), (double)di);
+
+ /* Until we give up */
+ for (iter = 0; iter < maxit; iter++) {
+
+ /* Search in direction sdir */
+ linesearch(di, cp, fp, dp, sdir, cpnew, &fp, maxstep, func, fdata);
+
+ for (i = 0; i < di; i++) {
+ sdir[i] = cpnew[i] - cp[i]; /* Update the line direction, */
+ cp[i] = cpnew[i]; /* and the current point. */
+ }
+
+ /* Test for convergence on x */
+ test = 0.0;
+ for (i = 0 ; i < di; i++) {
+ double tt = fabs(sdir[i]) / FMAX(fabs(cp[i]), 1.0);
+ if (tt > test)
+ test = tt;
+ }
+
+ if (test < TOLX) {
+ break;
+ }
+
+ for (i = 0; i < di; i++) /* Save previous partial deriv. */
+ lastdp[i] = dp[i];
+
+ (*dfunc)(fdata, dp, cp); /* and get the new gradient. */
+
+ test = 0.0; /* Test for convergence on zero gradient. */
+ den = FMAX(fp, 1.0);
+
+ for (i = 0; i < di; i++) {
+ double tt = fabs(dp[i]) * FMAX(fabs(cp[i]),1.0) / den;
+ if (tt > test)
+ test = tt;
+ }
+
+ if (test < ftol) {
+ break;
+ }
+
+ for (i = 0 ; i < di; i++)
+ lastdp[i] = dp[i] - lastdp[i]; /* Compute diference of gradients, */
+
+ for (i = 0; i < di; i++) { /* and difernce times current matrix. */
+ hlastdp[i] = 0.0;
+ for (j = 0; j < di; j++)
+ hlastdp[i] += hessian[i][j] * lastdp[j];
+ }
+
+ /* Calculate dot products for the denominator */
+ fac = fae = sumdg = sumsdir = 0.0;
+ for (i = 0; i < di; i++) {
+ fac += lastdp[i] * sdir[i];
+ fae += lastdp[i] * hlastdp[i];
+ sumdg += lastdp[i] * lastdp[i];
+ sumsdir += sdir[i] * sdir[i];
+ }
+ if (fac > sqrt(EPS * sumdg * sumsdir)) { /* Skip update if fac not sufficiently posive */
+ fac = 1.0/fac;
+ fad = 1.0/fae;
+ /* The vector that makes BFGS diferent from DFP: */
+ for (i = 0; i < di;i++)
+ lastdp[i] = fac * sdir[i] - fad * hlastdp[i];
+ for (i = 0; i < di;i++) { /* The BFGS updating formula: */
+ for (j = i; j < di; j++) {
+ hessian[i][j] += fac * sdir[i] * sdir[j]
+ - fad * hlastdp[i] * hlastdp[j] + fae * lastdp[i] * lastdp[j];
+ hessian[j][i] = hessian[i][j];
+ }
+ }
+ }
+ for (i = 0; i < di; i++) { /* Now calculate the next direction to go, */
+ sdir[i] = 0.0;
+ for (j = 0; j < di; j++)
+ sdir[i] -= hessian[i][j] * dp[j];
+ }
+ }
+
+ free_dvector(cpnew, 0, di-1);
+ free_dvector(hlastdp, 0, di-1);
+ free_dmatrix(hessian, 0, di-1, 0, di-1);
+ free_dvector(lastdp, 0, di-1);
+ free_dvector(dp, 0, di-1);
+ free_dvector(sdir, 0, di-1);
+ if (rv != NULL)
+ *rv = fp;
+ return 0;
+}
+
+#define ALPHA 1.0e-4 /* Ensures sucesscient decrease in function value. */
+#define LTOLX 1.0e-7 /* Convergence criterion on linesearch. */
+
+void linesearch(
+int di,
+double cpold[],
+double fpold,
+double dp[], /* Partial derivative */
+double sdir[], /* Current value */
+double cpnew[],
+double *pfp, /* Return value */
+double maxstep,
+double (*func)(void *fdata, double tp[]),
+void *fdata
+) {
+ double sum, slope;
+ double slen, slen_min;
+ double test, fval;
+ int i;
+
+ for (sum = 0.0, i = 0; i < di; i++)
+ sum += sdir[i] * sdir[i];
+ sum = sqrt(sum);
+
+ if (sum > maxstep) {
+ for (i = 0; i < di; i++)
+ sdir[i] *= maxstep/sum; /* Scale if attempted step is too big. */
+ }
+ for (slope = 0.0, i = 0; i < di; i++)
+ slope += dp[i] * sdir[i];
+
+ if (slope >= 0.0)
+ error("Roundoff problem in linesearch.");
+
+ test = 0.0;
+ for (i = 0;i < di; i++) {
+ double tt = fabs(sdir[i])/FMAX(fabs(cpold[i]), 1.0);
+ if (tt > test)
+ test = tt;
+ }
+
+ slen_min = TOLX/test;
+ slen = 1.0; /* Try full step */
+
+ /* Start of iteration loop. */
+ for (;;) {
+ double slen_2 = slen, slen_t;
+
+ for (i = 0; i < di;i++)
+ cpnew[i] = cpold[i] + slen * sdir[i];
+
+ *pfp = (*func)(fdata, cpnew);
+
+ if (slen < slen_min) {
+ for (i = 0; i < di; i++)
+ cpnew[i] = cpold[i];
+ return;
+
+ } else if (*pfp <= (fpold + ALPHA * slen * slope))
+ return;
+
+ /* Backtracking */
+ else {
+ /* First time through */
+ if (slen == 1.0)
+ slen_t = -slope/(2.0 * (*pfp - fpold - slope));
+ /* 2nd and subsequent times through */
+ else {
+ double aa, bb;
+ double rhs_1, rhs_2;
+
+ rhs_1 = *pfp - fpold - slen * slope;
+ rhs_2 = fval - fpold - slen_2 * slope;
+ aa = (rhs_1/(slen * slen) - rhs_2/(slen_2 * slen_2))/(slen - slen_2);
+ bb = (-slen_2 * rhs_1/(slen * slen)+slen * rhs_2/(slen_2 * slen_2))/(slen - slen_2);
+ if (aa == 0.0)
+ slen_t = -slope/(2.0 * bb);
+ else {
+ double dd = bb * bb - 3.0 * aa * slope;
+ if (dd < 0.0)
+ slen_t = 0.5 * slen;
+ else if (bb <= 0.0)
+ slen_t = (-bb + sqrt(dd))/(3.0 * aa);
+ else
+ slen_t = -slope/(bb + sqrt(dd));
+ }
+ if (slen_t > 0.5 * slen)
+ slen_t = 0.5 * slen;
+ }
+ }
+ slen_2 = slen;
+ fval = *pfp;
+ slen = FMAX(slen_t, 0.1 * slen);
+ }
+}