New upstream version 2.0.0upstream/2.0.0

1 files changed, 225 insertions, 81 deletions

diff --git a/numlib/dhsx.c b/numlib/dhsx.c
index 180d15c..e9da0eb 100644..100755
--- a/numlib/dhsx.c
+++ b/numlib/dhsx.c
@@ -20,7 +20,9 @@
 
 #undef DEBUG
 
-static void simplexinit(int di, double *cp, double *r,double **p);
+int dhsx_debug = 1;
+
+static void simplexinit(int di, double *cp, double **p, double *r, double sv, int ii);
 static double trypoint(int di,double *cp, double **p, double *y, int hix, double hpfac,
                 double (*funk)(void *fdata, double *tp), void *fdata, double *tryp);
 
@@ -28,68 +30,98 @@ static double trypoint(int di,double *cp, double **p, double *y, int hix, double
 
 #define ALPHA 0.7			/* Extrapolate hight point through oposite face factor */
 #define GAMMA 1.4			/* Aditional extrapolation if ALPHA is good */ 
-#define BETA 0.4			/* One dimensional contraction factor */
+#define BETA 0.4			/* One dimensional contraction factor (smaller is more) */
+#define DELTA 0.5			/* Multi dimensional contraction factor (smaller is more) */
 #define NONEXP 2			/* non expanding passes */
 
 #else			/* Standard tuning values */
 
-#define ALPHA 1.0			/* Extrapolate hight point through oposite face factor */
-#define GAMMA 2.0			/* Aditional extrapolation if ALPHA is good */ 
-#define BETA 0.5			/* One dimensional contraction factor */
-#define NONEXP 2			/* non expanding passes */
+#define ALPHA 1.0			/* [1.0] Extrapolate hight point through oposite face factor */
+#define GAMMA 2.0			/* [2.0] Aditional extrapolation if ALPHA is good */ 
+#define BETA 0.4			/* [0.5] One dimensional contraction factor (smaller is more) */
+#define DELTA 0.4			/* [0.5] Multi dimensional contraction factor (smaller is more) */
+#define NONEXP 3			/* [3] non expanding passes */
 
 #endif
 
-
 /* Down hill simplex function */
 /* return 0 on sucess, 1 on failure due to excessive itterations */
 /* Result will be in cp */
-/* Arrays start at 0 */
 int dhsx(
 double *rv,				/* If not NULL, return the residual error */
 int di,					/* Dimentionality */
-double *cp,				/* Initial starting point */
+double *cp,				/* Initial starting point, return minimum */
 double *s,				/* Size of initial search area */
 double ftol,			/* Finishing tollerance of error change */
-double atol,			/* Absolute return value tollerance */
+double athr,			/* Absolute return value threshold. (Set high to not use) */
 int maxit,				/* Maximum iterations allowed */
 double (*funk)(void *fdata, double *tp),		/* Error function to evaluate */
 void *fdata				/* Data needed by function */
 ) {
+	int ii = 0;			/* Initial simplex orientation */
 	int i, j;
-	int lox, hix, nhix;	/* Lowest point index, highest point, next highest point */
-	int nsp = di+1;		/* Number of simplex verticy points */
 	int nit;			/* Number of iterations */
+	int nsp = di+1;		/* Number of simplex verticy points */
 	double tryy, ysave;
 	double tol;
-	double **p;			/* Simplex array */
-	double *y;			/* values of func at verticies */
+	double **p;			/* Current simplex array */
+	double *y;			/* Values of func at verticies */
+	double **p2;		/* Trial simplex array */
+	double *y2;			/* Trial values of func at verticies */
+	int lox, hix, nhix;	/* Lowest point index, highest point, next highest point */
 	double *tryp;		/* Temporary used by trypoint() */
 
 	/* Allocate array arrays */
-	y = dvector(0, di);				/* Value of function at verticies */
-	tryp = dvector(0, di-1);		
-	p = dmatrix(0, di+1, 0, di);	/* Vertex array of dimentions */
+	tryp = dvector(0, di-1);		/* Trial value */		
+	p = dmatrix(0, nsp-1, 0, di-1);	/* Vertex array of dimentions */
+	y = dvector(0, nsp-1);			/* Value of function at verticies */
+	p2 = dmatrix(0, nsp-1, 0, di-1);	/* Trial vertex array of dimentions */
+	y2 = dvector(0, nsp-1);			/* Trial value of function at verticies */
 	
 	/* Init the search simplex */
-	simplexinit(di, cp, s, p);
+	simplexinit(di, cp, p, s, 1.0, ii);
+
+	/* Compute initial y (function) values at simplex verticies */
+	for (i = 0; i < nsp; i++)			/* For all verticies */
+		y[i] = (*funk)(fdata, p[i]);	/* Compute error function */
 
-	/* Compute current center point position */
-	for (j = 0; j < di; j++) {	/* For all dimensions */
+	/* Locate verticy with best value */
+	lox = 0;
+	for (i = 0; i < nsp; i++) {
+		if (y[i] < y[lox]) 
+			lox = i;
+	}
+	tryy = (*funk)(fdata, cp);	/* Value at initial point */
+
+	/* If our initial point is better than any of the simplex verticies */
+	if (y[lox] > tryy) {
+		/* Move all the verticies to match moving lox to cp */
+		for (i = 0; i < nsp; i++) {
+			if (i == lox)
+				continue;
+			for (j = 0; j < di; j++)
+				p[i][j] += cp[j] - p[lox][j]; 
+			y[i] = (*funk)(fdata, p[i]);	/* Compute error function */
+		}
+		/* Make lox be the input point */
+		for (j = 0; j < di; j++)
+			p[lox][j] = cp[j];
+		y[lox] = tryy;
+	} 
+
+	/* Compute current center point location as sum of verticies. */
+	/* (We use this to compute moves) */
+	for (j = 0; j < di; j++) {					/* For all dimensions */
 		double sum;
 		for (i = 0, sum = 0.0; i < nsp; i++)	/* For all verticies */
 			sum += p[i][j];
 		cp[j] = sum;
 	}
 
-	/* Compute initial y (function) values at verticies */
-	for (i = 0; i < nsp; i++)		/* For all verticies */
-		y[i] = (*funk)(fdata,p[i]);	/* Compute error function */
-
 	/* Untill we find a solution or give up */
-	for (nit = 0; nit < maxit; nit++) {
-		/* Find highest, next highest and lowest vertex */
+	for (nit = 0; ; nit++) {
 
+		/* Find highest, next highest and lowest vertex */
 		lox = nhix = hix = 0;
 		for (i = 0; i < nsp; i++) {
 			if (y[i] < y[lox]) 
@@ -104,82 +136,189 @@ void *fdata				/* Data needed by function */
 
 		tol = y[hix] - y[lox];
 
-#ifdef DEBUG	/* 2D */
-		printf("Current vs = %f,%f  %f,%f  %f,%f\n",
-		        p[0].c[0],p[0].c[1],p[1].c[0],p[1].c[1],p[2].c[0],p[2].c[1]);
-		printf("Current errs = %e    %e    %e\n",y[0],y[1],y[2]);
-		printf("Current sxs = %d    %d    %d\n",sy[0]->no,sy[1]->no,sy[2]->no);
-		printf("Current y[hix] = %e\n",y[hix]);
+#ifdef DEBUG
+		if (dhsx_debug) {
+			printf("Current vs =\n");
+			for (i = 0; i < nsp; i++)
+				printf(" %d: %s\n",i,debPdv(di, p[i]));
+			printf("Current errs = %s\n",debPdv(nsp,y));
+			printf("Current y[lox] = %e, y[hix] = %e\n",y[lox], y[hix]);
+		}
+#endif /* DEBUG */
+
+		/* If we look like we are about to finish, */
+		/* see if we should re-start with a new simplex. */
+		if (tol < ftol && y[lox] < athr 	/* Found an adequate solution */
+		 && nit < maxit) {
+			double scale = 0.0;
+			int lox2;
+
+#ifdef DEBUG
+			if (dhsx_debug) printf(" nit %d, tol %e\n",nit, tol);
+#endif /* DEBUG */
+
+			/* compute center location */
+			tryy = 1.0/nsp;
+			for (j = 0; j < di; j++)		/* For all dimensions */
+				cp[j] *= tryy;				/* Set cp to center point of simplex */
+
+			/* Compute scaled distance of vertexes from center */
+			for (i = 0; i < nsp; i++) {
+				double dist = 0.0;
+				for (j = 0; j < di; j++) {
+					double tt = (cp[j] - p[i][j])/s[j];
+					dist += tt * tt;
+				}
+				scale += sqrt(dist);
+			}
+			scale /= (double)nsp;		/* Average scale compared to starting simplex */
+#ifdef DEBUG
+			if (dhsx_debug) printf(" ave scale = %f\n",scale);
+#endif /* DEBUG */
+
+			/* Enlarge search space, but not more than initial */
+			scale *= 10.0;
+			if (scale > 1.0)
+				scale = 1.0;
+
+			/* Compute trial simplex with different orientation */
+			if (++ii >= (di+1))
+				ii = 0;
+
+			/* Init the search simplex */
+			simplexinit(di, cp, p2, s, scale, ii);
+
+			/* Compute y (function) values at simplex verticies */
+			for (i = 0; i < nsp; i++)			/* For all verticies */
+				y2[i] = (*funk)(fdata, p2[i]);	/* Compute error function */
+
+			/* Locate verticy with best value */
+			lox2 = 0;
+			for (i = 0; i < nsp; i++) {
+				if (y2[i] < y2[lox2]) 
+					lox2 = i;
+			}
+#ifdef DEBUG
+			if (dhsx_debug) printf(" y2lox %f ylox %f\n",y2[lox2], y[lox]);
+#endif /* DEBUG */
+
+			/* If any of its vertexes are better than current best, switch */
+			/* to it and continue (i.e. re-start) */
+			if (y2[lox2] < y[lox]) {
+
+#ifdef DEBUG
+				if (dhsx_debug) printf(" restarting\n");
 #endif /* DEBUG */
 
-		if (tol < ftol && y[lox] < atol) {	/* Found an adequate solution */
-							/* (allow 10 x range for disambiguator) */
-			/* set cp[] to best point, and return error value of that point */
-			tryy = 1.0/(di+1);
+				for (i = 0; i < nsp; i++) {
+					for (j = 0; j < di; j++)
+						p[i][j] = p2[i][j];
+					y[i] = y2[i];
+				}
+
+				/* Compute current center point location as sum of verticies. */
+				/* (We use this to compute moves) */
+				for (j = 0; j < di; j++) {					/* For all dimensions */
+					double sum;
+					for (i = 0, sum = 0.0; i < nsp; i++)	/* For all verticies */
+						sum += p[i][j];
+					cp[j] = sum;
+				}
+
+				/* Find highest, next highest and lowest vertex */
+				lox = nhix = hix = 0;
+				for (i = 0; i < nsp; i++) {
+					if (y[i] < y[lox]) 
+						lox = i;
+					if (y[i] > y[hix]) {
+						nhix = hix;
+						hix = i;
+					} else if (y[i] > y[nhix]) {
+						nhix = i;
+					}
+				}
+
+				tol = y[hix] - y[lox];
+			}
+		}
+
+		if ((tol < ftol && y[lox] < athr) 	/* Found an adequate solution */
+		 || ((nit+1) >= maxit)) {				/* Or we are about to fail */
+
+			/* convert cp[] to center point location, */
+			/* and use best out of it and any simplex verticy. */
+			tryy = 1.0/nsp;
 			for (j = 0; j < di; j++)		/* For all dimensions */
 				cp[j] *= tryy;				/* Set cp to center point of simplex */
 #ifdef DEBUG
-			printf("C point = %f,%f\n",cp[0],cp[1]);
+			if (dhsx_debug) printf("C point = %s\n",debPdv(di,cp));
 #endif
-			tryy = (*funk)(fdata,cp);		/* Compute error function */
+			tryy = (*funk)(fdata, cp);		/* Compute error function */
 
 			if (tryy > y[lox]) {			/* Center point is not the best */
+#ifdef DEBUG
+				if (dhsx_debug) printf("C point val %f is not best, using sx %d val %f instead\n",tryy,lox,y[lox]);
+#endif
 				tryy = y[lox];
 				for (j = 0; j < di; j++)
 					cp[j] = p[lox][j];
 			}
-			free_dmatrix(p, 0, di+1, 0, di);
+			free_dvector(y2, 0, nsp-1);
+			free_dmatrix(p2, 0, nsp-1, 0, di-1);
+			free_dvector(y, 0, nsp-1);
+			free_dmatrix(p, 0, nsp-1, 0, di-1);
 			free_dvector(tryp, 0, di-1);
-			free_dvector(y, 0, di);
-printf("~1 itterations = %d\n",nit);
+#ifdef DEBUG
+			if (dhsx_debug) printf("Total itterations = %d\n",nit);
+#endif
 			if (rv != NULL)
 				*rv = tryy;
+			if ((nit+1) >= maxit)
+				return 1;	/* Failed */
 			return 0;
 		}
 
-		if (nit > NONEXP) {	/* Only try expanding after a couple of iterations */
+		/* Only try expanding after a couple of iterations */
+		if (nit > NONEXP) {
 			/* Try moving the high point through the oposite face by ALPHA */
 #ifdef DEBUG
-			printf("dhsx: try moving high point %d through oposite face",hix);
+			if (dhsx_debug) printf("dhsx: try moving high point %d through oposite face",hix);
 #endif
 			tryy = trypoint(di, cp, p, y, hix, -ALPHA, funk, fdata, tryp);
 		}
 
+		/* If gave good result, continue on in that direction */
 		if (nit > NONEXP && tryy <= y[lox]) {
 #ifdef DEBUG
-			verbose(4,"dhsx: moving high through oposite face worked");
+			if (dhsx_debug) printf("dhsx: moving high through oposite face worked");
 #endif
-			/* Gave good result, so continue on in that direction */
-			tryy=trypoint(di,cp,p,y,hix,GAMMA,funk,fdata,tryp);
+			tryy = trypoint(di, cp, p, y, hix, GAMMA, funk, fdata, tryp);
 
 
+		/* else if ALPHA move made things worse, do a one dimensional */
+		/* contraction by a factor BETA */
 		} else if (nit <= NONEXP || tryy >= y[nhix]) {
 
-			/* else if ALPHA move made things worse, do a one dimensional */
-			/* contraction by a factor BETA */
 #ifdef DEBUG
-			verbose(4,"dhsx: else try moving contracting point %d, y[ini] = %f",hix,y[hix]);
+			if (dhsx_debug) printf("dhsx: else try moving contracting point %d, y[ini] = %f",hix,y[hix]);
 #endif
 			ysave = y[hix];
 			tryy = trypoint(di, cp, p, y, hix, BETA, funk, fdata, tryp);
 			
 			if (tryy >= ysave) {
 #ifdef DEBUG
-				verbose(4,"dhsx: contracting didn't work, try contracting other points to low");
+				if (dhsx_debug) printf("dhsx: contracting didn't work, try contracting other points to low");
 #endif
 				/* That still didn't help us, so move all the */
-				/* other points towards the high point */
+				/* other points towards the low point */
 				for (i = 0; i < nsp; i++) {	/* For all verts except low */
 					if (i != lox) {
-						for (j = 0; j < di; j++) {	/* For all dimensions */
-							cp[j] = 0.5 * (p[i][j] + p[lox][j]);
-							p[i][j] = cp[j];
-						}
-						/* Compute function value for new point */
-						y[i] = (*funk)(fdata,p[i]);		/* Compute error function */
+						for (j = 0; j < di; j++) 	/* For all dimensions */
+							p[i][j] = DELTA * p[i][j] + (1.0 - DELTA) * p[lox][j];
+						y[i] = (*funk)(fdata, p[i]);	/* Compute function value for new point */
 					}
 				}
-				/* Re-compute current center point value */
+				/* Re-compute current center point location */
 				for (j = 0; j < di; j++) {
 					double sum;
 					for (i = 0,sum = 0.0;i<nsp;i++)
@@ -188,22 +327,17 @@ printf("~1 itterations = %d\n",nit);
 				}
 			} else {
 #ifdef DEBUG
-				verbose(4,"dhsx: contracting point %d worked, tryy = %e, ysave = %e",hix,tryy,ysave);
+				if (dhsx_debug) printf("dhsx: contracting point %d worked, tryy = %e, ysave = %e",hix,tryy,ysave);
 #endif
 			}
 		}
 	}
-	free_dmatrix(p, 0, di+1, 0, di);
-	free_dvector(tryp, 0, di-1);
-	free_dvector(y, 0, di);
-	if (rv != NULL)
-		*rv = tryy;
-	return 1;	/* Failed */
 }
 
 /* Try moving the high point through the opposite face */
 /* by a factor of fac, and replaces the high point if */
-/* that proves to be better.                          */
+/* that proves to be better.  Return the failed or new */
+/* function value. */
 static double trypoint(
 int di,					/* Dimentionality */
 double *cp,				/* nsp * center coord/Returned coordinate */
@@ -229,7 +363,7 @@ double *tryp			/* temporary array of size di-1 */
 	/* If new high point pos. is better */
 	if (tryy < y[hix]) {
 #ifdef DEBUG
-		printf("Try gave improved %e from sx %d",tryy,lsxp->last_fwd->no);
+		if (dhsx_debug) printf("Try gave improved %e from sx %d",tryy,hix);
 #endif
 		y[hix] = tryy;		/* Replace func val of hi with trial */
 
@@ -239,8 +373,7 @@ double *tryp			/* temporary array of size di-1 */
 		}
 	} else {
 #ifdef DEBUG
-		verbose(4,"Try gave worse %e from sx %d",tryy,
-		               lsxp->last_fwd == NULL ? -999 : lsxp->last_fwd->no);
+		if (dhsx_debug) printf("Try gave worse %e from sx %d",tryy,hix);
 #endif
 	}
 	return tryy;		/* Function value of trial point */
@@ -251,17 +384,19 @@ double *tryp			/* temporary array of size di-1 */
 static void
 simplexinit(
 int di,			/* Dimentionality */
-double *cp,		/* Initial solution values */
+double *cp,		/* Initial solution location */
+double **p,		/* Simplex to initialize */
 double *s,		/* initial radius for each dimention */
-double **p		/* Simplex to initialize */
+double sv,		/* Radius scaling value */
+int ii			/* Coordinate to start with */
 ) {
 	double bb;
 	double hh = 0.5;			/* Constant */
 	double rr = sqrt(3.0)/2.0;	/* Constant */
-	int i,j;
-	for (i = 0; i <= di; i++) {	/* For each vertex */
+	int i, j;
+	for (i = 0; i < (di+1); i++) {	/* For each vertex */
 		/* The bounding points form a equalateral simplex */
-		/* whose vertexes are on a spehere about the data */
+		/* whose vertexes are on a sphere about the data */
 		/* point center. The coordinate sequence is: */
 		/*  B = sphere radius */
 		/*	H = 0.5         */
@@ -287,16 +422,25 @@ double **p		/* Simplex to initialize */
 		/*      etc.     */
 		bb = 1.0;		/* Initial unscaled radius */
 		for (j = 0; j < di; j++) {	/* For each coordinate in vertex */
-			if (j > i)
-				p[i][j] = cp[j] + s[j] * 0.0;		/* If beyond last */
-			else if (j == i)		/* If last non-zero */
-				p[i][j] = cp[j] + s[j] * bb;
-			else if (i == di && j == (di-1)) /* If last of last */
-				p[i][j] = cp[j] + s[j] * -1.0 * bb;
+			if (j > ii)
+				p[i][j] = cp[j] + sv * s[j] * 0.0;		/* If beyond last */
+			else if (j == ii)		/* If last non-zero */
+				p[i][j] = cp[j] + sv * s[j] * bb;
+			else if (ii == di && j == (di-1)) /* If last of last */
+				p[i][j] = cp[j] + sv * s[j] * -1.0 * bb;
 			else					/* If before last */
-				p[i][j] = cp[j] + s[j] * -hh * bb;
+				p[i][j] = cp[j] + sv * s[j] * -hh * bb;
 			bb *= rr;
 		}
+		/* Increment coordinate number with wrap around */
+		if (++ii >= (di+1))
+			ii = 0;
+	}
+#ifdef DEBUG
+	if (dhsx_debug) {
+		for (i = 0; i < (di+1); i++)
+			printf(" p[%d] = %s\n",i,debPdv(di,p[i]));
 	}
+#endif
 }