summaryrefslogtreecommitdiff
path: root/numlib
diff options
context:
space:
mode:
authorJörg Frings-Fürst <debian@jff-webhosting.net>2018-07-11 22:20:14 +0200
committerJörg Frings-Fürst <debian@jff-webhosting.net>2018-07-11 22:20:14 +0200
commit7beb00cd8d28c3d5893ce3db907a828d64afdea9 (patch)
tree395a3dee2fe197b8284dee02c5f527889df78413 /numlib
parente2d30e0583c047a4bedf4c8d0c86320f1b3fd8ed (diff)
parenta0442ed58dee48a521ea053083ea967894507898 (diff)
Update upstream source from tag 'upstream/2.0.1+repack'
Update to upstream version '2.0.1+repack' with Debian dir 6edb5dd2df9aca152662fc4a8f72bd6d86f8552e
Diffstat (limited to 'numlib')
-rwxr-xr-xnumlib/Jamfile4
-rwxr-xr-xnumlib/afiles5
-rwxr-xr-xnumlib/dhsx.c11
-rwxr-xr-xnumlib/dhsx.h6
-rwxr-xr-xnumlib/dnsq.c1
-rwxr-xr-xnumlib/dnsq.h3
-rwxr-xr-xnumlib/dnsqtest.c2
-rwxr-xr-xnumlib/gnewt.c413
-rwxr-xr-xnumlib/gnewt.h59
-rwxr-xr-xnumlib/ludecomp.c73
-rwxr-xr-xnumlib/ludecomp.h26
-rwxr-xr-xnumlib/numlib.h2
-rwxr-xr-xnumlib/numsup.c200
-rwxr-xr-xnumlib/numsup.h45
-rwxr-xr-xnumlib/powell.c712
-rwxr-xr-xnumlib/powell.h23
-rwxr-xr-xnumlib/quadprog.c35
-rwxr-xr-xnumlib/quadprog.h18
-rwxr-xr-xnumlib/rand.c21
-rwxr-xr-xnumlib/rand.h10
-rwxr-xr-xnumlib/roots.c217
-rwxr-xr-xnumlib/roots.h23
-rwxr-xr-xnumlib/tconjgrad.c162
23 files changed, 1862 insertions, 209 deletions
diff --git a/numlib/Jamfile b/numlib/Jamfile
index 2d5edaf..e2a06fd 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 varmet.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c quadprog.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 gnewt.c roots.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 qptest.c ;
+MainsFromSources dnsqtest.c tpowell.c tconjgrad.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c qptest.c ;
# Compile .c as .m
if $(OS) = MACOSX {
diff --git a/numlib/afiles b/numlib/afiles
index b2138b2..e427b7d 100755
--- a/numlib/afiles
+++ b/numlib/afiles
@@ -4,6 +4,8 @@ afiles
Jamfile
dnsq.c
dnsq.h
+gnewt.c
+gnewt.h
dnsqtest.c
numlib.h
numsup.c
@@ -11,6 +13,7 @@ numsup.h
powell.h
powell.c
tpowell.c
+tconjgrad.c
varmet.h
varmet.c
dhsx.h
@@ -35,5 +38,7 @@ aatree.c
quadprog.h
quadprog.c
qptest.c
+roots.h
+roots.c
ui.h
ui.c
diff --git a/numlib/dhsx.c b/numlib/dhsx.c
index e9da0eb..aa9f462 100755
--- a/numlib/dhsx.c
+++ b/numlib/dhsx.c
@@ -20,7 +20,7 @@
#undef DEBUG
-int dhsx_debug = 1;
+int dhsx_debug = 0;
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,
@@ -92,9 +92,15 @@ void *fdata /* Data needed by function */
lox = i;
}
tryy = (*funk)(fdata, cp); /* Value at initial point */
+#ifdef DEBUG
+ if (dhsx_debug) printf(" initial point %s = %e\n",debPdv(di,cp),tryy);
+#endif /* DEBUG */
/* If our initial point is better than any of the simplex verticies */
if (y[lox] > tryy) {
+#ifdef DEBUG
+ if (dhsx_debug) printf(" initial point is better than surrounding simplex\n");
+#endif /* DEBUG */
/* Move all the verticies to match moving lox to cp */
for (i = 0; i < nsp; i++) {
if (i == lox)
@@ -263,6 +269,9 @@ void *fdata /* Data needed by function */
for (j = 0; j < di; j++)
cp[j] = p[lox][j];
}
+#ifdef DEBUG
+ else if (dhsx_debug) printf("C point val %f is best\n",tryy);
+#endif
free_dvector(y2, 0, nsp-1);
free_dmatrix(p2, 0, nsp-1, 0, di-1);
free_dvector(y, 0, nsp-1);
diff --git a/numlib/dhsx.h b/numlib/dhsx.h
index 78cdb10..8a4e1ff 100755
--- a/numlib/dhsx.h
+++ b/numlib/dhsx.h
@@ -14,9 +14,9 @@
/* return 0 on sucess, 1 on failure due to excessive itterations */
/* Result will be in cp */
int dhsx(
- double *rv, /* If not NULL, return the residual error */
- int di, /* Dimentionality */
- double *cp, /* Initial starting point, return minimum */
+ 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) */
diff --git a/numlib/dnsq.c b/numlib/dnsq.c
index 75f00a8..31bebe4 100755
--- a/numlib/dnsq.c
+++ b/numlib/dnsq.c
@@ -112,6 +112,7 @@ int dnsqe(
info = 4;
if (info == 0)
warning("dnsqe: invalid input parameter.");
+
return info;
} /* dnsqe */
diff --git a/numlib/dnsq.h b/numlib/dnsq.h
index e06e562..28dd5de 100755
--- a/numlib/dnsq.h
+++ b/numlib/dnsq.h
@@ -84,7 +84,8 @@ int dnsq_jac( /* Return < 0 on abort */
);
#define M_LARGE 1.79e+308
-#define M_DIVER 2.22e-15
+#define M_DIVER 2.22e-15 /* Machine precision (10 x IEEE double) */
+#define M_SQRT_DIVER 4.71e-8 /* sqrt(M_DIVER) */
/* Simplified dnsq() */
int dnsqe(
diff --git a/numlib/dnsqtest.c b/numlib/dnsqtest.c
index 3f02819..f05bed4 100755
--- a/numlib/dnsqtest.c
+++ b/numlib/dnsqtest.c
@@ -74,7 +74,7 @@ int main(void)
/* Unless high precision solutions are required, */
/* this is the recommended setting. */
- tol = sqrt(M_DIVER);
+ tol = M_SQRT_DIVER;
info = dnsqe(NULL, fcn, NULL, n, x, ss, fvec, tol, tol, 0, nprint);
fnorm = denorm(n, fvec);
diff --git a/numlib/gnewt.c b/numlib/gnewt.c
new file mode 100755
index 0000000..c95cec7
--- /dev/null
+++ b/numlib/gnewt.c
@@ -0,0 +1,413 @@
+
+/*
+ * Copyright 2018 Graeme 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.
+ */
+
+#include "numsup.h"
+#include "ludecomp.h"
+#include "gnewt.h" /* Public interface definitions */
+
+#undef DEBUG
+
+#ifdef DEBUG
+# define DBG(xx) printf xx;
+#else
+# define DBG(xx)
+#endif
+
+
+#define TOLX 1.0e-7 /* Convergence criterion on delx */
+#define STPMX 100.0 /* Maximum step multiplier */
+
+static void apxjac(int n, double *x, double *fvec, double **df, void *fdata,
+ void (*fcn)(void *fdata, int n, double *x, double *fvec));
+
+static int linesearch(int n, double *xold, double fold, double *delf, double *delx,
+ double *x, double *fvec, double *fp, double maxstep, void *fdata,
+ void (*fcn)(void *fdata, int n, double *x, double *f), int *pfit, int maxjac, int it);
+
+#define FMAX(A, B) ((A) > (B) ? (A) : (B))
+
+int gnewt(
+ void *fdata, /* Opaque pointer to pass to fcn() and jac() */
+ void (*fcn)(void *fdata, int n, double *x, double *fvec),
+ /* Pointer to function we are solving */
+ void (*jac)(void *fdata, int n, double *x, double **fjac),
+ /* Function to compute jacobian */
+ int n, /* Number of functions and variables */
+ double x[], /* Initial solution estimate, returns final solution */
+ double rfvec[], /* Optionaly return soln. function values */
+ double xtol, /* Desired tollerance of root */
+ double ftol, /* Desired tollerance of the solution */
+ int maxfcn, /* Maximum number of function itterations */
+ int maxjac /* Maximum number of jacobian itterations */
+) {
+ int i, j, it, fit, jit, *pivx, _pivx[10];
+ double f, fold; /* half magnitide squared of fvec[] */
+ double *delf, _delf[10]; /* del f where f = 0.5 F.F */
+ double *fvec, _fvec[10]; /* F(x) */
+ double **fjac, *_fjac[11], __fjac[10 * 10];
+ double *xold, _xold[10];
+ double bigfx, bigx, maxstep;
+ double *delx, _delx[10]; /* Full step delta x */
+ double sum;
+ int rv = 0;
+
+#ifdef DEBUG
+ double *fvec_check;
+ double **fjac_check;
+#endif
+
+ DBG(("gnewt:\n"))
+
+ fit = jit = 0;
+
+ /* Do local vector/array allocations */
+ if (n <= 10) {
+ pivx = _pivx;
+ if (rfvec == NULL) {
+ fvec = _fvec;
+ } else
+ fvec = rfvec;
+ _fjac[0] = __fjac;
+ fjac = _fjac+1; /* dmatrix_reset() will setup fjac */
+ xold = _xold;
+ delf = _delf;
+ delx = _delx;
+ } else {
+ pivx = ivector(0, n-1); /* LU decomp. pivod record */
+ if (rfvec == NULL) {
+ fvec = dvector(0, n-1); /* Function value */
+ } else
+ fvec = rfvec;
+ fjac = dmatrix(0, n-1, 0, n-1); /* Jacobian matrix */
+ xold = dvector(0, n-1); /* Previous value of x[] */
+ delf = dvector(0, n-1); /* del f */
+ delx = dvector(0, n-1); /* Full step delta x */
+ }
+#ifdef DEBUG
+ fvec_check = dvector(0, n-1);
+ fjac_check = dmatrix(0, n-1, 0, n-1);
+#endif
+
+ /* Initial function value */
+ fcn(fdata, n, x, fvec);
+ fit++;
+
+ DBG((" x %s\n",debPdv(n,x)))
+ DBG((" fvec %s\n",debPdv(n,fvec)))
+
+ /* Compute half magnitide squared of function value at x */
+ for (sum = 0.0, i = 0; i < n; i++)
+ sum += fvec[i] * fvec[i];
+ f = 0.5 * sum;
+ DBG((" f %f\n",f))
+
+ /* test for initial value being a root */
+ for (bigfx = 0.0, i = 0; i < n; i++) {
+ double tt = fabs(fvec[i]);
+ if (tt > bigfx)
+ bigfx = tt;
+ }
+ if (bigfx < (0.01 * ftol)) {
+ goto done;
+ }
+
+ /* Compute line search x maximum step size */
+ for (sum = 0.0, i = 0 ; i < n; i++)
+ sum += x[i] * x[i];
+ maxstep = STPMX * FMAX(sqrt(sum), (double)n);
+ DBG((" maxstep %f\n",maxstep))
+
+ /* Until we are done */
+ for (it = 0; fit < maxfcn && jit < maxjac; it++) {
+ double rip;
+
+ DBG((" fit %d jit %d\n",fit,jit))
+
+ /* Compute Jacobian matrix */
+ if (jac != NULL) {
+ /* lu_decomp may have swapped rows - so fix it */
+ dmatrix_reset(fjac, 0, n-1, 0, n-1);
+ jac(fdata, n, x, fjac); /* User function */
+ } else {
+ apxjac(n, x, fvec, fjac, fdata, fcn); /* Numerical aproximation */
+ }
+ jit++;
+
+#ifdef DEBUG
+ copy_dmatrix(fjac_check, fjac, 0, n-1, 0, n-1);
+
+ DBG((" fjac = \n"))
+ for (i = 0; i < n; i++)
+ DBG((" %d: %s\n",i,debPdv(n, fjac[i])))
+ DBG(("\n"))
+#endif
+
+ /* Compute del f for the line search. */
+ for (i = 0; i < n; i++) {
+ for (sum = 0.0, j = 0; j < n; j++)
+ sum += fjac[j][i] * fvec[j]; /* Hmm. df/dx . f */
+ delf[i] = sum;
+ }
+
+ /* Save current values of x and f to be able to monitor progres */
+ for (i = 0; i < n; i++)
+ xold[i] = x[i];
+ fold = f;
+
+ /* Desired delta f to make F(x) == 0 */
+ for (i = 0; i < n; i++)
+ delx[i] = -fvec[i];
+ DBG((" -fvec %s\n",debPdv(n,delx)))
+
+ /* Solve for delta x using Jacobian and desired delta f */
+ if (lu_decomp(fjac, n, pivx, &rip)) {
+ rv = 2;
+ goto done;
+ }
+ lu_backsub(fjac, n, pivx, delx);
+ DBG((" delx %s\n",debPdv(n,delx)))
+
+#ifdef DEBUG
+ matrix_vect_mult(fvec_check, n, fjac_check, n, n, delx, n);
+ DBG((" check -fvec : %s\n\n",debPdv(n,fvec_check)))
+#endif
+
+ if ((rv = linesearch(n, xold, fold, delf, delx, x, fvec, &f, maxstep, fdata, fcn,
+ &fit, maxfcn, it)) != 0) {
+ if (rv != 1) { /* Not run out of itterations error */
+ DBG((" linesearch failed with %d\n",rv))
+ goto done;
+ }
+ }
+
+ DBG((" after linesearch:\n"))
+ DBG((" x %s\n",debPdv(n,x)))
+ DBG((" fvec %s\n",debPdv(n,fvec)))
+
+ /* See if f() has converged */
+ for (bigfx = 0.0, i = 0; i < n; i++) {
+ if (fabs(fvec[i]) > bigfx)
+ bigfx = fabs(fvec[i]);
+ }
+ DBG((" bigfx %f ftol %f\n",bigfx,ftol))
+ if (bigfx < ftol) {
+ goto done;
+ }
+
+ /* Could check for zero gradient problem here... */
+
+ /* See if x[] has converged */
+ for (bigx = 0.0, i = 0; i < n; i++) {
+ double tt = (fabs(x[i] - xold[i]))/FMAX(fabs(x[i]), 1.0);
+ if (tt > bigx)
+ bigx = tt;
+ }
+ DBG((" bigx %f xtol %f\n",bigx,xtol))
+ if (bigx < xtol)
+ goto done;
+ }
+
+ rv = 1;
+
+ done:;
+
+ if (n > 10) {
+ if (fvec != rfvec)
+ free_dvector(fvec, 0, n-1);
+ free_dvector(xold, 0, n-1);
+ free_dvector(delx, 0, n-1);
+ free_dvector(delf, 0, n-1);
+ free_dmatrix(fjac, 0, n-1, 0, n-1);
+ free_ivector(pivx, 0, n-1);
+ }
+#ifdef DEBUG
+ free_dvector(fvec_check,0, n-1);
+ free_dmatrix(fjac_check,0, n-1, 0, n-1);
+#endif
+
+ return rv;
+}
+
+/* - - - - - - - - */
+
+#define ALF 1.0e-4 /* Ensures sufficient decrease in function value. */
+
+/* Search for a step size that makes progress */
+/* Return nz on error */
+static int linesearch(
+ int n,
+ double *xold,
+ double fold,
+ double *delf, /* del f */
+ double *delx, /* full step delta x[] */
+ double *x, /* in/out current x[] */
+ double *fvec, /* return fvec at x[] */
+ double *fp, /* in/out f value */
+ double maxstep, /* maximum x step */
+ void *fdata, /* Context for fcn */
+ void (*fcn)(void *fdata, int n, double *x, double *f),
+ int *pfit, /* Inc number of itts */
+ int maxfcn, /* Max function its */
+ int it /* Caller iteration count */
+) {
+ int i;
+ double f = *fp, f2;
+ double lmda1, lmda2, min_lmda;
+ double sum, slope, bigx;
+
+ DBG(("linesearch:\n"))
+
+ /* Comute magnitude of step */
+ for (sum = 0.0, i = 0; i < n; i++)
+ sum += delx[i] * delx[i];
+ sum = sqrt(sum);
+
+ /* re-scale if step is too big */
+ if (sum > maxstep) {
+ for (i = 0; i < n; i++)
+ delx[i] *= maxstep/sum;
+ }
+
+ for (slope = 0.0, i = 0; i < n; i++)
+ slope += delf[i] * delx[i];
+ if (slope >= 0.0) {
+ DBG((" slope %f >= 0.0\n",slope))
+ return 3;
+ }
+
+ bigx = 0.0;
+ for (i = 0;i < n; i++) {
+ double tt = fabs(delx[i])/FMAX(fabs(xold[i]), 1.0);
+ if (tt > bigx)
+ bigx = tt;
+ }
+ min_lmda = TOLX/bigx;
+
+ /* Try full Newton step first */
+ lmda1 = 1.0;
+
+ DBG((" lmda1 %f min_lmda %f\n",lmda1, min_lmda))
+
+ /* Top of loop */
+ for (; *pfit < maxfcn; it++) {
+ double tmp_lmda;
+
+ DBG((" lmda1 %f\n",lmda1))
+
+ /* Take step */
+ for (i = 0;i < n;i++)
+ x[i] = xold[i] + lmda1 * delx[i];
+
+ /* Compute f = 0.5 F.F at x */
+ fcn(fdata, n, x, fvec);
+ (*pfit)++;
+ DBG((" x %s\n",debPdv(n,x)))
+ DBG((" fvec %s\n",debPdv(n,fvec)))
+ for (sum = 0.0, i = 0; i < n; i++)
+ sum += fvec[i] * fvec[i];
+ f = 0.5 * sum;
+
+//if (it == 0) printf(" linesearch: At 1st full step f %f -> %f\n", *fp, f);
+
+ /* Convergence on delx. */
+ if (lmda1 < min_lmda) {
+
+ for (i = 0; i < n; i++)
+ x[i] = xold[i];
+ return 0;
+
+ } else if (f <= fold + ALF * lmda1 * slope) {
+ *fp = f;
+ return 0; /* Sufficient function decrease */
+
+ } else { /* Backtrack. */
+ if (lmda1 == 1.0) /* First time */
+ tmp_lmda = -slope/(2.0 * (f - fold-slope));
+
+ else { /* Subsequent backtracks */
+ double c, d, e;
+ double a, b, rhs1, rhs2;
+
+ rhs1 = f - fold - slope * lmda1;
+ rhs2 = f2 - fold - slope * lmda2;
+ c = rhs1/(lmda1 * lmda1);
+ d = rhs2/(lmda2 * lmda2);
+ e = lmda1 - lmda2;
+ a = (c - d)/e;
+ b = (-lmda2 * c + lmda1 * d)/e;
+ if (a == 0.0)
+ tmp_lmda = -slope/(2.0 * b);
+ else {
+ double disc = b * b - 3.0 * a * slope;
+
+ if (disc < 0.0)
+ tmp_lmda = 0.5 * lmda1;
+ else if (b <= 0.0)
+ tmp_lmda = (-b + sqrt(disc))/(3.0 * a);
+ else
+ tmp_lmda = -slope/(b + sqrt(disc));
+ }
+ if (tmp_lmda > 0.5 * lmda1)
+ tmp_lmda = 0.5 * lmda1;
+ }
+ }
+ lmda2 = lmda1;
+ lmda1 = FMAX(tmp_lmda, lmda1 * 0.1);
+ f2 = f;
+ }
+
+ *fp = f;
+
+ return 1;
+}
+
+/* - - - - - - - - */
+
+/* Compute forward difference as aprox. Jacobian matrix */
+
+#define JEPS 1.0e-8 /* Aprox. sqrt of machine precision */
+
+static void apxjac(
+ int n, /* Dimensions */
+ double *x, /* Location x to compute Jacobian */
+ double *fvec, /* Function value at x */
+ double **df, /* Return Jacobian */
+ void *fdata, /* fcn() context */
+ void (*fcn)(void *fdata, int n, double *x, double *fvec)
+) {
+ int i, j;
+ double h, temp, *f, _f[10];
+
+ if (n <= 10)
+ f = _f;
+ else
+ f = dvector(0, n);
+
+ for (j = 0; j < n; j++) {
+ temp = x[j];
+
+ h = JEPS * fabs(temp);
+ if (h == 0.0)
+ h = JEPS;
+ x[j] = temp + h; /* Add delta */
+ h = x[j] - temp; /* Actual delta with fp precision limits */
+
+ fcn(fdata, n, x, f);
+
+ x[j] = temp; /* Restore value */
+
+ for (i = 0; i < n; i++)
+ df[i][j] = (f[i] - fvec[i])/h;
+ }
+
+ if (f != _f)
+ free_dvector(f, 0, n-1);
+}
+
+
diff --git a/numlib/gnewt.h b/numlib/gnewt.h
new file mode 100755
index 0000000..322d511
--- /dev/null
+++ b/numlib/gnewt.h
@@ -0,0 +1,59 @@
+#ifndef GNEWT_H
+#define GNEWT_H
+
+/* Global newton non-linear equation solver */
+
+/*
+ * Copyright 2018 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
+
+/*
+
+ return values:
+
+ 0 Success
+
+ 1 Ran out of itterations
+
+ 2 Inverting Jacobian failed
+
+ */
+
+int gnewt(
+ void *fdata, /* Opaque pointer to pass to fcn() and jac() */
+ void (*fcn)(void *fdata, int n, double *x, double *fvec),
+ /* Pointer to function we are solving */
+ void (*jac)(void *fdata, int n, double *x, double **fjac),
+ /* Function to compute jacobian */
+ int n, /* Number of functions and variables */
+ double x[], /* Initial solution estimate, returns final solution */
+ double rfvec[], /* Optionaly return soln. function values */
+ double xtol, /* Desired tollerance of root */
+ double ftol, /* Desired tollerance of the solution */
+ int maxfcn, /* Maximum number of function itterations */
+ int maxjac /* Maximum number of jacobian itterations */
+);
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif /* GNEWT_H */
+
+
+
+
+
+
+
+
+
+
diff --git a/numlib/ludecomp.c b/numlib/ludecomp.c
index b756bfe..06fc6a8 100755
--- a/numlib/ludecomp.c
+++ b/numlib/ludecomp.c
@@ -163,6 +163,7 @@ int n /* Dimensionality */
/* Decompose the square matrix A[][] into lower and upper triangles */
/* NOTE that it returns transposed inverse by normal convention. */
+/* NOTE that rows get swaped by swapping matrix pointers! */
/* Use sym_matrix_trans() to fix this. */
/* Return 1 if the matrix is singular. */
int
@@ -260,14 +261,14 @@ double *rip /* Row interchange parity, +/- 1.0, used for determinant */
return 0;
}
-/* Solve a set of simultaneous equations from the */
+/* Solve a set of simultaneous equations A.x = b from the */
/* LU decomposition, by back substitution. */
void
lu_backsub(
double **a, /* A[][] LU decomposed matrix */
int n, /* Dimensionality */
int *pivx, /* Pivoting row permutations record */
-double *b /* Input B[] vecor, return X[] */
+double *b /* Input B[] vector, return X[] */
) {
int i, j;
int nvi; /* When >= 0, indicates non-vanishing B[] index */
@@ -441,15 +442,13 @@ 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(
double **out, /* Output[0..N-1][0..M-1] */
double **in, /* Input[0..M-1][0..N-1] input matrix */
-int m, /* Rows */
-int n /* Columns */
+int m, /* In Rows */
+int n /* In Columns */
) {
int rv = 0;
double **tr; /* Transpose */
@@ -490,13 +489,73 @@ int n /* Columns */
}
free_dmatrix(sq, 0, m-1, 0, m-1);
}
-
free_dmatrix(tr, 0, n-1, 0, m-1);
return rv;
}
+/* ----------------------------------------------------------------- */
+
+// ~~~ Hmm. Need to verify this code is correct...
+
+/* Use Cholesky decomposition on a symetric positive-definite matrix. */
+/* Only the upper triangle of the matrix A is accessed. */
+/* L returns the decomposition */
+/* Return nz if A is not positive-definite */
+int llt_decomp(double **L, double **A, int n) {
+ int i, j, k;
+ double sum;
+
+ /* Scan though upper triangle */
+ 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];
+ }
+
+ if (i != j) {
+ L[j][i] = sum/L[i][i];
+ } else {
+ if (sum <= 0.0)
+ return 1;
+ L[i][i] = sqrt(sum);
+ }
+ }
+ }
+ return 0;
+}
+
+/* Solve a set of simultaneous equations A.x = b from the */
+/* LLt decomposition, by back substitution. */
+void llt_backsub(
+double **L, /* A[][] LLt decomposition in lower triangle */
+int n, /* Dimensionality */
+double *b, /* Input B[] */
+double *x /* Return X[] (may be same as B[]) */
+) {
+ int i, k;
+ double sum;
+
+ /* Solve L.y = b, storing y in x. */
+ for (i = 0; i < n; i++) {
+ sum = b[i];
+ for (k = i-1; k >= 0; k--)
+ sum -= L[i][k] * x[k];
+ x[i] = sum/L[i][i];
+ }
+
+ /* Solve Lt.x = y */
+ for (i = n; i >= 0; i--) {
+ sum = x[i];
+ for (k = i+1 ; k < n; k++)
+ sum -= L[k][i] * x[k];
+ x[i] = sum/L[i][i];
+ }
+}
diff --git a/numlib/ludecomp.h b/numlib/ludecomp.h
index 1075a5b..3f92ef8 100755
--- a/numlib/ludecomp.h
+++ b/numlib/ludecomp.h
@@ -38,6 +38,7 @@ int n /* Dimensionality */
);
/* Decompose the square matrix A[][] into lower and upper triangles */
+/* NOTE that rows get swaped by swapping matrix pointers! */
/* Return 1 if the matrix is singular. */
int
lu_decomp(
@@ -89,17 +90,34 @@ 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(
double **out, /* Output[0..N-1][0..M-1] */
double **in, /* Input[0..M-1][0..N-1] input matrix */
-int m, /* Rows */
-int n /* Columns */
+int m, /* In Rows */
+int n /* In Columns */
);
+/* - - - - - - - - - - - - - - - - - - - */
+
+/* Use Cholesky decomposition on a symetric positive-definite matrix. */
+/* Only the upper triangle of the matrix A is accessed. */
+/* L returns the decomposition */
+/* Return nz if A is not positive-definite */
+int llt_decomp(double **L, double **A, int n);
+
+
+/* Solve a set of simultaneous equations A.x = b from the */
+/* LLt decomposition, by back substitution. */
+void llt_backsub(
+double **L, /* A[][] LLt decomposition in lower triangle */
+int n, /* Dimensionality */
+double *b, /* Input B[] */
+double *x /* Return X[] (may be same as B[]) */
+);
+
+
#ifdef __cplusplus
}
#endif
diff --git a/numlib/numlib.h b/numlib/numlib.h
index 430ce7a..28ec9ab 100755
--- a/numlib/numlib.h
+++ b/numlib/numlib.h
@@ -6,6 +6,7 @@
#include "numsup.h" /* Support routines, macros */
#include "dnsq.h" /* Non-linear equation solver */
+#include "gnewt.h" /* Global Newton 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 */
@@ -16,5 +17,6 @@
#include "sobol.h" /* Sub-random vector generators */
#include "aatree.h" /* Anderson balanced binary tree */
#include "quadprog.h" /* Quadradic Programming solution */
+#include "roots.h" /* Quadratic, Cubic and Quartic root solving */
#endif /* NUMLIB_H */
diff --git a/numlib/numsup.c b/numlib/numsup.c
index eab81ed..ce76337 100755
--- a/numlib/numsup.c
+++ b/numlib/numsup.c
@@ -1080,6 +1080,29 @@ int nch
free((char *)(m+nrl-1));
}
+/* In case rows have been swapped, reset the pointers */
+void dmatrix_reset(
+double **m,
+int nrl, /* Row low index */
+int nrh, /* Row high index */
+int ncl, /* Col low index */
+int nch /* Col high index */
+) {
+ int i;
+ int cols;
+
+ if (nrh < nrl) /* Prevent failure for 0 dimension */
+ nrh = nrl;
+ if (nch < ncl)
+ nch = ncl;
+
+ cols = nch - ncl + 1;
+
+ m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */
+ for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */
+ m[i] = m[i-1] + cols;
+}
+
/* --------------------- */
/* 2D diagonal half matrix vector malloc/free */
/* A half matrix must have equal rows and columns, */
@@ -1854,19 +1877,61 @@ int matrix_vect_mult(
return 0;
}
+/* Multiply a 0 based transposed matrix by a vector */
+/* d may be same as v */
+int matrix_trans_vect_mult(
+ double *d, int nd,
+ double **m, int nr, int nc,
+ double *v, int nv
+) {
+ int i, j;
+ double *_v = v, vv[20];
+
+ 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 != nr)
+ return 1;
+
+ /* Output vector must match matrix rows */
+ if (nd != nc)
+ return 1;
+
+ for (i = 0; i < nd; i++) {
+ d[i] = 0.0;
+ for (j = 0; j < nv; j++)
+ d[i] += m[j][i] * _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;
+ if (v == 0.0)
+ memset((char *)d, 0, len * sizeof(double));
+ else {
+ 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];
+ memmove((char *)d, (char *)s, len * sizeof(double));
}
@@ -1900,6 +1965,35 @@ void vect_sub(
d[i] -= v[i];
}
+/* Scale a vector, */
+/* d may be same as v */
+void vect_scale(double *d, double *s, double scale, int len) {
+ int i;
+
+ for (i = 0; i < len; i++)
+ d[i] = s[i] * scale;
+}
+
+/* Take dot product of two vectors */
+double vect_dot(double *s1, double *s2, int len) {
+ int i;
+ double rv = 0.0;
+ for (i = 0; i < len; i++)
+ rv += s1[i] * s2[i];
+ return rv;
+}
+
+/* Return the vectors magnitude */
+double vect_mag(double *s, int len) {
+ int i;
+ double rv = 0.0;
+
+ for (i = 0; i < len; i++)
+ rv += s[i] * s[i];
+
+ return sqrt(rv);
+}
+
/* - - - - - - - - - - - - - - - - - - - - - - */
@@ -2019,6 +2113,85 @@ void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc) {
a1logd(g_log, 0, "\n");
}
+/* ============================================================================ */
+/* C matrix support */
+
+/* Clip a vector to the range 0.0 .. 1.0 */
+/* and return any clipping margine */
+double vect_ClipNmarg(int n, double *out, double *in) {
+ int j;
+ double tt, marg = 0.0;
+ for (j = 0; j < n; j++) {
+ out[j] = in[j];
+ if (out[j] < 0.0) {
+ tt = 0.0 - out[j];
+ out[j] = 0.0;
+ if (tt > marg)
+ marg = tt;
+ } else if (out[j] > 1.0) {
+ tt = out[j] - 1.0;
+ out[j] = 1.0;
+ if (tt > marg)
+ marg = tt;
+ }
+ }
+ return marg;
+}
+
+/*
+
+ mat in out
+
+[ ] [] []
+[ ] [] []
+[ ] * [] => []
+[ ] [] []
+[ ] [] []
+
+ */
+
+/* Multiply N vector by NxN transform matrix */
+/* Organization is mat[out][in] */
+void vect_MulByNxN(int n, double *out, double *mat, double *in) {
+ int i, j;
+ double _tt[20], *tt = _tt;
+
+ if (n > 20)
+ tt = dvector(0, n-1);
+
+ for (i = 0; i < n; i++) {
+ tt[i] = 0.0;
+ for (j = 0; j < n; j++)
+ tt[i] += mat[i * n + j] * in[j];
+ }
+
+ for (i = 0; i < n; i++)
+ out[i] = tt[i];
+
+ if (n > 20)
+ free_dvector(tt, 0, n-1);
+}
+
+/* Transpose an NxN matrix */
+void matrix_TransposeNxN(int n, double *out, double *in) {
+ int i, j;
+
+ if (in != out) {
+ for (i = 0; i < n; i++)
+ for (j = 0; j < n; j++)
+ out[i * n + j] = in[j * n + i];
+ } else {
+ for (i = 0; i < n; i++) {
+ for (j = i+1; j < n; j++) {
+ double tt;
+ tt = out[i * n + j];
+ out[i * n + j] = out[j * n + i];
+ out[j * n + i] = tt;
+ }
+ }
+ }
+}
+
/* Print C double matrix to g_log debug */
/* id identifies matrix */
/* pfx used at start of each line */
@@ -2694,9 +2867,9 @@ char *debPiv(int di, int *p) {
return buf[ix];
}
-/* Print a double color vector to a string. */
+/* Print a double color vector to a string with format. */
/* Returned static buffer is re-used every 5 calls. */
-char *debPdv(int di, double *p) {
+char *debPdvf(int di, char *fmt, double *p) {
static char buf[5][DEB_MAX_CHAN * 100];
static int ix = 0;
int e;
@@ -2705,6 +2878,9 @@ char *debPdv(int di, double *p) {
if (p == NULL)
return "(null)";
+ if (fmt == NULL)
+ fmt = "%.8f";
+
if (++ix >= 5)
ix = 0;
bp = buf[ix];
@@ -2715,11 +2891,17 @@ char *debPdv(int di, double *p) {
for (e = 0; e < di; e++) {
if (e > 0)
*bp++ = ' ';
- sprintf(bp, "%.8f", p[e]); bp += strlen(bp);
+ sprintf(bp, fmt, p[e]); bp += strlen(bp);
}
return buf[ix];
}
+/* Print a double color vector to a string. */
+/* Returned static buffer is re-used every 5 calls. */
+char *debPdv(int di, double *p) {
+ return debPdvf(di, NULL, p);
+}
+
/* Print a float color vector to a string. */
/* Returned static buffer is re-used every 5 calls. */
char *debPfv(int di, float *p) {
diff --git a/numlib/numsup.h b/numlib/numsup.h
index 86ad6a8..972d5b3 100755
--- a/numlib/numsup.h
+++ b/numlib/numsup.h
@@ -411,6 +411,7 @@ void free_dvector(double *v,int nl,int nh);
double **dmatrix(int nrl, int nrh, int ncl, int nch);
double **dmatrixz(int nrl, int nrh, int ncl, int nch);
void free_dmatrix(double **m, int nrl, int nrh, int ncl, int nch);
+void dmatrix_reset(double **m, int nrl, int nrh, int ncl, int nch);
double **dhmatrix(int nrl, int nrh, int ncl, int nch);
double **dhmatrixz(int nrl, int nrh, int ncl, int nch);
@@ -486,6 +487,14 @@ int matrix_vect_mult(
double *v, int nv
);
+/* Multiply a 0 based transposed matrix by a vector */
+/* d may be same as v */
+int matrix_trans_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);
@@ -498,16 +507,21 @@ 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
-);
+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
-);
+void vect_sub(double *d, double *v, int len);
+
+/* Scale a vector, */
+/* d may be same as v */
+void vect_scale(double *d, double *s, double scale, int len);
+
+/* Take dot product of two vectors */
+double vect_dot(double *s1, double *s2, int len);
+/* Return the vectors magnitude */
+double vect_mag(double *s, int len);
/* Diagnostics */
/* id identifies matrix/vector */
@@ -524,6 +538,20 @@ 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);
+/* ===================================================== */
+/* C matrix support */
+
+/* Clip a vector to the range 0.0 .. 1.0 */
+/* and return any clipping margine */
+double vect_ClipNmarg(int n, double *out, double *in);
+
+/* Multiply N vector by NxN transform matrix */
+/* Organization is mat[out][in] */
+void vect_MulByNxN(int n, double *out, double *mat, double *in);
+
+/* Transpose an NxN matrix */
+void matrix_TransposeNxN(int n, double *out, double *in);
+
/* Dump C type matrix */
void adump_C_dmatrix(a1log *log, char *id, char *pfx, double *a, int nr, int nc);
@@ -631,10 +659,15 @@ char *debPiv(int di, int *p);
/* Returned static buffer is re-used every 5 calls. */
char *debPdv(int di, double *p);
+/* Print a double vector to a string with format. */
+/* Returned static buffer is re-used every 5 calls. */
+char *debPdvf(int di, char *fmt, double *p);
+
/* Print a float vector to a string. */
/* Returned static buffer is re-used every 5 calls. */
char *debPfv(int di, float *p);
+
/*******************************************/
/* Numerical diagnostics */
diff --git a/numlib/powell.c b/numlib/powell.c
index 47acc15..7fb57a7 100755
--- a/numlib/powell.c
+++ b/numlib/powell.c
@@ -40,8 +40,10 @@
#include "numsup.h"
#include "powell.h"
-#undef SLOPE_SANITY_CHECK /* exermental */
-#undef ABSTOL /* Make tollerance absolute */
+#undef SLOPE_SANITY_CHECK /* [und] expermental */
+#undef ABSTOL /* [und] Make tollerance absolute */
+#undef USE_LINMIND /* [und] Use limind for conjgrad (typically slower) */
+
/* Some debugging printfs (not comprehensive): */
#undef PDEBUG /* Powell debug */
#undef CDEBUG /* Conjgrad debug */
@@ -71,6 +73,7 @@
# define LDBG(xxx)
#endif
+/* -------------------------------------- */
/* Standard interface for powell function */
/* return 0 on sucess, 1 on failure due to excessive itterions */
/* Result will be in cp */
@@ -91,10 +94,10 @@ void (*prog)(void *pdata, int perc), /* Optional progress percentage callback *
void *pdata /* Opaque data needed by prog() */
) {
int i;
- double **dmtx; /* Direction vector */
- double *spt; /* Sarting point before exploring all the directions */
- double *xpt; /* Extrapolated point */
- double *svec; /* Search vector */
+ double **dmtx, *_dmtx[10], __dmtx[10 * 10] = { 0.0 }; /* Direction vector */
+ double *spt, _spt[10]; /* Sarting point before exploring all the directions */
+ double *xpt, _xpt[10]; /* Extrapolated point */
+ double *svec, _svec[10]; /* Search vector */
int iter;
double retv; /* Returned function value at p */
double stopth; /* Current stop threshold */
@@ -102,10 +105,20 @@ void *pdata /* Opaque data needed by prog() */
double curdel; /* Current change in function value */
int pc = 0; /* Percentage complete */
- dmtx = dmatrixz(0, di-1, 0, di-1); /* Zero filled */
- spt = dvector(0,di-1);
- xpt = dvector(0,di-1);
- svec = dvector(0,di-1);
+ if (di <= 10) {
+ int j;
+ for (j = i = 0; i < di; i++, j += di)
+ _dmtx[i] = __dmtx + j;
+ dmtx = _dmtx;
+ spt = _spt;
+ xpt = _xpt;
+ svec = _svec;
+ } else {
+ dmtx = dmatrixz(0, di-1, 0, di-1); /* Zero filled */
+ spt = dvector(0, di-1);
+ xpt = dvector(0, di-1);
+ svec = dvector(0, di-1);
+ }
/* Create initial direction matrix by */
/* placing search start on diagonal */
@@ -213,11 +226,14 @@ void *pdata /* Opaque data needed by prog() */
}
//printf("~1 iters = %d\n",iter);
+
/* Free up all the temporary vectors and matrix */
- free_dvector(svec,0,di-1);
- free_dvector(xpt,0,di-1);
- free_dvector(spt,0,di-1);
- free_dmatrix(dmtx, 0, di-1, 0, di-1);
+ if (di > 10) {
+ free_dvector(svec, 0, di-1);
+ free_dvector(xpt, 0, di-1);
+ free_dvector(spt, 0, di-1);
+ free_dmatrix(dmtx, 0, di-1, 0, di-1);
+ }
if (prog != NULL) /* Report final progress */
prog(pdata, 100);
@@ -232,12 +248,323 @@ void *pdata /* Opaque data needed by prog() */
return 1; /* Failed due to execessive itterations */
}
+/* - - - - - - - - - - - - - - - - - */
+
+#define POWELL_GOLD 1.618034
+#define POWELL_CGOLD 0.3819660
+#define POWELL_MAXIT 100
+
+/* 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 for func() */
+{
+ int i;
+ double ax, xx, bx; /* Search vector multipliers */
+ double af, xf, bf; /* Function values at those points */
+ double *xt, _xt[10]; /* Trial point */
+
+ if (di <= 10)
+ xt = _xt;
+ else
+ xt = dvector(0, di-1); /* Vector for trial point */
+
+ /* -------------------------- */
+ /* First bracket the solution */
+
+ 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 */
+ /* best current solution ?) */
+ ax = 0.0;
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + ax * xi[i];
+ af = (*func)(fdata, xt);
+
+ /* xx being vector offset 0.618 */
+ xx = 1.0/POWELL_GOLD;
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + xx * xi[i];
+ xf = (*func)(fdata, xt);
+
+ 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) {
+ double tt;
+ tt = ax; ax = xx; xx = tt;
+ tt = af; af = xf; xf = tt;
+ }
+ 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);
+
+ 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))) {
+ LDBG((" linmin: giving up because slope is too shallow\n"))
+ if (xt != _xt)
+ free_dvector(xt,0,di-1);
+
+ if (bf < xf) {
+ xf = bf;
+ xx = bx;
+ }
+ goto done;
+ }
+#endif /* SLOPE_SANITY_CHECK */
+
+ /* While not bracketed */
+ while (xf > bf) {
+ double ulim, ux, uf;
+ double tt, r, q;
+
+ 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);
+ r = (xx - ax) * (xf - bf);
+ tt = q - r;
+ if (tt >= 0.0 && tt < 1e-20) /* If +ve too small */
+ tt = 1e-20;
+ else if (tt <= 0.0 && tt > -1e-20) /* If -ve too small */
+ tt = -1e-20;
+ ux = xx - ((xx - bx) * q - (xx - ax) * r) / (2.0 * tt);
+ ulim = xx + 100.0 * (bx - xx); /* Extrapolation limit */
+
+//printf("~1 ux = %f, ulim = %f\n",ux,ulim);
+ if ((xx - ux) * (ux - bx) > 0.0) { /* u is between x and b */
+
+ for (i = 0; i < di; i++) /* Evaluate u */
+ xt[i] = cp[i] + ux * xi[i];
+ uf = (*func)(fdata, xt);
+
+//printf("~1 u is between x and b, uf = %f\n",uf);
+
+ if (uf < bf) { /* Minimum is between x and b */
+//printf("~1 min is between x and b\n");
+ ax = xx; af = xf;
+ xx = ux; xf = uf;
+ break;
+ } else if (uf > xf) { /* Minimum is between a and u */
+//printf("~1 min is between a and u\n");
+ bx = ux; bf = uf;
+ break;
+ }
+
+ /* Parabolic fit didn't work, look further out in direction of b */
+ ux = bx + POWELL_GOLD * (bx-xx);
+//printf("~1 parabolic fit didn't work,look further in direction of b (%f)\n",ux);
+
+ } else if ((bx - ux) * (ux - ulim) > 0.0) { /* u is between b and limit */
+ for (i = 0; i < di; i++) /* Evaluate u */
+ xt[i] = cp[i] + ux * xi[i];
+ uf = (*func)(fdata, xt);
+
+//printf("~1 u is between b and limit uf = %f\n",uf);
+ if (uf > bf) { /* Minimum is between x and u */
+//printf("~1 min is between x and uf\n");
+ ax = xx; af = xf;
+ xx = bx; xf = bf;
+ bx = ux; bf = uf;
+ break;
+ }
+ xx = bx; xf = bf; /* Continue looking */
+ bx = ux; bf = uf;
+ ux = bx + POWELL_GOLD * (bx - xx); /* Test beyond b */
+//printf("~1 continue looking beyond b (%f)\n",ux);
+
+ } else if ((ux - ulim) * (ulim - bx) >= 0.0) { /* u is beyond limit */
+ ux = ulim;
+//printf("~1 use limit\n");
+ } else { /* u is to left side of x ? */
+ ux = bx + POWELL_GOLD * (bx-xx);
+//printf("~1 look gold beyond b (%f)\n",ux);
+ }
+ /* Evaluate u, and move into place at b */
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + ux * xi[i];
+ uf = (*func)(fdata, xt);
+//printf("~1 lookup ux %f value uf = %f\n",ux,uf);
+ ax = xx; af = xf;
+ xx = bx; xf = bf;
+ bx = ux; bf = uf;
+//printf("~1 move along to the right (a<-x, x<-b, b-<u)\n");
+ }
+ 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);
+
+ /* --------------------------------------- */
+ /* Now use brent minimiser bewteen a and b */
+ {
+ /* a and b bracket solution */
+ /* x is best function value so far */
+ /* w is second best function value so far */
+ /* v is previous second best, or third best */
+ /* u is most recently tested point */
+ double wx, vx, ux; /* Search vector multipliers */
+ double wf, vf = 0.0, uf; /* Function values at those points */
+ int iter;
+ double de = 0.0; /* Distance moved on previous step */
+ double e = 0.0; /* Distance moved on 2nd previous step */
+
+ /* Make sure a and b are in ascending order */
+ if (ax > bx) {
+ double tt;
+ tt = ax; ax = bx; bx = tt;
+ tt = af; af = bf; bf = tt;
+ }
+
+ wx = vx = xx; /* Initial values of other center points */
+ wf = xf = xf;
+
+ for (iter = 1; iter <= POWELL_MAXIT; iter++) {
+ double mx = 0.5 * (ax + bx); /* m is center of bracket values */
+#ifdef ABSTOL
+ double tol1 = ftol; /* Absolute tollerance */
+#else
+ double tol1 = ftol * fabs(xx) + 1e-10;
+#endif
+ double tol2 = 2.0 * tol1;
+
+ LDBG((" linmin it %d: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",iter,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))) {
+ LDBG((" linmin: We're done because %e <= %e\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)))
+ break;
+ }
+
+ LDBG((" linmin: e %e tol2 %e\n",e,tol1))
+
+ if (fabs(e) > tol1) { /* Do a trial parabolic fit */
+ double te, p, q, r;
+ r = (xx-wx) * (xf-vf);
+ q = (xx-vx) * (xf-wf);
+ p = (xx-vx) * q - (xx-wx) * r;
+ q = 2.0 * (q - r);
+ if (q > 0.0)
+ p = -p;
+ else
+ q = -q;
+ te = e; /* Save previous e value */
+ e = de; /* Previous steps distance moved */
+
+ 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;
+ 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 */
+ if ((ux - ax) < tol2 || (bx - ux) < tol2) {
+ if ((mx - xx) > 0.0) /* Don't use parabolic, use tol1 */
+ de = tol1; /* tol1 is +ve */
+ else
+ de = -tol1;
+ }
+ 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;
+ LDBG((" linmin: Continuing golden section search\n" ))
+ }
+
+ if (fabs(de) >= tol1) { /* If de moves as much as tol1 would */
+ ux = xx + de; /* use it */
+ 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;
+ LDBG((" linmin: ux = %f = xx %f + tol1 %e\n",ux,xx,tol1))
+ } else {
+ ux = xx - tol1;
+ LDBG((" linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1))
+ }
+ }
+
+ /* Evaluate function */
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + ux * xi[i];
+ 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 {
+ bx = xx; bf = xf; /* New upper bracket */
+ }
+ 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 */
+ } 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 {
+ bx = ux; bf = uf; /* New upper bracket */
+ }
+ if (uf <= wf || wx == xx) { /* New 2nd best solution, or equal best */
+ vx = wx; vf = wf; /* New previous 2nd best solution */
+ wx = ux; wf = uf; /* New 2nd best from latest */
+ } 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 */
+ }
+ }
+ }
+ /* !!! should do something if iter > POWELL_MAXIT !!!! */
+ /* Solution is at xx, xf */
+ }
+
+ done:;
+
+ /* Compute solution vector at xx */
+ LDBG((" linmin: computing soln from best at %f val %f\n",xx,xf))
+ for (i = 0; i < di; i++)
+ cp[i] += xx * xi[i];
+
+ if (xt != _xt)
+ free_dvector(xt,0,di-1);
+//printf("~~~ line minimizer returning %e\n",xf);
+ return xf;
+}
+
+#undef POWELL_GOLD
+#undef POWELL_CGOLD
+#undef POWELL_MAXIT
+
/* -------------------------------------- */
-/* Conjugate Gradient optimiser */
+/* Conjugate Gradient optimiser using partial derivatives. */
/* 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. */
+/* but this seems to be slower, so we don't use it. */
int conjgrad(
double *rv, /* If not NULL, return the residual error */
int di, /* Dimentionality */
@@ -250,79 +577,93 @@ double ftol, /* Relative tollerance of error change to stop on */
#endif
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 */
+double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient & function to evaluate */
+ /* dfunc() should return DFUNC_NRV if it doesn't return function value */
void *fdata, /* Opaque data needed by function */
void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */
void *pdata /* Opaque data needed by prog() */
) {
int i, iter;
- double *svec; /* Search vector */
- double *gvec; /* G direction vector */
- double *hvec; /* H direction vector */
+ double *svec, _svec[10]; /* Search vector */
+ double *ssvec, _ssvec[10]; /* s[] scaled search vector */
+ double *gvec, _gvec[10]; /* G direction vector */
+ double *hvec, _hvec[10]; /* H direction vector */
double retv; /* Returned function value at p */
double stopth; /* Current stop threshold */
double startdel = -1.0; /* Initial change in function value */
double curdel; /* Current change in function value */
- double svec_sca; /* initial svec scale factor */
+ double brat; /* svec to s[] ratio */
+ double svec_sca; /* svec scale factor */
int pc = 0; /* Percentage complete */
- svec = dvector(0,di-1);
- gvec = dvector(0,di-1);
- hvec = dvector(0,di-1);
+ if (di <= 10) {
+ svec = _svec;
+ ssvec = _ssvec;
+ gvec = _gvec;
+ hvec = _hvec;
+ } else {
+ svec = dvector(0, di-1);
+ ssvec = dvector(0, di-1);
+ gvec = dvector(0, di-1);
+ hvec = dvector(0, di-1);
+ }
+
+ CDBG(("conjgrad with di %d\n", di))
if (prog != NULL) /* Report initial progress */
prog(pdata, pc);
- /* Initial gradient function evaluation */
+ /* Initial function and gradient evaluation */
retv = (*dfunc)(fdata, svec, cp);
-
- /* svec[] seems to be large after this. */
- /* Rescale it to conform to maximum of s[] */
- for (svec_sca = 0.0, i = 0; i < di; i++) {
- if (fabs(svec[i]) > svec_sca)
- svec_sca = fabs(svec[i]);
+ if (retv == DFUNC_NRV)
+ retv = (*func)(fdata, cp);
+
+ /* svec[] seems to be large after this. Compute scaled version that */
+ /* has maximum of s[] so that line search is guided by the search radius. */
+ for (brat = 0.0, i = 0; i < di; i++) {
+ double rat = fabs(svec[i]) / fabs(s[i]);
+ if (rat > brat)
+ brat = rat;
}
- /* set scale so largest <= 1 */
- if (svec_sca < 1e-12)
- svec_sca = 1.0;
- else
- svec_sca = 1.0/svec_sca;
-
- CDBG((" initial dir = %s\n", debPdv(di,svec)));
- CDBG((" initial retv = %f\n",retv));
+ svec_sca = 1.0/brat;
/* 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 */
+ svec[i] = gvec[i] = hvec[i] = -svec[i]; /* Inverse gradient */
+ ssvec[i] = svec[i] * svec_sca; /* Scale the search vector to s[] size */
}
- CDBG(("Initial svec = %s\n", debPdv(di,svec)));
+
+ CDBG((" initial dir = %s\n", debPdv(di, ssvec)));
+ CDBG((" initial retv = %f\n",retv));
/* 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 */
- CDBG(("conjrad: about to do linmin\n"))
+ CDBG(("conjrad it %d: about to do linmind\n",iter))
pretv = retv;
- retv = linmin(cp, svec, di, ftol, func, fdata);
+#ifdef USE_LINMIND
+ retv = linmind(cp, ssvec, di, ftol, func, dfunc, fdata);
+#else
+ retv = linmin(cp, ssvec, di, ftol, func, fdata);
+#endif
#ifdef ABSTOL
stopth = ftol; /* Absolute tollerance */
#else
- stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON); // Old code
+ stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON);
#endif
curdel = fabs(pretv - retv);
CDBG((" this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel));
if (startdel < 0.0) {
startdel = curdel;
- } else {
+ } else if (prog != NULL) { /* Update percentage */
int tt;
tt = (int)(100.0 * pow((log(curdel) - log(startdel))/(log(stopth) - log(startdel)), 4.0) + 0.5);
if (tt > pc && tt < 100) {
pc = tt;
- if (prog != NULL) /* Report initial progress */
- prog(pdata, pc);
+ prog(pdata, pc); /* Report initial progress */
}
}
@@ -336,7 +677,7 @@ void *pdata /* Opaque data needed by prog() */
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)));
+ CDBG((" pderiv = %s\n", debPdv(di, svec)));
/* Compute gamma */
for (gamnum = gamden = 0.0, i = 0; i < di; i++) {
@@ -360,25 +701,26 @@ void *pdata /* Opaque data needed by prog() */
svec[i] = hvec[i] = gvec[i] + gam * hvec[i];
}
- /* svec[] seems to be large after this. */
- /* Rescale it to conform to maximum of s[] */
- for (svec_sca = 0.0, i = 0; i < di; i++) {
- if (fabs(svec[i]) > svec_sca)
- svec_sca = fabs(svec[i]);
+ /* svec[] seems to be large after this. Compute scaled version that */
+ /* has maximum of s[] so that line search is guided by the search radius. */
+ for (brat = 0.0, i = 0; i < di; i++) {
+ double rat = fabs(svec[i]) / fabs(s[i]);
+ if (rat > brat)
+ brat = rat;
}
- /* set scale so largest <= 1 */
- if (svec_sca < 1e-12)
- svec_sca = 1.0;
- else
- svec_sca = 1.0/svec_sca;
+ svec_sca = 1.0/brat;
for (i = 0; i < di; i++)
- svec[i] = svec[i] * s[i] * svec_sca;
- CDBG((" svec = %s\n", debPdv(di,svec)));
+ ssvec[i] = svec[i] * svec_sca;
+
+ CDBG((" ssvec = %s\n", debPdv(di,ssvec)));
}
/* Free up all the temporary vectors and matrix */
- free_dvector(hvec,0,di-1);
- free_dvector(gvec,0,di-1);
- free_dvector(svec,0,di-1);
+ if (di > 10) {
+ free_dvector(hvec, 0, di-1);
+ free_dvector(gvec, 0, di-1);
+ free_dvector(ssvec, 0, di-1);
+ free_dvector(svec, 0, di-1);
+ }
if (prog != NULL) /* Report final progress */
prog(pdata, 100);
@@ -394,14 +736,15 @@ void *pdata /* Opaque data needed by prog() */
return 1; /* Failed due to execessive itterations */
}
-/*------------------------------*/
#define POWELL_GOLD 1.618034
-#define POWELL_CGOLD 0.3819660
#define POWELL_MAXIT 100
-/* Line bracketing and minimisation routine. */
+/* Line bracketing and minimisation routine using derivatives */
+/* This is not used, because it typically makes it slower */
+/* - it may take less itterations, but each itteration uses */
+/* a func() and dfunc() call, at least doubling itter overhead. */
/* Return value at minimum. */
-double linmin(
+double linmind(
double cp[], /* Start point, and returned value */
double xi[], /* Search vector */
int di, /* Dimensionality */
@@ -410,23 +753,29 @@ 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 */
+double (*func)(void *fdata, double tp[]), /* Error function to evaluate */
+double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient function to evaluate */
+ /* dfunc() should return DFUNC_NRV if it doesn't return function value */
void *fdata) /* Opaque data for func() */
{
int i;
double ax, xx, bx; /* Search vector multipliers */
double af, xf, bf; /* Function values at those points */
- double *xt, XT[10]; /* Trial point */
+ double *xt, _xt[10]; /* Trial point */
+ double *df, _df[10]; /* Derivative vector */
- if (di <= 10)
- xt = XT;
- else
+ if (di <= 10) {
+ xt = _xt;
+ df = _df;
+ } else {
xt = dvector(0, di-1); /* Vector for trial point */
+ df = dvector(0, di-1); /* Vector for trial point */
+ }
/* -------------------------- */
/* First bracket the solution */
- LDBG(("linmin: Bracketing solution\n"))
+ LDBG((" linmind: Bracketing solution\n"))
/* The line is measured as startpoint + offset * search vector. */
/* (Search isn't symetric, but it seems to depend on cp being */
@@ -442,7 +791,7 @@ void *fdata) /* Opaque data for func() */
xt[i] = cp[i] + xx * xi[i];
xf = (*func)(fdata, xt);
- LDBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
+ LDBG((" linmind: 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) {
@@ -450,33 +799,31 @@ void *fdata) /* Opaque data for func() */
tt = ax; ax = xx; xx = tt;
tt = af; af = xf; xf = tt;
}
- LDBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf))
+ LDBG((" linmind: 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);
- LDBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+ LDBG((" linmind: 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))) {
- LDBG(("linmin: giving up because slope is too shallow\n"))
- if (xt != XT)
- free_dvector(xt,0,di-1);
+ LDBG((" linmind: giving up because slope is too shallow\n"))
+ if (di > 10) {
+ free_dvector(df, 0, di-1);
+ free_dvector(xt, 0, di-1);
+ }
if (bf < xf) {
xf = bf;
xx = bx;
}
-
- /* Compute solution vector */
- for (i = 0; i < di; i++)
- cp[i] += xx * xi[i];
- return xf;
+ goto done;
}
#endif /* SLOPE_SANITY_CHECK */
@@ -485,8 +832,7 @@ void *fdata) /* Opaque data for func() */
double ulim, ux, uf;
double tt, r, q;
-// 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((" linmind: 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 */
@@ -559,7 +905,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");
}
- LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+ LDBG((" linmind: 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);
@@ -571,8 +917,10 @@ void *fdata) /* Opaque data for func() */
/* w is second best function value so far */
/* v is previous second best, or third best */
/* u is most recently tested point */
+
double wx, vx, ux; /* Search vector multipliers */
double wf, vf = 0.0, uf; /* Function values at those points */
+ double xd, wd, vd, ud; /* Derivative values at those points */
int iter;
double de = 0.0; /* Distance moved on previous step */
double e = 0.0; /* Distance moved on 2nd previous step */
@@ -587,6 +935,16 @@ void *fdata) /* Opaque data for func() */
wx = vx = xx; /* Initial values of other center points */
wf = xf = xf;
+ /* Lookup derivative at x (we already have xf from bracketing) */
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + xx * xi[i];
+ (*dfunc)(fdata, df, xt);
+ for (xd = 0.0, i = 0; i < di; i++)
+ xd += xi[i] * df[i];
+ wd = ud = xd;
+
+ LDBG((" linmind: xx %f, deriv. xd %f\n",xx,xd))
+
for (iter = 1; iter <= POWELL_MAXIT; iter++) {
double mx = 0.5 * (ax + bx); /* m is center of bracket values */
#ifdef ABSTOL
@@ -596,113 +954,169 @@ void *fdata) /* Opaque data for func() */
#endif
double tol2 = 2.0 * tol1;
- LDBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf))
+ LDBG((" linmind it %d: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",iter, 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))) {
- LDBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)))
+ LDBG((" linmind: We're done because %e <= %e\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)))
break;
}
- if (fabs(e) > tol1) { /* Do a trial parabolic fit */
- double te, p, q, r;
- r = (xx-wx) * (xf-vf);
- q = (xx-vx) * (xf-wf);
- p = (xx-vx) * q - (xx-wx) * r;
- q = 2.0 * (q - r);
- if (q > 0.0)
- p = -p;
- else
- q = -q;
+ LDBG((" linmind: e %f tol2 %f\n",e,tol1))
+
+ if (fabs(e) > tol1) { /* Do a trial secant fit */
+ double te;
+ double dx1, dx2; /* Secant extrapolation points */
+ double ux1, ux2;
+ int ch1, ch2;
+
+ LDBG((" linmind: Doing trial secant fit\n" ))
+
+ dx2 = dx1 = 2.0 * (bx - ax); /* Default to values out of the ax..bx bracket */
+
+ /* Extrapolated points from last two points (secant method) */
+ if (wd != xd)
+ dx1 = (wx - xx) * xd/(xd - wd);
+ if (vd != xd)
+ dx2 = (vx - xx) * xd/(xd - vd);
+
+ ux1 = xx + dx1;
+ ux2 = xx + dx2;
+
+ /* Check which one is reasonable */
+ ch1 = (ax - ux1) * (ux1 - bx) > 0.0 && xd * dx1 < 0.0;
+ ch2 = (ax - ux2) * (ux2 - bx) > 0.0 && xd * dx2 < 0.0;
+
+ LDBG((" linmind: Doing dx1 %f dx2 %f ux1 %f ux2 %f ch1 %d ch2 %d\n",dx1,dx2,ux1,ux2,ch1,ch2))
+
te = e; /* Save previous e value */
e = de; /* Previous steps distance moved */
- LDBG(("linmin: Trial parabolic fit\n" ))
+ if (!ch1 && !ch2)
+ goto bisect;
- 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;
- 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 */
- if ((ux - ax) < tol2 || (bx - ux) < tol2) {
- if ((mx - xx) > 0.0) /* Don't use parabolic, use tol1 */
- de = tol1; /* tol1 is +ve */
- else
- de = -tol1;
- }
- LDBG(("linmin: Using parabolic fit\n" ))
+ /* Use smallest or the one that's valid */
+ if (ch1 && ch2)
+ de = fabs(dx1) < fabs(dx2) ? dx1 : dx2;
+ if (ch1)
+ de = dx1;
+ else if (ch2)
+ de = dx2;
+
+ LDBG((" linmind: set de %f\n",de))
+
+ if (fabs(de) > fabs(0.5 * te)) {
+ LDBG((" linmind: abs(de) %f > abs(te/2 = %f)\n",fabs(de),fabs(0.5 * te)))
+ goto bisect;
}
- } else { /* Keep using the golden section search */
- e = ((xx >= mx) ? ax-xx : bx-xx); /* Override previous distance moved */
- de = POWELL_CGOLD * e;
- LDBG(("linmin: Continuing golden section search\n" ))
+
+ ux = xx + de;
+
+ if ((ux - ax) < tol2 || (bx - ux) < tol2) {
+ if ((mx - xx) < 0.0)
+ de = -fabs(tol1);
+ else
+ de = fabs(tol1);
+ LDBG((" linmind: Set de to tol1 %f\n",de))
+ }
+#ifdef LDEBUG
+ else {
+ LDBG((" linmind: Using secant fit de %f\n",de))
+ }
+#endif
+
+ /* else bisect picking side using sign of derivative */
+ } else {
+ bisect:
+ e = (xd >= 0.0 ? ax - xx : bx -xx);
+ de = 0.5 * e;
+ LDBG((" linmind: Continuing bisection search de %f\n",de))
}
- if (fabs(de) >= tol1) { /* If de moves as much as tol1 would */
+ if (fabs(de) >= tol1) { /* If de moves as much as tol1 or more */
ux = xx + de; /* use it */
- LDBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de))
+
+ /* Evaluate function */
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + ux * xi[i];
+ uf = (*func)(fdata, xt);
+
+ LDBG((" linmind: ux = %f = xx %f + de %f, uf %f\n",ux,xx,de,uf))
+
} else { /* else move by tol1 in direction de */
if (de > 0.0) {
ux = xx + tol1;
- LDBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1))
+ LDBG((" linmind: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1))
} else {
ux = xx - tol1;
- LDBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1))
+ LDBG((" linmind: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1))
+ }
+ /* Evaluate function */
+ for (i = 0; i < di; i++)
+ xt[i] = cp[i] + ux * xi[i];
+ uf = (*func)(fdata, xt);
+
+ LDBG((" linmind: uf %f\n",uf))
+
+ if (uf > xf) { /* If tol1 step downhill takes us uphill, we're done */
+ goto done;
}
}
- /* Evaluate function */
- for (i = 0; i < di; i++)
- xt[i] = cp[i] + ux * xi[i];
- uf = (*func)(fdata, xt);
+ /* Evaluate derivative at trial point */
+ (*dfunc)(fdata, df, xt);
+ for (ud = 0.0, i = 0; i < di; i++)
+ ud += xi[i] * df[i];
+
+ LDBG((" linmind: ux %f, deriv. ud %f\n",ux,ud))
+ /* Houskeeping: */
if (uf <= xf) { /* Found new best solution */
- LDBG(("linmin: found new best solution at %f val %f\n",ux,uf))
+ LDBG((" linmind: found new best solution at %f val %f dval %f\n",ux,uf,ud))
if (ux >= xx) {
- ax = xx; af = xf; /* New lower bracket */
+ ax = xx; af = xf; /* New lower bracket */
} else {
bx = xx; bf = xf; /* New upper bracket */
}
- 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 */
- } 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))
+ vx = wx; vf = wf; vd = wd; /* New previous 2nd best solution */
+ wx = xx; wf = xf; wd = xd; /* New 2nd best solution from previous best */
+ xx = ux; xf = uf; xd = ud; /* New best solution from latest */
+
+ } else { /* Found a worse solution */
+ LDBG((" linmind: found new worse solution at %f val %f dval %f\n",ux,uf,ud))
+ LDBG((" linmind: current best at %f val %f dval %f\n",xx,xf,xd))
if (ux < xx) {
ax = ux; af = uf; /* New lower bracket */
} else {
bx = ux; bf = uf; /* New upper bracket */
}
if (uf <= wf || wx == xx) { /* New 2nd best solution, or equal best */
- vx = wx; vf = wf; /* New previous 2nd best solution */
- wx = ux; wf = uf; /* New 2nd best from latest */
+ vx = wx; vf = wf; vd = wd; /* New previous 2nd best solution */
+ wx = ux; wf = uf; wd = ud; /* New 2nd best from latest */
} 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 */
+ vx = ux; vf = uf; vd = ud; /* New previous 2nd best from latest */
}
}
- }
- /* !!! should do something if iter > POWELL_MAXIT !!!! */
+ } /* Next itter */
+
+ /* !!! should do something if iter > POWELL_MAXIT ??? */
/* Solution is at xx, xf */
+ done:;
+ if (di > 10) {
+ free_dvector(df, 0, di-1);
+ free_dvector(xt, 0, di-1);
+ }
/* Compute solution vector */
- LDBG(("linmin: computing soln from best at %f val %f\n",xx,xf))
+ LDBG((" linmind: computing soln from best at %f val %f dval %f\n",xx,xf,xd))
for (i = 0; i < di; i++)
cp[i] += xx * xi[i];
- }
- if (xt != XT)
- free_dvector(xt,0,di-1);
-//printf("~~~ line minimizer returning %e\n",xf);
+ } /* Minimizer context */
+
return xf;
}
#undef POWELL_GOLD
-#undef POWELL_CGOLD
#undef POWELL_MAXIT
-/**************************************************/
diff --git a/numlib/powell.h b/numlib/powell.h
index 4b86573..642cb65 100755
--- a/numlib/powell.h
+++ b/numlib/powell.h
@@ -15,6 +15,8 @@
extern "C" {
#endif
+#define DFUNC_NRV 1e100
+
/* Standard interface for powell function */
/* return 0 on sucess, 1 on failure due to excessive itterations */
/* Result will be in cp */
@@ -32,7 +34,7 @@ void (*prog)(void *pdata, int perc), /* Optional progress percentage callback *
void *pdata /* Opaque data needed by prog() */
);
-/* Conjugate Gradient optimiser */
+/* Conjugate Gradient optimiser using partial derivatives. */
/* return 0 on sucess, 1 on failure due to excessive itterations */
/* Result will be in cp */
int conjgrad(
@@ -43,7 +45,8 @@ 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 */
+double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient & function to evaluate */
+ /* dfunc() should return DFUNC_NRV if it doesn't return function value */
void *fdata, /* Opaque data needed by function */
void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */
void *pdata /* Opaque data needed by prog() */
@@ -69,6 +72,22 @@ double (*func)(void *fdata, double tp[]), /* Error function to evaluate */
void *fdata /* Opaque data for func() */
);
+/* Line bracketing and minimisation routine using derivatives. */
+/* Return value at minimum. */
+double linmind(
+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 */
+double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient & function to evaluate */
+void *fdata /* Opaque data for func() */
+);
+
#ifdef __cplusplus
}
#endif
diff --git a/numlib/quadprog.c b/numlib/quadprog.c
index 847601f..4e94cec 100755
--- a/numlib/quadprog.c
+++ b/numlib/quadprog.c
@@ -233,7 +233,7 @@ double quadprog( /* Return solution cost, QP_INFEASIBLE if infeasible/error */
/* and compute the current solution value */
f_value = 0.5 * scalar_product(g0, x, n);
#ifdef TRACE_SOLVER
- printf("Unconstrained solution: %f",f_value);
+ printf("Unconstrained solution: f_value %f\n",f_value);
print_vector("x", x, n);
#endif
@@ -314,9 +314,11 @@ l1: iter++;
#endif
if (fabs(psi) <= m * DBL_EPSILON * c1 * c2* 100.0) {
- /* numerically there are not infeasibilities anymore */
+ /* numerically there are no infeasibilities anymore */
+#ifdef TRACE_SOLVER
+ printf("numerically there are no infeasibilities anymore\n");
+#endif
q = iq;
-
goto done;
}
@@ -338,8 +340,10 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
}
}
if (ss >= 0.0) {
+#ifdef TRACE_SOLVER
+ printf(" ss %f >= 0.0\n",ss);
+#endif
q = iq;
-
goto done;
}
@@ -398,7 +402,7 @@ l2a:
/* 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);
+ printf("Step size: %e (t1 = %e, t2 = %e\n",t,t1,t2);
#endif
/* Step 2c: determine new S-pair and take step: */
@@ -407,6 +411,9 @@ l2a:
if (t >= QP_INFEASIBLE) {
/* QPP is infeasible */
// FIXME: unbounded to raise
+#ifdef TRACE_SOLVER
+ printf(" QPP is infeasible\n");
+#endif
q = iq;
f_value = QP_INFEASIBLE;
goto done;
@@ -449,18 +456,21 @@ l2a:
if (fabs(t - t2) < DBL_EPSILON) {
#ifdef TRACE_SOLVER
- printf("Full step has taken %f\n",t);
+ printf("Full step has been taken %f\n",t);
print_vector("x", x, n);
#endif
- /* full step has taken */
- /* add constraint ip to the active set*/
+ /* full step has been taken */
+ /* Add constraint ip to the active set*/
if (!add_constraint(R, J, d, &iq, &R_norm, n)) {
+#ifdef TRACE_SOLVER
+ printf("add_constraint failed - removing constraint ant step\n",t);
+#endif
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 ?
+ print_ivector("iai", iai, m+p);
#endif
for (i = 0; i < m; i++)
iai[i] = i;
@@ -477,7 +487,7 @@ l2a:
#ifdef TRACE_SOLVER
print_matrix("R", R, n, n);
print_ivector("A", A, iq);
- print_ivector("iai", iai, iq); // ?? iq size of m+p ?
+ print_ivector("iai", iai, m + p);
#endif
goto l1;
}
@@ -532,6 +542,11 @@ l2a:
free_svector(iaexcl, 0, m + p-1);
}
+#ifdef TRACE_SOLVER
+ printf("Returning f_value %e\n",f_value);
+ print_vector("Returning x", x, n);
+#endif
+
return f_value;
}
diff --git a/numlib/quadprog.h b/numlib/quadprog.h
index 7d28ec8..d091b75 100755
--- a/numlib/quadprog.h
+++ b/numlib/quadprog.h
@@ -25,20 +25,20 @@ 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
+ CE^t x + ce0 = 0
+ CI^t x + ci0 >= 0
The matrix and vectors dimensions are as follows:
- G: n * n
- g0: n
+ G: n * n
+ g0: n
- CE: n * p
- ce0: p
+ CE: n * p
+ ce0: p
- CI: n * m
- ci0: m
+ CI: n * m
+ ci0: m
- x: n
+ 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.
diff --git a/numlib/rand.c b/numlib/rand.c
index 9e58de3..e2fdfdf 100755
--- a/numlib/rand.c
+++ b/numlib/rand.c
@@ -39,12 +39,18 @@ double ranno(void) {
return rand32_th(NULL, 0) / 4294967295.0;
}
-/* Return a random double in the range min to max */
+/* Return a uniform random double in the range min to max */
double
d_rand(double min, double max) {
return d_rand_th(NULL, min, max);
}
+/* Return a squared distribution random double in the range min to max */
+double
+d2_rand(double min, double max) {
+ return d2_rand_th(NULL, min, max);
+}
+
/* Return a random integer in the range min to max inclusive */
int
i_rand(int min, int max) {
@@ -81,6 +87,7 @@ unsigned int seed /* Optional seed. Non-zero re-initialized with that seed */
p = &g_rand;
if (seed != 0) {
+//printf("~1 rand 0x%x seed 0x%x\n",p,seed);
p->pvs_inited = 0;
p->ran = seed;
}
@@ -98,21 +105,29 @@ unsigned int seed /* Optional seed. Non-zero re-initialized with that seed */
p->last = p->pvs[i]; /* Value generated */
p->pvs[i] = p->ran = PSRAND32(p->ran); /* New value */
+//printf("~1 rand 0x%x ret 0x%x\n",p,p->last-1);
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 rand32_th(p, 0) / 4294967295.0;
}
-/* Return a random double in the range min to max */
+/* Return a uniform 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 squared distribution random double in the range min to max */
+double
+d2_rand_th(rand_state *p, double min, double max) {
+ double val = ranno_th(p);
+ return min + (max - min) * val * val;
+}
+
/* Return a random integer in the range min to max inclusive */
int
i_rand_th(rand_state *p, int min, int max) {
diff --git a/numlib/rand.h b/numlib/rand.h
index e190fcd..0339b24 100755
--- a/numlib/rand.h
+++ b/numlib/rand.h
@@ -24,9 +24,12 @@ 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(int min, int max);
-/* Return a random double in the range min to max inclusive */
+/* Return a uniform random double in the range min to max inclusive */
double d_rand(double min, double max);
+/* Return a squared distribution random double in the range min to max inclusive */
+double d2_rand(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 */
@@ -61,9 +64,12 @@ 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 */
+/* Return a uniform random double in the range min to max inclusive */
double d_rand_th(rand_state *p, double min, double max);
+/* Return a squared distribution random double in the range min to max inclusive */
+double d2_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 */
diff --git a/numlib/roots.c b/numlib/roots.c
new file mode 100755
index 0000000..04e1526
--- /dev/null
+++ b/numlib/roots.c
@@ -0,0 +1,217 @@
+
+/*
+ * Roots3And4.c
+ *
+ * Utility functions to find cubic and quartic roots.
+ *
+ * Coefficients are passed like this:
+ *
+ * c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
+ *
+ * The functions return the number of non-complex roots and
+ * put the values into the s array.
+ *
+ * Author: Jochen Schwarze (schwarze@isa.de)
+ *
+ * Jan 26, 1990 Version for Graphics Gems
+ * Oct 11, 1990 Fixed sign problem for negative q's in SolveQuartic
+ * (reported by Mark Podlipec),
+ * Old-style function definitions,
+ * IsZero() as a macro
+ * Nov 23, 1990 Some systems do not declare acos() and cbrt() in
+ * <math.h>, though the functions exist in the library.
+ * If large coefficients are used, EQN_EPS should be
+ * reduced considerably (e.g. to 1E-30), results will be
+ * correct but multiple roots might be reported more
+ * than once.
+ * Apr 18, 2018 Reformat for inclusing in ArgyllCMS - GWG
+ *
+ * The authors and the publisher hold no copyright restrictions
+ * on any of these files; this source code is public domain, and
+ * is freely available to the entire computer graphics community
+ * for study, use, and modification. We do request that the
+ * comment at the top of each file, identifying the original
+ * author and its original publication in the book Graphics
+ * Gems, be retained in all programs that use these files.
+ *
+ */
+
+#include <math.h>
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/* epsilon surrounding for near zero values */
+#define EQN_EPS 1e-9
+#define IsZero(x) ((x) > -EQN_EPS && (x) < EQN_EPS)
+
+#define CBRT(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
+ ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
+
+int SolveQuadric(double c[3], double s[2]) {
+ double p, q, D;
+
+ /* normal form: x^2 + px + q = 0 */
+ p = c[1] / (2 * c[2]);
+ q = c[0] / c[2];
+
+ D = p * p - q;
+
+ if (IsZero(D)) {
+ s[ 0 ] = - p;
+ return 1;
+ } else if (D < 0) {
+ return 0;
+ } else /* if (D > 0) */ {
+ double sqrt_D = sqrt(D);
+
+ s[0] = sqrt_D - p;
+ s[1] = - sqrt_D - p;
+ return 2;
+ }
+}
+
+
+int SolveCubic(double c[4], double s[3]) {
+ int i, num;
+ double sub;
+ double A, B, C;
+ double sq_A, p, q;
+ double cb_p, D;
+
+ /* normal form: x^3 + Ax^2 + Bx + C = 0 */
+ A = c[2] / c[3];
+ B = c[1] / c[3];
+ C = c[0] / c[3];
+
+ /* substitute x = y - A/3 to eliminate quadric term: */
+ /* x^3 +px + q = 0 */
+ sq_A = A * A;
+ p = 1.0/3 * (-1.0/3 * sq_A + B);
+ q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
+
+ /* use Cardano's formula */
+ cb_p = p * p * p;
+ D = q * q + cb_p;
+
+ if (IsZero(D)) {
+ if (IsZero(q)) { /* one triple solution */
+ s[0] = 0.0;
+ num = 1;
+ } else { /* one single and one double solution */
+ double u = CBRT(-q);
+ s[0] = 2.0 * u;
+ s[1] = - u;
+ num = 2;
+ }
+ } else if (D < 0) { /* Casus irreducibilis: three real solutions */
+ double phi = 1.0/3 * acos(-q / sqrt(-cb_p));
+ double t = 2 * sqrt(-p);
+
+ s[0] = t * cos(phi);
+ s[1] = -t * cos(phi + M_PI / 3.0);
+ s[2] = -t * cos(phi - M_PI / 3.0);
+ num = 3;
+ } else { /* one real solution */
+ double sqrt_D = sqrt(D);
+ double u = CBRT(sqrt_D - q);
+ double v = -CBRT(sqrt_D + q);
+
+ s[0] = u + v;
+ num = 1;
+ }
+
+ /* resubstitute */
+ sub = 1.0/3.0 * A;
+
+ for (i = 0; i < num; i++)
+ s[i] -= sub;
+
+ return num;
+}
+
+
+int SolveQuartic(double c[5], double s[4]) {
+ double coeffs[4];
+ double z, u, v, sub;
+ double A, B, C, D;
+ double sq_A, p, q, r;
+ int i, num;
+
+ /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
+ A = c[3] / c[4];
+ B = c[2] / c[4];
+ C = c[1] / c[4];
+ D = c[0] / c[4];
+
+ /* substitute x = y - A/4 to eliminate cubic term:
+ x^4 + px^2 + qx + r = 0 */
+ sq_A = A * A;
+ p = - 3.0/8 * sq_A + B;
+ q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
+ r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
+
+ if (IsZero(r)) {
+ /* no absolute term: y(y^3 + py + q) = 0 */
+
+ coeffs[0] = q;
+ coeffs[1] = p;
+ coeffs[2] = 0;
+ coeffs[3] = 1.0;
+
+ num = SolveCubic(coeffs, s);
+
+ s[num++] = 0;
+ } else {
+ /* solve the resolvent cubic ... */
+ coeffs[0] = 1.0/2.0 * r * p - 1.0/8.0 * q * q;
+ coeffs[1] = - r;
+ coeffs[2] = - 1.0/2.0 * p;
+ coeffs[3] = 1.0;
+
+ (void) SolveCubic(coeffs, s);
+
+ /* ... and take the one real solution ... */
+ z = s[0];
+
+ /* ... to build two quadric equations */
+ u = z * z - r;
+ v = 2 * z - p;
+
+ if (IsZero(u))
+ u = 0;
+ else if (u > 0)
+ u = sqrt(u);
+ else
+ return 0;
+
+ if (IsZero(v))
+ v = 0;
+ else if (v > 0)
+ v = sqrt(v);
+ else
+ return 0;
+
+ coeffs[0] = z - u;
+ coeffs[1] = q < 0 ? -v : v;
+ coeffs[2] = 1.0;
+
+ num = SolveQuadric(coeffs, s);
+
+ coeffs[0]= z + u;
+ coeffs[1] = q < 0 ? v : -v;
+ coeffs[2] = 1.0;
+
+ num += SolveQuadric(coeffs, s + num);
+ }
+
+ /* resubstitute */
+ sub = 1.0/4.0 * A;
+
+ for (i = 0; i < num; i++)
+ s[i] -= sub;
+
+ return num;
+}
+
+
diff --git a/numlib/roots.h b/numlib/roots.h
new file mode 100755
index 0000000..e14b72d
--- /dev/null
+++ b/numlib/roots.h
@@ -0,0 +1,23 @@
+
+/*
+ * Roots3And4.c
+ *
+ * Utility functions to find cubic and quartic roots,
+ * coefficients are passed like this:
+ *
+ * c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
+ *
+ * The functions return the number of non-complex roots and
+ * put the values into the s array.
+ *
+ * Author: Jochen Schwarze (schwarze@isa.de)
+ * (From Graphics Gems I)
+ */
+
+int SolveQuadric(double c[3], double s[2]);
+
+int SolveCubic(double c[4], double s[3]);
+
+int SolveQuartic(double c[5], double s[4]);
+
+
diff --git a/numlib/tconjgrad.c b/numlib/tconjgrad.c
new file mode 100755
index 0000000..aa98871
--- /dev/null
+++ b/numlib/tconjgrad.c
@@ -0,0 +1,162 @@
+/* Code to test the conjgrad minimiser */
+/*
+ * Copyright 1999, 2018 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.
+ */
+
+#include <stdio.h>
+#include "numlib.h"
+
+/* Final approximate solution: */
+
+double expect[9] = {
+ -0.5706545E+00,
+ -0.6816283E+00,
+ -0.7017325E+00,
+ -0.7042129E+00,
+ -0.7013690E+00,
+ -0.6918656E+00,
+ -0.6657920E+00,
+ -0.5960342E+00,
+ -0.4164121E+00 };
+
+double fcn( /* Return function value */
+ void *fdata, /* Opaque data pointer */
+ double tp[] /* Multivriate input value */
+);
+
+static double dfcn(
+ void *fdata,
+ double dp[],
+ double tp[]
+);
+
+#define N 9
+
+static void progress(void *pdata, int perc) {
+ printf("%c% 3d%%",cr_char,perc);
+ if (perc == 100)
+ printf("\n");
+ fflush(stdout);
+}
+
+int main(void)
+{
+ double cp[N]; /* Function input values */
+ double s[N]; /* Search area */
+ double err;
+ int j;
+ int nprint = 0; /* Itteration debugging print = off */
+ int rc;
+
+ error_program = "tpowell"; /* Set global error reporting string */
+ check_if_not_interactive();
+
+ /* The following starting values provide a rough solution. */
+ for (j = 0; j < N; j++) {
+ cp[j] = -1.f;
+ s[j] = 0.9;
+ }
+
+ nprint = 0;
+
+ /* Set tol to the square root of the machine precision. */
+ /* Unless high precision solutions are required, */
+ /* this is the recommended setting. */
+
+ rc = conjgrad(
+ &err,
+ N, /* Dimentionality */
+ cp, /* Initial starting point */
+ s, /* Size of initial search area */
+ 0.00000001, /* Tollerance of error change to stop on */
+ 1000, /* Maximum iterations allowed */
+ fcn, /* Error function to evaluate */
+ dfcn, /* Partial derivative function */
+ NULL, /* Opaque data needed by function */
+ progress, /* Progress callback */
+ NULL /* Context for callback */
+ );
+
+
+ fprintf(stdout,"Status = %d, final approximate solution err = %f:\n",rc,err);
+ for (j = 0; j < N; j++) {
+ fprintf(stdout,"cp[%d] = %e, expect %e\n",j,cp[j],expect[j]);
+ }
+
+ return 0;
+} /* main() */
+
+/* Function being minimized */
+double fcn( /* Return function value */
+void *fdata, /* Opaque data pointer */
+double tp[] /* Multivriate input value */
+) {
+ double err, tt;
+ double temp, temp1, temp2;
+ int k;
+
+ /* Function Body */
+ err = 0.0;
+ for (k = 0; k < N; ++k) {
+ temp = (3.0 - 2.0 * tp[k]) * tp[k];
+ temp1 = 0.0;
+ if (k != 0) {
+ temp1 = tp[k-1];
+ }
+ temp2 = 0.0;
+ if (k != ((N)-1))
+ temp2 = tp[k+1];
+ tt = temp - temp1 - 2.0 * temp2 + 1.0;
+ err += tt * tt;
+ }
+ err = sqrt(err);
+//printf("Returning %16.14f\n",err);
+ return err;
+}
+
+
+/* Compute aprox. forward difference */
+
+#define JEPS 1.0e-8 /* Aprox. sqrt of machine precision */
+
+static double dfcn(
+void *fdata,
+double dp[],
+double tp[]
+) {
+ int i;
+ double d, h, temp;
+
+ d = fcn(fdata, tp);
+
+ for (i = 0; i < N; i++) {
+ temp = tp[i];
+
+ h = JEPS * fabs(temp);
+ if (h == 0.0)
+ h = JEPS;
+ tp[i] = temp + h; /* Add delta */
+ h = tp[i] - temp; /* Actual delta with fp precision limits */
+
+ dp[i] = (fcn(fdata, tp) - d)/h;
+
+ tp[i] = temp; /* Restore value */
+ }
+
+ return DFUNC_NRV;
+}
+
+
+
+
+
+
+
+
+
+
+