PaGMO  1.1.5
ZeroFinder.h
1 /*****************************************************************************
2  * Copyright (C) 2004-2015 The PaGMO development team, *
3  * Advanced Concepts Team (ACT), European Space Agency (ESA) *
4  * *
5  * https://github.com/esa/pagmo *
6  * *
7  * act@esa.int *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the *
21  * Free Software Foundation, Inc., *
22  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
23  *****************************************************************************/
24 
25 #ifndef ZEROFINDER_H
26 #define ZEROFINDER_H
27 
28 #define NUMERATOR(dab,dcb,fa,fb,fc) fb*(dab*fc*(fc-fb)-fa*dcb*(fa-fb))
29 #define DENOMINATOR(fa,fb,fc) (fc-fb)*(fa-fb)*(fa-fc)
30 
31 
32 namespace ZeroFinder
33 {
34 
36  // with some parameters
37  /* The ()-operator with one double argument
38  * has to be overloaded for a derived class
39  * The return value is the ordinate computed for
40  * the abscissa-argument.
41  */
42  class Function1D
43  {
44  public:
45  Function1D(const double &a, const double &b):p1(a),p2(b) {}
46  // parameters
47  const double p1,p2;
48  };
49 
50  class Function1D_7param
51  {
52  public:
53  Function1D_7param(const double &a, const double &b, const double &c,
54  const double &d, const double &e, const double &f,
55  const double &g):p1(a),p2(b),p3(c),p4(d),p5(e),p6(f),p7(g) {}
56  // parameters
57  const double p1,p2,p3,p4,p5,p6,p7;
58  };
59 
60 
61  class FZero
62  {
63  private:
64  double a, c; // lower and upper bounds
65 
66  public:
67  FZero(const double &a_, const double &c_):a(a_),c(c_) {} // constructor
68  // fzero procedure
69  template <class Functor>
70  double FindZero(const Functor &f);
71  };
72 
73  //-------------------------------------------------------------------------------------//
74  // This part is an adaption of the 'Amsterdam method', which is an inversre quadratic //
75  // interpolation - bisection method //
76  // See http://mymathlib.webtrellis.net/roots/amsterdam.html //
77  //
78  //-------------------------------------------------------------------------------------//
79  template <class Functor>
80  double FZero::FindZero(const Functor &f)
81  {
82  static const int max_iterations = 500;
83  static const double tolerance = 1e-15;
84 
85  double fa = f(a), b = 0.5 * ( a + c ), fc = f(c), fb = fa * fc;
86  double delta, dab, dcb;
87  int i;
88 
89  // If the initial estimates do not bracket a root, set the err flag and //
90  // return. If an initial estimate is a root, then return the root. //
91 
92  if ( fb >= 0.0 ) {
93  if ( fb > 0.0 ) { return 0.0; }
94  else {return ( fa == 0.0 ) ? a : c;}
95  }
96 
97  // Insure that the initial estimate a < c. //
98 
99  if ( a > c ) {
100  delta = a; a = c; c = delta; delta = fa; fa = fc; fc = delta;
101  }
102 
103  // If the function at the left endpoint is positive, and the function //
104  // at the right endpoint is negative. Iterate reducing the length //
105  // of the interval by either bisection or quadratic inverse //
106  // interpolation. Note that the function at the left endpoint //
107  // remains nonnegative and the function at the right endpoint remains //
108  // nonpositive. //
109 
110  if ( fa > 0.0 )
111  for ( i = 0; i < max_iterations; i++) {
112 
113  // Are the two endpoints within the user specified tolerance ?
114 
115  if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
116 
117  // No, Continue iteration.
118 
119  fb = f(b);
120 
121  // Check that we are converging or that we have converged near //
122  // the left endpoint of the inverval. If it appears that the //
123  // interval is not decreasing fast enough, use bisection. //
124  if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
125  if ( ( b - a ) < tolerance ) {
126  if ( fb > 0 ) {
127  a = b; fa = fb; b = 0.5 * ( a + c ); continue;
128  }
129  else {
130  return b;
131  }
132  }
133 
134  // Check that we are converging or that we have converged near //
135  // the right endpoint of the inverval. If it appears that the //
136  // interval is not decreasing fast enough, use bisection. //
137 
138  if ( ( c - b ) < tolerance ) {
139  if ( fb < 0 ) {
140  c = b; fc = fb; b = 0.5 * ( a + c ); continue;
141  }
142  else {
143  return b;
144  }
145  }
146 
147  // If quadratic inverse interpolation is feasible, try it. //
148 
149  if ( ( fa > fb ) && ( fb > fc ) ) {
150  delta = DENOMINATOR(fa,fb,fc);
151  if ( delta != 0.0 ) {
152  dab = a - b;
153  dcb = c - b;
154  delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
155 
156  // Will the new estimate of the root be within the //
157  // interval? If yes, use it and update interval. //
158  // If no, use the bisection method. //
159 
160  if ( delta > dab && delta < dcb ) {
161  if ( fb > 0.0 ) { a = b; fa = fb; }
162  else if ( fb < 0.0 ) { c = b; fc = fb; }
163  else return b;
164  b += delta;
165  continue;
166  }
167  }
168  }
169 
170  // If not, use the bisection method. //
171 
172  fb > 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
173  b = 0.5 * ( a + c );
174  }
175  else
176 
177  // If the function at the left endpoint is negative, and the function //
178  // at the right endpoint is positive. Iterate reducing the length //
179  // of the interval by either bisection or quadratic inverse //
180  // interpolation. Note that the function at the left endpoint //
181  // remains nonpositive and the function at the right endpoint remains //
182  // nonnegative. //
183 
184  for ( i = 0; i < max_iterations; i++) {
185  if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
186  fb = f(b);
187 
188  if ( ( b - a ) < tolerance ) {
189  if ( fb < 0 ) {
190  a = b; fa = fb; b = 0.5 * ( a + c ); continue;
191  } else {
192  return b;
193  }
194  }
195 
196  if ( ( c - b ) < tolerance ) {
197  if ( fb > 0 ) {
198  c = b; fc = fb; b = 0.5 * ( a + c ); continue;
199  } else {
200  return b;
201  }
202  }
203 
204  if ( ( fa < fb ) && ( fb < fc ) ) {
205  delta = DENOMINATOR(fa,fb,fc);
206  if ( delta != 0.0 ) {
207  dab = a - b;
208  dcb = c - b;
209  delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
210  if ( delta > dab && delta < dcb ) {
211  if ( fb < 0.0 ) { a = b; fa = fb; }
212  else if ( fb > 0.0 ) { c = b; fc = fb; }
213  else return b;
214  b += delta;
215  continue;
216  }
217  }
218  }
219  fb < 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
220  b = 0.5 * ( a + c );
221  }
222  return b;
223  }
224 }
225 
226 #undef NUMERATOR
227 #undef DENOMINATOR
228 
229 #endif
Generated by   doxygen 1.8.9.1