#include <stdio.h>
#include <math.h>
#include "gsl/gsl_linalg.h"
#include "gsl/gsl_poly.h"
namespace libpetey {
//maximum number of re-brackets on the same side of the root:
int glob_supnewt_maxnrebr=3;
//note: for smooth ("well-behaved") functions with local non-zero third
//moments, the algorithm has a tendency to land repeatedly on one side
//of the root, slowing down convergence. This issue needs to be addressed.
template <class real>
real nearest_of_three(real value,
real x1,
real x2,
real x3) {
real diff1, diff2, diff3;
real result;
diff1=fabs(value-x1);
diff2=fabs(value-x2);
diff3=fabs(value-x3);
if (diff1 > diff2) {
if (diff2 > diff3) result = x3; else result = x2;
} else {
if (diff1 > diff3) result = x3; else result = x1;
}
return result;
}
//minimization function which brackets the root
//and then approximates root by fitting a third-
//order polynomial
template <class real>
real supernewton(void (*funcd) (real, void *, real *, real *),
void *params, //fucntion parameters
real x1, //first bracket
real x2, //second bracket
real xtol, //desired tolerance in x direction
real ytol, //desired tolerance in y direction
long maxiter, //maximum number of iterations
long &err, //error code--positive for success
real &y1, //to avoid re-calculating
real &dydx1, //these items
real y2,
real dydx2)
{
//function evaluations at the brackets:
/*
real y1;
real dydx1;
real y2;
real dydx2;
*/
//polynomial coefficients:
double a1, b1; //might as well carry over from double to double
real x0; //solution
real y0; //solution value for y
real dydx0; //solution value for dydx
//intermediates in calculation:
real dx;
long nroot;
//roots of polynomial:
double dx0_1, dx0_2, dx0_3; //these need to be double because they're
//returned from a GSL routine
real x0_4;
//number of iterations:
long i;
//x and y error:
real xerr, yerr;
real xerr1, xerr2;
//approximate slope of the function:
real m;
int gsl_status; //error state from GSL calls
int nss=0; //number of re-brackets on the same side
err=0;
i=0;
do {
/*
printf("Brackets: [%g, %g]\n", x1, x2);
printf(" [%g, %g]\n", y1, y2);
printf(" [%g, %g]\n", dydx1, dydx2);
*/
if (abs(nss) >= glob_supnewt_maxnrebr) {
real y0a, dydx0a;
//take the gap between the root and the close bracket and double it:
if (nss < 0) {
//printf("Root has fallen on right side too many times: %g %g %g\n", x1, x0, x2);
x0=x2-2*(x2-x0);
} else if (nss > 0) {
//printf("Root has fallen on left side too many times: %g %g %g\n", x1, x0, x2);
x0=x0+2*(x0-x1);
}
//evaluate the function at the approximated root
(*funcd) (x0, params, &y0a, &dydx0a);
//printf("y(%g)=%g\n", x0, y0a);
//if it's outside the bracket (it shouldn't be),
//or the new approximated "root" is still on the same side
//we just use a bisection step:
if (x0 < x1 || x0 > x2 || y0a*y0 > 0) {
x0=(x2+x1)/2;
//evaluate the function at the approximated root
(*funcd) (x0, params, &y0, &dydx0);
} else {
y0=y0a;
dydx0=dydx0a;
}
nss=0;
} else {
i++;
//a simple coordinate shift vastly simplifies this
//calculation (rather than using a matrix solver):
dx=x2-x1;
b1=(3*(y2-y1)/dx-2*dydx1-dydx2)/dx;
a1=(dydx2-dydx1-2*b1*dx)/3/dx/dx;
{
//printf("Polynomial coeffs.:%f, %f, %f, %f\n", a1, b1, dydx1, y1);
//solve the cubic:
nroot=gsl_poly_solve_cubic(b1/a1, dydx1/a1, y1/a1, &dx0_1, &dx0_2, &dx0_3);
if (nroot==1) {
//if there is only one root AND it's finite AND it advances the solution
//we use that one:
x0=(x1+x2)/2;
if (isfinite(dx0_1)) {
if (fabs(x1+dx0_1-x0) < fabs((x1-x2)/2)) x0=x1+dx0_1;
}
//printf("Root: %f\n", x1+dx0_1);
} else if (nroot=3) {
real xdiff, xdiffmin;
x0_4=(x1+x2)/2;
//printf("Roots: %f, %f, %f\n", x1+dx0_1, x1+dx0_2, x1+dx0_3);
//x0=nearest_of_three(x0_4, (real) x0_1, (real) x0_2, (real) x0_3);
//if there are three,
//fuck nearest_of_three, this is better:
x0=x0_4;
xdiffmin=fabs((x1-x2)/2);
if (isfinite(dx0_1)) {
xdiff=fabs(x1+dx0_1-x0_4);
if (xdiff < xdiffmin) {
xdiffmin=xdiff;
x0=x1+dx0_1;
}
}
if (isfinite(dx0_2)) {
xdiff=fabs(x1+dx0_2-x0_4);
if (xdiff < xdiffmin) {
xdiffmin=xdiff;
x0=x1+dx0_2;
}
}
if (isfinite(dx0_3)) {
xdiff=fabs(x1+dx0_3-x0_4);
if (xdiff < xdiffmin) {
xdiffmin=xdiff;
x0=x1+dx0_3;
}
}
} else {
//aparently this never happens (I checked the source)
fprintf(stderr, "Supernewton: gsl_poly_solve_cubic returned an error\n");
err=-i;
break;
}
}
//evaluate the function at the approximated root
(*funcd) (x0, params, &y0, &dydx0);
}
//m=(y2-y1)/(x2-x1);
yerr=fabs(y0);
//approximate the x error from the y error and approx. slope of func.:
//xerr=fabs(y0/m);
//xerr1=fabs(x0-x1);
//xerr2=fabs(x0-x2);
xerr=fabs(x1-x2);
//if (xerr1 < xerr2) xerr=xerr1; else xerr=xerr2;
//test for convergence:
//(*note* this convergence test is designed for speed, NOT accuracy)
if (yerr < ytol || xerr < xtol) {
//if (yerr < ytol) {
err=i;
break;
}
if (i > maxiter) {
fprintf(stderr, "Maximum number of iterations exceeded in supernewton\n");
err = -maxiter;
break;
printf("Brackets: [%f, %f]\n", x1, x2);
printf("Polynomial coeffs.:%f, %f, %f, %f\n", a1, b1, dydx1, y1);
printf("Roots: %f, %f, %f\n", dx0_1, dx0_2, dx0_3);
}
//printf("Brackets: [%f, %f]\n", x1, x2);
//rebracket the "true" root:
//(unless it's fallen on the same side of the approximated root
//too many times...)
if (y0*y1 > 0) {
if (nss > 0) nss++; else nss=1;
if (abs(nss)<glob_supnewt_maxnrebr) {
x1=x0;
y1=y0;
dydx1=dydx0;
}
} else {
if (nss < 0) nss--; else nss=-1;
if (abs(nss)<glob_supnewt_maxnrebr) {
x2=x0;
y2=y0;
dydx2=dydx0;
}
}
/*
xerr=fabs(x1-x2);
if (xerr < xtol) {
x0=(x1+x2)/2;
y0=(y1+y2)/2;
dydx0=(dydx1+dydx2)/2;
err=i;
break;
}
*/
} while(1);
//place the function values in the fourth and third last parameters:
y1=y0;
dydx1=dydx0;
//printf("Supernewton: %d iterations required to reach convergence\n", i);
return x0;
}
template float supernewton<float>(void (*funcd) (float, void *, float *, float *),
void *params,
float x1,
float x2,
float xtol,
float ytol,
long maxiter,
long &err,
float &y1,
float &dydx1,
float y2,
float dydx2);
template double supernewton<double>(void (*funcd) (double, void *, double *, double *),
void *params,
double x1,
double x2,
double xtol,
double ytol,
long maxiter,
long &err,
double &y1,
double &dydx1,
double y2,
double dydx2);
} //end namespace libpetey