boost/math/tools/toms748_solve.hpp

   1 //  (C) Copyright John Maddock 2006.
   2 //  Use, modification and distribution are subject to the
   3 //  Boost Software License, Version 1.0. (See accompanying file
   4 //  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
   5
   6 #ifndef BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
   7 #define BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
   8
   9 #ifdef _MSC_VER
  10 #pragma once
  11 #endif
  12
  13 #include <boost/math/tools/precision.hpp>
  14 #include <boost/math/policies/error_handling.hpp>
  15 #include <boost/math/tools/config.hpp>
  16 #include <boost/math/special_functions/sign.hpp>
  17 #include <boost/cstdint.hpp>
  18 #include <limits>
  19
  20 namespace boost{ namespace math{ namespace tools{
  21
  22 template <class T>
  23 class eps_tolerance
  24 {
  25 public:
  26    eps_tolerance(unsigned bits)
  27    {
  28       BOOST_MATH_STD_USING
  29       eps = (std::max)(T(ldexp(1.0F, 1-bits)), T(2 * tools::epsilon<T>()));
  30    }
  31    bool operator()(const T& a, const T& b)
  32    {
  33       BOOST_MATH_STD_USING
  34       return (fabs(a - b) / (std::min)(fabs(a), fabs(b))) <= eps;
  35    }
  36 private:
  37    T eps;
  38 };
  39
  40 struct equal_floor
  41 {
  42    equal_floor(){}
  43    template <class T>
  44    bool operator()(const T& a, const T& b)
  45    {
  46       BOOST_MATH_STD_USING
  47       return floor(a) == floor(b);
  48    }
  49 };
  50
  51 struct equal_ceil
  52 {
  53    equal_ceil(){}
  54    template <class T>
  55    bool operator()(const T& a, const T& b)
  56    {
  57       BOOST_MATH_STD_USING
  58       return ceil(a) == ceil(b);
  59    }
  60 };
  61
  62 struct equal_nearest_integer
  63 {
  64    equal_nearest_integer(){}
  65    template <class T>
  66    bool operator()(const T& a, const T& b)
  67    {
  68       BOOST_MATH_STD_USING
  69       return floor(a + 0.5f) == floor(b + 0.5f);
  70    }
  71 };
  72
  73 namespace detail{
  74
  75 template <class F, class T>
  76 void bracket(F f, T& a, T& b, T c, T& fa, T& fb, T& d, T& fd)
  77 {
  78    //
  79    // Given a point c inside the existing enclosing interval
  80    // [a, b] sets a = c if f(c) == 0, otherwise finds the new
  81    // enclosing interval: either [a, c] or [c, b] and sets
  82    // d and fd to the point that has just been removed from
  83    // the interval.  In other words d is the third best guess
  84    // to the root.
  85    //
  86    BOOST_MATH_STD_USING  // For ADL of std math functions
  87    T tol = tools::epsilon<T>() * 2;
  88    //
  89    // If the interval [a,b] is very small, or if c is too close
  90    // to one end of the interval then we need to adjust the
  91    // location of c accordingly:
  92    //
  93    if((b - a) < 2 * tol * a)
  94    {
  95       c = a + (b - a) / 2;
  96    }
  97    else if(c <= a + fabs(a) * tol)
  98    {
  99       c = a + fabs(a) * tol;
 100    }
 101    else if(c >= b - fabs(b) * tol)
 102    {
 103       c = b - fabs(a) * tol;
 104    }
 105    //
 106    // OK, lets invoke f(c):
 107    //
 108    T fc = f(c);
 109    //
 110    // if we have a zero then we have an exact solution to the root:
 111    //
 112    if(fc == 0)
 113    {
 114       a = c;
 115       fa = 0;
 116       d = 0;
 117       fd = 0;
 118       return;
 119    }
 120    //
 121    // Non-zero fc, update the interval:
 122    //
 123    if(boost::math::sign(fa) * boost::math::sign(fc) < 0)
 124    {
 125       d = b;
 126       fd = fb;
 127       b = c;
 128       fb = fc;
 129    }
 130    else
 131    {
 132       d = a;
 133       fd = fa;
 134       a = c;
 135       fa= fc;
 136    }
 137 }
 138
 139 template <class T>
 140 inline T safe_div(T num, T denom, T r)
 141 {
 142    //
 143    // return num / denom without overflow,
 144    // return r if overflow would occur.
 145    //
 146    BOOST_MATH_STD_USING  // For ADL of std math functions
 147
 148    if(fabs(denom) < 1)
 149    {
 150       if(fabs(denom * tools::max_value<T>()) <= fabs(num))
 151          return r;
 152    }
 153    return num / denom;
 154 }
 155
 156 template <class T>
 157 inline T secant_interpolate(const T& a, const T& b, const T& fa, const T& fb)
 158 {
 159    //
 160    // Performs standard secant interpolation of [a,b] given
 161    // function evaluations f(a) and f(b).  Performs a bisection
 162    // if secant interpolation would leave us very close to either
 163    // a or b.  Rationale: we only call this function when at least
 164    // one other form of interpolation has already failed, so we know
 165    // that the function is unlikely to be smooth with a root very
 166    // close to a or b.
 167    //
 168    BOOST_MATH_STD_USING  // For ADL of std math functions
 169
 170    T tol = tools::epsilon<T>() * 5;
 171    T c = a - (fa / (fb - fa)) * (b - a);
 172    if((c <= a + fabs(a) * tol) || (c >= b - fabs(b) * tol))
 173       return (a + b) / 2;
 174    return c;
 175 }
 176
 177 template <class T>
 178 T quadratic_interpolate(const T& a, const T& b, T const& d,
 179                            const T& fa, const T& fb, T const& fd,
 180                            unsigned count)
 181 {
 182    //
 183    // Performs quadratic interpolation to determine the next point,
 184    // takes count Newton steps to find the location of the
 185    // quadratic polynomial.
 186    //
 187    // Point d must lie outside of the interval [a,b], it is the third
 188    // best approximation to the root, after a and b.
 189    //
 190    // Note: this does not guarentee to find a root
 191    // inside [a, b], so we fall back to a secant step should
 192    // the result be out of range.
 193    //
 194    // Start by obtaining the coefficients of the quadratic polynomial:
 195    //
 196    T B = safe_div(T(fb - fa), T(b - a), tools::max_value<T>());
 197    T A = safe_div(T(fd - fb), T(d - b), tools::max_value<T>());
 198    A = safe_div(T(A - B), T(d - a), T(0));
 199
 200    if(a == 0)
 201    {
 202       // failure to determine coefficients, try a secant step:
 203       return secant_interpolate(a, b, fa, fb);
 204    }
 205    //
 206    // Determine the starting point of the Newton steps:
 207    //
 208    T c;
 209    if(boost::math::sign(A) * boost::math::sign(fa) > 0)
 210    {
 211       c = a;
 212    }
 213    else
 214    {
 215       c = b;
 216    }
 217    //
 218    // Take the Newton steps:
 219    //
 220    for(unsigned i = 1; i <= count; ++i)
 221    {
 222       //c -= safe_div(B * c, (B + A * (2 * c - a - b)), 1 + c - a);
 223       c -= safe_div(T(fa+(B+A*(c-b))*(c-a)), T(B + A * (2 * c - a - b)), T(1 + c - a));
 224    }
 225    if((c <= a) || (c >= b))
 226    {
 227       // Oops, failure, try a secant step:
 228       c = secant_interpolate(a, b, fa, fb);
 229    }
 230    return c;
 231 }
 232
 233 template <class T>
 234 T cubic_interpolate(const T& a, const T& b, const T& d,
 235                     const T& e, const T& fa, const T& fb,
 236                     const T& fd, const T& fe)
 237 {
 238    //
 239    // Uses inverse cubic interpolation of f(x) at points
 240    // [a,b,d,e] to obtain an approximate root of f(x).
 241    // Points d and e lie outside the interval [a,b]
 242    // and are the third and forth best approximations
 243    // to the root that we have found so far.
 244    //
 245    // Note: this does not guarentee to find a root
 246    // inside [a, b], so we fall back to quadratic
 247    // interpolation in case of an erroneous result.
 248    //
 249    BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b
 250       << " d = " << d << " e = " << e << " fa = " << fa << " fb = " << fb
 251       << " fd = " << fd << " fe = " << fe);
 252    T q11 = (d - e) * fd / (fe - fd);
 253    T q21 = (b - d) * fb / (fd - fb);
 254    T q31 = (a - b) * fa / (fb - fa);
 255    T d21 = (b - d) * fd / (fd - fb);
 256    T d31 = (a - b) * fb / (fb - fa);
 257    BOOST_MATH_INSTRUMENT_CODE(
 258       "q11 = " << q11 << " q21 = " << q21 << " q31 = " << q31
 259       << " d21 = " << d21 << " d31 = " << d31);
 260    T q22 = (d21 - q11) * fb / (fe - fb);
 261    T q32 = (d31 - q21) * fa / (fd - fa);
 262    T d32 = (d31 - q21) * fd / (fd - fa);
 263    T q33 = (d32 - q22) * fa / (fe - fa);
 264    T c = q31 + q32 + q33 + a;
 265    BOOST_MATH_INSTRUMENT_CODE(
 266       "q22 = " << q22 << " q32 = " << q32 << " d32 = " << d32
 267       << " q33 = " << q33 << " c = " << c);
 268
 269    if((c <= a) || (c >= b))
 270    {
 271       // Out of bounds step, fall back to quadratic interpolation:
 272       c = quadratic_interpolate(a, b, d, fa, fb, fd, 3);
 273    BOOST_MATH_INSTRUMENT_CODE(
 274       "Out of bounds interpolation, falling back to quadratic interpolation. c = " << c);
 275    }
 276
 277    return c;
 278 }
 279
 280 } // namespace detail
 281
 282 template <class F, class T, class Tol, class Policy>
 283 std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, const T& fax, const T& fbx, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
 284 {
 285    //
 286    // Main entry point and logic for Toms Algorithm 748
 287    // root finder.
 288    //
 289    BOOST_MATH_STD_USING  // For ADL of std math functions
 290
 291    static const char* function = "boost::math::tools::toms748_solve<%1%>";
 292
 293    boost::uintmax_t count = max_iter;
 294    T a, b, fa, fb, c, u, fu, a0, b0, d, fd, e, fe;
 295    static const T mu = 0.5f;
 296
 297    // initialise a, b and fa, fb:
 298    a = ax;
 299    b = bx;
 300    if(a >= b)
 301       policies::raise_domain_error(
 302          function,
 303          "Parameters a and b out of order: a=%1%", a, pol);
 304    fa = fax;
 305    fb = fbx;
 306
 307    if(tol(a, b) || (fa == 0) || (fb == 0))
 308    {
 309       max_iter = 0;
 310       if(fa == 0)
 311          b = a;
 312       else if(fb == 0)
 313          a = b;
 314       return std::make_pair(a, b);
 315    }
 316
 317    if(boost::math::sign(fa) * boost::math::sign(fb) > 0)
 318       policies::raise_domain_error(
 319          function,
 320          "Parameters a and b do not bracket the root: a=%1%", a, pol);
 321    // dummy value for fd, e and fe:
 322    fe = e = fd = 1e5F;
 323
 324    if(fa != 0)
 325    {
 326       //
 327       // On the first step we take a secant step:
 328       //
 329       c = detail::secant_interpolate(a, b, fa, fb);
 330       detail::bracket(f, a, b, c, fa, fb, d, fd);
 331       --count;
 332       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 333
 334       if(count && (fa != 0) && !tol(a, b))
 335       {
 336          //
 337          // On the second step we take a quadratic interpolation:
 338          //
 339          c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
 340          e = d;
 341          fe = fd;
 342          detail::bracket(f, a, b, c, fa, fb, d, fd);
 343          --count;
 344          BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 345       }
 346    }
 347
 348    while(count && (fa != 0) && !tol(a, b))
 349    {
 350       // save our brackets:
 351       a0 = a;
 352       b0 = b;
 353       //
 354       // Starting with the third step taken
 355       // we can use either quadratic or cubic interpolation.
 356       // Cubic interpolation requires that all four function values
 357       // fa, fb, fd, and fe are distinct, should that not be the case
 358       // then variable prof will get set to true, and we'll end up
 359       // taking a quadratic step instead.
 360       //
 361       T min_diff = tools::min_value<T>() * 32;
 362       bool prof = (fabs(fa - fb) < min_diff) || (fabs(fa - fd) < min_diff) || (fabs(fa - fe) < min_diff) || (fabs(fb - fd) < min_diff) || (fabs(fb - fe) < min_diff) || (fabs(fd - fe) < min_diff);
 363       if(prof)
 364       {
 365          c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
 366          BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
 367       }
 368       else
 369       {
 370          c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
 371       }
 372       //
 373       // re-bracket, and check for termination:
 374       //
 375       e = d;
 376       fe = fd;
 377       detail::bracket(f, a, b, c, fa, fb, d, fd);
 378       if((0 == --count) || (fa == 0) || tol(a, b))
 379          break;
 380       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 381       //
 382       // Now another interpolated step:
 383       //
 384       prof = (fabs(fa - fb) < min_diff) || (fabs(fa - fd) < min_diff) || (fabs(fa - fe) < min_diff) || (fabs(fb - fd) < min_diff) || (fabs(fb - fe) < min_diff) || (fabs(fd - fe) < min_diff);
 385       if(prof)
 386       {
 387          c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 3);
 388          BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
 389       }
 390       else
 391       {
 392          c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
 393       }
 394       //
 395       // Bracket again, and check termination condition, update e:
 396       //
 397       detail::bracket(f, a, b, c, fa, fb, d, fd);
 398       if((0 == --count) || (fa == 0) || tol(a, b))
 399          break;
 400       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 401       //
 402       // Now we take a double-length secant step:
 403       //
 404       if(fabs(fa) < fabs(fb))
 405       {
 406          u = a;
 407          fu = fa;
 408       }
 409       else
 410       {
 411          u = b;
 412          fu = fb;
 413       }
 414       c = u - 2 * (fu / (fb - fa)) * (b - a);
 415       if(fabs(c - u) > (b - a) / 2)
 416       {
 417          c = a + (b - a) / 2;
 418       }
 419       //
 420       // Bracket again, and check termination condition:
 421       //
 422       e = d;
 423       fe = fd;
 424       detail::bracket(f, a, b, c, fa, fb, d, fd);
 425       if((0 == --count) || (fa == 0) || tol(a, b))
 426          break;
 427       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 428       //
 429       // And finally... check to see if an additional bisection step is
 430       // to be taken, we do this if we're not converging fast enough:
 431       //
 432       if((b - a) < mu * (b0 - a0))
 433          continue;
 434       //
 435       // bracket again on a bisection:
 436       //
 437       e = d;
 438       fe = fd;
 439       detail::bracket(f, a, b, T(a + (b - a) / 2), fa, fb, d, fd);
 440       --count;
 441       BOOST_MATH_INSTRUMENT_CODE("Not converging: Taking a bisection!!!!");
 442       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 443    } // while loop
 444
 445    max_iter -= count;
 446    if(fa == 0)
 447    {
 448       b = a;
 449    }
 450    else if(fb == 0)
 451    {
 452       a = b;
 453    }
 454    return std::make_pair(a, b);
 455 }
 456
 457 template <class F, class T, class Tol>
 458 inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, const T& fax, const T& fbx, Tol tol, boost::uintmax_t& max_iter)
 459 {
 460    return toms748_solve(f, ax, bx, fax, fbx, tol, max_iter, policies::policy<>());
 461 }
 462
 463 template <class F, class T, class Tol, class Policy>
 464 inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
 465 {
 466    max_iter -= 2;
 467    std::pair<T, T> r = toms748_solve(f, ax, bx, f(ax), f(bx), tol, max_iter, pol);
 468    max_iter += 2;
 469    return r;
 470 }
 471
 472 template <class F, class T, class Tol>
 473 inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter)
 474 {
 475    return toms748_solve(f, ax, bx, tol, max_iter, policies::policy<>());
 476 }
 477
 478 template <class F, class T, class Tol, class Policy>
 479 std::pair<T, T> bracket_and_solve_root(F f, const T& guess, T factor, bool rising, Tol tol, boost::uintmax_t& max_iter, const Policy& pol)
 480 {
 481    BOOST_MATH_STD_USING
 482    static const char* function = "boost::math::tools::bracket_and_solve_root<%1%>";
 483    //
 484    // Set up inital brackets:
 485    //
 486    T a = guess;
 487    T b = a;
 488    T fa = f(a);
 489    T fb = fa;
 490    //
 491    // Set up invocation count:
 492    //
 493    boost::uintmax_t count = max_iter - 1;
 494
 495    if((fa < 0) == (guess < 0 ? !rising : rising))
 496    {
 497       //
 498       // Zero is to the right of b, so walk upwards
 499       // until we find it:
 500       //
 501       while((boost::math::sign)(fb) == (boost::math::sign)(fa))
 502       {
 503          if(count == 0)
 504             policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol);
 505          //
 506          // Heuristic: every 20 iterations we double the growth factor in case the
 507          // initial guess was *really* bad !
 508          //
 509          if((max_iter - count) % 20 == 0)
 510             factor *= 2;
 511          //
 512          // Now go ahead and move our guess by "factor":
 513          //
 514          a = b;
 515          fa = fb;
 516          b *= factor;
 517          fb = f(b);
 518          --count;
 519          BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
 520       }
 521    }
 522    else
 523    {
 524       //
 525       // Zero is to the left of a, so walk downwards
 526       // until we find it:
 527       //
 528       while((boost::math::sign)(fb) == (boost::math::sign)(fa))
 529       {
 530          if(fabs(a) < tools::min_value<T>())
 531          {
 532             // Escape route just in case the answer is zero!
 533             max_iter -= count;
 534             max_iter += 1;
 535             return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0));
 536          }
 537          if(count == 0)
 538             policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol);
 539          //
 540          // Heuristic: every 20 iterations we double the growth factor in case the
 541          // initial guess was *really* bad !
 542          //
 543          if((max_iter - count) % 20 == 0)
 544             factor *= 2;
 545          //
 546          // Now go ahead and move are guess by "factor":
 547          //
 548          b = a;
 549          fb = fa;
 550          a /= factor;
 551          fa = f(a);
 552          --count;
 553          BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
 554       }
 555    }
 556    max_iter -= count;
 557    max_iter += 1;
 558    std::pair<T, T> r = toms748_solve(
 559       f,
 560       (a < 0 ? b : a),
 561       (a < 0 ? a : b),
 562       (a < 0 ? fb : fa),
 563       (a < 0 ? fa : fb),
 564       tol,
 565       count,
 566       pol);
 567    max_iter += count;
 568    BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
 569    return r;
 570 }
 571
 572 template <class F, class T, class Tol>
 573 inline std::pair<T, T> bracket_and_solve_root(F f, const T& guess, const T& factor, bool rising, Tol tol, boost::uintmax_t& max_iter)
 574 {
 575    return bracket_and_solve_root(f, guess, factor, rising, tol, max_iter, policies::policy<>());
 576 }
 577
 578 } // namespace tools
 579 } // namespace math
 580 } // namespace boost
 581
 582
 583 #endif // BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
 584