summaryrefslogtreecommitdiff
path: root/numlib
diff options
context:
space:
mode:
authorJörg Frings-Fürst <debian@jff-webhosting.net>2017-12-03 20:38:41 +0100
committerJörg Frings-Fürst <debian@jff-webhosting.net>2017-12-03 20:38:41 +0100
commitba627dd9ecb578e9852c7b9cce67ec63199d1acf (patch)
tree27c4258311ca8c8ed7ff67a8a0bc7280e8fcae79 /numlib
parent69aec3b712232e93600ecd741269fed1f90b412a (diff)
parent3abb40d43649adb3807180692d8579c405524675 (diff)
Merge branch 'release/2.0.0+repack-1'2.0.0+repack-1
Diffstat (limited to 'numlib')
-rwxr-xr-x[-rw-r--r--]numlib/Jamfile4
-rwxr-xr-x[-rw-r--r--]numlib/LUtest.c0
-rwxr-xr-x[-rw-r--r--]numlib/License.txt0
-rwxr-xr-x[-rw-r--r--]numlib/Readme.txt0
-rwxr-xr-x[-rw-r--r--]numlib/aatree.c0
-rwxr-xr-x[-rw-r--r--]numlib/aatree.h3
-rwxr-xr-x[-rw-r--r--]numlib/afiles5
-rwxr-xr-x[-rw-r--r--]numlib/dhsx.c306
-rwxr-xr-x[-rw-r--r--]numlib/dhsx.h28
-rwxr-xr-x[-rw-r--r--]numlib/dnsq.c0
-rwxr-xr-x[-rw-r--r--]numlib/dnsq.h0
-rwxr-xr-x[-rw-r--r--]numlib/dnsqtest.c0
-rwxr-xr-x[-rw-r--r--]numlib/ludecomp.c16
-rwxr-xr-x[-rw-r--r--]numlib/ludecomp.h14
-rwxr-xr-x[-rw-r--r--]numlib/numlib.h2
-rwxr-xr-x[-rw-r--r--]numlib/numsup.c327
-rwxr-xr-x[-rw-r--r--]numlib/numsup.h88
-rwxr-xr-x[-rw-r--r--]numlib/powell.c121
-rwxr-xr-x[-rw-r--r--]numlib/powell.h24
-rwxr-xr-xnumlib/qptest.c105
-rwxr-xr-xnumlib/quadprog.c845
-rwxr-xr-xnumlib/quadprog.h67
-rwxr-xr-x[-rw-r--r--]numlib/rand.c128
-rwxr-xr-x[-rw-r--r--]numlib/rand.h40
-rwxr-xr-x[-rw-r--r--]numlib/sobol.c0
-rwxr-xr-x[-rw-r--r--]numlib/sobol.h0
-rwxr-xr-x[-rw-r--r--]numlib/soboltest.c0
-rwxr-xr-x[-rw-r--r--]numlib/svd.c0
-rwxr-xr-x[-rw-r--r--]numlib/svd.h35
-rwxr-xr-x[-rw-r--r--]numlib/svdtest.c0
-rwxr-xr-x[-rw-r--r--]numlib/tdhsx.c0
-rwxr-xr-x[-rw-r--r--]numlib/tpowell.c0
-rwxr-xr-x[-rw-r--r--]numlib/ui.c0
-rwxr-xr-x[-rw-r--r--]numlib/ui.h0
-rwxr-xr-xnumlib/varmet.c290
-rwxr-xr-xnumlib/varmet.h55
-rwxr-xr-x[-rw-r--r--]numlib/zbrent.c0
-rwxr-xr-x[-rw-r--r--]numlib/zbrent.h0
-rwxr-xr-x[-rw-r--r--]numlib/zbrenttest.c0
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