New upstream version 2.0.0upstream/2.0.0

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);
+	}
+}