summaryrefslogtreecommitdiff
path: root/numlib/dhsx.c
blob: 180d15c98366ba82ea25f88054e44ffb4a1041fd (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302

/* This is better for stocastic optimisation, where the function */
/* being evaluated may have a random component, or is not smooth. */

/*
 * Copyright 1999 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.
 */

/* A general purpose downhill simplex multivariate optimser, */
/* based on the Nelder and Mead algorithm. */
/* 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. */

#include "numsup.h"

#undef DEBUG

static void simplexinit(int di, double *cp, double *r,double **p);
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);

#ifdef NEVER	/* Experimental */

#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 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 */

#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 *s,				/* Size of initial search area */
double ftol,			/* Finishing tollerance of error change */
double atol,			/* Absolute return value tollerance */
int maxit,				/* Maximum iterations allowed */
double (*funk)(void *fdata, double *tp),		/* Error function to evaluate */
void *fdata				/* Data needed by function */
) {
	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 */
	double tryy, ysave;
	double tol;
	double **p;			/* Simplex array */
	double *y;			/* values of func at verticies */
	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 */
	
	/* Init the search simplex */
	simplexinit(di, cp, s, p);

	/* Compute current center point position */
	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 */

		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];

#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]);
#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 (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]);
#endif
			tryy = (*funk)(fdata,cp);		/* Compute error function */

			if (tryy > y[lox]) {			/* Center point is not the best */
				tryy = y[lox];
				for (j = 0; j < di; j++)
					cp[j] = p[lox][j];
			}
			free_dmatrix(p, 0, di+1, 0, di);
			free_dvector(tryp, 0, di-1);
			free_dvector(y, 0, di);
printf("~1 itterations = %d\n",nit);
			if (rv != NULL)
				*rv = tryy;
			return 0;
		}

		if (nit > NONEXP) {	/* Only try expanding after a couple of iterations */
			/* Try moving the high point through the oposite face by ALPHA */
#ifdef 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 (nit > NONEXP && tryy <= y[lox]) {
#ifdef DEBUG
			verbose(4,"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);


		} 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]);
#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");
#endif
				/* That still didn't help us, so move all the */
				/* other points towards the high 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 */
					}
				}
				/* Re-compute current center point value */
				for (j = 0; j < di; j++) {
					double sum;
					for (i = 0,sum = 0.0;i<nsp;i++)
						sum += p[i][j];
					cp[j] = sum;
				}
			} else {
#ifdef DEBUG
				verbose(4,"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.                          */
static double trypoint(
int di,					/* Dimentionality */
double *cp,				/* nsp * center coord/Returned coordinate */
double **p,				/* Starting/Current simplex (modified by dhsx) */
double *y,				/* values of func at verticies */
int hix,				/* Index of high point we are moving */
double hpfac,			/* factor to move high point */
double (*funk)(void *fdata, double tp[]),		/* Error function to evaluate */
void *fdata,			/* Data needed by function */
double *tryp			/* temporary array of size di-1 */
) {
	int j;
	double tt, tryy;

	/* Compute trial high point */
	tt = (1.0 - hpfac)/di;
	for (j = 0; j < di; j++) 
		tryp[j] = cp[j] * tt - p[hix][j] * (tt - hpfac);

	/* Evaluate trial point */
	tryy = (*funk)(fdata, tryp);	/* Compute error function */

	/* 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);
#endif
		y[hix] = tryy;		/* Replace func val of hi with trial */

		for (j = 0; j < di; j++) {
			cp[j] += tryp[j] - p[hix][j];	/* Recompute cp */
			p[hix][j] = tryp[j];	/* Replace co-ords of hi with trial */
		}
	} else {
#ifdef DEBUG
		verbose(4,"Try gave worse %e from sx %d",tryy,
		               lsxp->last_fwd == NULL ? -999 : lsxp->last_fwd->no);
#endif
	}
	return tryy;		/* Function value of trial point */
}


/* Make up an initial simplex for dhsx routine */
static void
simplexinit(
int di,			/* Dimentionality */
double *cp,		/* Initial solution values */
double *s,		/* initial radius for each dimention */
double **p		/* Simplex to initialize */
) {
	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 */
		/* The bounding points form a equalateral simplex */
		/* whose vertexes are on a spehere about the data */
		/* point center. The coordinate sequence is: */
		/*  B = sphere radius */
		/*	H = 0.5         */
		/*	R = sqrt(3)/2   */
		/*   0  0  0 +B     */
		/*   0  0  0 -B     */

		/*   0  0   0  +B   */
		/*   0  0  +RB -HB  */
		/*   0  0  -RB -HB  */

		/*   0  0      0   +B   */
		/*   0  0     +RB  -HB  */
		/*   0  +RRb  -HRB -HB  */
		/*   0  -RRb  -HRB -HB  */

		/*   0       0      0   +B   */
		/*   0       0     +RB  -HB  */
		/*   0      +RRb   -HRB -HB  */
		/*   +RRRb  -HRRb  -HRB -HB  */
		/*   -RRRb  -HRRb  -HRB -HB  */

		/*      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;
			else					/* If before last */
				p[i][j] = cp[j] + s[j] * -hh * bb;
			bb *= rr;
		}
	}
}