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(4 * tools::epsilon<T>()));
  30    }
  31    bool operator()(const T& a, const T& b)
  32    {
  33       BOOST_MATH_STD_USING
  34       return fabs(a - b) <= (eps * (std::min)(fabs(a), fabs(b)));
  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       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 426       BOOST_MATH_INSTRUMENT_CODE(" tol = " << T((fabs(a) - fabs(b)) / fabs(a)));
 427       if((0 == --count) || (fa == 0) || tol(a, b))
 428          break;
 429       //
 430       // And finally... check to see if an additional bisection step is
 431       // to be taken, we do this if we're not converging fast enough:
 432       //
 433       if((b - a) < mu * (b0 - a0))
 434          continue;
 435       //
 436       // bracket again on a bisection:
 437       //
 438       e = d;
 439       fe = fd;
 440       detail::bracket(f, a, b, T(a + (b - a) / 2), fa, fb, d, fd);
 441       --count;
 442       BOOST_MATH_INSTRUMENT_CODE("Not converging: Taking a bisection!!!!");
 443       BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
 444    } // while loop
 445
 446    max_iter -= count;
 447    if(fa == 0)
 448    {
 449       b = a;
 450    }
 451    else if(fb == 0)
 452    {
 453       a = b;
 454    }
 455    return std::make_pair(a, b);
 456 }
 457
 458 template <class F, class T, class Tol>
 459 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)
 460 {
 461    return toms748_solve(f, ax, bx, fax, fbx, tol, max_iter, policies::policy<>());
 462 }
 463
 464 template <class F, class T, class Tol, class Policy>
 465 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)
 466 {
 467    max_iter -= 2;
 468    std::pair<T, T> r = toms748_solve(f, ax, bx, f(ax), f(bx), tol, max_iter, pol);
 469    max_iter += 2;
 470    return r;
 471 }
 472
 473 template <class F, class T, class Tol>
 474 inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter)
 475 {
 476    return toms748_solve(f, ax, bx, tol, max_iter, policies::policy<>());
 477 }
 478
 479 template <class F, class T, class Tol, class Policy>
 480 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)
 481 {
 482    BOOST_MATH_STD_USING
 483    static const char* function = "boost::math::tools::bracket_and_solve_root<%1%>";
 484    //
 485    // Set up inital brackets:
 486    //
 487    T a = guess;
 488    T b = a;
 489    T fa = f(a);
 490    T fb = fa;
 491    //
 492    // Set up invocation count:
 493    //
 494    boost::uintmax_t count = max_iter - 1;
 495
 496    if((fa < 0) == (guess < 0 ? !rising : rising))
 497    {
 498       //
 499       // Zero is to the right of b, so walk upwards
 500       // until we find it:
 501       //
 502       while((boost::math::sign)(fb) == (boost::math::sign)(fa))
 503       {
 504          if(count == 0)
 505             policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol);
 506          //
 507          // Heuristic: every 20 iterations we double the growth factor in case the
 508          // initial guess was *really* bad !
 509          //
 510          if((max_iter - count) % 20 == 0)
 511             factor *= 2;
 512          //
 513          // Now go ahead and move our guess by "factor":
 514          //
 515          a = b;
 516          fa = fb;
 517          b *= factor;
 518          fb = f(b);
 519          --count;
 520          BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
 521       }
 522    }
 523    else
 524    {
 525       //
 526       // Zero is to the left of a, so walk downwards
 527       // until we find it:
 528       //
 529       while((boost::math::sign)(fb) == (boost::math::sign)(fa))
 530       {
 531          if(fabs(a) < tools::min_value<T>())
 532          {
 533             // Escape route just in case the answer is zero!
 534             max_iter -= count;
 535             max_iter += 1;
 536             return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0));
 537          }
 538          if(count == 0)
 539             policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol);
 540          //
 541          // Heuristic: every 20 iterations we double the growth factor in case the
 542          // initial guess was *really* bad !
 543          //
 544          if((max_iter - count) % 20 == 0)
 545             factor *= 2;
 546          //
 547          // Now go ahead and move are guess by "factor":
 548          //
 549          b = a;
 550          fb = fa;
 551          a /= factor;
 552          fa = f(a);
 553          --count;
 554          BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
 555       }
 556    }
 557    max_iter -= count;
 558    max_iter += 1;
 559    std::pair<T, T> r = toms748_solve(
 560       f,
 561       (a < 0 ? b : a),
 562       (a < 0 ? a : b),
 563       (a < 0 ? fb : fa),
 564       (a < 0 ? fa : fb),
 565       tol,
 566       count,
 567       pol);
 568    max_iter += count;
 569    BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
 570    return r;
 571 }
 572
 573 template <class F, class T, class Tol>
 574 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)
 575 {
 576    return bracket_and_solve_root(f, guess, factor, rising, tol, max_iter, policies::policy<>());
 577 }
 578
 579 } // namespace tools
 580 } // namespace math
 581 } // namespace boost
 582
 583
 584 #endif // BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
 585