comparison DEPENDENCIES/generic/include/boost/math/tools/toms748_solve.hpp @ 16:2665513ce2d3

Add boost headers

+//  (C) Copyright John Maddock 2006.
+//  Use, modification and distribution are subject to the
+//  Boost Software License, Version 1.0. (See accompanying file
+//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
+
+#ifndef BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
+#define BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
+
+#ifdef _MSC_VER
+#pragma once
+#endif
+
+#include <boost/math/tools/precision.hpp>
+#include <boost/math/policies/error_handling.hpp>
+#include <boost/math/tools/config.hpp>
+#include <boost/math/special_functions/sign.hpp>
+#include <boost/cstdint.hpp>
+#include <limits>
+
+namespace boost{ namespace math{ namespace tools{
+
+template <class T>
+class eps_tolerance
+{
+public:
+   eps_tolerance(unsigned bits)
+   {
+      BOOST_MATH_STD_USING
+      eps = (std::max)(T(ldexp(1.0F, 1-bits)), T(4 * tools::epsilon<T>()));
+   }
+   bool operator()(const T& a, const T& b)
+   {
+      BOOST_MATH_STD_USING
+      return fabs(a - b) <= (eps * (std::min)(fabs(a), fabs(b)));
+   }
+private:
+   T eps;
+};
+
+struct equal_floor
+{
+   equal_floor(){}
+   template <class T>
+   bool operator()(const T& a, const T& b)
+   {
+      BOOST_MATH_STD_USING
+      return floor(a) == floor(b);
+   }
+};
+
+struct equal_ceil
+{
+   equal_ceil(){}
+   template <class T>
+   bool operator()(const T& a, const T& b)
+   {
+      BOOST_MATH_STD_USING
+      return ceil(a) == ceil(b);
+   }
+};
+
+struct equal_nearest_integer
+{
+   equal_nearest_integer(){}
+   template <class T>
+   bool operator()(const T& a, const T& b)
+   {
+      BOOST_MATH_STD_USING
+      return floor(a + 0.5f) == floor(b + 0.5f);
+   }
+};
+
+namespace detail{
+
+template <class F, class T>
+void bracket(F f, T& a, T& b, T c, T& fa, T& fb, T& d, T& fd)
+{
+   //
+   // Given a point c inside the existing enclosing interval
+   // [a, b] sets a = c if f(c) == 0, otherwise finds the new 
+   // enclosing interval: either [a, c] or [c, b] and sets
+   // d and fd to the point that has just been removed from
+   // the interval.  In other words d is the third best guess
+   // to the root.
+   //
+   BOOST_MATH_STD_USING  // For ADL of std math functions
+   T tol = tools::epsilon<T>() * 2;
+   //
+   // If the interval [a,b] is very small, or if c is too close 
+   // to one end of the interval then we need to adjust the
+   // location of c accordingly:
+   //
+   if((b - a) < 2 * tol * a)
+   {
+      c = a + (b - a) / 2;
+   }
+   else if(c <= a + fabs(a) * tol)
+   {
+      c = a + fabs(a) * tol;
+   }
+   else if(c >= b - fabs(b) * tol)
+   {
+      c = b - fabs(a) * tol;
+   }
+   //
+   // OK, lets invoke f(c):
+   //
+   T fc = f(c);
+   //
+   // if we have a zero then we have an exact solution to the root:
+   //
+   if(fc == 0)
+   {
+      a = c;
+      fa = 0;
+      d = 0;
+      fd = 0;
+      return;
+   }
+   //
+   // Non-zero fc, update the interval:
+   //
+   if(boost::math::sign(fa) * boost::math::sign(fc) < 0)
+   {
+      d = b;
+      fd = fb;
+      b = c;
+      fb = fc;
+   }
+   else
+   {
+      d = a;
+      fd = fa;
+      a = c;
+      fa= fc;
+   }
+}
+
+template <class T>
+inline T safe_div(T num, T denom, T r)
+{
+   //
+   // return num / denom without overflow,
+   // return r if overflow would occur.
+   //
+   BOOST_MATH_STD_USING  // For ADL of std math functions
+
+   if(fabs(denom) < 1)
+   {
+      if(fabs(denom * tools::max_value<T>()) <= fabs(num))
+         return r;
+   }
+   return num / denom;
+}
+
+template <class T>
+inline T secant_interpolate(const T& a, const T& b, const T& fa, const T& fb)
+{
+   //
+   // Performs standard secant interpolation of [a,b] given
+   // function evaluations f(a) and f(b).  Performs a bisection
+   // if secant interpolation would leave us very close to either
+   // a or b.  Rationale: we only call this function when at least
+   // one other form of interpolation has already failed, so we know
+   // that the function is unlikely to be smooth with a root very
+   // close to a or b.
+   //
+   BOOST_MATH_STD_USING  // For ADL of std math functions
+
+   T tol = tools::epsilon<T>() * 5;
+   T c = a - (fa / (fb - fa)) * (b - a);
+   if((c <= a + fabs(a) * tol) || (c >= b - fabs(b) * tol))
+      return (a + b) / 2;
+   return c;
+}
+
+template <class T>
+T quadratic_interpolate(const T& a, const T& b, T const& d,
+                           const T& fa, const T& fb, T const& fd, 
+                           unsigned count)
+{
+   //
+   // Performs quadratic interpolation to determine the next point,
+   // takes count Newton steps to find the location of the
+   // quadratic polynomial.
+   //
+   // Point d must lie outside of the interval [a,b], it is the third
+   // best approximation to the root, after a and b.
+   //
+   // Note: this does not guarentee to find a root
+   // inside [a, b], so we fall back to a secant step should
+   // the result be out of range.
+   //
+   // Start by obtaining the coefficients of the quadratic polynomial:
+   //
+   T B = safe_div(T(fb - fa), T(b - a), tools::max_value<T>());
+   T A = safe_div(T(fd - fb), T(d - b), tools::max_value<T>());
+   A = safe_div(T(A - B), T(d - a), T(0));
+
+   if(A == 0)
+   {
+      // failure to determine coefficients, try a secant step:
+      return secant_interpolate(a, b, fa, fb);
+   }
+   //
+   // Determine the starting point of the Newton steps:
+   //
+   T c;
+   if(boost::math::sign(A) * boost::math::sign(fa) > 0)
+   {
+      c = a;
+   }
+   else
+   {
+      c = b;
+   }
+   //
+   // Take the Newton steps:
+   //
+   for(unsigned i = 1; i <= count; ++i)
+   {
+      //c -= safe_div(B * c, (B + A * (2 * c - a - b)), 1 + c - a);
+      c -= safe_div(T(fa+(B+A*(c-b))*(c-a)), T(B + A * (2 * c - a - b)), T(1 + c - a));
+   }
+   if((c <= a) || (c >= b))
+   {
+      // Oops, failure, try a secant step:
+      c = secant_interpolate(a, b, fa, fb);
+   }
+   return c;
+}
+
+template <class T>
+T cubic_interpolate(const T& a, const T& b, const T& d, 
+                    const T& e, const T& fa, const T& fb, 
+                    const T& fd, const T& fe)
+{
+   //
+   // Uses inverse cubic interpolation of f(x) at points 
+   // [a,b,d,e] to obtain an approximate root of f(x).
+   // Points d and e lie outside the interval [a,b]
+   // and are the third and forth best approximations
+   // to the root that we have found so far.
+   //
+   // Note: this does not guarentee to find a root
+   // inside [a, b], so we fall back to quadratic
+   // interpolation in case of an erroneous result.
+   //
+   BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b
+      << " d = " << d << " e = " << e << " fa = " << fa << " fb = " << fb 
+      << " fd = " << fd << " fe = " << fe);
+   T q11 = (d - e) * fd / (fe - fd);
+   T q21 = (b - d) * fb / (fd - fb);
+   T q31 = (a - b) * fa / (fb - fa);
+   T d21 = (b - d) * fd / (fd - fb);
+   T d31 = (a - b) * fb / (fb - fa);
+   BOOST_MATH_INSTRUMENT_CODE(
+      "q11 = " << q11 << " q21 = " << q21 << " q31 = " << q31
+      << " d21 = " << d21 << " d31 = " << d31);
+   T q22 = (d21 - q11) * fb / (fe - fb);
+   T q32 = (d31 - q21) * fa / (fd - fa);
+   T d32 = (d31 - q21) * fd / (fd - fa);
+   T q33 = (d32 - q22) * fa / (fe - fa);
+   T c = q31 + q32 + q33 + a;
+   BOOST_MATH_INSTRUMENT_CODE(
+      "q22 = " << q22 << " q32 = " << q32 << " d32 = " << d32
+      << " q33 = " << q33 << " c = " << c);
+
+   if((c <= a) || (c >= b))
+   {
+      // Out of bounds step, fall back to quadratic interpolation:
+      c = quadratic_interpolate(a, b, d, fa, fb, fd, 3);
+   BOOST_MATH_INSTRUMENT_CODE(
+      "Out of bounds interpolation, falling back to quadratic interpolation. c = " << c);
+   }
+
+   return c;
+}
+
+} // namespace detail
+
+template <class F, class T, class Tol, class Policy>
+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)
+{
+   //
+   // Main entry point and logic for Toms Algorithm 748
+   // root finder.
+   //
+   BOOST_MATH_STD_USING  // For ADL of std math functions
+
+   static const char* function = "boost::math::tools::toms748_solve<%1%>";
+
+   boost::uintmax_t count = max_iter;
+   T a, b, fa, fb, c, u, fu, a0, b0, d, fd, e, fe;
+   static const T mu = 0.5f;
+
+   // initialise a, b and fa, fb:
+   a = ax;
+   b = bx;
+   if(a >= b)
+      policies::raise_domain_error(
+         function, 
+         "Parameters a and b out of order: a=%1%", a, pol);
+   fa = fax;
+   fb = fbx;
+
+   if(tol(a, b) || (fa == 0) || (fb == 0))
+   {
+      max_iter = 0;
+      if(fa == 0)
+         b = a;
+      else if(fb == 0)
+         a = b;
+      return std::make_pair(a, b);
+   }
+
+   if(boost::math::sign(fa) * boost::math::sign(fb) > 0)
+      policies::raise_domain_error(
+         function, 
+         "Parameters a and b do not bracket the root: a=%1%", a, pol);
+   // dummy value for fd, e and fe:
+   fe = e = fd = 1e5F;
+
+   if(fa != 0)
+   {
+      //
+      // On the first step we take a secant step:
+      //
+      c = detail::secant_interpolate(a, b, fa, fb);
+      detail::bracket(f, a, b, c, fa, fb, d, fd);
+      --count;
+      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+
+      if(count && (fa != 0) && !tol(a, b))
+      {
+         //
+         // On the second step we take a quadratic interpolation:
+         //
+         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
+         e = d;
+         fe = fd;
+         detail::bracket(f, a, b, c, fa, fb, d, fd);
+         --count;
+         BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+      }
+   }
+
+   while(count && (fa != 0) && !tol(a, b))
+   {
+      // save our brackets:
+      a0 = a;
+      b0 = b;
+      //
+      // Starting with the third step taken
+      // we can use either quadratic or cubic interpolation.
+      // Cubic interpolation requires that all four function values
+      // fa, fb, fd, and fe are distinct, should that not be the case
+      // then variable prof will get set to true, and we'll end up
+      // taking a quadratic step instead.
+      //
+      T min_diff = tools::min_value<T>() * 32;
+      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);
+      if(prof)
+      {
+         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 2);
+         BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
+      }
+      else
+      {
+         c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
+      }
+      //
+      // re-bracket, and check for termination:
+      //
+      e = d;
+      fe = fd;
+      detail::bracket(f, a, b, c, fa, fb, d, fd);
+      if((0 == --count) || (fa == 0) || tol(a, b))
+         break;
+      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+      //
+      // Now another interpolated step:
+      //
+      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);
+      if(prof)
+      {
+         c = detail::quadratic_interpolate(a, b, d, fa, fb, fd, 3);
+         BOOST_MATH_INSTRUMENT_CODE("Can't take cubic step!!!!");
+      }
+      else
+      {
+         c = detail::cubic_interpolate(a, b, d, e, fa, fb, fd, fe);
+      }
+      //
+      // Bracket again, and check termination condition, update e:
+      //
+      detail::bracket(f, a, b, c, fa, fb, d, fd);
+      if((0 == --count) || (fa == 0) || tol(a, b))
+         break;
+      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+      //
+      // Now we take a double-length secant step:
+      //
+      if(fabs(fa) < fabs(fb))
+      {
+         u = a;
+         fu = fa;
+      }
+      else
+      {
+         u = b;
+         fu = fb;
+      }
+      c = u - 2 * (fu / (fb - fa)) * (b - a);
+      if(fabs(c - u) > (b - a) / 2)
+      {
+         c = a + (b - a) / 2;
+      }
+      //
+      // Bracket again, and check termination condition:
+      //
+      e = d;
+      fe = fd;
+      detail::bracket(f, a, b, c, fa, fb, d, fd);
+      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+      BOOST_MATH_INSTRUMENT_CODE(" tol = " << T((fabs(a) - fabs(b)) / fabs(a)));
+      if((0 == --count) || (fa == 0) || tol(a, b))
+         break;
+      //
+      // And finally... check to see if an additional bisection step is 
+      // to be taken, we do this if we're not converging fast enough:
+      //
+      if((b - a) < mu * (b0 - a0))
+         continue;
+      //
+      // bracket again on a bisection:
+      //
+      e = d;
+      fe = fd;
+      detail::bracket(f, a, b, T(a + (b - a) / 2), fa, fb, d, fd);
+      --count;
+      BOOST_MATH_INSTRUMENT_CODE("Not converging: Taking a bisection!!!!");
+      BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b);
+   } // while loop
+
+   max_iter -= count;
+   if(fa == 0)
+   {
+      b = a;
+   }
+   else if(fb == 0)
+   {
+      a = b;
+   }
+   return std::make_pair(a, b);
+}
+
+template <class F, class T, class Tol>
+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)
+{
+   return toms748_solve(f, ax, bx, fax, fbx, tol, max_iter, policies::policy<>());
+}
+
+template <class F, class T, class Tol, class Policy>
+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)
+{
+   max_iter -= 2;
+   std::pair<T, T> r = toms748_solve(f, ax, bx, f(ax), f(bx), tol, max_iter, pol);
+   max_iter += 2;
+   return r;
+}
+
+template <class F, class T, class Tol>
+inline std::pair<T, T> toms748_solve(F f, const T& ax, const T& bx, Tol tol, boost::uintmax_t& max_iter)
+{
+   return toms748_solve(f, ax, bx, tol, max_iter, policies::policy<>());
+}
+
+template <class F, class T, class Tol, class Policy>
+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)
+{
+   BOOST_MATH_STD_USING
+   static const char* function = "boost::math::tools::bracket_and_solve_root<%1%>";
+   //
+   // Set up inital brackets:
+   //
+   T a = guess;
+   T b = a;
+   T fa = f(a);
+   T fb = fa;
+   //
+   // Set up invocation count:
+   //
+   boost::uintmax_t count = max_iter - 1;
+
+   if((fa < 0) == (guess < 0 ? !rising : rising))
+   {
+      //
+      // Zero is to the right of b, so walk upwards
+      // until we find it:
+      //
+      while((boost::math::sign)(fb) == (boost::math::sign)(fa))
+      {
+         if(count == 0)
+            policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", b, pol);
+         //
+         // Heuristic: every 20 iterations we double the growth factor in case the
+         // initial guess was *really* bad !
+         //
+         if((max_iter - count) % 20 == 0)
+            factor *= 2;
+         //
+         // Now go ahead and move our guess by "factor":
+         //
+         a = b;
+         fa = fb;
+         b *= factor;
+         fb = f(b);
+         --count;
+         BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
+      }
+   }
+   else
+   {
+      //
+      // Zero is to the left of a, so walk downwards
+      // until we find it:
+      //
+      while((boost::math::sign)(fb) == (boost::math::sign)(fa))
+      {
+         if(fabs(a) < tools::min_value<T>())
+         {
+            // Escape route just in case the answer is zero!
+            max_iter -= count;
+            max_iter += 1;
+            return a > 0 ? std::make_pair(T(0), T(a)) : std::make_pair(T(a), T(0)); 
+         }
+         if(count == 0)
+            policies::raise_evaluation_error(function, "Unable to bracket root, last nearest value was %1%", a, pol);
+         //
+         // Heuristic: every 20 iterations we double the growth factor in case the
+         // initial guess was *really* bad !
+         //
+         if((max_iter - count) % 20 == 0)
+            factor *= 2;
+         //
+         // Now go ahead and move are guess by "factor":
+         //
+         b = a;
+         fb = fa;
+         a /= factor;
+         fa = f(a);
+         --count;
+         BOOST_MATH_INSTRUMENT_CODE("a = " << a << " b = " << b << " fa = " << fa << " fb = " << fb << " count = " << count);
+      }
+   }
+   max_iter -= count;
+   max_iter += 1;
+   std::pair<T, T> r = toms748_solve(
+      f, 
+      (a < 0 ? b : a), 
+      (a < 0 ? a : b), 
+      (a < 0 ? fb : fa), 
+      (a < 0 ? fa : fb), 
+      tol, 
+      count, 
+      pol);
+   max_iter += count;
+   BOOST_MATH_INSTRUMENT_CODE("max_iter = " << max_iter << " count = " << count);
+   return r;
+}
+
+template <class F, class T, class Tol>
+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)
+{
+   return bracket_and_solve_root(f, guess, factor, rising, tol, max_iter, policies::policy<>());
+}
+
+} // namespace tools
+} // namespace math
+} // namespace boost
+
+
+#endif // BOOST_MATH_TOOLS_SOLVE_ROOT_HPP
+