AstroToolbox/ZeroFinder.h

   1 // ------------------------------------------------------------------------ //
   2 // This source file is part of the 'ESA Advanced Concepts Team's                        //
   3 // Space Mechanics Toolbox' software.                                       //
   4 //                                                                          //
   5 // The source files are for research use only,                              //
   6 // and are distributed WITHOUT ANY WARRANTY. Use them on your own risk.     //
   7 //                                                                          //
   8 // Copyright (c) 2004-2007 European Space Agency                            //
   9 // ------------------------------------------------------------------------ //
  10
  11 #ifndef ZEROFINDER_H
  12 #define ZEROFINDER_H
  13
  14 #define NUMERATOR(dab,dcb,fa,fb,fc) fb*(dab*fc*(fc-fb)-fa*dcb*(fa-fb))
  15 #define DENOMINATOR(fa,fb,fc) (fc-fb)*(fa-fb)*(fa-fc)
  16
  17 /// Namespace
  18 namespace ZeroFinder
  19 {
  20
  21         /// Class for one dimensional functions
  22         //  with some parameters
  23         /**  The ()-operator with one double argument
  24          *  has to be overloaded for a derived class
  25          *  The return value is the ordinate computed for
  26          *  the abscissa-argument.
  27          */
  28         class Function1D
  29         {
  30         public:
  31         Function1D(const double &a, const double &b):p1(a),p2(b) {}
  32                 // parameters
  33                 const double p1,p2;
  34         };
  35
  36         class Function1D_7param
  37         {
  38         public:
  39         Function1D_7param(const double &a, const double &b, const double &c,
  40                           const double &d, const double &e, const double &f,
  41                           const double &g):p1(a),p2(b),p3(c),p4(d),p5(e),p6(f),p7(g) {}
  42                 // parameters
  43                 const double p1,p2,p3,p4,p5,p6,p7;
  44         };
  45
  46
  47         class FZero
  48         {
  49                 private:
  50                         double a, c; // lower and upper bounds
  51
  52                 public:
  53                         FZero(const double &a_, const double &c_):a(a_),c(c_) {} // constructor
  54                         // fzero procedure
  55                         template <class Functor>
  56                         double FindZero(const Functor &f);
  57         };
  58
  59         //-------------------------------------------------------------------------------------//
  60         // This part is an adaption of the 'Amsterdam method', which is an inversre quadratic  //
  61         // interpolation - bisection method                                                                                                        //
  62         // See http://mymathlib.webtrellis.net/roots/amsterdam.html                                                        //
  63         //
  64         //-------------------------------------------------------------------------------------//
  65         template <class Functor>
  66         double FZero::FindZero(const Functor &f)
  67         {
  68                 static const int max_iterations = 500;
  69                 static const double tolerance = 1e-15;
  70
  71                 double fa = f(a), b = 0.5 * ( a + c ), fc = f(c), fb = fa * fc;
  72                 double delta, dab, dcb;
  73                 int i;
  74
  75               // If the initial estimates do not bracket a root, set the err flag and //
  76               // return.  If an initial estimate is a root, then return the root.     //
  77
  78             double err = 0;
  79                 if ( fb >= 0.0 )
  80                         if ( fb > 0.0 )  { err = -1; return 0.0; }
  81                         else return ( fa == 0.0 ) ? a : c;
  82
  83               // Insure that the initial estimate a < c. //
  84
  85                 if ( a > c ) {
  86                         delta = a; a = c; c = delta; delta = fa; fa = fc; fc = delta;
  87                 }
  88
  89               // If the function at the left endpoint is positive, and the function //
  90               // at the right endpoint is negative.  Iterate reducing the length    //
  91               // of the interval by either bisection or quadratic inverse           //
  92               // interpolation.  Note that the function at the left endpoint        //
  93               // remains nonnegative and the function at the right endpoint remains //
  94               // nonpositive.                                                       //
  95
  96                 if ( fa > 0.0 )
  97                         for ( i = 0; i < max_iterations; i++) {
  98
  99                     // Are the two endpoints within the user specified tolerance ?
 100
 101                                 if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
 102
 103                                         // No, Continue iteration.
 104
 105                                 fb = f(b);
 106
 107                     // Check that we are converging or that we have converged near //
 108                     // the left endpoint of the inverval.  If it appears that the  //
 109                     // interval is not decreasing fast enough, use bisection.      //
 110                                 if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
 111                                 if ( ( b - a ) < tolerance )
 112                                         if ( fb > 0 ) {
 113                                                 a = b; fa = fb; b = 0.5 * ( a + c ); continue;
 114                                         }
 115                                         else return b;
 116
 117                     // Check that we are converging or that we have converged near //
 118                     // the right endpoint of the inverval.  If it appears that the //
 119                     // interval is not decreasing fast enough, use bisection.      //
 120
 121                                 if ( ( c - b ) < tolerance )
 122                                         if ( fb < 0 ) {
 123                                                 c = b; fc = fb; b = 0.5 * ( a + c ); continue;
 124                                         }
 125                                         else return b;
 126
 127                     // If quadratic inverse interpolation is feasible, try it. //
 128
 129                                 if (  ( fa > fb ) && ( fb > fc ) ) {
 130                                         delta = DENOMINATOR(fa,fb,fc);
 131                                         if ( delta != 0.0 ) {
 132                                                 dab = a - b;
 133                                                 dcb = c - b;
 134                                                 delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
 135
 136                                                 // Will the new estimate of the root be within the   //
 137                                                 // interval?  If yes, use it and update interval.    //
 138                                                 // If no, use the bisection method.                  //
 139
 140                                                 if ( delta > dab && delta < dcb ) {
 141                                                         if ( fb > 0.0 ) { a = b; fa = fb; }
 142                                                         else if ( fb < 0.0 )  { c = b; fc = fb; }
 143                                                         else return b;
 144                                                         b += delta;
 145                                                         continue;
 146                                                 }
 147                                         }
 148                                 }
 149
 150                     // If not, use the bisection method. //
 151
 152                                 fb > 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
 153                                 b = 0.5 * ( a + c );
 154                         }
 155                         else
 156
 157                         // If the function at the left endpoint is negative, and the function //
 158                         // at the right endpoint is positive.  Iterate reducing the length    //
 159                         // of the interval by either bisection or quadratic inverse           //
 160                         // interpolation.  Note that the function at the left endpoint        //
 161                         // remains nonpositive and the function at the right endpoint remains //
 162                         // nonnegative.                                                       //
 163
 164                         for ( i = 0; i < max_iterations; i++) {
 165                                 if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
 166                                 fb = f(b);
 167
 168                                 if ( ( b - a ) < tolerance )
 169                     if ( fb < 0 ) {
 170                                         a = b; fa = fb; b = 0.5 * ( a + c ); continue;
 171                     }
 172                     else return b;
 173
 174                                 if ( ( c - b ) < tolerance )
 175                                         if ( fb > 0 ) {
 176                                                 c = b; fc = fb; b = 0.5 * ( a + c ); continue;
 177                                         }
 178                                         else return b;
 179
 180                                 if (  ( fa < fb ) && ( fb < fc ) ) {
 181                                         delta = DENOMINATOR(fa,fb,fc);
 182                                         if ( delta != 0.0 ) {
 183                                                 dab = a - b;
 184                                                 dcb = c - b;
 185                                                 delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
 186                                                 if ( delta > dab && delta < dcb ) {
 187                                                         if ( fb < 0.0 ) { a = b; fa = fb; }
 188                                                         else if ( fb > 0.0 )  { c = b; fc = fb; }
 189                                                         else return b;
 190                                                         b += delta;
 191                                                         continue;
 192                                                 }
 193                                         }
 194                                 }
 195                                 fb < 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
 196                                 b = 0.5 * ( a + c );
 197                         }
 198                         err = -2;
 199                         return  b;
 200         }
 201 }
 202
 203 #undef NUMERATOR
 204 #undef DENOMINATOR
 205
 206 #endif