diff options
Diffstat (limited to 'numlib')
| -rwxr-xr-x[-rw-r--r--] | numlib/Jamfile | 4 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/LUtest.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/License.txt | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/Readme.txt | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/aatree.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/aatree.h | 3 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/afiles | 5 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/dhsx.c | 306 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/dhsx.h | 28 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/dnsq.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/dnsq.h | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/dnsqtest.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/ludecomp.c | 16 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/ludecomp.h | 14 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/numlib.h | 2 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/numsup.c | 327 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/numsup.h | 88 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/powell.c | 121 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/powell.h | 24 | ||||
| -rwxr-xr-x | numlib/qptest.c | 105 | ||||
| -rwxr-xr-x | numlib/quadprog.c | 845 | ||||
| -rwxr-xr-x | numlib/quadprog.h | 67 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/rand.c | 128 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/rand.h | 40 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/sobol.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/sobol.h | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/soboltest.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/svd.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/svd.h | 35 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/svdtest.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/tdhsx.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/tpowell.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/ui.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/ui.h | 0 | ||||
| -rwxr-xr-x | numlib/varmet.c | 290 | ||||
| -rwxr-xr-x | numlib/varmet.h | 55 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/zbrent.c | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/zbrent.h | 0 | ||||
| -rwxr-xr-x[-rw-r--r--] | numlib/zbrenttest.c | 0 |
39 files changed, 2267 insertions, 236 deletions
diff --git a/numlib/Jamfile b/numlib/Jamfile index 1053612..2d5edaf 100644..100755 --- a/numlib/Jamfile +++ b/numlib/Jamfile @@ -20,13 +20,13 @@ Headers = numlib.h libui.h ; HDRS = ../h ; # Numeric library -Library libnum.lib : numsup.c dnsq.c powell.c dhsx.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c ; +Library libnum.lib : numsup.c dnsq.c powell.c dhsx.c varmet.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c quadprog.c ; # Link all utilities with libnum LINKLIBS = libnum ; # All test programs are made from a single source file -MainsFromSources dnsqtest.c tpowell.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c ; +MainsFromSources dnsqtest.c tpowell.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c qptest.c ; # Compile .c as .m if $(OS) = MACOSX { diff --git a/numlib/LUtest.c b/numlib/LUtest.c index feb2277..feb2277 100644..100755 --- a/numlib/LUtest.c +++ b/numlib/LUtest.c diff --git a/numlib/License.txt b/numlib/License.txt index a871fcf..a871fcf 100644..100755 --- a/numlib/License.txt +++ b/numlib/License.txt diff --git a/numlib/Readme.txt b/numlib/Readme.txt index 8cc0173..8cc0173 100644..100755 --- a/numlib/Readme.txt +++ b/numlib/Readme.txt diff --git a/numlib/aatree.c b/numlib/aatree.c index 2f38b1d..2f38b1d 100644..100755 --- a/numlib/aatree.c +++ b/numlib/aatree.c diff --git a/numlib/aatree.h b/numlib/aatree.h index 4212b09..325bf1e 100644..100755 --- a/numlib/aatree.h +++ b/numlib/aatree.h @@ -4,6 +4,8 @@ /* Andersson binary balanced tree library + Log n performance on insert/erase + > Created (Julienne Walker): September 10, 2005 This code is in the public domain. Anyone may @@ -33,6 +35,7 @@ typedef struct aat_atree aat_atree_t; typedef struct aat_atrav aat_atrav_t; /* User-defined item handling */ +/* Return -1, 0, +1 */ typedef int (*cmp_f) ( const void *p1, const void *p2 ); /* Andersson tree functions */ diff --git a/numlib/afiles b/numlib/afiles index 29d6699..b2138b2 100644..100755 --- a/numlib/afiles +++ b/numlib/afiles @@ -11,6 +11,8 @@ numsup.h powell.h powell.c tpowell.c +varmet.h +varmet.c dhsx.h dhsx.c tdhsx.c @@ -30,5 +32,8 @@ sobol.h soboltest.c aatree.h aatree.c +quadprog.h +quadprog.c +qptest.c ui.h ui.c 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 } diff --git a/numlib/dhsx.h b/numlib/dhsx.h index 828b8b2..78cdb10 100644..100755 --- a/numlib/dhsx.h +++ b/numlib/dhsx.h @@ -11,29 +11,23 @@ #endif /* Down hill simplex function */ -/* return err on sucess, -1.0 on failure */ -/* Result will be in cp */ -/* Arrays start at 0 */ - -/* Standard interface for optimizer 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, /* Tollerance of error change to stop on */ -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 */ + double *rv, /* If not NULL, return the residual error */ + int di, /* Dimentionality */ + double *cp, /* Initial starting point, return minimum */ + double *s, /* Size of initial search area */ + double ftol, /* Finishing tollerance of error change */ + 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 */ ); double dhsx_funk( /* Return function value */ - void *fdata, /* Opaque data pointer */ - double tp[]); /* Multivriate input value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ #ifdef __cplusplus } diff --git a/numlib/dnsq.c b/numlib/dnsq.c index 75f00a8..75f00a8 100644..100755 --- a/numlib/dnsq.c +++ b/numlib/dnsq.c diff --git a/numlib/dnsq.h b/numlib/dnsq.h index e06e562..e06e562 100644..100755 --- a/numlib/dnsq.h +++ b/numlib/dnsq.h diff --git a/numlib/dnsqtest.c b/numlib/dnsqtest.c index 3f02819..3f02819 100644..100755 --- a/numlib/dnsqtest.c +++ b/numlib/dnsqtest.c diff --git a/numlib/ludecomp.c b/numlib/ludecomp.c index 72e014a..b756bfe 100644..100755 --- a/numlib/ludecomp.c +++ b/numlib/ludecomp.c @@ -162,6 +162,8 @@ int n /* Dimensionality */ } /* Decompose the square matrix A[][] into lower and upper triangles */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use sym_matrix_trans() to fix this. */ /* Return 1 if the matrix is singular. */ int lu_decomp( @@ -339,6 +341,8 @@ int *pivx /* Pivoting row permutations record */ /* Invert a matrix A using lu decomposition */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */ /* Return 1 if the matrix is singular, 0 if OK */ int lu_invert( @@ -385,7 +389,10 @@ int n /* Dimensionality */ return 0; } -#ifdef NEVER /* It's not clear that this is correct */ +/* Invert a matrix A using lu decomposition, and polish it. */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */ +/* Return 1 if the matrix is singular, 0 if OK */ int lu_polished_invert( double **a, /* A[][] input matrix, returns inversion of A */ @@ -413,8 +420,8 @@ int n /* Dimensionality */ return i; } - for (k = 0; k < 10; k++) { - matrix_mult(t1, n, n, aa, n, n, a, n, n); + for (k = 0; k < 20; k++) { + matrix_trans_mult(t1, n, n, aa, n, n, a, n, n); for (i = 0; i < n; i++) { for (j = 0; j < n; j++) { t2[i][j] = a[i][j]; @@ -432,9 +439,10 @@ int n /* Dimensionality */ free_dmatrix(t2, 0, n-1, 0, n-1); return 0; } -#endif /* Pseudo-Invert matrix A using lu decomposition */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use matrix_trans() to fix this, or use matrix_trans_mult(). */ /* Return 1 if the matrix is singular, 0 if OK */ int lu_psinvert( diff --git a/numlib/ludecomp.h b/numlib/ludecomp.h index cdb4382..1075a5b 100644..100755 --- a/numlib/ludecomp.h +++ b/numlib/ludecomp.h @@ -69,6 +69,8 @@ int *pivx /* Pivoting row permutations record */ ); /* Invert a matrix A using lu decomposition */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */ /* Return 1 if the matrix is singular, 0 if OK */ int lu_invert( @@ -76,7 +78,19 @@ double **a, /* A[][] input matrix, returns inversion of A */ int n /* Dimensionality */ ); +/* Invert a matrix A using lu decomposition, and polish it. */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use sym_matrix_trans() to fix this, or use matrix_trans_mult() */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_polished_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +); + /* Pseudo-Invert matrix A using lu decomposition */ +/* NOTE that it returns transposed inverse by normal convention. */ +/* Use matrix_trans() to fix this, or use matrix_trans_mult(). */ /* Return 1 if the matrix is singular, 0 if OK */ int lu_psinvert( diff --git a/numlib/numlib.h b/numlib/numlib.h index aa34b2d..430ce7a 100644..100755 --- a/numlib/numlib.h +++ b/numlib/numlib.h @@ -8,11 +8,13 @@ #include "dnsq.h" /* Non-linear equation solver */ #include "powell.h" /* Powell multi dimentional minimiser */ #include "dhsx.h" /* Downhill simplex multi dimentional minimiser */ +#include "varmet.h" /* Variable Metric multi dimentional minimiser */ #include "ludecomp.h" /* LU decomposition matrix solver */ #include "svd.h" /* Singular Value decomposition matrix solver */ #include "zbrent.h" /* 1 dimentional brent root search */ #include "rand.h" /* Random number generators */ #include "sobol.h" /* Sub-random vector generators */ #include "aatree.h" /* Anderson balanced binary tree */ +#include "quadprog.h" /* Quadradic Programming solution */ #endif /* NUMLIB_H */ diff --git a/numlib/numsup.c b/numlib/numsup.c index 39011ae..eab81ed 100644..100755 --- a/numlib/numsup.c +++ b/numlib/numsup.c @@ -483,17 +483,34 @@ a1log *del_a1log(a1log *log) { return NULL; } +/* Set the debug level. */ +void a1log_debug(a1log *log, int level) { + if (log != NULL) { + log->debug = level; + } +} + +/* Set the vebosity level. */ +void a1log_verb(a1log *log, int level) { + if (log != NULL) { + log->verb = level; + } +} + /* Set the tag. Note that the tage string is NOT copied, just referenced */ void a1log_tag(a1log *log, char *tag) { - log->tag = tag; + if (log != NULL) { + log->tag = tag; + } } /* Log a verbose message if level >= verb */ void a1logv(a1log *log, int level, char *fmt, ...) { + if (log != NULL) { if (log->verb >= level) { va_list args; - + A1LOG_LOCK(log, 0); va_start(args, fmt); log->logv(log->cntx, log, fmt, args); @@ -511,7 +528,7 @@ void a1logd(a1log *log, int level, char *fmt, ...) { A1LOG_LOCK(log, 1); va_start(args, fmt); - log->loge(log->cntx, log, fmt, args); + log->logd(log->cntx, log, fmt, args); va_end(args); A1LOG_UNLOCK(log); } @@ -601,6 +618,8 @@ void a1logue(a1log *log) { void adump_bytes(a1log *log, char *pfx, unsigned char *buf, int base, int len) { int i, j, ii; char oline[200] = { '\000' }, *bp = oline; + if (pfx == NULL) + pfx = ""; for (i = j = 0; i < len; i++) { if ((i % 16) == 0) bp += sprintf(bp,"%s%04x:",pfx,base+i); @@ -827,7 +846,7 @@ void osx_userinitiated_start() { } /* Create a reason string */ - str = objc_msgSend(objc_getClass("NSString"), sel_getUid("alloc")); + str = objc_msgSend((id)objc_getClass("NSString"), sel_getUid("alloc")); str = objc_msgSend(str, sel_getUid("initWithUTF8String:"), "ArgyllCMS"); /* Start activity that tells App Nap to mind its own business. */ @@ -842,8 +861,9 @@ void osx_userinitiated_end() { if (osx_userinitiated_cnt == 0 && osx_userinitiated_activity != nil) { a1logd(g_log, 7, "OS X - User Initiated Activity end"); objc_msgSend( - objc_msgSend(objc_getClass("NSProcessInfo"), sel_getUid("processInfo")), - sel_getUid("endActivity:"), osx_userinitiated_activity); + objc_msgSend((id)objc_getClass("NSProcessInfo"), + sel_getUid("processInfo")), sel_getUid("endActivity:"), + osx_userinitiated_activity); osx_userinitiated_activity = nil; } } @@ -885,7 +905,7 @@ void osx_latencycritical_start() { } /* Create a reason string */ - str = objc_msgSend(objc_getClass("NSString"), sel_getUid("alloc")); + str = objc_msgSend((id)objc_getClass("NSString"), sel_getUid("alloc")); str = objc_msgSend(str, sel_getUid("initWithUTF8String:"), "Measuring Color"); /* Start activity that tells App Nap to mind its own business. */ @@ -900,8 +920,9 @@ void osx_latencycritical_end() { if (osx_latencycritical_cnt == 0 && osx_latencycritical_activity != nil) { a1logd(g_log, 7, "OS X - Latency Critical Activity end"); objc_msgSend( - objc_msgSend(objc_getClass("NSProcessInfo"), sel_getUid("processInfo")), - sel_getUid("endActivity:"), osx_latencycritical_activity); + objc_msgSend((id)objc_getClass("NSProcessInfo"), + sel_getUid("processInfo")), sel_getUid("endActivity:"), + osx_latencycritical_activity); osx_latencycritical_activity = nil; } } @@ -957,7 +978,7 @@ int nh /* Highest index */ } /* --------------------- */ -/* 2D Double vector malloc/free */ +/* 2D Double matrix malloc/free */ double **dmatrix( int nrl, /* Row low index */ int nrh, /* Row high index */ @@ -1179,7 +1200,7 @@ int nch } /* --------------------- */ -/* 2D vector copy */ +/* matrix copy */ void copy_dmatrix( double **dst, double **src, @@ -1713,6 +1734,19 @@ void matrix_trans(double **d, double **s, int nr, int nc) { } } +/* Transpose a 0 base symetrical matrix in place */ +void sym_matrix_trans(double **m, int n) { + int i, j; + + for (i = 0; i < n; i++) { + for (j = i+1; j < n; j++) { + double tt = m[j][i]; + m[j][i] = m[i][j]; + m[i][j] = tt; + } + } +} + /* Multiply two 0 based matricies */ /* Return nz on matching error */ int matrix_mult( @@ -1746,20 +1780,260 @@ int matrix_mult( return 0; } -/* Diagnostic - print to g_log debug */ -void matrix_print(char *c, double **a, int nr, int nc) { +/* Matrix multiply transpose of s1 by s2 */ +/* 0 based matricies, */ +/* This is usefull for using results of lu_invert() */ +int matrix_trans_mult( + double **d, int nr, int nc, + double **ts1, int nr1, int nc1, + double **s2, int nr2, int nc2 +) { + int i, j, k; + + /* s1 and s2 must mesh */ + if (nr1 != nr2) + return 1; + + /* Output rows = s1 rows */ + if (nr != nc1) + return 2; + + /* Output colums = s2 columns */ + if (nc != nc2) + return 2; + + for (i = 0; i < nc1; i++) { + for (j = 0; j < nc2; j++) { + d[i][j] = 0.0; + for (k = 0; k < nr1; k++) { + d[i][j] += ts1[k][i] * s2[k][j]; + } + } + } + + return 0; +} + +/* Multiply a 0 based matrix by a vector */ +/* d may be same as v */ +int matrix_vect_mult( + double *d, int nd, + double **m, int nr, int nc, + double *v, int nv +) { int i, j; - a1logd(g_log, 0, "%s, %d x %d\n",c,nr,nc); + double *_v = v, vv[20]; - for (j = 0; j < nr; j++) { - a1logd(g_log, 0, " "); - for (i = 0; i < nc; i++) { - a1logd(g_log, 0, " %.2f",a[j][i]); + if (d == v) { + if (nv <= 20) { + _v = vv; + } else { + _v = dvector(0, nv-1); } + for (j = 0; j < nv; j++) + _v[j] = v[j]; + } + + /* Input vector must match matrix columns */ + if (nv != nc) + return 1; + + /* Output vector must match matrix rows */ + if (nd != nr) + return 1; + + for (i = 0; i < nd; i++) { + d[i] = 0.0; + for (j = 0; j < nv; j++) + d[i] += m[i][j] * _v[j]; + } + + if (_v != v && _v != vv) + free_dvector(_v, 0, nv-1); + + return 0; +} + + +/* Set zero based dvector */ +void vect_set(double *d, double v, int len) { + int i; + for (i = 0; i < len; i++) + d[i] = v; +} + +/* Copy zero based dvector */ +void vect_cpy(double *d, double *s, int len) { + int i; + for (i = 0; i < len; i++) + d[i] = s[i]; +} + + +/* Negate and copy a vector, d = -v */ +/* d may be same as v */ +void vect_neg(double *d, double *s, int len) { + int i; + for (i = 0; i < len; i++) + d[i] = -s[i]; +} + +/* Add two vectors */ +/* d may be same as v */ +void vect_add( + double *d, + double *v, int len +) { + int i; + + for (i = 0; i < len; i++) + d[i] += v[i]; +} + +/* Subtract two vectors, d -= v */ +/* d may be same as v */ +void vect_sub( + double *d, double *v, int len +) { + int i; + for (i = 0; i < len; i++) + d[i] -= v[i]; +} + + +/* - - - - - - - - - - - - - - - - - - - - - - */ + +/* Print double matrix to g_log debug */ +/* id identifies matrix */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_dmatrix(a1log *log, char *id, char *pfx, double **a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc); + + for (j = 0; j < nr; j++) { + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%f%s",a[j][i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); + } +} + +/* Print float matrix to g_log debug */ +/* id identifies matrix */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_fmatrix(a1log *log, char *id, char *pfx, float **a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc); + + for (j = 0; j < nr; j++) { + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%f%s",a[j][i], i < (nc-1) ? ", " : ""); a1logd(g_log, 0, "\n"); } } +/* Print int matrix to g_log debug */ +/* id identifies matrix */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_imatrix(a1log *log, char *id, char *pfx, int **a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc); + + for (j = 0; j < nr; j++) { + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%d%s",a[j][i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); + } +} + +/* Print short matrix to g_log debug */ +/* id identifies matrix */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_smatrix(a1log *log, char *id, char *pfx, short **a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc); + + for (j = 0; j < nr; j++) { + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%d%s",a[j][i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); + } +} + +/* Print double vector to g_log debug */ +/* id identifies vector */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_dvector(a1log *log, char *id, char *pfx, double *a, int nc) { + int i; + a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc); + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); +} + +/* Print float vector to g_log debug */ +/* id identifies vector */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_fvector(a1log *log, char *id, char *pfx, float *a, int nc) { + int i; + a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc); + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); +} + +/* Print int vector to g_log debug */ +/* id identifies vector */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_ivector(a1log *log, char *id, char *pfx, int *a, int nc) { + int i; + a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc); + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%d%s",a[i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); +} + +/* Print short vector to g_log debug */ +/* id identifies vector */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc) { + int i; + a1logd(g_log, 0, "%s%s[%d]\n",pfx,id,nc); + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%d%s",a[i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); +} + +/* Print C double matrix to g_log debug */ +/* id identifies matrix */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ +void adump_C_dmatrix(a1log *log, char *id, char *pfx, double *a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s%s[%d][%d]\n",pfx,id,nr,nc); + + for (j = 0; j < nr; j++, a += nc) { + a1logd(g_log, 0, "%s ",pfx); + for (i = 0; i < nc; i++) + a1logd(g_log, 0, "%f%s",a[i], i < (nc-1) ? ", " : ""); + a1logd(g_log, 0, "\n"); + } +} /*******************************************/ /* Platform independent IEE754 conversions */ @@ -2286,10 +2560,12 @@ void msec_sleep(unsigned int msec) { } -#if defined(__APPLE__) && !defined(CLOCK_MONOTONIC) +#if defined(__APPLE__) /* && !defined(CLOCK_MONOTONIC) */ #include <mach/mach_time.h> +/* Return the current time in msec */ +/* since the first invokation of msec_time() */ unsigned int msec_time() { mach_timebase_info_data_t timebase; static uint64_t startup = 0; @@ -2400,6 +2676,9 @@ char *debPiv(int di, int *p) { int e; char *bp; + if (p == NULL) + return "(null)"; + if (++ix >= 5) ix = 0; bp = buf[ix]; @@ -2418,11 +2697,14 @@ char *debPiv(int di, int *p) { /* Print a double color vector to a string. */ /* Returned static buffer is re-used every 5 calls. */ char *debPdv(int di, double *p) { - static char buf[5][DEB_MAX_CHAN * 16]; + static char buf[5][DEB_MAX_CHAN * 100]; static int ix = 0; int e; char *bp; + if (p == NULL) + return "(null)"; + if (++ix >= 5) ix = 0; bp = buf[ix]; @@ -2441,11 +2723,14 @@ char *debPdv(int di, double *p) { /* Print a float color vector to a string. */ /* Returned static buffer is re-used every 5 calls. */ char *debPfv(int di, float *p) { - static char buf[5][DEB_MAX_CHAN * 16]; + static char buf[5][DEB_MAX_CHAN * 100]; static int ix = 0; int e; char *bp; + if (p == NULL) + return "(null)"; + if (++ix >= 5) ix = 0; bp = buf[ix]; diff --git a/numlib/numsup.h b/numlib/numsup.h index 9d55daf..86ad6a8 100644..100755 --- a/numlib/numsup.h +++ b/numlib/numsup.h @@ -78,6 +78,14 @@ # endif #endif +#ifndef INLINE +# ifdef _MSC_VER +# define INLINE __inline +# else +# define INLINE inline +# endif +#endif + #else /* !__STDC_VERSION__ */ #ifdef _MSC_VER @@ -100,6 +108,10 @@ # define ATTRIBUTE_NORETURN __declspec(noreturn) #endif +#ifndef INLINE +# define INLINE __inline +#endif + #else /* !_MSC_VER */ /* The following works on a lot of modern systems, including */ @@ -131,6 +143,9 @@ #ifndef ATTRIBUTE_NORETURN # define ATTRIBUTE_NORETURN __attribute__((noreturn)) #endif +#ifndef INLINE +# define INLINE inline +#endif /* INLINE */ #endif /* !_MSC_VER */ #endif /* !__STDC_VERSION__ */ @@ -299,6 +314,12 @@ a1log *new_a1log_d(a1log *log); /* Returns NULL */ a1log *del_a1log(a1log *log); +/* Set the debug logging level. */ +void a1log_debug(a1log *log, int level); + +/* Set the vebosity level. */ +void a1log_verb(a1log *log, int level); + /* Set the tag. Note that the tag string is NOT copied, just referenced */ void a1log_tag(a1log *log, char *tag); @@ -398,6 +419,7 @@ void free_dhmatrix(double **m, int nrl, int nrh, int ncl, int nch); void copy_dmatrix(double **dst, double **src, int nrl, int nrh, int ncl, int nch); void copy_dmatrix_to3x3(double dst[3][3], double **src, int nrl, int nrh, int ncl, int nch); +/* Convert from C matrix to matrix */ double **convert_dmatrix(double *a,int nrl,int nrh,int ncl,int nch); void free_convert_dmatrix(double **m,int nrl,int nrh,int ncl,int nch); @@ -437,6 +459,9 @@ void free_smatrix(short **m,int nrl,int nrh,int ncl,int nch); /* Transpose a 0 base matrix */ void matrix_trans(double **d, double **s, int nr, int nc); +/* Transpose a 0 base symetrical matrix in place */ +void sym_matrix_trans(double **m, int n); + /* Matrix multiply 0 based matricies */ int matrix_mult( double **d, int nr, int nc, @@ -444,8 +469,63 @@ int matrix_mult( double **s2, int nr2, int nc2 ); -/* Diagnostic */ -void matrix_print(char *c, double **a, int nr, int nc); +/* Matrix multiply transpose of s1 by s2 */ +/* 0 based matricies, */ +/* This is usefull for using results of lu_invert() */ +int matrix_trans_mult( + double **d, int nr, int nc, + double **ts1, int nr1, int nc1, + double **s2, int nr2, int nc2 +); + +/* Multiply a 0 based matrix by a vector */ +/* d may be same as v */ +int matrix_vect_mult( + double *d, int nd, + double **m, int nr, int nc, + double *v, int nv +); + +/* Set zero based dvector */ +void vect_set(double *d, double v, int len); + +/* Copy zero based dvector */ +void vect_cpy(double *d, double *s, int len); + +/* Negate and copy a vector, d = -v */ +/* d may be same as v */ +void vect_neg(double *d, double *s, int len); + +/* Add two vectors, d += v */ +/* d may be same as v */ +void vect_add( + double *d, double *v, int len +); + +/* Subtract two vectors, d -= v */ +/* d may be same as v */ +void vect_sub( + double *d, double *v, int len +); + + +/* Diagnostics */ +/* id identifies matrix/vector */ +/* pfx used at start of each line */ +/* Assumed indexed from 0 */ + +void adump_dmatrix(a1log *log, char *id, char *pfx, double **a, int nr, int nc); +void adump_fmatrix(a1log *log, char *id, char *pfx, float **a, int nr, int nc); +void adump_imatrix(a1log *log, char *id, char *pfx, int **a, int nr, int nc); +void adump_smatrix(a1log *log, char *id, char *pfx, short **a, int nr, int nc); + +void adump_dvector(a1log *log, char *id, char *pfx, double *a, int nc); +void adump_fvector(a1log *log, char *id, char *pfx, float *a, int nc); +void adump_ivector(a1log *log, char *id, char *pfx, int *a, int nc); +void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc); + +/* Dump C type matrix */ +void adump_C_dmatrix(a1log *log, char *id, char *pfx, double *a, int nr, int nc); /* =========================================================== */ @@ -547,11 +627,11 @@ double usec_time(); /* Returned static buffer is re-used every 5 calls. */ char *debPiv(int di, int *p); -/* Print a double color vector to a string. */ +/* Print a double vector to a string. */ /* Returned static buffer is re-used every 5 calls. */ char *debPdv(int di, double *p); -/* Print a float color vector to a string. */ +/* Print a float vector to a string. */ /* Returned static buffer is re-used every 5 calls. */ char *debPfv(int di, float *p); diff --git a/numlib/powell.c b/numlib/powell.c index 5d2adac..47acc15 100644..100755 --- a/numlib/powell.c +++ b/numlib/powell.c @@ -42,18 +42,34 @@ #undef SLOPE_SANITY_CHECK /* exermental */ #undef ABSTOL /* Make tollerance absolute */ -#undef DEBUG /* Some debugging printfs (not comprehensive) */ + /* Some debugging printfs (not comprehensive): */ +#undef PDEBUG /* Powell debug */ +#undef CDEBUG /* Conjgrad debug */ +#undef LDEBUG /* Line min debug */ + +#if defined(PDEBUG) +# undef PDBG +# define PDBG(xxx) printf xxx ; +#else +# undef PDBG +# define PDBG(xxx) +#endif -#ifdef DEBUG -#undef DBG -#define DBG(xxx) printf xxx ; +#if defined(CDEBUG) +# undef CDBG +# define CDBG(xxx) printf xxx ; #else -#undef DBG -#define DBG(xxx) +# undef CDBG +# define CDBG(xxx) #endif -static double linmin(double p[], double xi[], int n, double ftol, - double (*func)(void *fdata, double tp[]), void *fdata); +#if defined(LDEBUG) +# undef LDBG +# define LDBG(xxx) printf xxx ; +#else +# undef LDBG +# define LDBG(xxx) +#endif /* Standard interface for powell function */ /* return 0 on sucess, 1 on failure due to excessive itterions */ @@ -120,7 +136,7 @@ void *pdata /* Opaque data needed by prog() */ /* Loop over all directions in the set */ for (i = 0; i < di; i++) { - DBG(("Looping over direction %d\n",i)) + PDBG(("Looping over direction %d\n",i)) for (j = 0; j < di; j++) /* Extract this direction to make search vector */ svec[j] = dmtx[j][i]; @@ -161,10 +177,10 @@ void *pdata /* Opaque data needed by prog() */ /* reached a suitable tollerance, then finish */ if (iter > 1 && curdel <= stopth) { //printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth); - DBG(("Reached stop tollerance because curdel %f <= stopth %f\n",curdel,stopth)) + PDBG(("Reached stop tollerance because curdel %f <= stopth %f\n",curdel,stopth)) break; } - DBG(("Not stopping because curdel %f > stopth %f\n",curdel,stopth)) + PDBG(("Not stopping because curdel %f > stopth %f\n",curdel,stopth)) //printf("~1 ### recomputing direction\n"); for (i = 0; i < di; i++) { @@ -212,7 +228,7 @@ void *pdata /* Opaque data needed by prog() */ if (iter < maxit) return 0; - DBG(("powell: returning 1 due to excessive itterations\n")) + PDBG(("powell: returning 1 due to excessive itterations\n")) return 1; /* Failed due to execessive itterations */ } @@ -225,7 +241,7 @@ void *pdata /* Opaque data needed by prog() */ int conjgrad( double *rv, /* If not NULL, return the residual error */ int di, /* Dimentionality */ -double cp[], /* Initial starting point */ +double cp[], /* Initial starting point and return value */ double s[], /* Size of initial search area */ #ifdef ABSTOL double ftol, /* Absolute tollerance of error change to stop on */ @@ -257,7 +273,7 @@ void *pdata /* Opaque data needed by prog() */ if (prog != NULL) /* Report initial progress */ prog(pdata, pc); - /* Initial function evaluation */ + /* Initial gradient function evaluation */ retv = (*dfunc)(fdata, svec, cp); /* svec[] seems to be large after this. */ @@ -272,21 +288,22 @@ void *pdata /* Opaque data needed by prog() */ else svec_sca = 1.0/svec_sca; -//printf("~1 ### initial dir = %f %f\n", svec[0],svec[1]); -//printf("~1 ### initial retv = %f\n",retv); + CDBG((" initial dir = %s\n", debPdv(di,svec))); + CDBG((" initial retv = %f\n",retv)); + /* Initial vector setup */ for (i = 0; i < di; i++) { gvec[i] = hvec[i] = -svec[i]; /* Inverse gradient */ svec[i] = s[i] * -svec[i] * svec_sca; /* Scale the search vector */ } -//printf("~1 ### svec = %f %f\n", svec[0],svec[1]); + CDBG(("Initial svec = %s\n", debPdv(di,svec))); /* Itterate untill we converge on a solution, or give up. */ for (iter = 1; iter < maxit; iter++) { double gamden, gamnum, gam; double pretv; /* Previous function return value */ - DBG(("conjrad: about to do linmin\n")) + CDBG(("conjrad: about to do linmin\n")) pretv = retv; retv = linmin(cp, svec, di, ftol, func, fdata); @@ -296,7 +313,7 @@ void *pdata /* Opaque data needed by prog() */ stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON); // Old code #endif curdel = fabs(pretv - retv); -//printf("~1 ### this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel); + CDBG((" this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel)); if (startdel < 0.0) { startdel = curdel; } else { @@ -312,31 +329,30 @@ void *pdata /* Opaque data needed by prog() */ /* If we have had at least one change of direction and */ /* reached a suitable tollerance, then finish */ if (iter > 1 && curdel <= stopth) { -//printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth); + CDBG((" stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth)); break; } -//printf("~1 ### Not stopping on itter %d because curdel %f > stopth %f\n",iter, curdel,stopth); + CDBG((" not stopping on itter %d because curdel %f > stopth %f\n",iter, curdel,stopth)); - DBG(("conjrad: recomputing direction\n")) -//printf("~1 ### recomputing direction\n"); + CDBG(("conjrad: recomputing direction\n")) (*dfunc)(fdata, svec, cp); /* (Don't use retv as it wrecks stop test) */ + CDBG((" pderiv = %s\n", debPdv(di,svec))); -//printf("~1 ### pderiv = %f %f\n", svec[0],svec[1]); /* Compute gamma */ for (gamnum = gamden = 0.0, i = 0; i < di; i++) { gamnum += svec[i] * (gvec[i] + svec[i]); gamden += gvec[i] * gvec[i]; } -//printf("~1 ### gamnum = %f, gamden = %f\n", gamnum,gamden); + CDBG((" gamnum = %f, gamden = %f\n", gamnum,gamden)); if (gamden == 0.0) { /* Gradient is exactly zero */ - DBG(("conjrad: gradient is exactly zero\n")) + CDBG(("conjrad: gradient is exactly zero\n")) break; } gam = gamnum/gamden; - DBG(("conjrad: gamma = %f = %f/%f\n",gam,gamnum,gamden)) -//printf("~1 ### gvec[] = %f %f, gamma = %f, hvec = %f %f\n", gvec[0],gvec[1],gam,hvec[0],hvec[1]); + CDBG(("conjrad: gamma = %f = %f/%f\n",gam,gamnum,gamden)); + CDBG((" gvec[] = %s, hvec = %s\n", debPdv(di,gvec),debPdv(di,hvec))); /* Adjust seach direction */ for (i = 0; i < di; i++) { @@ -357,8 +373,7 @@ void *pdata /* Opaque data needed by prog() */ svec_sca = 1.0/svec_sca; for (i = 0; i < di; i++) svec[i] = svec[i] * s[i] * svec_sca; -//printf("~1 ### svec = %f %f\n", svec[0],svec[1]); - + CDBG((" svec = %s\n", debPdv(di,svec))); } /* Free up all the temporary vectors and matrix */ free_dvector(hvec,0,di-1); @@ -371,7 +386,7 @@ void *pdata /* Opaque data needed by prog() */ if (rv != NULL) *rv = retv; -//printf("~1 ### done\n"); + CDBG((" conjgrad returning = %f\n", retv)); if (iter < maxit) return 0; @@ -386,7 +401,7 @@ void *pdata /* Opaque data needed by prog() */ /* Line bracketing and minimisation routine. */ /* Return value at minimum. */ -static double linmin( +double linmin( double cp[], /* Start point, and returned value */ double xi[], /* Search vector */ int di, /* Dimensionality */ @@ -411,7 +426,7 @@ void *fdata) /* Opaque data for func() */ /* -------------------------- */ /* First bracket the solution */ - DBG(("linmin: Bracketing solution\n")) + LDBG(("linmin: Bracketing solution\n")) /* The line is measured as startpoint + offset * search vector. */ /* (Search isn't symetric, but it seems to depend on cp being */ @@ -427,7 +442,7 @@ void *fdata) /* Opaque data for func() */ xt[i] = cp[i] + xx * xi[i]; xf = (*func)(fdata, xt); - DBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) + LDBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) /* Fix it so that we are decreasing from point a -> x */ if (xf > af) { @@ -435,21 +450,21 @@ void *fdata) /* Opaque data for func() */ tt = ax; ax = xx; xx = tt; tt = af; af = xf; xf = tt; } - DBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) + LDBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) bx = xx + POWELL_GOLD * (xx-ax); /* Guess b beyond a -> x */ for (i = 0; i < di; i++) xt[i] = cp[i] + bx * xi[i]; bf = (*func)(fdata, xt); - DBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + LDBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) #ifdef SLOPE_SANITY_CHECK /* If we're not seeing a slope indicitive of progress */ /* of order ftol, give up straight away */ if (2000.0 * fabs(xf - bf) <= ftol * (fabs(xf) + fabs(bf)) && 2000.0 * fabs(af - xf) <= ftol * (fabs(af) + fabs(xf))) { - DBG(("linmin: giving up because slope is too shallow\n")) + LDBG(("linmin: giving up because slope is too shallow\n")) if (xt != XT) free_dvector(xt,0,di-1); @@ -470,9 +485,9 @@ void *fdata) /* Opaque data for func() */ double ulim, ux, uf; double tt, r, q; -// DBG(("linmin: Not bracketed a:%f:%f x:%f%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) - DBG(("linmin: Not bracketed because xf %f > bf %f\n",xf, bf)) - DBG((" ax = %f, xx = %f, bx = %f\n",ax,xx,bx)) +// LDBG(("linmin: Not bracketed a:%f:%f x:%f%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + LDBG(("linmin: Not bracketed because xf %f > bf %f\n",xf, bf)) + LDBG((" ax = %f, xx = %f, bx = %f\n",ax,xx,bx)) /* Compute ux by parabolic interpolation from a, x & b */ q = (xx - bx) * (xf - af); @@ -544,7 +559,7 @@ void *fdata) /* Opaque data for func() */ bx = ux; bf = uf; //printf("~1 move along to the right (a<-x, x<-b, b-<u)\n"); } - DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) /* Got bracketed minimum between a -> x -> b */ //printf("~1 got bracketed minimum at %f (%f), %f (%f), %f (%f)\n",ax,af,xx,xf,bx,bf); @@ -581,12 +596,12 @@ void *fdata) /* Opaque data for func() */ #endif double tol2 = 2.0 * tol1; - DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) /* See if we're done */ //printf("~1 linmin check %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)); if (fabs(xx - mx) <= (tol2 - 0.5 * (bx - ax))) { - DBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax))) + LDBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax))) break; } @@ -603,13 +618,13 @@ void *fdata) /* Opaque data for func() */ te = e; /* Save previous e value */ e = de; /* Previous steps distance moved */ - DBG(("linmin: Trial parabolic fit\n" )) + LDBG(("linmin: Trial parabolic fit\n" )) if (fabs(p) >= fabs(0.5 * q * te) || p <= q * (ax-xx) || p >= q * (bx-xx)) { /* Give up on the parabolic fit, and use the golden section search */ e = ((xx >= mx) ? ax-xx : bx-xx); /* Override previous distance moved */ de = POWELL_CGOLD * e; - DBG(("linmin: Moving to golden section search\n" )) + LDBG(("linmin: Moving to golden section search\n" )) } else { /* Use parabolic fit */ de = p/q; /* Change in xb */ ux = xx + de; /* Trial point according to parabolic fit */ @@ -619,24 +634,24 @@ void *fdata) /* Opaque data for func() */ else de = -tol1; } - DBG(("linmin: Using parabolic fit\n" )) + LDBG(("linmin: Using parabolic fit\n" )) } } else { /* Keep using the golden section search */ e = ((xx >= mx) ? ax-xx : bx-xx); /* Override previous distance moved */ de = POWELL_CGOLD * e; - DBG(("linmin: Continuing golden section search\n" )) + LDBG(("linmin: Continuing golden section search\n" )) } if (fabs(de) >= tol1) { /* If de moves as much as tol1 would */ ux = xx + de; /* use it */ - DBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de)) + LDBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de)) } else { /* else move by tol1 in direction de */ if (de > 0.0) { ux = xx + tol1; - DBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1)) + LDBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1)) } else { ux = xx - tol1; - DBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1)) + LDBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1)) } } @@ -646,6 +661,7 @@ void *fdata) /* Opaque data for func() */ uf = (*func)(fdata, xt); if (uf <= xf) { /* Found new best solution */ + LDBG(("linmin: found new best solution at %f val %f\n",ux,uf)) if (ux >= xx) { ax = xx; af = xf; /* New lower bracket */ } else { @@ -654,8 +670,9 @@ void *fdata) /* Opaque data for func() */ vx = wx; vf = wf; /* New previous 2nd best solution */ wx = xx; wf = xf; /* New 2nd best solution from previous best */ xx = ux; xf = uf; /* New best solution from latest */ - DBG(("linmin: found new best solution\n")) } else { /* Found a worse solution */ + LDBG(("linmin: found new worse solution at %f val %f\n",ux,uf)) + LDBG(("linmin: current best at %f val %f\n",xx,xf)) if (ux < xx) { ax = ux; af = uf; /* New lower bracket */ } else { @@ -667,13 +684,13 @@ void *fdata) /* Opaque data for func() */ } else if (uf <= vf || vx == xx || vx == wx) { /* New 3rd best, or equal 1st & 2nd */ vx = ux; vf = uf; /* New previous 2nd best from latest */ } - DBG(("linmin: found new worse solution\n")) } } /* !!! should do something if iter > POWELL_MAXIT !!!! */ /* Solution is at xx, xf */ /* Compute solution vector */ + LDBG(("linmin: computing soln from best at %f val %f\n",xx,xf)) for (i = 0; i < di; i++) cp[i] += xx * xi[i]; } diff --git a/numlib/powell.h b/numlib/powell.h index a1db8b6..4b86573 100644..100755 --- a/numlib/powell.h +++ b/numlib/powell.h @@ -54,18 +54,20 @@ double powell_funk( /* Return function value */ void *fdata, /* Opaque data pointer */ double tp[]); /* Multivriate input value */ -/* Line in multi-dimensional space minimiser */ -double brentnd( /* vector multiplier return value */ -double ax, /* Minimum of multiplier range */ -double bx, /* Starting point multiplier of search */ -double cx, /* Maximum of multiplier range */ -double ftol, /* Tollerance to stop search */ -double *xmin, /* Return value of multiplier at minimum */ -int n, /* Dimensionality */ +/* Line bracketing and minimisation routine. */ +/* Return value at minimum. */ +double linmin( +double cp[], /* Start point, and returned value */ +double xi[], /* Search vector */ +int di, /* Dimensionality */ +#ifdef ABSTOL +double ftol, /* Absolute tolerance to stop on */ +#else +double ftol, /* Relative tolerance to stop on */ +#endif double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ -void *fdata, /* Opaque data */ -double pcom[], /* Base vector point */ -double xicom[]); /* Vector that will be multiplied and added to pcom[] */ +void *fdata /* Opaque data for func() */ +); #ifdef __cplusplus } diff --git a/numlib/qptest.c b/numlib/qptest.c new file mode 100755 index 0000000..cc826f1 --- /dev/null +++ b/numlib/qptest.c @@ -0,0 +1,105 @@ +/* + + This file contains just an example on how to set-up the matrices for using with + the quadprog() function. + + The test problem is the following: + + Given: + G = 4 -2 g0^T = [6 0] + -2 4 + + Solve: + min f(x) = 1/2 x G x + g0 x + s.t. + x_1 + x_2 = 3 + x_1 >= 0 + x_2 >= 0 + x_1 + x_2 >= 2 + + The solution is x^T = [1 2] and f(x) = 12 + +*/ + +#include "stdio.h" +#include "numlib.h" +#include "quadprog.h" + +int main() { + double **GG, **G, **CE, **CI; + double *g0, *ce0, *ci0, *x; + int n, m, p; + int i, j; + double f, sum = 0.0; + + n = 2; + + /* Orginal, unchanged G */ + GG = dmatrix(0, n-1, 0, n-1); + GG[0][0] = 4.0; + GG[0][1] = -2.0; + GG[1][0] = -2.0; + GG[1][1] = 4.0; + + /* Working copy - gets modified */ + G = dmatrix(0, n-1, 0, n-1); + copy_dmatrix(G, GG, 0, n-1, 0, n-1); + + g0 = dvector(0, n-1); + g0[0] = 6.0; + g0[1] = 0.0; + + p = 1; + + CE = dmatrix(0, n-1, 0, p-1); + CE[0][0] = 1.0; + CE[1][0] = 1.0; + + ce0 = dvector(0, p-1); + ce0[0] = -3.0; + + m = 3; + CI = dmatrix(0, n-1, 0, m-1); + CI[0][0] = 1.0; + CI[0][1] = 0.0; + CI[0][2] = 1.0; + CI[1][0] = 0.0; + CI[1][1] = 1.0; + CI[1][2] = 1.0; + + ci0 = dvector(0, m-1); + ci0[0] = 0.0; + ci0[1] = 0.0; + ci0[2] = -2.0; + + x = dvector(0, n-1); + + f = quadprog(x, G, g0, CE, ce0, CI, ci0, n, p, m); + + if (f == QP_INFEASIBLE) + printf("Failed to find solution\n"); + else { + printf("Function value at %f %f = %f\n",x[0],x[1],f); + + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) + sum += x[i] * GG[i][j] * x[j]; + sum *= 0.5; + + for (i = 0; i < n; i++) + sum += g0[i] * x[i]; + + printf("Double check funtion value %f\n",sum); + } + + free_dmatrix(GG, 0, n-1, 0, n-1); + free_dmatrix(G, 0, n-1, 0, n-1); + free_dvector(g0, 0, n-1); + free_dmatrix(CE, 0, n-1, 0, p-1); + free_dvector(ce0, 0, p-1); + free_dmatrix(CI, 0, n-1, 0, m-1); + free_dvector(ci0, 0, m-1); + free_dvector(x, 0, n-1); + + return 0; +} diff --git a/numlib/quadprog.c b/numlib/quadprog.c new file mode 100755 index 0000000..847601f --- /dev/null +++ b/numlib/quadprog.c @@ -0,0 +1,845 @@ + +/* + Quadradic Programming soliution. + + Based on Luca Di Gaspero's QuadProgpp++, made available with + the following license: + + MIT License + + Copyright (c) 2007-2016 Luca Di Gaspero + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + + Translation to C by Graeme W. Gill, Copyright 2017, also licensed under the + above license. +*/ + +/* + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + for the solution of a (convex) Quadratic Programming problem + by means of an active-set dual method. + +The problem is in the form: + +min 0.5 * x G x + g0 x +s.t. + CE^T x + ce0 = 0 + CI^T x + ci0 >= 0 + + The matrix and vectors dimensions are as follows: + G: n * n + g0: n + + CE: n * p + ce0: p + + CI: n * m + ci0: m + + x: n + + References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving + strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. + +*/ + +#include "numsup.h" +#include "quadprog.h" + +#undef TRACE_SOLVER /* Print progress */ + +/* Utility functions for updating some data needed by the solution method */ +INLINE static void compute_d(double *d, double **J, double *np, int n); +INLINE static void update_z(double *z, double **J, double *d, int iq, int n); +INLINE static void update_r(double **R, double *r, double *d, int iq, int n); +static int add_constraint(double **R, double **J, double *d, int *piq, double *prnorm, int n); +INLINE static void delete_constraint(double **R, double **J, int *A, double *u, + int n, int p, int *piq, int l); + +/* Utility functions for computing the Cholesky decomposition and solving */ +/* linear systems */ +static void cholesky_decomposition(double **A, int n); +static void cholesky_solve(double **L, double *x, double *b, int n); +INLINE static void forward_elimination(double **L, double *y, double *b, int n); +INLINE static void backward_elimination(double **U, double *x, double *y, int n); + +/* Utility functions for computing the scalar product and the euclidean */ +/* distance between two numbers */ +INLINE static double scalar_product(double *x, double *y, int n); +INLINE static double distance(double a, double b); + +/* Utility functions for printing vectors and matrices */ +static void print_matrix(char* name, double **A, int n, int m); + +static void print_vector(char* name, double *v, int n); +static void print_ivector(char* name, int *v, int n); + +#define FMIN(A,B) ((A) < (B) ? (A) : (B)) + +#define FMAX(A,B) ((A) > (B) ? (A) : (B)) + +#define ASZ 10 +#define VSZ 20 + +double quadprog( /* Return solution cost, QP_INFEASIBLE if infeasible/error */ + double *x, /* Return x[n] value */ + double **G, /* G[n][n] Quadratic combination matrix - modified */ + double *g0, /* g0[n] Direct vector */ + double **CE, /* CE[n][p] Equality constraint matrix */ + double *ce0, /* ce0[p] Equality constraing constants */ + double **CI, /* CI[n][m] Constraint matrix */ + double *ci0, /* cie[m] Constraint constants */ + int n, /* Number of variables */ + int p, /* Number of equalities */ + int m /* Number of constraints */ +) { + int i, j, k, l; /* indices */ + int ip; /* index of the constraint to be added to the active set */ + double f_value = QP_INFEASIBLE, psi, c1, c2, sum, ss, R_norm; + double inf; + double t, t1, t2; /* t is the step length, which is the minimum of the partial */ + /* step length t1 and the full step length t2 */ + int q, iq, iter = 0; + + double **R, **J; + double *s, *z, *r, *d, *np, *u, *x_old, *u_old; + int *A, *A_old, *iai; + short *iaexcl; + + double *_R[ASZ], __R[ASZ][ASZ], *_J[ASZ], __J[ASZ][ASZ]; + double _s[VSZ], _z[VSZ], _r[VSZ], _d[VSZ], _np[VSZ], _u[VSZ], _x_old[VSZ], _u_old[VSZ]; + int _A[VSZ], _A_old[VSZ], _iai[VSZ]; + short _iaexcl[VSZ]; + + /* Avoid malloc's if we can.. */ + if (n <= ASZ) { + R = _R; + J = _J; + for (i = 0; i < n; i++) { + _R[i] = __R[i]; + _J[i] = __J[i]; + } + } else { + R = dmatrix(0, n-1, 0, n-1); + J = dmatrix(0, n-1, 0, n-1); + } + + if (n <= VSZ) { + x_old = _x_old; + z = _z; + d = _d; + np = _np; + } else { + x_old = dvector(0, n-1); + z = dvector(0, n-1); + d = dvector(0, n-1); + np = dvector(0, n-1); + } + + if ((m + p) <= VSZ) { + s = _s; + r = _r; + u = _u; + u_old = _u_old; + A = _A; + A_old = _A_old; + iai = _iai; + iaexcl = _iaexcl; + } else { + s = dvector(0, m + p-1); + r = dvector(0, m + p-1); + u = dvector(0, m + p-1); + u_old = dvector(0, m + p-1); + A = ivector(0, m + p-1); + A_old = ivector(0, m + p-1); + iai = ivector(0, m + p-1); + iaexcl = svector(0, m + p-1); + } + + q = 0; /* size of the active set A (containing the indices of the active constraints) */ + +#ifdef TRACE_SOLVER + printf("\nStarting solve_quadprog\n"); + print_matrix("G", G, n, n); + print_vector("g0", g0, n); + print_matrix("CE", CE, n, p); + print_vector("ce0", ce0, p); + print_matrix("CI", CI, n, m); + print_vector("ci0", ci0, m); +#endif + + /* + * Preprocessing phase + */ + + /* compute the trace of the original matrix G */ + c1 = 0.0; + for (i = 0; i < n; i++) + c1 += G[i][i]; + /* decompose the matrix G in the form L^T L */ + cholesky_decomposition(G, n); +#ifdef TRACE_SOLVER + print_matrix("G", G, n, n); +#endif + + /* initialize the matrix R */ + for (i = 0; i < n; i++) { + d[i] = 0.0; + for (j = 0; j < n; j++) + R[i][j] = 0.0; + } + R_norm = 1.0; /* this variable will hold the norm of the matrix R */ + + /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */ + c2 = 0.0; + for (i = 0; i < n; i++) { + d[i] = 1.0; + forward_elimination(G, z, d, n); + for (j = 0; j < n; j++) + J[i][j] = z[j]; + c2 += z[i]; + d[i] = 0.0; + } +#ifdef TRACE_SOLVER + print_matrix("J", J, n, n); +#endif + + /* c1 * c2 is an estimate for cond(G) */ + + /* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x */ + /* this is a feasible point in the dual space */ + /* x = G^-1 * g0 */ + cholesky_solve(G, x, g0, n); + for (i = 0; i < n; i++) + x[i] = -x[i]; + /* and compute the current solution value */ + f_value = 0.5 * scalar_product(g0, x, n); +#ifdef TRACE_SOLVER + printf("Unconstrained solution: %f",f_value); + print_vector("x", x, n); +#endif + + /* Add equality constraints to the working set A */ + iq = 0; + for (i = 0; i < p; i++) { + for (j = 0; j < n; j++) + np[j] = CE[j][i]; + compute_d(d, J, np, n); + update_z(z, J, d, iq, n); + update_r(R, r, d, iq, n); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, n); + print_vector("z", z, n); + print_vector("r", r, iq); + print_vector("d", d, n); +#endif + + /* compute full step length t2: i.e., the minimum step in primal space s.t. */ + /* the contraint becomes feasible */ + t2 = 0.0; + if (fabs(scalar_product(z, z, n)) > DBL_EPSILON) /* i.e. z != 0 */ + t2 = (-scalar_product(np, x, n) - ce0[i]) / scalar_product(z, np, n); + + /* set x = x + t2 * z */ + for (k = 0; k < n; k++) + x[k] += t2 * z[k]; + + /* set u = u+ */ + u[iq] = t2; + for (k = 0; k < iq; k++) + u[k] -= t2 * r[k]; + + /* compute the new solution value */ + f_value += 0.5 * (t2 * t2) * scalar_product(z, np, n); + A[i] = -i - 1; + + if (!add_constraint(R, J, d, &iq, &R_norm, n)) { + /* Equality constraints are linearly dependent */ +#ifdef TRACE_SOLVER + printf("Constraints are linearly dependent\n"); +#endif + goto done; + } + } + + /* set iai = K \ A */ + for (i = 0; i < m; i++) + iai[i] = i; + + /* Hmm. Use of crossing goto's is a little Fortran like... */ + +l1: iter++; +#ifdef TRACE_SOLVER + print_vector("x", x, n); +#endif + /* step 1: choose a violated constraint */ + for (i = p; i < iq; i++) { + ip = A[i]; + iai[ip] = -1; + } + + /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ + ss = 0.0; + psi = 0.0; /* this value will contain the sum of all infeasibilities */ + ip = 0; /* ip will be the index of the chosen violated constraint */ + for (i = 0; i < m; i++) { + iaexcl[i] = 1; + sum = 0.0; + for (j = 0; j < n; j++) + sum += CI[j][i] * x[j]; + sum += ci0[i]; + s[i] = sum; + psi += FMIN(0.0, sum); + } +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + + if (fabs(psi) <= m * DBL_EPSILON * c1 * c2* 100.0) { + /* numerically there are not infeasibilities anymore */ + q = iq; + + goto done; + } + + /* save old values for u and A */ + for (i = 0; i < iq; i++) { + u_old[i] = u[i]; + A_old[i] = A[i]; + } + + /* and for x */ + for (i = 0; i < n; i++) + x_old[i] = x[i]; + +l2: /* Step 2: check for feasibility and determine a new S-pair */ + for (i = 0; i < m; i++) { + if (s[i] < ss && iai[i] != -1 && iaexcl[i]) { + ss = s[i]; + ip = i; + } + } + if (ss >= 0.0) { + q = iq; + + goto done; + } + + /* set np = n[ip] */ + for (i = 0; i < n; i++) + np[i] = CI[i][ip]; + /* set u = [u 0]^T */ + u[iq] = 0.0; + /* add ip to the active set A */ + A[iq] = ip; + +#ifdef TRACE_SOLVER + printf("Trying with constraint %d\n",ip); + print_vector("np", np, n); +#endif + +l2a: + /* Step 2a: determine step direction */ + /* compute z = H np: the step direction in the primal space (through J, see the paper) */ + compute_d(d, J, np, n); + update_z(z, J, d, iq, n); + /* compute N* np (if q > 0): the negative of the step direction in the dual space */ + update_r(R, r, d, iq, n); +#ifdef TRACE_SOLVER + printf("Step direction z\n"); + print_vector("z", z, n); + print_vector("r", r, iq + 1); + print_vector("u", u, iq + 1); + print_vector("d", d, n); + print_ivector("A", A, iq + 1); +#endif + + /* Step 2b: compute step length */ + l = 0; + /* Compute t1: partial step length (maximum step in dual space */ + /* without violating dual feasibility */ + t1 = QP_INFEASIBLE; /* +inf */ + /* find the index l s.t. it reaches the minimum of u+[x] / r */ + for (k = p; k < iq; k++) { + if (r[k] > 0.0) { + if (u[k] / r[k] < t1) { + t1 = u[k] / r[k]; + l = A[k]; + } + } + } + /* Compute t2: full step length (minimum step in primal space */ + /* such that the constraint ip becomes feasible */ + if (fabs(scalar_product(z, z, n)) > DBL_EPSILON) { /* i.e. z != 0 */ + t2 = -s[ip] / scalar_product(z, np, n); + if (t2 < 0) /* patch suggested by Takano Akio for handling numerical inconsistencies */ + t2 = QP_INFEASIBLE; + } else + t2 = QP_INFEASIBLE; /* +inf */ + + /* the step is chosen as the minimum of t1 and t2 */ + t = FMIN(t1, t2); +#ifdef TRACE_SOLVER + printf("Step sizes: %f (t1 = %f, t2 = %f\n",t,t1,t2); +#endif + + /* Step 2c: determine new S-pair and take step: */ + + /* case (i): no step in primal or dual space */ + if (t >= QP_INFEASIBLE) { + /* QPP is infeasible */ + // FIXME: unbounded to raise + q = iq; + f_value = QP_INFEASIBLE; + goto done; + } + /* case (ii): step in dual space */ + if (t2 >= QP_INFEASIBLE) { + /* set u = u + t * [-r 1] and drop constraint l from the active set A */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; + iai[l] = l; + delete_constraint(R, J, A, u, n, p, &iq, l); +#ifdef TRACE_SOLVER + printf(" in dual space: %f\n",f_value); + print_vector("x", x, n); + print_vector("z", z, n); + print_ivector("A", A, iq + 1); +#endif + goto l2a; + } + + /* case (iii): step in primal and dual space */ + + /* set x = x + t * z */ + for (k = 0; k < n; k++) + x[k] += t * z[k]; + /* update the solution value */ + f_value += t * scalar_product(z, np, n) * (0.5 * t + u[iq]); + /* u = u + t * [-r 1] */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; +#ifdef TRACE_SOLVER + printf(" in both spaces: %f\n",f_value); + print_vector("x", x, n); + print_vector("u", u, iq + 1); + print_vector("r", r, iq + 1); + print_ivector("A", A, iq + 1); +#endif + + if (fabs(t - t2) < DBL_EPSILON) { +#ifdef TRACE_SOLVER + printf("Full step has taken %f\n",t); + print_vector("x", x, n); +#endif + /* full step has taken */ + /* add constraint ip to the active set*/ + if (!add_constraint(R, J, d, &iq, &R_norm, n)) { + iaexcl[ip] = 0; + delete_constraint(R, J, A, u, n, p, &iq, ip); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, n); + print_ivector("A", A, iq); + print_ivector("iai", iai, iq); // ?? iq size of m+p ? +#endif + for (i = 0; i < m; i++) + iai[i] = i; + for (i = p; i < iq; i++) { + A[i] = A_old[i]; + u[i] = u_old[i]; + iai[A[i]] = -1; + } + for (i = 0; i < n; i++) + x[i] = x_old[i]; + goto l2; /* go to step 2 */ + } else + iai[ip] = -1; +#ifdef TRACE_SOLVER + print_matrix("R", R, n, n); + print_ivector("A", A, iq); + print_ivector("iai", iai, iq); // ?? iq size of m+p ? +#endif + goto l1; + } + + /* a patial step has taken */ +#ifdef TRACE_SOLVER + printf("Partial step has taken %f\n",t); + print_vector("x", x, n); +#endif + /* drop constraint l */ + iai[l] = l; + delete_constraint(R, J, A, u, n, p, &iq, l); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, n); + print_ivector("A", A, iq); +#endif + + /* update s[ip] = CI * x + ci0 */ + sum = 0.0; + for (k = 0; k < n; k++) + sum += CI[k][ip] * x[k]; + s[ip] = sum + ci0[ip]; + +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + goto l2a; + + done:; + + /* Cleanup and return f value */ + if (n > ASZ) { + free_dmatrix(R, 0, n-1, 0, n-1); + free_dmatrix(J, 0, n-1, 0, n-1); + } + + if (n > VSZ) { + free_dvector(x_old, 0, n-1); + free_dvector(z, 0, n-1); + free_dvector(d, 0, n-1); + free_dvector(np, 0, n-1); + } + + if ((m + p) > VSZ) { + free_dvector(s, 0, m + p-1); + free_dvector(r, 0, m + p-1); + free_dvector(u, 0, m + p-1); + free_dvector(u_old, 0, m + p-1); + free_ivector(A, 0, m + p-1); + free_ivector(A_old, 0, m + p-1); + free_ivector(iai, 0, m + p-1); + free_svector(iaexcl, 0, m + p-1); + } + + return f_value; +} + +INLINE static void compute_d(double *d, double **J, double *np, int n) { + int i, j; + double sum; + + /* compute d = H^T * np */ + for (i = 0; i < n; i++) { + sum = 0.0; + for (j = 0; j < n; j++) + sum += J[j][i] * np[j]; + d[i] = sum; + } +} + +INLINE static void update_z(double *z, double **J, double *d, int iq, int n) { + int i, j; + + /* setting of z = H * d */ + for (i = 0; i < n; i++) { + z[i] = 0.0; + for (j = iq; j < n; j++) + z[i] += J[i][j] * d[j]; + } +} + +INLINE static void update_r(double **R, double *r, double *d, int iq, int n) { + int i, j; + double sum; + + /* setting of r = R^-1 d */ + for (i = iq - 1; i >= 0; i--) { + sum = 0.0; + for (j = i + 1; j < iq; j++) + sum += R[i][j] * r[j]; + r[i] = (d[i] - sum) / R[i][i]; + } +} + +static int add_constraint(double **R, double **J, double *d, int *piq, double *prnorm, int n) { + int iq = *piq; + int i, j, k; + double cc, ss, h, t1, t2, xny; + +#ifdef TRACE_SOLVER + printf("Add constraint %d",iq); +#endif + + /* we have to find the Givens rotation which will reduce the element */ + /* d[j] to zero. if it is already zero we don't have to do anything, except */ + /* of decreasing j */ + for (j = n - 1; j >= iq + 1; j--) { + /* The Givens rotation is done with the matrix (cc cs, cs -cc). */ + /* If cc is one, then element (j) of d is zero compared with element */ + /* (j - 1). Hence we don't have to do anything. */ + /* If cc is zero, then we just have to switch column (j) and column (j - 1) */ + /* of J. Since we only switch columns in J, we have to be careful how we */ + /* update d depending on the sign of gs. */ + /* Otherwise we have to apply the Givens rotation to these columns. */ + /* The i - 1 element of d has to be updated to h. */ + cc = d[j - 1]; + ss = d[j]; + h = distance(cc, ss); + if (fabs(h) < DBL_EPSILON) /* h == 0 */ + continue; + d[j] = 0.0; + ss = ss / h; + cc = cc / h; + if (cc < 0.0) { + cc = -cc; + ss = -ss; + d[j - 1] = -h; + } else + d[j - 1] = h; + xny = ss / (1.0 + cc); + for (k = 0; k < n; k++) { + t1 = J[k][j - 1]; + t2 = J[k][j]; + J[k][j - 1] = t1 * cc + t2 * ss; + J[k][j] = xny * (t1 + J[k][j - 1]) - t2; + } + } + /* update the number of constraints added*/ + *piq = ++iq; + /* To update R we have to put the iq components of the d vector into column iq - 1 of R */ + for (i = 0; i < iq; i++) + R[i][iq - 1] = d[i]; +#ifdef TRACE_SOLVER + printf("/%d\n", iq); + print_matrix("R", R, iq, iq); + print_matrix("J", J, n, n); + print_vector("d", d, iq); +#endif + + if (fabs(d[iq - 1]) <= DBL_EPSILON * *prnorm) { + /* problem degenerate */ + return 0; + } + *prnorm = (double)FMAX(*prnorm, fabs(d[iq - 1])); + + return 1; +} + +static void delete_constraint(double **R, double **J, int *A, double *u, + int n, int p, int *piq, int l) { + int iq = *piq; + int i, j, k, qq = -1; // just to prevent warnings from smart compilers + double cc, ss, h, xny, t1, t2; + +#ifdef TRACE_SOLVER + printf("Delete constraint %d %d",l,iq); +#endif + + /* Find the index qq for active constraint l to be removed */ + for (i = p; i < iq; i++) { + if (A[i] == l) { + qq = i; + break; + } + } + + /* remove the constraint from the active set and the duals */ + for (i = qq; i < iq - 1; i++) { + A[i] = A[i + 1]; + u[i] = u[i + 1]; + for (j = 0; j < n; j++) + R[j][i] = R[j][i + 1]; + } + + A[iq - 1] = A[iq]; + u[iq - 1] = u[iq]; + A[iq] = 0; + u[iq] = 0.0; + for (j = 0; j < iq; j++) + R[j][iq - 1] = 0.0; + + /* constraint has been fully removed */ + *piq = --iq; + +#ifdef TRACE_SOLVER + printf("/%d\n",iq); +#endif + + if (iq == 0) + return; + + for (j = qq; j < iq; j++) { + cc = R[j][j]; + ss = R[j + 1][j]; + h = distance(cc, ss); + if (fabs(h) < DBL_EPSILON) // h == 0 + continue; + cc = cc / h; + ss = ss / h; + R[j + 1][j] = 0.0; + if (cc < 0.0) { + R[j][j] = -h; + cc = -cc; + ss = -ss; + } else + R[j][j] = h; + + xny = ss / (1.0 + cc); + for (k = j + 1; k < iq; k++) { + t1 = R[j][k]; + t2 = R[j + 1][k]; + R[j][k] = t1 * cc + t2 * ss; + R[j + 1][k] = xny * (t1 + R[j][k]) - t2; + } + for (k = 0; k < n; k++) { + t1 = J[k][j]; + t2 = J[k][j + 1]; + J[k][j] = t1 * cc + t2 * ss; + J[k][j + 1] = xny * (J[k][j] + t1) - t2; + } + } +} + +INLINE static double distance(double a, double b) { + double a1, b1, t; + a1 = fabs(a); + b1 = fabs(b); + if (a1 > b1) { + t = (b1 / a1); + return a1 * sqrt(1.0 + t * t); + } else if (b1 > a1) { + t = (a1 / b1); + return b1 * sqrt(1.0 + t * t); + } + return a1 * sqrt(2.0); +} + + +INLINE static double scalar_product(double *x, double *y, int n) { + int i; + double sum; + + sum = 0.0; + for (i = 0; i < n; i++) + sum += x[i] * y[i]; + return sum; +} + +// !!! this doesn't work for semi-definite matricies, ie. +// they will jave diagonal sum == 0.0 !!! +static void cholesky_decomposition(double **A, int n) { + int i, j, k; + double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = A[i][j]; + for (k = i - 1; k >= 0; k--) + sum -= A[i][k] * A[j][k]; +#ifdef TRACE_SOLVER + printf("sum[%d][%d] = %f\n",i,j,sum); +#endif + if (i == j) { + if (sum <= 0.0) + { + // raise error + print_matrix("A", A, n, n); + error("QuadProg:cholesky decomposition, matrix is not postive definite, sum: %e",sum); + } + A[i][i] = sqrt(sum); + } else + A[j][i] = sum / A[i][i]; + } + for (k = i + 1; k < n; k++) + A[i][k] = A[k][i]; + } +} + +static void cholesky_solve(double **L, double *x, double *b, int n) { + double *y, _y[VSZ]; + + if (n <= VSZ) + y = _y; + else + y = dvector(0, n-1); + + /* Solve L * y = b */ + forward_elimination(L, y, b, n); + + /* Solve L^T * x = y */ + backward_elimination(L, x, y, n); + + if (n > VSZ) + free_dvector(y, 0, n-1); +} + +INLINE static void forward_elimination(double **L, double *y, double *b, int n) { + int i, j; + + y[0] = b[0] / L[0][0]; + for (i = 1; i < n; i++) { + y[i] = b[i]; + for (j = 0; j < i; j++) + y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +INLINE static void backward_elimination(double **U, double *x, double *y, int n) { + int i, j; + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) { + x[i] = y[i]; + for (j = i + 1; j < n; j++) + x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +/* --------------------------------------------------- */ + +static void print_matrix(char *name, double **A, int n, int m) { + int i, j; + + printf("%s: \n",name); + for (i = 0; i < n; i++) { + printf(" "); + for (j = 0; j < m; j++) + printf("%f%s", A[i][j], j < (m-1) ? ", " : ""); + printf("\n"); + } + printf("\n"); +} + +static void print_vector(char *name, double *v, int n) { + int i; + + printf("%s: \n",name); + printf(" "); + for (i = 0; i < n; i++) + printf("%f%s", v[i], i < (n-1) ? ", " : ""); + printf("\n\n"); +} + +static void print_ivector(char *name, int *v, int n) { + int i; + + printf("%s: \n",name); + printf(" "); + for (i = 0; i < n; i++) + printf("%d%s", v[i], i < (n-1) ? ", " : ""); + printf("\n\n"); +} + diff --git a/numlib/quadprog.h b/numlib/quadprog.h new file mode 100755 index 0000000..7d28ec8 --- /dev/null +++ b/numlib/quadprog.h @@ -0,0 +1,67 @@ + +#ifndef QUADPROG_H +#define QUADPROG_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* + Quadradic Programming solution. + + Based on Luca Di Gaspero's QuadProgpp++, made available under the MIT license. + + Translation to C by Graeme W. Gill, Copyright 2017, also licensed under the + MIT license. +*/ + +/* + + The quadprog_solve() function implements the algorithm of Goldfarb and Idnani + for the solution of a (convex) Quadratic Programming problem + by means of an active-set dual method. + +The problem is in the form: + +min 0.5 * x G x + g0 x +s.t. + CE^T x + ce0 = 0 + CI^T x + ci0 >= 0 + + The matrix and vectors dimensions are as follows: + G: n * n + g0: n + + CE: n * p + ce0: p + + CI: n * m + ci0: m + + x: n + + References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving + strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. + +*/ + +#define QP_INFEASIBLE 1.0E300 + +double quadprog( /* Return solution cost, QP_INFEASIBLE if infeasible/error */ + double *x, /* Return x[n] value */ + double **G, /* G[n][n] Quadratic combination matrix - modified */ + double *g0, /* g0[n] Direct vector */ + double **CE, /* CE[n][p] Equality constraint matrix */ + double *ce0, /* ce0[p] Equality constraing constants */ + double **CI, /* CI[n][m] Constraint matrix */ + double *ci0, /* cie[m] Constraint constants */ + int n, /* Number of variables */ + int p, /* Number of equalities */ + int m /* Number of constraints */ +); + +#ifdef __cplusplus + } +#endif + +#endif /*define QUADPROG_H */ diff --git a/numlib/rand.c b/numlib/rand.c index 436de52..9e58de3 100644..100755 --- a/numlib/rand.c +++ b/numlib/rand.c @@ -19,84 +19,128 @@ /* (From Knuth & H.W.Lewis) */ #define PSRAND32L(S) ((S) * 1664525L + 1013904223L) +/* - - - - - - - - - - - - - - - */ +/* Global state random generator */ + +static rand_state g_rand = { 0 }; /* Use default seed for global state generator */ + /* Return a 32 bit number between 0 and 4294967295 */ /* Use Knuth shuffle to improve PSRAND32 sequence */ unsigned int rand32( /* Return 32 bit random number */ unsigned int seed /* Optional seed. Non-zero re-initialized with that seed */ ) { -#define TSIZE 2843 /* Prime */ - static unsigned int last, ran = 0x12345678; /* Default seed */ - static unsigned int pvs[TSIZE]; - static int pvs_inited = 0; - int i; - - if (seed != 0) - { - pvs_inited = 0; - ran = seed;; - } - - /* Init random storage locations */ - if (pvs_inited == 0) - { - for (i = 0; i < TSIZE; i++) - pvs[i] = ran = PSRAND32(ran); - last = ran; - pvs_inited = 1; - } - i = last % TSIZE; /* New location */ - last = pvs[i]; /* Value generated */ - pvs[i] = ran = PSRAND32(ran); /* New value */ - - return last-1; + return rand32_th(NULL, seed); } /* return a random number between 0.0 and 1.0 */ /* based on rand32 */ double ranno(void) { - return rand32(0) / 4294967295.0; + return rand32_th(NULL, 0) / 4294967295.0; } /* Return a random double in the range min to max */ double d_rand(double min, double max) { - double tt; - tt = ranno(); - return min + (max - min) * tt; + return d_rand_th(NULL, min, max); } /* Return a random integer in the range min to max inclusive */ int i_rand(int min, int max) { - double tt; - tt = ranno(); - return min + (int)floor(0.5 + ((double)(max - min)) * tt); + return i_rand_th(NULL, min, max); } - /* Return a random floating point number with a gausian/normal */ /* distribution, centered about 0.0, with standard deviation 1.0 */ /* This uses the Box-Muller transformation */ double norm_rand(void) { - static int r2 = 0; /* Can use 2nd number generated */ - static double nr2; + return norm_rand_th(NULL); +} + +/* - - - - - - - - - - - - - - - */ +/* Explicit state random generator */ + +/* Init rand_state to default */ +void rand_init(rand_state *p) { + if (p == NULL) + p = &g_rand; + p->pvs_inited = 0; + p->ran = 0; +} + +/* Return a 32 bit number between 0 and 4294967295 */ +/* Use Knuth shuffle to improve PSRAND32 sequence */ +unsigned int +rand32_th(rand_state *p, +unsigned int seed /* Optional seed. Non-zero re-initialized with that seed */ +) { + int i; + + if (p == NULL) + p = &g_rand; + + if (seed != 0) { + p->pvs_inited = 0; + p->ran = seed; + } + + /* Init random storage locations */ + if (p->pvs_inited == 0) { + if (p->ran == 0) + p->ran = RAND_SEED; + for (i = 0; i < RAND_TSIZE; i++) + p->pvs[i] = p->ran = PSRAND32(p->ran); + p->last = p->ran; + p->pvs_inited = 1; + } + i = p->last % RAND_TSIZE; /* New location */ + p->last = p->pvs[i]; /* Value generated */ + p->pvs[i] = p->ran = PSRAND32(p->ran); /* New value */ + + return p->last-1; +} + +/* return a random number between 0.0 and 1.0 */ +/* based on rand32 */ +double ranno_th(rand_state *p) { + return rand32_th(NULL, 0) / 4294967295.0; +} + +/* Return a random double in the range min to max */ +double +d_rand_th(rand_state *p, double min, double max) { + return min + (max - min) * ranno_th(p); +} + +/* Return a random integer in the range min to max inclusive */ +int +i_rand_th(rand_state *p, int min, int max) { + return min + (int)floor(0.5 + ((double)(max - min)) * ranno_th(p)); +} + +/* Return a random floating point number with a gausian/normal */ +/* distribution, centered about 0.0, with standard deviation 1.0 */ +/* This uses the Box-Muller transformation */ +double norm_rand_th(rand_state *p) { + if (p == NULL) + p = &g_rand; - if (r2 == 0) { + if (p->r2 == 0) { double v1, v2, t1, t2, r1; do { - v1 = d_rand(-1.0, 1.0); - v2 = d_rand(-1.0, 1.0); + v1 = d_rand_th(p, -1.0, 1.0); + v2 = d_rand_th(p, -1.0, 1.0); t1 = v1 * v1 + v2 * v2; } while (t1 == 0.0 || t1 >= 1.0); t2 = sqrt(-2.0 * log(t1)/t1); - nr2 = v2 * t2; /* One for next time */ - r2 = 1; + p->nr2 = v2 * t2; /* One for next time */ + p->r2 = 1; r1 = v1 * t2; return r1; } else { - r2 = 0; - return nr2; + p->r2 = 0; + return p->nr2; } } diff --git a/numlib/rand.h b/numlib/rand.h index 46f79f2..e190fcd 100644..100755 --- a/numlib/rand.h +++ b/numlib/rand.h @@ -13,6 +13,9 @@ extern "C" { #endif +/* - - - - - - - - - - - - - - - */ +/* Global state random generator */ + /* Return a random number between 0 and 4294967294 */ unsigned int rand32( /* Return 32 bit random number */ @@ -29,6 +32,43 @@ double d_rand(double min, double max); /* and an average deviation of 0.564 */ double norm_rand(void); +/* - - - - - - - - - - - - - - - */ +/* Explicit state random generator */ +/* Use NULL for global state */ + +#define RAND_TSIZE 2843 /* Prime */ +#define RAND_SEED 0x12345678 /* Default seed */ + +/* Should set to 0 to default intialize */ +typedef struct { + int pvs_inited; + unsigned int ran, last; + unsigned int pvs[RAND_TSIZE]; + + /* normal distribution 2nd value */ + int r2; /* 2nd value available */ + double nr2; /* 2nd value */ +} rand_state; + +/* Init rand_state to default */ +void rand_init(rand_state *p); + +/* Return a random number between 0 and 4294967294 */ +unsigned int +rand32_th(rand_state *p, +unsigned int seed); /* Optional seed. Non-zero re-initialized with that seed */ + +/* Return a random integer in the range min to max inclusive */ +int i_rand_th(rand_state *p, int min, int max); + +/* Return a random double in the range min to max inclusive */ +double d_rand_th(rand_state *p, double min, double max); + +/* Return a random floating point number with a gausian/normal */ +/* distribution, centered about 0.0, with standard deviation 1.0 */ +/* and an average deviation of 0.564 */ +double norm_rand_th(rand_state *p); + #ifdef __cplusplus } #endif diff --git a/numlib/sobol.c b/numlib/sobol.c index 40a2c38..40a2c38 100644..100755 --- a/numlib/sobol.c +++ b/numlib/sobol.c diff --git a/numlib/sobol.h b/numlib/sobol.h index 08ac5c0..08ac5c0 100644..100755 --- a/numlib/sobol.h +++ b/numlib/sobol.h diff --git a/numlib/soboltest.c b/numlib/soboltest.c index 32c26ee..32c26ee 100644..100755 --- a/numlib/soboltest.c +++ b/numlib/soboltest.c diff --git a/numlib/svd.c b/numlib/svd.c index fdb8edd..fdb8edd 100644..100755 --- a/numlib/svd.c +++ b/numlib/svd.c diff --git a/numlib/svd.h b/numlib/svd.h index 59b080d..6a89210 100644..100755 --- a/numlib/svd.h +++ b/numlib/svd.h @@ -13,13 +13,44 @@ extern "C" { #endif +/* + + U[] decomposes A[]'s columns into orthogonal, singular vectors. + U[]'s columns are vectors that sum to 1.0, i.e. they leave a vectors normal unchanged. + The inverse of U[] is its transpose. + U[]'s columns corresponding to non-zero W[] are the orthonormal vectors that span + the (input) RANGE space. Columns corresponding to zero W[] are zero. + + W[] will return singlular values, i.e. the weighting of the singular vectors. + It's inverse is is the reciprical of its elements. + + V[] composes the singular vectors back into A[]'s rows. + V[]'s columns and rows are orthonormal. + V[]'s columns corresponding to non-zero W[] are the orthonormal vectors that span + the (output) RANGE space. + v[]'s columns corresponding to zero W[] are the (output) othonormal vectors that span + the NULL space. + The inverse of V[] is its transpose. + + To re-form, A = U.W.Vt, i.e. multiply by transpose of V. + + i.e. mult. input vector[m] by U[] converts to [n] compact, orthogonal + basis vectors. W then scales them appropiately, setting null space + vectors to 0. V[] then transforms from the orthogonal basis vectors + to A[]'s output space. + + To reveal NULL space, make sure n >= m, since U[] vectors corrsponding + to zero's are set to zero. + +*/ + /* Compute Singular Value Decomposition of A = U.W.Vt */ /* Return status value: */ /* 0 - no error */ /* 1 - m < n error */ int svdecomp( -double **a, /* A[0..m-1][0..n-1], return U[][] */ -double *w, /* return W[0..n-1] */ +double **a, /* A[0..m-1][0..n-1], return U[0..m-1][0..n-1] */ +double *w, /* return W[0..n-1] = singular values */ double **v, /* return V[0..n-1][0..n-1] (not transpose!) */ int m, /* Number of equations */ int n /* Number of unknowns */ diff --git a/numlib/svdtest.c b/numlib/svdtest.c index 100120d..100120d 100644..100755 --- a/numlib/svdtest.c +++ b/numlib/svdtest.c diff --git a/numlib/tdhsx.c b/numlib/tdhsx.c index d1738c4..d1738c4 100644..100755 --- a/numlib/tdhsx.c +++ b/numlib/tdhsx.c diff --git a/numlib/tpowell.c b/numlib/tpowell.c index f7b0aad..f7b0aad 100644..100755 --- a/numlib/tpowell.c +++ b/numlib/tpowell.c diff --git a/numlib/ui.c b/numlib/ui.c index 390a332..390a332 100644..100755 --- a/numlib/ui.c +++ b/numlib/ui.c diff --git a/numlib/ui.h b/numlib/ui.h index a282c26..a282c26 100644..100755 --- a/numlib/ui.h +++ b/numlib/ui.h 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); + } +} diff --git a/numlib/varmet.h b/numlib/varmet.h new file mode 100755 index 0000000..5f24445 --- /dev/null +++ b/numlib/varmet.h @@ -0,0 +1,55 @@ +#ifndef VARMET_H +#define VARMET_H + +/* Variable Metric multivariate minimiser */ + +/* + * Copyright 2000, 2007 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. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* Variable Metric Gradient optimiser */ +/* return 0 on sucess, 1 on failure due to excessive itterations */ +/* Result will be in cp */ +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 */ +); + +/* Example user function declarations */ +double varmet_funk( /* Return function value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ + +/* Line in multi-dimensional space minimiser */ +double brentnd( /* vector multiplier return value */ +double ax, /* Minimum of multiplier range */ +double bx, /* Starting point multiplier of search */ +double cx, /* Maximum of multiplier range */ +double ftol, /* Tollerance to stop search */ +double *xmin, /* Return value of multiplier at minimum */ +int n, /* Dimensionality */ +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata, /* Opaque data */ +double pcom[], /* Base vector point */ +double xicom[]); /* Vector that will be multiplied and added to pcom[] */ + +#ifdef __cplusplus + } +#endif + +#endif /* VARMET_H */ diff --git a/numlib/zbrent.c b/numlib/zbrent.c index a811818..a811818 100644..100755 --- a/numlib/zbrent.c +++ b/numlib/zbrent.c diff --git a/numlib/zbrent.h b/numlib/zbrent.h index 2f8aac4..2f8aac4 100644..100755 --- a/numlib/zbrent.h +++ b/numlib/zbrent.h diff --git a/numlib/zbrenttest.c b/numlib/zbrenttest.c index d7b44d3..d7b44d3 100644..100755 --- a/numlib/zbrenttest.c +++ b/numlib/zbrenttest.c |