167 lines
6.1 KiB
C++
167 lines
6.1 KiB
C++
#ifndef DEF_TOMS748
|
|
#define DEF_TOMS748
|
|
|
|
#include <tuple>
|
|
|
|
namespace TOMS748
|
|
{
|
|
namespace internal
|
|
{
|
|
// 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(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;
|
|
}
|
|
}// namespace internal
|
|
|
|
/// Algorithm 4.1 from TOMS748 of robust root-solving.
|
|
/// Use this version if f(a) and f(b) have already been computed.
|
|
template<typename Func, typename T> std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T fa, T fb, T tol, 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(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(fabs(fa) < fabs(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(fabs(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
|
|
}
|
|
|
|
template<typename Func, typename T> std::tuple<T,T,bool> TOMS748_solve1(Func f, T a, T b, T tol, int Nmax = 1000)
|
|
{
|
|
return TOMS748_solve1(f, a, b, f(a), f(b), tol, Nmax);
|
|
}
|
|
}// namespace TOMS748
|
|
|
|
#endif
|