xue@11: /*
xue@11:     Harmonic sinusoidal modelling and tools
xue@11: 
xue@11:     C++ code package for harmonic sinusoidal modelling and relevant signal processing.
xue@11:     Centre for Digital Music, Queen Mary, University of London.
xue@11:     This file copyright 2011 Wen Xue.
xue@11: 
xue@11:     This program is free software; you can redistribute it and/or
xue@11:     modify it under the terms of the GNU General Public License as
xue@11:     published by the Free Software Foundation; either version 2 of the
xue@11:     License, or (at your option) any later version.
xue@11: */
xue@1: //---------------------------------------------------------------------------
xue@1: 
xue@1: #include <math.h>
xue@1: #include <memory.h>
xue@1: #include <stdlib.h>
xue@1: #include "opt.h"
xue@1: #include "matrix.h"
xue@1: 
Chris@5: /** @file opt.h - optimization routines */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
xue@1: //macro nsign: judges if two double-precision floating pointer numbers have different signs
xue@1: #define nsign(x1, x2) (0x80&(((char*)&x1)[7]^((char*)&x2)[7]))
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function GradientMax: gradient method maximizing F(x)
xue@1: 
xue@1:   In: function F(x) and its derivative dF(x)
xue@1:       start[dim]: initial value of x
xue@1:       ep: a stopping threshold
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: start[dim]: the final x
xue@1: 
xue@1:   Returnx max F(final x)
xue@1: */
xue@1: double GradientMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*),
xue@1:   int dim, double* start, double ep, int maxiter)
xue@1: {
xue@1:   double oldG, G=F(dim, start, params), *dG=new double[dim], *X=new double[dim];
xue@1: 
xue@1:   int iter=0;
xue@1:   do
xue@1:   {
xue@1:     oldG=G;
xue@1: 
xue@1:     dF(dG, dim, start, params);
xue@1:     Search1Dmaxfrom(params, F, dim, start, dG, 1e-2, 1e-6, 1e6, X, &G, ep);
xue@1:     memcpy(start, X, sizeof(double)*dim);
xue@1: 
xue@1:     iter++;
xue@1:   }
xue@1:   while (iter<maxiter && G>oldG*(1+ep));
xue@1:   return G;
xue@1: }//GradientMax
xue@1: 
Chris@5: /**
xue@1:   function GradientOneMax: gradient method maximizing F(x), which searches $dim dimensions one after
xue@1:   another instead of taking the gradient descent direction.
xue@1: 
xue@1:   In: function F(x) and its derivative dF(x)
xue@1:       start[dim]: initial value of x
xue@1:       ep: a stopping threshold
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: start[dim]: the final x
xue@1: 
xue@1:   Returnx max F(final x)
xue@1: */
xue@1: double GradientOneMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*),
xue@1:   int dim, double* start, double ep, int maxiter)
xue@1: {
xue@1:   double oldG, G=F(dim, start, params), *dG=new double[dim], *X=new double[dim];
xue@1: 
xue@1:   int iter=0;
xue@1:   do
xue@1:   {
xue@1:     oldG=G;
xue@1:     for (int d=0; d<dim; d++)
xue@1:     {
xue@1:       dF(dG, dim, start, params);
xue@1:       for (int i=0; i<dim; i++) if (d!=i) dG[i]=0;
xue@1:       if (dG[d]!=0)
xue@1:       {
xue@1:         Search1Dmaxfrom(params, F, dim, start, dG, 1e-2, 1e-6, 1e6, X, &G, ep);
xue@1:         memcpy(start, X, sizeof(double)*dim);
xue@1:       }
xue@1:     }
xue@1:     iter++;
xue@1:   }
xue@1:   while (iter<maxiter && G>oldG*(1+ep));
xue@1:   return G;
xue@1: }//GradientOneMax
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function mindenormaldouble: returns the minimal denormal double-precision floating point number.
xue@1: */
xue@1: double mindenormaldouble()
xue@1: {
xue@1:   double result=0;
xue@1:   *((unsigned char*)&result)=1;
xue@1:   return result;
xue@1: }//mindenormaldouble
xue@1: 
Chris@5: /**
xue@1:   function minnormaldouble: returns the minimal normal double-precision floating point number.
xue@1: */
xue@1: double minnormaldouble()
xue@1: {
xue@1:   double result=0;
xue@1:   ((unsigned char*)&result)[6]=16;
xue@1:   return result;
xue@1: }//minnormaldouble
xue@1: 
xue@1: /*
xue@1:   //function nextdouble: returns the next double value after x in the direction of dir
xue@1: */
xue@1: double nextdouble(double x, double dir)
xue@1: {
xue@1:   if (x==0)
xue@1:   {
xue@1:     if (dir<0) return -mindenormaldouble();
xue@1:     else return mindenormaldouble();
xue@1:   }
xue@1:   else if ((x>0)^(dir>0)) *((__int64*)&x)-=1;
xue@1:   else *((__int64*)&x)+=1;
xue@1:   return x;
xue@1: }//nextdouble
xue@1: 
Chris@5: /**
xue@1:   function Newton: Newton method for finding the root of y(x)
xue@1: 
xue@1:   In: function y(x) and its derivative y'(x)
xue@1:       params: additional argument for calling y and y'
xue@1:       x0: inital value of x
xue@1:       epx, epy: stopping thresholds on x and y respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: x0: the root
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: double Newton(double& x0, double (*y)(double, void*), double (*y1)(double, void*), void* params, double epx, int maxiter, double epy)
xue@1: {
xue@1:   double y0=y(x0, params), x2, y2, dx, dy, x3, y3;
xue@1:   if (y0==0) return 0;
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     dy=y1(x0, params);
xue@1:     if (dy==0) return -1;
xue@1:     x2=x0-y0/dy;
xue@1:     y2=y(x2, params);
xue@1:     while (fabs(y2)>fabs(y0))
xue@1:     {
xue@1:       x2=(x2+x0)/2;
xue@1:       y2=y(x2, params);
xue@1:     }
xue@1:     if (y2==0)
xue@1:     {
xue@1:       x0=x2;
xue@1:       return 0;
xue@1:     }
xue@1:     dx=fabs(x0-x2);
xue@1:     if (dx==0)
xue@1:     {
xue@1:       if (fabs(y0)<epy) return 0;
xue@1:       else return -1;
xue@1:     }
xue@1:     else if (dx<epx)
xue@1:     {
xue@1:       if nsign(y2, y0)
xue@1:       {
xue@1:         x0=x2;
xue@1:         return dx;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         x3=2*x2-x0;
xue@1:         y3=y(x3, params);
xue@1:         if (y3==0)
xue@1:         {
xue@1:           x0=x3;
xue@1:           return 0;
xue@1:         }
xue@1:         if nsign(y2, y3)
xue@1:         {
xue@1:           x0=x2;
xue@1:           return dx;
xue@1:         }
xue@1:         else if (fabs(y3)<fabs(y2))
xue@1:         {
xue@1:           x2=x3, y2=y3;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     x0=x2;
xue@1:     y0=y2;
xue@1:     iter++;
xue@1:   }
xue@1:   if (fabs(y0)<epy) return 0;
xue@1:   else return -1;
xue@1: }//Newton
xue@1: 
Chris@5: /**
xue@1:   function Newton: Newton method for finding the root of y(x), for use when it's more efficient for y'
xue@1:   and y to be calculated in one function call
xue@1: 
xue@1:   In: function dy(x) that computes y and y' at x
xue@1:       params: additional argument for calling dy
xue@1:       yshift: the byte shift in params where dy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       epx, epy: stopping thresholds on x and y respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: x0: the root
xue@1: 
xue@1:   Returns 0 if successful.
xue@1: */
xue@1: int Newton(double& x0, double (*dy)(double, void*), void* params, int yshift, double epx, int maxiter, double epy, double xmin, double xmax)
xue@1: {
xue@1:   double y0, x2, y2, dx, dy0, dy2, dy3, x3, y3;
xue@1:   dy0=dy(x0, params);
xue@1:   y0=*(double*)((char*)params+yshift);
xue@1:   if (y0==0) return 0;
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (dy0==0) return -1;
xue@1:     dx=-y0/dy0;
xue@1: 
xue@1:     x2=x0+dx;
xue@1:     dy2=dy(x2, params);
xue@1:     y2=*(double*)((char*)params+yshift);
xue@1:     //make sure the iteration yields a y closer to 0
xue@1:     while (fabs(y2)>fabs(y0))
xue@1:     {
xue@1:       dx/=2;
xue@1:       x2=x0+dx;
xue@1:       dy2=dy(x2, params);
xue@1:       y2=*(double*)((char*)params+yshift);
xue@1:     }
xue@1:     //the above may fail due to precision problem, i.e. x0+dx=x0
xue@1:     if (x2==x0)
xue@1:     {
xue@1:       if (fabs(y0)<epy) return 0;
xue@1:       else
xue@1:       {
xue@1:         x2=nextdouble(x0, dx);
xue@1:         dy2=dy(x2, params);
xue@1:         y2=*(double*)((char*)params+yshift);
xue@1:         if (y2==0)
xue@1:         {
xue@1:           x0=x2;
xue@1:           return 0;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           dx=x2-x0;
xue@1:           while (y2==y0)
xue@1:           {
xue@1:             dx*=2;
xue@1:             x2=x0+dx;
xue@1:             dy2=dy(x2, params);
xue@1:             y2=*(double*)((char*)params+yshift);
xue@1:           }
xue@1:           if nsign(y2, y0)
xue@1:             return fabs(dx);
xue@1:           else if (fabs(y2)>fabs(y0))
xue@1:             return -1;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     else if (y2==0)
xue@1:     {
xue@1:       x0=x2;
xue@1:       return 0;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       double fabsdx=fabs(dx);
xue@1:       if (fabsdx<epx)
xue@1:       {
xue@1:         if nsign(y2, y0)
xue@1:         {
xue@1:           x0=x2;
xue@1:           return fabsdx;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           x3=x2+dx;
xue@1:           dy3=dy(x3, params);
xue@1:           y3=*(double*)((char*)params+yshift);
xue@1:           if (y3==0){x0=x3; return 0;}
xue@1:           if nsign(y2, y3)
xue@1:           {
xue@1:             x0=x2;
xue@1:             return dx;
xue@1:           }
xue@1:           else if (fabs(y3)<fabs(y2))
xue@1:           {
xue@1:             x2=x3, y2=y3, dy2=dy3;
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     if (x2<xmin || x2>xmax) return -1;
xue@1:     x0=x2;
xue@1:     y0=y2;
xue@1:     dy0=dy2;
xue@1:     iter++;
xue@1:   }
xue@1:   if (fabs(y0)<epy) return 0;
xue@1:   else return -1;
xue@1: }//Newton
xue@1: 
Chris@5: /**
xue@1:   function init_Newton_1d: finds an initial x and interval (xa, xb) from which to search for an extremum
xue@1: 
xue@1:   On calling, xa<x0<xb, y(x0)>=y(xa), y(x0)>=y(xb) (or y(x0)<=y(xa), y(x0)<=y(xb)).
xue@1:   On return, y(x0)>=y(xa), y(x0)>=y(xb) (or y(x0)<=y(xa), y(x0)<=y(xb)), and either y'(xa)>0, y'(xb)<0
xue@1:   (or y'(xa)<0, y'(xb)>0), or xb-xa<epx.
xue@1: 
xue@1:   In: function dy(x) that computes y and y' at x
xue@1:       params: additional argument for calling dy
xue@1:       yshift: the byte shift in params where dy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx: tolerable error of extremum
xue@1:   Out: improved initial value x0 and search range (xa, xb)
xue@1: 
xue@1:   Returns xb-xa if successful, -0.5 or -0.6 if y(x0) is not bigger(smaller) than both y(xa) and y(xb)
xue@1: */
xue@1: double init_Newton_1d(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx)
xue@1: {
xue@1:   double dy0, dya, dyb, y0, ya, yb;
xue@1:   dy0=dy(x0, params); y0=*(double*)((char*)params+yshift);
xue@1:   dya=dy(xa, params); ya=*(double*)((char*)params+yshift);
xue@1:   dyb=dy(xb, params); yb=*(double*)((char*)params+yshift);
xue@1:   if (y0>=ya && y0>=yb)
xue@1:   {
xue@1:     //look for xa<x0<xb, ya<y0>yb, so that dya>0, dyb<0
xue@1:     while ((dya<=0 || dyb>=0) && xb-xa>=epx)
xue@1:     {
xue@1:       double x1=(x0+xa)/2;
xue@1:       double dy1=dy(x1, params);
xue@1:       double y1=*(double*)((char*)params+yshift);
xue@1:       double x2=(x0+xb)/2;
xue@1:       double dy2=dy(x2, params);
xue@1:       double y2=*(double*)((char*)params+yshift);
xue@1:       if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:       else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;}
xue@1:       else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;}
xue@1:     }
xue@1:     return xb-xa;
xue@1:   }
xue@1:   else if (y0<=ya && y0<=yb)
xue@1:   {
xue@1:     //look for xa<x0<xb, ya>y0<yb, so that dya<0, dyb>0
xue@1:     while ((dya>=0 || dyb<=0) && xb-xa>=epx)
xue@1:     {
xue@1:       double x1=(x0+xa)/2;
xue@1:       double dy1=dy(x1, params);
xue@1:       double y1=*(double*)((char*)params+yshift);
xue@1:       double x2=(x0+xb)/2;
xue@1:       double dy2=dy(x2, params);
xue@1:       double y2=*(double*)((char*)params+yshift);
xue@1:       if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:       else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;}
xue@1:       else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;}
xue@1:     }
xue@1:     return xb-xa;
xue@1:   }
xue@1:   else//meaning y(x0) is not bigger(smaller) than both y(xa) and y(xb)
xue@1:   {
xue@1:     if (y0<=yb) return -0.5; //ya<=y0<=yb
xue@1:     if (y0<=ya) return -0.6; //ya>=y0>=yb
xue@1:   }
xue@1:   return -0;
xue@1: }//init_Newton_1d
xue@1: 
Chris@5: /**
xue@1:   function init_Newton_1d_max: finds an initial x and interval (xa, xb) from which to search for an
xue@1:   maximum.
xue@1: 
xue@1:   On calling xa<x0<xb, and it is assumed that y(x0)>=y(xa), y(x0)>=y(xb). If the conditions on y are met,
xue@1:   it find a new set of xa<x0<xb, so that y(x0)>y(xa), y(x0)>=y(xb), y'(xa)>0, y'(xb)<0, or xb-xa<epx,
xue@1:   and returns xb-xa>0; otherwise it returns a negative value showing the initial order of y(x0), y(xa)
xue@1:   and y(xb) as
xue@1:     -0.5 for ya<=y0<yb
xue@1:     -0.6 for yb<=y0<ya
xue@1:     -0.7 for y0<ya<yb
xue@1:     -0.8 for y0<yb<=ya
xue@1: 
xue@1:   In: function dy(x) that computes y and y' at x
xue@1:       params: additional argument for calling dy
xue@1:       yshift: the byte shift in params where dy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx: tolerable error of extremum
xue@1:   Out: improved initial value x0 and search range (xa, xb)
xue@1: 
xue@1:   Returns xb-xa if successful, a negative value as above if not.
xue@1: */
xue@1: double init_Newton_1d_max(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx)
xue@1: {
xue@1:   double dy0, dya, dyb, y0, ya, yb;
xue@1:   dy0=dy(x0, params); y0=*(double*)((char*)params+yshift);
xue@1:   dya=dy(xa, params); ya=*(double*)((char*)params+yshift);
xue@1:   dyb=dy(xb, params); yb=*(double*)((char*)params+yshift);
xue@1:   if (y0>=ya && y0>=yb)
xue@1:   {
xue@1:     //look for xa<x0<xb, ya<y0>yb, so that dya>0, dyb<0
xue@1:     while ((dya<=0 || dyb>=0) && xb-xa>=epx)
xue@1:     {
xue@1:       double x1=(x0+xa)/2;
xue@1:       double dy1=dy(x1, params);
xue@1:       double y1=*(double*)((char*)params+yshift);
xue@1:       double x2=(x0+xb)/2;
xue@1:       double dy2=dy(x2, params);
xue@1:       double y2=*(double*)((char*)params+yshift);
xue@1:       if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:       else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;}
xue@1:       else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;}
xue@1:     }
xue@1:     return xb-xa;
xue@1:   }
xue@1:   else//meaning y(x0) is not bigger than both y(xa) and y(xb)
xue@1:   {
xue@1:     if (y0<yb && y0>=ya) return -0.5; //ya<=y0<yb
xue@1:     if (y0<ya && y0>=yb) return -0.6; //ya>y0>=yb
xue@1:     if (ya<yb) return -0.7; //y0<ya<yb
xue@1:     return -0.8; //y0<yb<=ya
xue@1:   }
xue@1: }//init_Newton_1d_max
xue@1: 
Chris@5: /**
xue@1:   function init_Newton_1d_min: finds an initial x and interval (xa, xb) from which to search for an
xue@1:   manimum.
xue@1: 
xue@1:   On calling xa<x0<xb, and it is assumed that y(x0)<=y(xa), y(x0)<=y(xb). If the conditions on y are met,
xue@1:   it find a new set of xa<x0<xb, so that y(x0)<y(xa), y(x0)<=y(xb), y'(xa)<0, y'(xb)>0, or xb-xa<epx,
xue@1:   and returns xb-xa>0; otherwise it returns a negative value showing the initial order of y(x0), y(xa)
xue@1:   and y(xb) as
xue@1:     -0.5 for ya<=y0<yb
xue@1:     -0.6 for yb<=y0<ya
xue@1:     -0.7 for y0>yb>ya
xue@1:     -0.8 for y0>ya>=yb
xue@1: 
xue@1:   In: function dy(x) that computes y and y' at x
xue@1:       params: additional argument for calling dy
xue@1:       yshift: the byte shift in params where dy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx: tolerable error of extremum
xue@1:   Out: improved initial value x0 and search range (xa, xb)
xue@1: 
xue@1:   Returns xb-xa if successful, a negative value as above if not.
xue@1: */
xue@1: double init_Newton_1d_min(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx)
xue@1: {
xue@1:   double dy0, dya, dyb, y0, ya, yb;
xue@1:   dy0=dy(x0, params); y0=*(double*)((char*)params+yshift);
xue@1:   dya=dy(xa, params); ya=*(double*)((char*)params+yshift);
xue@1:   dyb=dy(xb, params); yb=*(double*)((char*)params+yshift);
xue@1:   if (y0<=ya && y0<=yb)
xue@1:   {
xue@1:     //look for xa<x0<xb, ya>y0<yb, so that dya<0, dyb>0
xue@1:     while ((dya>=0 || dyb<=0) && xb-xa>=epx)
xue@1:     {
xue@1:       double x1=(x0+xa)/2;
xue@1:       double dy1=dy(x1, params);
xue@1:       double y1=*(double*)((char*)params+yshift);
xue@1:       double x2=(x0+xb)/2;
xue@1:       double dy2=dy(x2, params);
xue@1:       double y2=*(double*)((char*)params+yshift);
xue@1:       if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:       else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;}
xue@1:       else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;}
xue@1:     }
xue@1:     return xb-xa;
xue@1:   }
xue@1:   else//meaning y(x0) is not bigger than both y(xa) and y(xb)
xue@1:   {
xue@1:     if (y0<yb && y0>=ya) return -0.5; //ya<=y0<yb
xue@1:     if (y0<ya && y0>=yb) return -0.6; //ya>y0>=yb
xue@1:     if (ya<yb) return -0.7; //ya<yb<y0
xue@1:     return -0.8; //yb<=ya<y0
xue@1:   }
xue@1: }//init_Newton_1d_min
xue@1: 
Chris@5: /**
xue@1:   function Newton_1d_max: Newton method for maximizing y(x). On calling y(x0)>=y(xa), y(x0)>=y(xb),
xue@1:   y'(xa)>0, y'(xb)<0
xue@1: 
xue@1:   In: function ddy(x) that computes y, y' and y" at x
xue@1:       params: additional argument for calling ddy
xue@1:       y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type
xue@1:       yshift: the byte shift in params where ddy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx, epdy: stopping threshold for x and y', respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: x0: the maximum
xue@1: 
xue@1:   Returns the error bound for the maximum x0, or
xue@1:        -2 if y(*) do not satisfy input conditions
xue@1:        -3 if y'(*) do not satisfy input conditions
xue@1: */
xue@1: double Newton_1d_max(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epdy, bool checkinput)
xue@1: {
xue@1:   double x1, y1, x2, y2, dx, dy1, dy2, dy3, x3, y3, ddy1, ddy2, ddy3;
xue@1:   double y0, ya, yb, dy0, dya, dyb, ddy0;
xue@1:   if (xb-xa<epx) {return xb-xa;}
xue@1:   ddy(xa, params);
xue@1:   dya=*(double*)((char*)params+y1shift), ya=*(double*)((char*)params+yshift);
xue@1:   ddy(xb, params);
xue@1:   dyb=*(double*)((char*)params+y1shift), yb=*(double*)((char*)params+yshift);
xue@1:   ddy0=ddy(x0, params);
xue@1:   dy0=*(double*)((char*)params+y1shift), y0=*(double*)((char*)params+yshift);
xue@1: 
xue@1:   if (checkinput)
xue@1:   {
xue@1:     if (y0>=ya && y0>=yb)
xue@1:     {
xue@1:       if (dya<=0 || dyb>=0) return -3;
xue@1:     }
xue@1:     else return -2;
xue@1:   }
xue@1: 
xue@1:   if (dy0==0) return 0;
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (ddy0>=0) //newton method not applicable
xue@1:     {
xue@1:       do
xue@1:       {
xue@1:         x1=(x0+xa)/2;
xue@1:         ddy1=ddy(x1, params);
xue@1:         dy1=*(double*)((char*)params+y1shift), y1=*(double*)((char*)params+yshift);
xue@1:         x2=(x0+xb)/2;
xue@1:         ddy2=ddy(x2, params);
xue@1:         dy2=*(double*)((char*)params+y1shift), y2=*(double*)((char*)params+yshift);
xue@1:         if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:         else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1, ddy0=ddy1;}
xue@1:         else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;}
xue@1:       }
xue@1:       while(ddy0>=0 && (dya<=0 || dyb>=0) && xb-xa>=epx);
xue@1:       if (xb-xa<epx) return xb-xa;
xue@1:     }
xue@1:     if (fabs(dy0)<epdy) return 0;
xue@1:     //x2: Newton point
xue@1:     //dx is the same direction as dy0 if ddy0<0
xue@1:     dx=-dy0/ddy0;
xue@1:     x2=x0+dx;
xue@1:     if (x2==x0) return 0;
xue@1: 
xue@1:     //make sure x2 lies in [xa, xb]
xue@1:     if (x2<xa) {x2=(xa+x0)/2; dx=x2-x0;}
xue@1:     else if (x2>xb) {x2=(xb+x0)/2; dx=x2-x0;}
xue@1:     ddy2=ddy(x2, params);
xue@1:     y2=*(double*)((char*)params+yshift);
xue@1: 
xue@1:     //make sure the iteration yields a larger y
xue@1:     if (y2<y0)
xue@1:     {
xue@1:       while (y2<y0 && dx>epx)
xue@1:       {
xue@1:         dx/=2;
xue@1:         x2=x0+dx;
xue@1:         ddy2=ddy(x2, params);
xue@1:         y2=*(double*)((char*)params+yshift);
xue@1:       }
xue@1:       if (y2<y0) return fabs(dx);
xue@1:     }
xue@1: 
xue@1:     //new x2 lies between xa and xb and y2>=y0
xue@1:     dy2=*(double*)((char*)params+y1shift);
xue@1:     if (fabs(dy2)<epdy){x0=x2; return 0;}
xue@1: 
xue@1:     double fabsdx=fabs(dx);
xue@1:     if (fabsdx<epx)
xue@1:     {
xue@1:       if nsign(dy0, dy2){x0=x2; return fabsdx;}
xue@1:       x3=x2+dx;
xue@1:       ddy3=ddy(x3, params);
xue@1:       dy3=*(double*)((char*)params+y1shift), y3=*(double*)((char*)params+yshift);
xue@1:       if (dy3==0) {x0=x3; return 0;}
xue@1:       if (y2>=y3) {x0=x2; return fabsdx;}
xue@1:       else x2=x3, y2=y3, dy2=dy3, ddy2=ddy3;
xue@1:     }
xue@1: 
xue@1:     if (x0<x2) xa=x0, ya=y0, dya=dy0;
xue@1:     else xb=x0, yb=y0, dyb=dy0;
xue@1:     x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;
xue@1:     iter++;
xue@1:   }
xue@1:   if (fabs(dy0)<epdy) return 0;
xue@1:   else return -1;
xue@1: }//Newton_1d_max
xue@1: 
Chris@5: /**
xue@10:   function Newton_1d_min: Newton method for minimizing y(x). On calling y(x0)<=y(xa), y(x0)<=y(xb),
xue@1:   y'(xa)<0, y'(xb)>0
xue@1: 
xue@1:   In: function ddy(x) that computes y, y' and y" at x
xue@1:       params: additional argument for calling ddy
xue@1:       y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type
xue@1:       yshift: the byte shift in params where ddy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx, epdy: stopping threshold for x and y', respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: x0: the maximum
xue@1: 
xue@1:   Returns the error bound for the maximum x0, or
xue@1:        -2 if y(*) do not satisfy input conditions
xue@1:        -3 if y'(*) do not satisfy input conditions
xue@1: */
xue@1: double Newton_1d_min(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epdy, bool checkinput)
xue@1: {
xue@1:   double x1, y1, x2, y2, dx, dy1, dy2, dy3, x3, y3, ddy1, ddy2, ddy3;
xue@1:   double y0, ya, yb, dy0, dya, dyb, ddy0;
xue@1:   if (xb-xa<epx) {return xb-xa;}
xue@1:   ddy(xa, params);
xue@1:   dya=*(double*)((char*)params+y1shift), ya=*(double*)((char*)params+yshift);
xue@1:   ddy(xb, params);
xue@1:   dyb=*(double*)((char*)params+y1shift), yb=*(double*)((char*)params+yshift);
xue@1:   ddy0=ddy(x0, params);
xue@1:   dy0=*(double*)((char*)params+y1shift), y0=*(double*)((char*)params+yshift);
xue@1: 
xue@1:   if (checkinput)
xue@1:   {
xue@1:     if (y0<=ya && y0<=yb)
xue@1:     {
xue@1:       if (dya>=0 || dyb<=0) return -3;
xue@1:     }
xue@1:     else return -2;
xue@1:   }
xue@1: 
xue@1:   if (dy0==0) return 0;
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (ddy0<=0) //newton method not applicable
xue@1:     {
xue@1:       do
xue@1:       {
xue@1:         x1=(x0+xa)/2;
xue@1:         ddy1=ddy(x1, params);
xue@1:         dy1=*(double*)((char*)params+y1shift), y1=*(double*)((char*)params+yshift);
xue@1:         x2=(x0+xb)/2;
xue@1:         ddy2=ddy(x2, params);
xue@1:         dy2=*(double*)((char*)params+y1shift), y2=*(double*)((char*)params+yshift);
xue@1:         if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2;
xue@1:         else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1, ddy0=ddy1;}
xue@1:         else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;}
xue@1:       }
xue@1:       while((dya>=0 || dyb<=0) && xb-xa>=epx);
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       //Newton point
xue@1:       //dx is the same direction as dy0 if ddy0<0
xue@1:       if (fabs(dy0)<epdy) return 0;
xue@1:       dx=-dy0/ddy0;
xue@1:       x2=x0+dx;
xue@1:       if (x2==x0) return 0;
xue@1:       //make sure x2 lies in [xa, xb]
xue@1:       if (x2<xa) {x2=(xa+x0)/2; dx=x2-x0;}
xue@1:       else if (x2>xb) {x2=(xb+x0)/2; dx=x2-x0;}
xue@1:       ddy2=ddy(x2, params);
xue@1:       y2=*(double*)((char*)params+yshift);
xue@1:       //make sure the iteration yields a larger y
xue@1:       if (y2>y0)
xue@1:       {
xue@1:         while (y2>y0)
xue@1:         {
xue@1:           dx/=2;
xue@1:           x2=x0+dx;
xue@1:           ddy2=ddy(x2, params);
xue@1:           y2=*(double*)((char*)params+yshift);
xue@1:         }
xue@1:         if (x2==x0) return 0; //dy0<0 but all y(x+(ep)dx)<0
xue@1:       }
xue@1:       dy2=*(double*)((char*)params+y1shift);
xue@1:       if (fabs(dy2)<epdy)
xue@1:       {
xue@1:         x0=x2;
xue@1:         return 0;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         double fabsdx=fabs(dx);
xue@1:         if (fabsdx<epx)
xue@1:         {
xue@1:           if nsign(dy2, dy0)
xue@1:           {
xue@1:             x0=x2;
xue@1:             return fabsdx;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             x3=x2+dx;
xue@1:             ddy3=ddy(x3, params);
xue@1:             dy3=*(double*)((char*)params+y1shift), y3=*(double*)((char*)params+yshift);
xue@1:             if (dy3==0) {x0=x3; return 0;}
xue@1:             if nsign(dy2, dy3)
xue@1:             {
xue@1:               x0=x2;
xue@1:               return dx;
xue@1:             }
xue@1:             else if (y3<y2)
xue@1:             {
xue@1:               x2=x3, y2=y3, dy2=dy3, ddy2=ddy3;
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       if (x0<x2) xa=x0, ya=y0, dya=dy0;
xue@1:       else xb=x0, yb=y0, dyb=dy0;
xue@1: 
xue@1:       x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;
xue@1:     }
xue@1: 
xue@1:     iter++;
xue@1:   }
xue@1:   if (fabs(dy0)<epdy) return 0;
xue@1:   else return -1;
xue@1: }//Newton_1d_min
xue@1: 
Chris@5: /**
xue@1:   function Newton1dmax: uses init_Newton_1d_max and Newton_1d_max to search for a local maximum of y.
xue@1: 
xue@1:   By default ddy and dy use the same parameter structure and returns the lower order derivatives at
xue@1:   specific offsets within that structure. However, since the same params is used, it's possible the same
xue@1:   variable may take different offsets when returned from ddy() and dy().
xue@1: 
xue@1:   On calling xa<x0<xb, and it is assumed that y0>=ya, y0>=yb. If the conditions on y are not met, it
xue@1:   returns a negative value showing the order of y0, ya and yb, i.e.
xue@1:     -0.5 for ya<=y0<yb
xue@1:     -0.6 for yb<=y0<ya
xue@1:     -0.7 for y0<ya<yb
xue@1:     -0.8 for y0<yb<=ya
xue@1: 
xue@1:   In: function ddy(x) that computes y, y' and y" at x
xue@1:       function dy(x) that computes y and y' at x
xue@1:       params: additional argument for calling ddy and dy
xue@1:       y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type
xue@1:       yshift: the byte shift in params where ddy(x) put the value of y(x) as double type
xue@1:       dyshift: the byte shift in params where dy(x) put the value of y(x) as double type
xue@1:       x0: inital value of x
xue@1:       (xa, xb): initial search range
xue@1:       epx, epdy: stopping threshold for x and y', respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: x0: the maximum
xue@1: 
xue@1:   Returns the error bound for maximum x0, or a negative value if the initial point does not satisfy
xue@1:   relevent assumptions.
xue@1: */
xue@1: double Newton1dmax(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double (*dy)(double, void*), int dyshift, double epx, int maxiter, double epdy, bool checkinput)
xue@1: {
xue@1:   double ep=init_Newton_1d_max(x0, xa, xb, dy, params, dyshift, epx);
xue@1:   if (ep>epx) ep=Newton_1d_max(x0, xa, xb, ddy, params, y1shift, yshift, epx, maxiter, epdy, true);
xue@1:   return ep;
xue@1: }//Newton1dmax
xue@1: 
xue@1: //function Newton1dmin: same as Newton1dmax, in the other direction.
xue@1: double Newton1dmin(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double (*dy)(double, void*), int dyshift, double epx, int maxiter, double epdy, bool checkinput)
xue@1: {
xue@1:   double ep=init_Newton_1d_min(x0, xa, xb, dy, params, dyshift, epx);
xue@1:   if (ep>epx) ep=Newton_1d_min(x0, xa, xb, ddy, params, y1shift, yshift, epx, maxiter, epdy, true);
xue@1:   return ep;
xue@1: }//Newton1dmin
xue@1: 
Chris@5: /**
xue@1:   function NewtonMax: looks for the maximum of y within [x0, x1] using Newton method.
xue@1: 
xue@1:   In: function ddy(x) that computes y, y' and y" at x
xue@1:       params: additional argument for calling ddy
xue@1:       y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type
xue@1:       yshift: the byte shift in params where ddy(x) put the value of y(x) as double type
xue@1:       (x0, x1): initial search range
xue@1:       epx, epy: stopping threshold for x and y, respectively
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: maximum x0
xue@1: 
xue@1:   Returns an error bound on maximum x0 if successful, a negative value if not.
xue@1: */
xue@1: double NewtonMax(double& x0, double x1, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epy)
xue@1: {
xue@1:   double y0, x2, y2, dx, dy0, dy2, dy3, x3, y3;
xue@1:   dy0=ddy(x0, params);
xue@1:   y0=*(double*)((char*)params+yshift);
xue@1:   if (y0==0) return 0;
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (dy0==0) return -1;
xue@1:     dx=-y0/dy0;
xue@1: 
xue@1:     x2=x0+dx;
xue@1:     dy2=ddy(x2, params);
xue@1:     y2=*(double*)((char*)params+yshift);
xue@1:     //make sure the iteration yields a y closer to 0
xue@1:     while (fabs(y2)>fabs(y0))
xue@1:     {
xue@1:       dx/=2;
xue@1:       x2=x0+dx;
xue@1:       dy2=ddy(x2, params);
xue@1:       y2=*(double*)((char*)params+yshift);
xue@1:     }
xue@1:     //the above may fail due to precision problem, i.e. x0+dx=x0
xue@1:     if (x2==x0)
xue@1:     {
xue@1:       if (fabs(y0)<epy) return 0;
xue@1:       else
xue@1:       {
xue@1:         x2=nextdouble(x0, dx);
xue@1:         dy2=ddy(x2, params);
xue@1:         y2=*(double*)((char*)params+yshift);
xue@1:         if (y2==0)
xue@1:         {
xue@1:           x0=x2;
xue@1:           return 0;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           dx=x2-x0;
xue@1:           while (y2==y0)
xue@1:           {
xue@1:             dx*=2;
xue@1:             x2=x0+dx;
xue@1:             dy2=ddy(x2, params);
xue@1:             y2=*(double*)((char*)params+yshift);
xue@1:           }
xue@1:           if nsign(y2, y0)
xue@1:             return fabs(dx);
xue@1:           else if (fabs(y2)>fabs(y0))
xue@1:             return -1;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     else if (y2==0)
xue@1:     {
xue@1:       x0=x2;
xue@1:       return 0;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       double fabsdx=fabs(dx);
xue@1:       if (fabsdx<epx)
xue@1:       {
xue@1:         if nsign(y2, y0)
xue@1:         {
xue@1:           x0=x2;
xue@1:           return fabsdx;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           x3=x2+dx;
xue@1:           dy3=ddy(x3, params);
xue@1:           y3=*(double*)((char*)params+yshift);
xue@1:           if (y3==0){x0=x3; return 0;}
xue@1:           if nsign(y2, y3)
xue@1:           {
xue@1:             x0=x2;
xue@1:             return dx;
xue@1:           }
xue@1:           else if (fabs(y3)<fabs(y2))
xue@1:           {
xue@1:             x2=x3, y2=y3, dy2=dy3;
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     x0=x2;
xue@1:     y0=y2;
xue@1:     dy0=dy2;
xue@1:     iter++;
xue@1:   }
xue@1:   if (fabs(y0)<epy) return 0;
xue@1:   else return -1;
xue@1: }//NewtonMax
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function rootsearchhalf: searches for the root of y(x) in (x1, x2) by half-split method. On calling
xue@1:   y(x1) and y(x2) must not have the same sign, or -1 is returned signaling a failure.
xue@1: 
xue@1:   In: function y(x),
xue@1:       params: additional arguments for calling y
xue@1:       (x1, x2): inital range
xue@1:       epx: stopping threshold on x
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: root x1
xue@1: 
xue@1:   Returns a positive error estimate if a root is found, -1 if not.
xue@1: */
xue@1: double rootsearchhalf(double&x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter)
xue@1: {
xue@1:   if (x1>x2) {double tmp=x1; x1=x2; x2=tmp;}
xue@1:   double y1=y(x1, params), y3, x3, ep2=epx*2;
xue@1:   if (y1==0) return 0;
xue@1:   else
xue@1:   {
xue@1:     double y2=y(x2, params);
xue@1:     if (y2==0)
xue@1:     {
xue@1:       x1=x2;
xue@1:       return 0;
xue@1:     }
xue@1:     else if nsign(y1, y2) //y1 and y2 have different signs
xue@1:     {
xue@1:       int iter=0;
xue@1:       while (x2-x1>ep2 && iter<maxiter)
xue@1:       {
xue@1:         x3=(x1+x2)/2;
xue@1:         y3=y(x3, params);
xue@1:         if (y3==0)
xue@1:         {
xue@1:           x1=x3;
xue@1:           return 0;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           if nsign(y1, y3) x2=x3, y2=y3;
xue@1:           else x1=x3, y1=y3;
xue@1:         }
xue@1:         iter++;
xue@1:       }
xue@1:       x1=(x1+x2)/2;
xue@1:       return x2-x1;
xue@1:     }
xue@1:     else
xue@1:       return -1;
xue@1:   }
xue@1: }//rootsearchhalf
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function rootsearchsecant: searches for the root of y(x) in (x1, x2) by secant method.
xue@1: 
xue@1:   In: function y(x),
xue@1:       params: additional arguments for calling y
xue@1:       (x1, x2): inital range
xue@1:       epx: stopping threshold on x
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: root x1
xue@1: 
xue@1:   Returns a positive error estimate if a root is found, -1 if not.
xue@1: */
xue@1: double rootsearchsecant(double& x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter)
xue@1: {
xue@1:   double y1=y(x1, params), y2=y(x2, params), y3, x3, dx, x0, y0;
xue@1:   if (y1==0) return 0;
xue@1:   if (y2==0)
xue@1:   {
xue@1:     x1=x2;
xue@1:     return 0;
xue@1:   }
xue@1:   if (fabs(y1)>fabs(y2))
xue@1:   {
xue@1:     x3=x1, y3=y1;
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     x3=x2, y3=y2;
xue@1:     x2=x1, y2=y1;
xue@1:   }
xue@1: 
xue@1:   int iter=0;
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     x1=(y3*x2-y2*x3)/(y3-y2);
xue@1:     y1=y(x1, params);
xue@1: 
xue@1:     dx=fabs(x1-x2);
xue@1:     if (dx<epx)
xue@1:     {
xue@1:       if nsign(y1, y2)
xue@1:       {
xue@1:         return dx;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         x0=2*x1-x2;
xue@1:         y0=y(x0, params);
xue@1:         if nsign(y0, y1)
xue@1:         {
xue@1:           return dx;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     x3=x2, y3=y2;
xue@1:     x2=x1, y2=y1;
xue@1:     iter++;
xue@1:   }
xue@1:   return -1;
xue@1: }//rootsearchsecant
xue@1: 
Chris@5: /**
xue@1:   function rootsearchbisecant: searches for the root of y(x) in (x1, x2) by bi-secant method. On calling
xue@1:   given that y(x1) and y(x2) have different signs. The bound interval (x1, x2) converges to 0, as
xue@1:   compared to standared secant method.
xue@1: 
xue@1:   In: function y(x),
xue@1:       params: additional arguments for calling y
xue@1:       (x1, x2): inital range
xue@1:       epx: stopping threshold on x
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: root x1
xue@1: 
xue@1:   Returns an estimated error bound if a root is found, a negative value if not.
xue@1: */
xue@1: double rootsearchbisecant(double&x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter)
xue@1: {
xue@1:   if (x1>x2) {double tmp=x1; x1=x2; x2=tmp;}
xue@1:   double y1=y(x1, params), y3, y4, dy2, dy3, x3, x4, ep2=epx*2;
xue@1:   if (y1==0) return 0;
xue@1:   else
xue@1:   {
xue@1:     double y2=y(x2, params);
xue@1:     if (y2==0)
xue@1:     {
xue@1:       x1=x2;
xue@1:       return 0;
xue@1:     }
xue@1:     else if nsign(y1, y2) //y1 and y2 have different signs
xue@1:     {
xue@1:       int iter=0;
xue@1:       while (x2-x1>ep2 && iter<maxiter)
xue@1:       {
xue@1:         dy2=y1-y2;
xue@1:         x3=(y1*x2-y2*x1)/dy2;
xue@1:         y3=y(x3, params);
xue@1:         if (y3==0)
xue@1:         {
xue@1:           x1=x3;
xue@1:           return 0;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           if nsign(y1, y3)
xue@1:           {
xue@1:             dy3=y3-y2;
xue@1:             if (dy3==0 || nsign(dy3, dy2)) x2=x3, y2=y3;
xue@1:             else
xue@1:             {
xue@1:               x4=(y3*x2-y2*x3)/dy3; //x4<x3
xue@1:               if (x4<=x1) x2=x3, y2=y3;
xue@1:               else
xue@1:               {
xue@1:                 y4=y(x4, params);
xue@1:                 if (y4==0)
xue@1:                 {
xue@1:                   x1=x4;
xue@1:                   return 0;
xue@1:                 }
xue@1:                 else
xue@1:                 {
xue@1:                   if nsign(y4, y3) x1=x4, y1=y4, x2=x3, y2=y3;
xue@1:                   else x2=x4, y2=y4;
xue@1:                 }
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             dy3=y1-y3;
xue@1:             if (dy3==0 || nsign(dy3, dy2)) x1=x3, y1=y3;
xue@1:             else
xue@1:             {
xue@1:               x4=(y1*x3-y3*x1)/dy3; //x4>x3
xue@1:               if (x4>=x2) x1=x3, y1=y3;
xue@1:               else
xue@1:               {
xue@1:                 y4=y(x4, params);
xue@1:                 if (y4==0)
xue@1:                 {
xue@1:                   x1=x4;
xue@1:                   return 0;
xue@1:                 }
xue@1:                 else
xue@1:                 {
xue@1:                   if nsign(y4, y3) x1=x3, y1=y3, x2=x4, y2=y4;
xue@1:                   else x1=x4, y1=y4;
xue@1:                 }
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:         iter++;
xue@1:       }
xue@1:       x1=(x1+x2)/2;
xue@1:       return x2-x1;
xue@1:     }
xue@1:     else
xue@1:       return -1;
xue@1:   }
xue@1: }//rootsearchbisecant
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Search1Dmax: 1-dimensional maximum search within interval [inf, sup] using 0.618 method
xue@1: 
xue@1:   In: function F(x),
xue@1:       params: additional arguments for calling F
xue@1:       (inf, sup): inital range
xue@1:       epx: stopping threshold on x
xue@1:       maxiter: maximal number of iterates
xue@1:       convcheck: specifies if convexity is checked at each interate, false recommended.
xue@1:   Out: x: the maximum
xue@1:        maxresult: the maximal value, if specified
xue@1: 
xue@1:   Returns an estimated error bound of x if successful, a negative value if not.
xue@1: */
xue@1: double Search1Dmax(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult, double ep, int maxiter, bool convcheck)
xue@1: {
xue@1:   double v0618=(sqrt(5.0)-1)/2, iv0618=1-v0618;
xue@1:   double l=sup-inf;
xue@1:   double startsup=sup, startinf=inf, startl=l;
xue@1:   double x1=inf+l*(1-v0618);
xue@1:   double x2=inf+l*v0618;
xue@1:   double result;
xue@1: 
xue@1:   double v0=F(inf, params);
xue@1:   double v1=F(x1, params);
xue@1:   double v2=F(x2, params);
xue@1:   double v3=F(sup, params);
xue@1:   int iter=0;
xue@1: 
xue@1:   if (v1<v0 && v1<v3 && v2<v0 && v2<v3) goto return1;
xue@1:   if (convcheck && (v1<v0*v0618+v3*iv0618 || v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1: 
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (l<startl*ep) break;
xue@1:     if (v1>v2)
xue@1:     {
xue@1:       sup=x2, v3=v2;
xue@1:       l=sup-inf;
xue@1:       x2=x1, v2=v1;
xue@1:       x1=inf+l*(1-v0618);
xue@1:       v1=F(x1, params);
xue@1:       if (convcheck && (v1<v0*v0618+v3*iv0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       inf=x1, v0=v1;
xue@1:       l=sup-inf;
xue@1:       x1=x2, v1=v2;
xue@1:       x2=inf+l*v0618;
xue@1:       v2=F(x2, params);
xue@1:       if (convcheck && (v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1:     }
xue@1:     iter++;
xue@1:   }
xue@1:   if (v1>v2) {x=x1; if (maxresult) maxresult[0]=v1; result=x1-inf;}
xue@1:   else {x=x2; if (maxresult) maxresult[0]=v2; result=sup-x2;}
xue@1:   return result;
xue@1: 
xue@1: return1: //where at some stage v(.) fails to be convex
xue@1:   if (v0>=v1 && v0>=v2 && v0>=v3){x=inf; if (maxresult) maxresult[0]=v0; result=(x==startinf)?(-1):(sup-x1);}
xue@1:   else if (v1>=v0 && v1>=v2 && v1>=v3){x=x1; if (maxresult) maxresult[0]=v1; result=x1-inf;}
xue@1:   else if (v2>=v0 && v2>=v1 && v2>=v3){x=x2; if (maxresult) maxresult[0]=v2; result=sup-x2;}
xue@1:   else {x=sup; if (maxresult) maxresult[0]=v3; result=(x==startsup)?(-1):(x2-inf);}
xue@1:   return result;
xue@1: }//Search1Dmax
xue@1: 
Chris@5: /**
xue@1:   function Search1DmaxEx: 1-dimensional maximum search using Search1Dmax, but allows setting a $len
xue@1:   argument so that a pre-location of the maximum is performed by sampling (inf, sup) at interval $len,
xue@1:   so that Search1Dmax will work on an interval no longer than 2*len.
xue@1: 
xue@1:   In: function F(x),
xue@1:       params: additional arguments for calling F
xue@1:       (inf, sup): inital range
xue@1:       len: rough sampling interval for pre-locating maximum
xue@1:       epx: stopping threshold on x
xue@1:       maxiter: maximal number of iterates
xue@1:       convcheck: specifies if convexity is checked at each interate, false recommended.
xue@1:   Out: x: the maximum
xue@1:        maxresult: the maximal value, if specified
xue@1: 
xue@1:   Returns an estimated error bound of x if successful, a negative value if not.
xue@1: */
xue@1: double Search1DmaxEx(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult, double ep, int maxiter, double len)
xue@1: {
xue@1:   int c=ceil((sup-inf)/len+1);
xue@1:   if (c<8) c=8;
xue@1:   double* vs=new double[c+1], step=(sup-inf)/c;
xue@1:   for (int i=0; i<=c; i++) vs[i]=F(inf+step*i, params);
xue@1:   int max=0; for (int i=1; i<=c; i++) if (vs[max]<vs[i]) max=i;
xue@1:   *maxresult=vs[max]; delete[] vs;
xue@1:   if (max>0 && max<c) return Search1Dmax(x, params, F, inf+step*(max-1), inf+step*(max+1), maxresult, ep, maxiter);
xue@1:   else if (max==0)
xue@1:   {
xue@1:     if (step<=ep) {x=inf; return ep;}
xue@1:     else return Search1DmaxEx(x, params, F, inf, inf+step, maxresult, ep, maxiter, len);
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     if (step<=ep) {x=sup; return ep;}
xue@1:     else return Search1DmaxEx(x, params, F, sup-step, sup, maxresult, ep, maxiter, len);
xue@1:   }
xue@1: }//Search1DmaxEx
xue@1: 
Chris@5: /**
xue@1:   function Search1Dmaxfrom: 1-dimensional maximum search from start[] towards start+direct[]. The
xue@1:   function tries to locate an interval ("step") for which the 4-point convexity rule confirms the
xue@1:   existence of a convex maximum.
xue@1: 
xue@1:   In: function F(x)
xue@1:       params: additional arguments for calling F
xue@1:       start[dim]: initial x[]
xue@1:       direct[dim]: search direction
xue@1:       step: initial step size
xue@1:       minstep: minimal step size in 4-point scheme between x0 and x3
xue@1:       maxstep: maximal step size in 4-point scheme between x0 and x3
xue@1:   Out: opt[dim]: the maximum, if specified
xue@1:        max: maximal value, if specified
xue@1: 
xue@1:   Returns x>0 that locally maximizes F(start+x*direct)
xue@1:     or x|-x>0 that maximizes F(start-x*direct) at calculated points if convex check fails.
xue@1: */
xue@1: double Search1Dmaxfrom(void* params, double (*F)(int, double*, void*), int dim, double* start, double* direct,
xue@1:   double step, double minstep, double maxstep, double* opt, double* max, double ep, int maxiter, bool convcheck)
xue@1: {
xue@1:   bool record=true;
xue@1:   if (!opt)
xue@1:   {
xue@1:     record=false;
xue@1:     opt=new double[dim];
xue@1:   }
xue@1:   
xue@1:   double v0618=(sqrt(5.0)-1)/2, iv0618=1-v0618;
xue@1:   double x3=step, x0=0, x1=-1, x2=-1, l=x3-x0;
xue@1: 
xue@1:   //prepares x0~x3, v0~v3, x0=0, v2>v3
xue@1:   memcpy(opt, start, sizeof(double)*dim);
xue@1:   double v0=F(dim, opt, params);
xue@1:   MultiAdd(dim, opt, start, direct, x3);
xue@1:   double v1, v2, v3=F(dim, opt, params);
xue@1:   if (v0>=v3)
xue@1:   {
xue@1:     x1=x0+l*iv0618;
xue@1:     MultiAdd(dim, opt, start, direct, x1);
xue@1:     v1=F(dim, opt, params);
xue@1:     x2=x0+l*v0618;
xue@1:     MultiAdd(dim, opt, start, direct, x2);
xue@1:     v2=F(dim, opt, params);
xue@1:     while (l>minstep && v0>v1)
xue@1:     {
xue@1:       x3=x2, v3=v2;
xue@1:       x2=x1, v2=v1;
xue@1:       l=x3-x0;
xue@1:       x1=x0+l*iv0618;
xue@1:       MultiAdd(dim, opt, start, direct, x1);
xue@1:       v1=F(dim, opt, params);
xue@1:     }
xue@1:     if (l<=minstep)
xue@1:     {
xue@1:       if (max) max[0]=v0;
xue@1:       if (!record) delete[] opt;
xue@1:       return x0;
xue@1:     }
xue@1:   }
xue@1:   else//v0<=v3
xue@1:   {
xue@1:     do
xue@1:     {
xue@1:       x1=x2, v1=v2;
xue@1:       x2=x3, v2=v3;
xue@1:       x3+=l*v0618;
xue@1:       l=x3-x0;
xue@1:       MultiAdd(dim, opt, start, direct, x3);
xue@1:       v3=F(dim, opt, params);
xue@1:     } while (v2<v3 && x3-x0<maxstep);
xue@1:     if (l>=maxstep) //increasing all the way to maxstep
xue@1:     {
xue@1:       if (max) max[0]=v3;
xue@1:       if (!record) delete opt;
xue@1:       return x3;
xue@1:     }
xue@1:     else if (x1<0)
xue@1:     {
xue@1:       x1=x0+l*iv0618;
xue@1:       MultiAdd(dim, opt, start, direct, x1);
xue@1:       v1=F(dim, opt, params);
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   double oldl=l;
xue@1: 
xue@1:   int iter=0;
xue@1: 
xue@1:   if (convcheck && (v1<v0*v0618+v3*iv0618 || v2<v0*iv0618+v3*v0618
xue@1:     || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1: 
xue@1:   while (iter<maxiter)
xue@1:   {
xue@1:     if (l<oldl*ep) break;
xue@1:     if (v1>v2)
xue@1:     {
xue@1:       x3=x2, v3=v2;
xue@1:       l=x3-x0;
xue@1:       x2=x1, v2=v1;
xue@1:       x1=x0+l*iv0618;
xue@1:       MultiAdd(dim, opt, start, direct, x1);
xue@1:       v1=F(dim, opt, params);
xue@1:       if (convcheck && (v1<v0*v0618+v3*iv0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       x0=x1, v0=v1;
xue@1:       l=x3-x0;
xue@1:       x1=x2, v1=v2;
xue@1:       x2=x0+l*v0618;
xue@1:       MultiAdd(dim, opt, start, direct, x2);
xue@1:       v2=F(dim, opt, params);
xue@1:       if (convcheck && (v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1;
xue@1:     }
xue@1:     iter++;
xue@1:   }
xue@1: 
xue@1:   if (!record) delete[] opt;
xue@1: 
xue@1:   if (max) max[0]=v1;
xue@1:   return x1;
xue@1: 
xue@1: return1:
xue@1:   if (!record) delete[] opt;
xue@1:   if (v1>v0) v0=v1,x0=x1; if (v2>v0) v0=v2, x0=x2; if (v3>v0) v0=v3, x0=x3;
xue@1:   if (max) max[0]=v0;
xue@1:   return -x0;
xue@1: }//Search1Dmaxfrom
xue@1: 
xue@1: