295 lines
11 KiB
C++
295 lines
11 KiB
C++
#ifndef DEF_TOMS748
|
|
#define DEF_TOMS748
|
|
|
|
#include <tuple>
|
|
|
|
namespace TOMS748
|
|
{
|
|
namespace internal
|
|
{
|
|
template<typename T> T sureAbs(const T & x) { return (x < T(0)) ? -x : x; }
|
|
|
|
// checks that none of the values are the same
|
|
// returns true if at least two values are identical
|
|
template<typename T>
|
|
bool checkTwoValuesIdentical(T fa, T fb, T fc, T fd)
|
|
{
|
|
bool same = false;
|
|
same |= fa == fb;
|
|
same |= fa == fc;
|
|
same |= fa == fd;
|
|
|
|
same |= fb == fc;
|
|
same |= fb == fd;
|
|
|
|
same |= fc == fd;
|
|
return same;
|
|
}
|
|
|
|
// finite differences 1st derivative given a, b, fa, fb
|
|
template<typename T> T fbracket1(T a, T b, T fa, T fb) { return (fb-fa)/(b-a); }
|
|
|
|
// finite differences 2nd derivative given a, b, d, fa, fb, fd
|
|
template<typename T> T fbracket2(T a, T b, T d, T fa, T fb, T fd) { return (fbracket1(b, d, fb, fd) - fbracket1(a, b, fa, fb))/(d-a); }
|
|
|
|
// standard bracketing routine
|
|
// returns ahat, bhat, d
|
|
// d is a point outside the new interval
|
|
template<typename T> std::tuple<T,T,T,T,T,T>
|
|
bracket(T a, T b, T c, T fa, T fb, T fc)
|
|
{
|
|
if(fa*fc < T(0))
|
|
return std::make_tuple(a, c, b, fa, fc, fb);
|
|
else
|
|
return std::make_tuple(c, b, a, fc, fb, fa);
|
|
}
|
|
|
|
// standard bracketing routine
|
|
// checks if f(c) is close enough to 0 and returns ok = true if it is the case
|
|
// returns ahat, bhat, d
|
|
// d is a point outside the new interval
|
|
template<typename T> std::tuple<T,T,T,T,T,T,bool>
|
|
bracketAndCheckConvergence(T a, T b, T c, T fa, T fb, T fc, T tol)
|
|
{
|
|
bool ok = false;
|
|
if(internal::sureAbs(fc) < tol)
|
|
ok = true;
|
|
if(fa*fc < T(0))
|
|
return std::make_tuple(a, c, b, fa, fc, fb, ok);
|
|
else
|
|
return std::make_tuple(c, b, a, fc, fb, fa, ok);
|
|
}
|
|
|
|
// finds an approximate solution to the quadratic P(x) = fa + f[a,b]*(x-a) + f[a,b,d]*(x-a)(x-b)
|
|
// with f[a,b] = fbracket1(a,b) and f[a,b,d] = fbracket2(a,b,d)
|
|
template<typename T, int k>
|
|
T NewtonQuadratic(T a, T b, T d, T fa, T fb, T fd)
|
|
{
|
|
T r;
|
|
T A = fbracket2(a,b,d,fa,fb,fd);
|
|
T B = fbracket1(a,b,fa,fb);
|
|
if(A == T(0))
|
|
return a - fa/B;
|
|
if(A*fa > T(0))
|
|
r = a;
|
|
else
|
|
r = b;
|
|
|
|
for(int i = 0 ; i < k ; i++)
|
|
r = r - B*r/(B + A*(T(2)*r - a - b));
|
|
return r;
|
|
}
|
|
|
|
// Inverse cubic interpolation evaluated at 0 (modified Aitken-Neville interpolation)
|
|
template<typename T>
|
|
T ipzero(T a, T b, T c, T d, T fa, T fb, T fc, T fd)
|
|
{
|
|
T Q11 = (c-d)*fc/(fd-fc);
|
|
T Q21 = (b-c)*fb/(fc-fb);
|
|
T Q31 = (a-b)*fa/(fb-fa);
|
|
T D21 = (b-c)*fc/(fc-fb);
|
|
T D31 = (a-b)*fb/(fb-fa);
|
|
T Q22 = (D21-Q11)*fb/(fd-fb);
|
|
T Q32 = (D31-Q21)*fa/(fc-fa);
|
|
T D32 = (D31-Q21)*fc/(fc-fa);
|
|
T Q33 = (D32-Q22)*fa/(fd-fa);
|
|
return a + Q31 + Q32 + Q33;
|
|
}
|
|
|
|
/// Sorts the pairs (abs(a), fa) and (abs(b), fb) in ascending order.
|
|
template<typename T>
|
|
void swap_abs_ab_fafb_if_not_ascending(T & a, T & b, T & fa, T & fb)
|
|
{
|
|
if(internal::sureAbs(b) < internal::sureAbs(a))
|
|
{
|
|
// swap a and b
|
|
T temp = a;
|
|
a = b;
|
|
b = temp;
|
|
// swap fa and fb
|
|
temp = fa;
|
|
fa = fb;
|
|
fb = temp;
|
|
}
|
|
}
|
|
|
|
/// Sorts the pairs (a, fa) and (b, fb) in ascending order.
|
|
template<typename T>
|
|
void swap_ab_fafb_if_not_ascending(T & a, T & b, T & fa, T & fb)
|
|
{
|
|
if(b < a)
|
|
{
|
|
// swap a and b
|
|
T temp = a;
|
|
a = b;
|
|
b = temp;
|
|
// swap fa and fb
|
|
temp = fa;
|
|
fa = fb;
|
|
fb = temp;
|
|
}
|
|
}
|
|
}// namespace internal
|
|
|
|
/// Algorithm 4.1 from TOMS748 of robust root-solving.
|
|
/// Use this version if f(a) and f(b) have already been computed.
|
|
/// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
|
|
///
|
|
/// Source : https://na.math.kit.edu/alefeld/download/1995_Algorithm_748_Enclosing_Zeros_of_Continuous_Functions.pdf
|
|
template<typename Func, typename T>
|
|
std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T fa, T fb, T tol, unsigned int Nmax = 1000)
|
|
{
|
|
using namespace internal;
|
|
T c, d, e, u, dbar, dhat, fc, fd, fe, fu, fdbar, fdhat;
|
|
T mu = 0.5;
|
|
bool ok;
|
|
c = a - fa/fbracket1(a, b, fa, fb); // 4.1.1 secant method
|
|
fc = f(c);
|
|
std::tie(a, b, d, fa, fb, fd, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol); // 4.1.2
|
|
if(ok) { return std::make_tuple(c, fc, true); }
|
|
e = d;
|
|
fe = fd;
|
|
// ---
|
|
for(unsigned int n = 2 ; n < Nmax ; n++) // 4.1.3
|
|
{
|
|
if(n == 2 || checkTwoValuesIdentical(fa, fb, fd, fe))
|
|
c = NewtonQuadratic<T,2>(a, b, d, fa, fb, fd);
|
|
else
|
|
{
|
|
c = ipzero(a, b, d, e, fa, fb, fd, fe);
|
|
if((c-a)*(c-b) >= T(0))
|
|
c = NewtonQuadratic<T,2>(a, b, d, fa, fb, fd);
|
|
}
|
|
// ---
|
|
fc = f(c);
|
|
std::tie(a, b, dbar, fa, fb, fdbar, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol); // 4.1.4
|
|
if(ok) { return std::make_tuple(c, fc, true); }
|
|
// ---
|
|
if(internal::sureAbs(fa) < internal::sureAbs(fb)) // 4.1.5
|
|
{
|
|
u = a;
|
|
fu = fa;
|
|
}
|
|
else
|
|
{
|
|
u = b;
|
|
fu = fb;
|
|
}
|
|
// ---
|
|
c = u - 2*fu/fbracket1(a, b, fa, fb); // 4.1.6
|
|
// ---
|
|
if(internal::sureAbs(c - u) > 0.5*(b - a)) // 4.1.7
|
|
c = 0.5*(b + a);
|
|
// ---
|
|
fc = f(c);
|
|
std::tie(a, b, dhat, fa, fb, fdhat, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol); // 4.1.8
|
|
if(ok) { return std::make_tuple(c, fc, true); }
|
|
// ---
|
|
if(b - a < mu*(b - a)) // 4.1.9
|
|
{
|
|
d = dhat;
|
|
e = dbar;
|
|
fd = fdhat;
|
|
fe = fdbar;
|
|
}
|
|
else
|
|
{
|
|
e = dhat;
|
|
fe = fdhat;
|
|
}
|
|
c = 0.5*(a+b);
|
|
fc = f(c);
|
|
std::tie(a, b, d, fa, fb, fd, ok) = bracketAndCheckConvergence(a, b, c, fa, fb, fc, tol);
|
|
if(ok) { return std::make_tuple(c, fc, true); }
|
|
}
|
|
return std::make_tuple(c, fc, false);// no solution found, return last estimate
|
|
}
|
|
|
|
/// Algorithm 4.1 from TOMS748 of robust root-solving.
|
|
/// Use this version if f(a) and f(b) have NOT already been computed.
|
|
/// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
|
|
template<typename Func, typename T>
|
|
std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T tol, unsigned int Nmax = 1000)
|
|
{
|
|
return TOMS748_solve1(f, a, b, f(a), f(b), tol, Nmax);
|
|
}
|
|
|
|
/// Finds a bracket [a b] that encloses a root of f : f(a)*f(b) < 0, starting from the initial guess x.
|
|
/// A typical value for the factor is 2.
|
|
/// The initial guess must be on the correct side of the real line : the algorithm will never cross the zero during the search.
|
|
/// The factor is increased every 10 iterations in order to speed up convergence for when the root is orders of magnitude away from the initial guess.
|
|
/// If the bracket returned by the function is too wide, try reducing the factor (while always keeping it > 1).
|
|
///
|
|
/// The function returns a tuple of the following values :
|
|
/// - T a : lower bound of the interval.
|
|
/// - T b : upper bound of the interval.
|
|
/// - T fa : function value at lower bound of the interval.
|
|
/// - T fb : function value at upper bound of the interval.
|
|
/// - bool ok : indicates whether the search was successful or not.
|
|
template<typename Func, typename T>
|
|
std::tuple<T,T,T,T,bool> findEnclosingBracket(Func f, T x, T factor, unsigned int Nmax = 100)
|
|
{
|
|
T a, b, fa, fb;
|
|
unsigned int i;
|
|
a = x;
|
|
b = x*factor; // try searching in the positive direction
|
|
fa = f(a);
|
|
fb = f(b);
|
|
internal::swap_abs_ab_fafb_if_not_ascending(a, b, fa, fb);
|
|
|
|
if(fa*fb <= T(0)) // if the original bracket is already enclosing the solution
|
|
{
|
|
internal::swap_ab_fafb_if_not_ascending(a, b, fa, fb);
|
|
return std::make_tuple(a, b, fa, fb, true);
|
|
}
|
|
|
|
if(internal::sureAbs(fa) < internal::sureAbs(fb)) // if fa is closer, search in the other direction
|
|
{
|
|
a = x/factor;
|
|
b = x;
|
|
fb = fa; // f(b) = f(x)
|
|
fa = f(a);
|
|
internal::swap_abs_ab_fafb_if_not_ascending(a, b, fa, fb);
|
|
}
|
|
i = 0;
|
|
while(i < Nmax && fa*fb > T(0))
|
|
{
|
|
if(internal::sureAbs(fa) < internal::sureAbs(fb)) // fa is closer to 0 than fb, extend the interval in the a direction (reducing it)
|
|
{
|
|
a = a / factor;
|
|
fa = f(a);
|
|
}
|
|
else // fb is closer to 0 than fa, extend the interval in the b direction (augmenting it)
|
|
{
|
|
b = b * factor;
|
|
fb = f(b);
|
|
}
|
|
i = i+1;
|
|
|
|
if(i % 10 == 0) // every 10 iterations, bump up the factor to speed up convergence
|
|
factor = factor * 5;
|
|
}
|
|
|
|
internal::swap_ab_fafb_if_not_ascending(a, b, fa, fb);
|
|
return std::make_tuple(a, b, fa, fb, static_cast<bool>(fa*fb <= T(0)));
|
|
}
|
|
|
|
/// Finds a bracket that encloses the solution and then calls TOMS748_solve1 to solve for the root of the function in the bracket.
|
|
/// Returns x, f(x), and a boolean indicating if the function converged or not (true if converged).
|
|
template<typename Func, typename T>
|
|
std::tuple<T,T,bool> bracket_and_solve(Func f, T x, T tol, T factor = T(2), unsigned int Nmax = 1000)
|
|
{
|
|
T a, b, fa, fb;
|
|
bool ok;
|
|
std::tie(a, b, fa, fb, ok) = findEnclosingBracket(f, x, factor, Nmax);
|
|
if(ok)
|
|
return TOMS748_solve1(f, a, b, fa, fb, tol, Nmax);
|
|
else
|
|
{
|
|
internal::swap_abs_ab_fafb_if_not_ascending(fa, fb, a, b);
|
|
return std::make_tuple(a, fa, false);
|
|
}
|
|
}
|
|
}// namespace TOMS748
|
|
|
|
#endif
|