// (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(2 * tools::epsilon<T>())); | |
} | |
bool operator()(const T& a, const T& b) | |
{ | |
BOOST_MATH_STD_USING | |
return (fabs(a - b) / (std::min)(fabs(a), fabs(b))) <= eps; | |
} | |
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); | |
if((0 == --count) || (fa == 0) || tol(a, b)) | |
break; | |
BOOST_MATH_INSTRUMENT_CODE(" a = " << a << " b = " << b); | |
// | |
// 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 | |