TOMS748/cpp/TOMS748.hpp

#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