src/Math/ZeroFinder.cpp

   1 /* Copyright_License {
   2
   3   XCSoar Glide Computer - http://www.xcsoar.org/
   4   Copyright (C) 2000-2013 The XCSoar Project
   5   A detailed list of copyright holders can be found in the file "AUTHORS".
   6
   7   This program is free software; you can redistribute it and/or
   8   modify it under the terms of the GNU General Public License
   9   as published by the Free Software Foundation; either version 2
  10   of the License, or (at your option) any later version.
  11
  12   This program is distributed in the hope that it will be useful,
  13   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15   GNU General Public License for more details.
  16
  17   You should have received a copy of the GNU General Public License
  18   along with this program; if not, write to the Free Software
  19   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  20 }
  21  */
  22 #include "ZeroFinder.hpp"
  23 #include <math.h>
  24 #include <assert.h>
  25 #include <algorithm>
  26 #include <limits>
  27
  28 #ifdef FIXED_MATH
  29 static constexpr fixed epsilon(fixed::internal(), 2);
  30 static const fixed sqrt_epsilon = sqrt(epsilon);
  31 #else
  32 /** machine tolerance */
  33 static constexpr fixed epsilon(std::numeric_limits<fixed>::epsilon());
  34 /** sqrt of machine tolerance */
  35 static const fixed sqrt_epsilon = sqrt(epsilon);
  36 #endif
  37 static const fixed r((3. - sqrt(5.0)) / 2); /* Gold section ratio */
  38
  39 #define fixed_threequaters fixed(0.75)
  40
  41 inline fixed
  42 ZeroFinder::tolerance_actual_min(const fixed x) const
  43 {
  44   return sqrt_epsilon * fabs(x) + tolerance * fixed_third;
  45 }
  46
  47 inline fixed
  48 ZeroFinder::tolerance_actual_zero(const fixed x) const
  49 {
  50   return Double(epsilon * fabs(x)) + Half(tolerance);
  51 }
  52
  53 //#define INSTRUMENT_ZERO
  54 #ifdef INSTRUMENT_ZERO
  55 unsigned long zero_skipped = 0;
  56 unsigned long zero_total = 0;
  57 #endif
  58
  59 inline bool
  60 ZeroFinder::solution_within_tolerance(const fixed x,
  61                                       const fixed tol_act)
  62 {
  63
  64   // are we away from the edges? if so, check improved solution
  65   const fixed x_minus = x-tol_act;
  66   if (xmin >= x_minus)
  67     return false;
  68   const fixed x_plus = x+tol_act;
  69   if (x_plus >= xmax)
  70     return false;
  71
  72   const fixed fx = f(x);
  73   if (f(x_plus)<fx)
  74     return false;
  75   if (f(x_minus)<fx)
  76     return false;
  77   // existing solution is good
  78   return true;
  79 }
  80
  81 /*
  82   Note:
  83   - When you can, use find_zero search, since it narrows in at rate of 1.6 per step compared
  84     to 1.3 for the find_min.
  85 */
  86
  87 /*
  88  ************************************************************************
  89  *                          C math library
  90  * function ZEROIN - obtain a function zero within the given range
  91  *
  92  * Input
  93  *      fixed zeroin(ax,bx,f,tol)
  94  *      fixed ax;                       Root will be seeked for within
  95  *      fixed bx;                       a range [ax,bx]
  96  *      fixed (*f)(fixed x);            Name of the function whose zero
  97  *                                      will be seeked for
  98  *      fixed tol;                      Acceptable tolerance for the root
  99  *                                      value.
 100  *                                      May be specified as 0.0 to cause
 101  *                                      the program to find the root as
 102  *                                      accurate as possible
 103  *
 104  * Output
 105  *      Zeroin returns an estimate for the root with accuracy
 106  *      4*epsilon*abs(x) + tol
 107  *
 108  * Algorithm
 109  *      G.Forsythe, M.Malcolm, C.Moler, Computer methods for mathematical
 110  *      computations. M., Mir, 1980, p.180 of the Russian edition
 111  *
 112  *      The function makes use of the bissection procedure combined with
 113  *      the linear or quadric inverse interpolation.
 114  *      At every step program operates on three abscissae - a, b, and c.
 115  *      b - the last and the best approximation to the root
 116  *      a - the last but one approximation
 117  *      c - the last but one or even earlier approximation than a that
 118  *              1) |f(b)| <= |f(c)|
 119  *              2) f(b) and f(c) have opposite signs, i.e. b and c confine
 120  *                 the root
 121  *      At every step Zeroin selects one of the two new approximations, the
 122  *      former being obtained by the bissection procedure and the latter
 123  *      resulting in the interpolation (if a,b, and c are all different
 124  *      the quadric interpolation is utilized, otherwise the linear one).
 125  *      If the latter (i.e. obtained by the interpolation) point is
 126  *      reasonable (i.e. lies within the current interval [b,c] not being
 127  *      too close to the boundaries) it is accepted. The bissection result
 128  *      is used in the other case. Therefore, the range of uncertainty is
 129  *      ensured to be reduced at least by the factor 1.6
 130  *
 131  ************************************************************************
 132  */
 133
 134 fixed ZeroFinder::find_zero(const fixed xstart) {
 135 #ifdef INSTRUMENT_ZERO
 136   zero_total++;
 137 #endif
 138   if ((xmin<=xstart) || (xstart<=xmax) ||
 139       (f(xstart)> sqrt_epsilon))
 140     return find_zero_actual(xstart);
 141 #ifdef INSTRUMENT_ZERO
 142   zero_skipped++;
 143 #endif
 144   return xstart;
 145 }
 146
 147 inline fixed
 148 ZeroFinder::find_zero_actual(const fixed xstart)
 149 {
 150   fixed a, b, c; // Abscissae, descr. see above
 151   fixed fa; // f(a)
 152   fixed fb; // f(b)
 153   fixed fc; // f(c)
 154
 155   bool b_best = true; // b is best and last called
 156
 157   c = a = xmin;
 158   fc = fa = f(a);
 159
 160   b = xmax;
 161   fb = f(b);
 162
 163   // Main iteration loop
 164   for (;;) {
 165     // Distance from the last but one to the last approximation
 166     fixed prev_step = b - a;
 167
 168     if (fabs(fc) < fabs(fb)) {
 169       // Swap data for b to be the best approximation
 170       a = b;
 171       b = c;
 172       c = a;
 173
 174       fa = fb;
 175       fb = fc;
 176       fc = fa;
 177
 178       b_best = false;
 179     } else {
 180       b_best = true;
 181     }
 182
 183     // Actual tolerance
 184     const fixed tol_act = tolerance_actual_zero(b);
 185
 186     // Step at this iteration
 187     fixed new_step = Half(c - b);
 188
 189     if (fabs(new_step) <= tol_act || fabs(fb) < sqrt_epsilon) {
 190       if (!b_best)
 191         // call once more
 192         fb = f(b);
 193
 194       // Acceptable approx. is found
 195       return b;
 196     }
 197
 198     // Decide if the interpolation can be tried
 199
 200     // If prev_step was large enough and was in true direction,
 201     // interpolation may be tried
 202     if (fabs(prev_step) >= tol_act && fabs(fa) > fabs(fb)) {
 203       // Interpolation step is calculated in the form p/q;
 204       // division operations is delayed until the last moment
 205       fixed p;
 206       fixed q;
 207
 208       const fixed cb = c - b;
 209       // If we have only two distinct points
 210       // -> linear interpolation can only be applied
 211       if (a == c) {
 212         const fixed t1 = fb / fa;
 213         p = cb * t1;
 214         q = fixed(1) - t1;
 215       } else {
 216         // Quadric inverse interpolation
 217         q = fa / fc;
 218         const fixed t1 = fb / fc;
 219         const fixed t2 = fb / fa;
 220         p = t2 * (cb * q * (q - t1) - (b - a) * (t1 - fixed(1)));
 221         q = (q - fixed(1)) * (t1 - fixed(1)) * (t2 - fixed(1));
 222       }
 223
 224       // p was calculated with the opposite sign;
 225       // make p positive and assign possible minus to q
 226       if (positive(p))
 227         q = -q;
 228       else
 229         p = -p;
 230
 231       // If b+p/q falls in [b,c] and isn't too large it is accepted
 232       // If p/q is too large then the bissection procedure can
 233       // reduce [b,c] range to more extent
 234       if (p < (fixed_threequaters * cb * q - Half(fabs(tol_act * q)))
 235           && p < fabs(Half(prev_step * q)))
 236         new_step = p / q;
 237     }
 238
 239     // Adjust the step to be not less than tolerance
 240     limit_tolerance(new_step, tol_act);
 241
 242     // Save the previous approx.
 243     a = b;
 244     fa = fb;
 245
 246     // Do step to a new approxim.
 247     b += new_step;
 248     fb = f(b);
 249
 250     // Adjust c for it to have a sign opposite to that of b
 251     if ((positive(fb) && positive(fc)) || (negative(fb) && negative(fc))) {
 252       c = a;
 253       fc = fa;
 254     }
 255
 256     assert(b >= xmin);
 257     assert(b <= xmax);
 258   }
 259 }
 260
 261
 262 /*
 263  ************************************************************************
 264  *                          C math library
 265  * function FMINBR - one-dimensional search for a function minimum
 266  *                        over the given range
 267  *
 268  * Input
 269  *      fixed fminbr(a,b,f,tolerance)
 270  *      fixed a;                        Minimum will be seeked for over
 271  *      fixed b;                        a range [a,b], a being < b.
 272  *      fixed (*f)(fixed x);            Name of the function whose minimum
 273  *                                      will be seeked for
 274  *      fixed tolerance;                        Acceptable tolerance for the minimum
 275  *                                      location. It have to be positive
 276  *                                      (e.g. may be specified as epsilon)
 277  *
 278  * Output
 279  *      Fminbr returns an estimate for the minimum location with accuracy
 280  *      3*sqrt_epsilon*abs(x) + tolerance.
 281  *      The function always obtains a local minimum which coincides with
 282  *      the global one only if a function under investigation being
 283  *      unimodular.
 284  *      If a function being examined possesses no local minimum within
 285  *      the given range, Fminbr returns 'a' (if f(a) < f(b)), otherwise
 286  *      it returns the right range boundary value b.
 287  *
 288  * Algorithm
 289  *      G.Forsythe, M.Malcolm, C.Moler, Computer methods for mathematical
 290  *      computations. M., Mir, 1980, p.202 of the Russian edition
 291  *
 292  *      The function makes use of the "gold section" procedure combined with
 293  *      the parabolic interpolation.
 294  *      At every step program operates three abscissae - x,v, and w.
 295  *      x - the last and the best approximation to the minimum location,
 296  *          i.e. f(x) <= f(a) or/and f(x) <= f(b)
 297  *          (if the function f has a local minimum in (a,b), then the both
 298  *          conditions are fulfiled after one or two steps).
 299  *      v,w are previous approximations to the minimum location. They may
 300  *      coincide with a, b, or x (although the algorithm tries to make all
 301  *      u, v, and w distinct). Points x, v, and w are used to construct
 302  *      interpolating parabola whose minimum will be treated as a new
 303  *      approximation to the minimum location if the former falls within
 304  *      [a,b] and reduces the range enveloping minimum more efficient than
 305  *      the gold section procedure.
 306  *      When f(x) has a second derivative positive at the minimum location
 307  *      (not coinciding with a or b) the procedure converges superlinearly
 308  *      at a rate order about 1.324
 309  *
 310  ************************************************************************
 311  */
 312
 313 fixed ZeroFinder::find_min(const fixed xstart) {
 314 #ifdef INSTRUMENT_ZERO
 315   zero_total++;
 316 #endif
 317   if (!solution_within_tolerance(xstart, tolerance_actual_min(xstart)))
 318     return find_min_actual(xstart);
 319 #ifdef INSTRUMENT_ZERO
 320   zero_skipped++;
 321 #endif
 322   return xstart;
 323 }
 324
 325 inline fixed
 326 ZeroFinder::find_min_actual(const fixed xstart)
 327 {
 328   fixed x, v, w; // Abscissae, descr. see above
 329   fixed fx; // f(x)
 330   fixed fv; // f(v)
 331   fixed fw; // f(w)
 332   fixed a = xmin;
 333   fixed b = xmax;
 334   bool x_best = true;
 335
 336   assert(positive(tolerance) && b > a);
 337
 338   /* First step - always gold section*/
 339   x = w = v = a + r * (b - a);
 340   fx = fw = fv = f(v);
 341
 342   // Main iteration loop
 343   for (;;) {
 344     // Range over which the minimum is seeked for
 345     const fixed range = b - a;
 346     const fixed middle_range = Half(a + b);
 347
 348     // Actual tolerance
 349     const fixed tol_act = tolerance_actual_min(x);
 350     const fixed double_tol_act = Double(tol_act);
 351
 352     if (fabs(x-middle_range) + Half(range) <= double_tol_act) {
 353       if (!x_best)
 354         // call once more
 355         fx = f(x);
 356
 357       // Acceptable approx. is found
 358       return x;
 359     }
 360
 361     // Step at this iteration
 362     // Obtain the gold section step
 363     fixed new_step = r * (x < middle_range ? b - x : a - x);
 364
 365     // Decide if the interpolation can be tried
 366
 367     // If x and w are distinct
 368     // interpolation may be tried
 369     if (fabs(x - w) >= tol_act) {
 370       // Interpolation step is calculated as p/q;
 371       // division operation is delayed until last moment
 372       fixed p;
 373       fixed q;
 374
 375       const fixed t = (x - w) * (fx - fv);
 376       q = (x - v) * (fx - fw);
 377       p = (x - v) * q - (x - w) * t;
 378       q = Double(q - t);
 379
 380       // q was calculated with the opposite sign;
 381       // make q positive and assign possible minus to p
 382       if (positive(q))
 383         p = -p;
 384       else
 385         q = -q;
 386
 387       // If x+p/q falls in [a,b] not too close to a and b,
 388       // and isn't too large it is accepted
 389       // If p/q is too large then the gold section procedure can
 390       // reduce [a,b] range to more extent
 391       if (fabs(p) < fabs(new_step * q) && p > q * (a - x + double_tol_act)
 392           && p < q * (b - x - double_tol_act))
 393         new_step = p / q;
 394     }
 395
 396     // Adjust the step to be not less than tolerance
 397     limit_tolerance(new_step, tol_act);
 398
 399     // Obtain the next approximation to min and reduce the enveloping range
 400     {
 401       // Tentative point for the min
 402       const fixed t = x + new_step;
 403       const fixed ft = f(t);
 404       // t is a better approximation
 405       if (ft <= fx) {
 406         // Reduce the range so that t would fall within it
 407         (t < x ? b : a) = x;
 408
 409         // Assign the best approx to x
 410         v = w;
 411         w = x;
 412         x = t;
 413
 414         fv = fw;
 415         fw = fx;
 416         fx = ft;
 417         x_best = false;
 418       } else {
 419         // x remains the better approx
 420         x_best = true;
 421         // Reduce the range enclosing x
 422         (t < x ? a : b) = t;
 423
 424         if ((ft <= fw) || (w == x)) {
 425           v = w;
 426           w = t;
 427           fv = fw;
 428           fw = ft;
 429         } else if ((ft <= fv) || (v == x) || (v == w)) {
 430           v = t;
 431           fv = ft;
 432         }
 433       }
 434     }
 435   }
 436 }