Algo748.hh

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco, Mattia Piazza and Enrico Bertolazzi.                       *
 *                                                                                               *
 * The Optimist project is distributed under the BSD 2-Clause License.                           *
 *                                                                                               *
 * Davide Stocco                          Mattia Piazza                        Enrico Bertolazzi *
 * University of Trento               University of Trento                  University of Trento *
 * davide.stocco@unitn.it            mattia.piazza@unitn.it           enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
#pragma once
 
#ifndef OPTIMIST_ROOTFINDER_ALGO748_HH
#define OPTIMIST_ROOTFINDER_ALGO748_HH
 
#include "Optimist/RootFinder/Bracketing.hh"
 
namespace Optimist
{
  namespace RootFinder
  {
 
    /*\
     |      _    _           _____ _  _    ___
     |     / \  | | __ _  __|___  | || |  ( _ )
     |    / _ \ | |/ _` |/ _ \ / /| || |_ / _ \
     |   / ___ \| | (_| | (_) / / |__   _| (_) |
     |  /_/   \_\_|\__, |\___/_/     |_|  \___/
     |             |___/
    \*/

    template <typename Real>
    class Algo748 : public Bracketing<Real, Algo748<Real>>
    {
    public:
      static constexpr bool requires_function{true};
      static constexpr bool requires_first_derivative{false};
      static constexpr bool requires_second_derivative{false};
 
      OPTIMIST_BASIC_CONSTANTS(Real)
 
      
      Algo748() {}

      std::string name_impl() const {return "Algo748";}
 
    private:
      bool all_different(Real a, Real b, Real c, Real d) const {
        return a != b && a != c && a != d && b != c && b != d && c != d;
      }

      template <typename FunctionLambda>
      bool bracketing(FunctionLambda && function)
      {
        #define CMD "Optimist::ScalarRootfinder::Algo748::bracketing(...): "
 
        {
          Real tolerance{0.7*this->m_tolerance_bracketing};
          Real hba{(this->m_b - this->m_a)/2.0};
          if (hba <= tolerance) {this->m_c = this->m_a + hba;}
          else if (this->m_c <= this->m_a + tolerance) {this->m_c = this->m_a + tolerance;}
          else if (this->m_c >= this->m_b - tolerance) {this->m_c = this->m_b - tolerance;}
        }
 
        // If f(c) = 0 set a = c and return true
        bool success{this->evaluate_function(std::forward<FunctionLambda>(function), this->m_c, this->m_fc)};
        OPTIMIST_ASSERT_WARNING(success,
          CMD "function evaluation failed during bracketing.");
        if (this->m_fc == 0) {
          this->m_a = this->m_c; this->m_fa = 0.0;
          this->m_d = 0.0;       this->m_fd = 0.0;
          return true;
        } else {
          // If f(c) is not zero, then determine the new enclosing interval
          if (this->m_fa*this->m_fc < 0) {
            // D <- B <- C
            this->m_d = this->m_b; this->m_fd = this->m_fb;
            this->m_b = this->m_c; this->m_fb = this->m_fc;
          } else {
            // D <- A <- C
            this->m_d = this->m_a; this->m_fd = this->m_fa;
            this->m_a = this->m_c; this->m_fa = this->m_fc;
          }
          return false;
        }
 
        #undef CMD
      }

      Real cubic_interpolation()
      {
        // Compute the coefficients for the inverse cubic interpolation
        Real d_1{this->m_b - this->m_a};
        Real d_2{this->m_d - this->m_a};
        Real d_3{this->m_e - this->m_a};
 
        Real dd_0{d_1/(this->m_fb - this->m_fa)};
        Real dd_1{(d_1-d_2)/(this->m_fb - this->m_fd)};
        Real dd_2{(d_2-d_3)/(this->m_fd - this->m_fe)};
 
        Real ddd_0 {(dd_0 - dd_1)/(this->m_fa - this->m_fd)};
        Real ddd_1 {(dd_1 - dd_2)/(this->m_fb - this->m_fe)};
 
        Real dddd_0{(ddd_0-ddd_1)/(this->m_fa-this->m_fe)};
 
        // Compute the approximate root
        Real root{this->m_a - this->m_fa*(dd_0 - this->m_fb*(ddd_0 - this->m_fd*dddd_0))};
        Real tolerance{0.7*this->m_tolerance_bracketing};
        if (root <= this->m_a + tolerance || root >= this->m_b - tolerance) {
          root = (this->m_a + this->m_b)/2.0;
        }
        return root;
      }

      Real newton_quadratic(Integer n)
      {
        // Compute the coefficients for the quadratic interpolation
        Real a_0{this->m_fa};
        Real a_1{(this->m_fb - this->m_fa)/(this->m_b - this->m_a)};
        Real a_2{((this->m_fd - this->m_fb)/(this->m_d - this->m_b)-a_1)/(this->m_d - this->m_a)};
 
        // Safeguard to avoid overflow
        if (a_2 == 0) {return this->m_a - a_0/a_1;}
 
        // Determine the starting point of newton steps
        Real c{a_2*this->m_fa > 0.0 ? this->m_a : this->m_b};
 
        // Start the safeguarded newton steps
        bool is_nonzero{true};
        Real pc{0.0}, pdc{0.0};
        for (Integer i{0}; i < n && is_nonzero ; ++i ) {
          pc  = a_0 + (a_1 + a_2*(c - this->m_b))*(c - this->m_a);
          pdc = a_1 + a_2*((2.0 * c) - (this->m_a + this->m_b));
          is_nonzero = pdc != 0;
          if (is_nonzero) {c -= pc/pdc;}
        }
 
        if (is_nonzero) {return c;}
        else {return this->m_a - a_0/a_1;}
      }
 
    public:
      template <typename FunctionLambda>
      Real find_root_impl(FunctionLambda && function)
      {
        #define CMD "Optimist::ScalarRootfinder::Algo748::find_root_impl(...): "
 
        bool success;
        Real tolerance_step{this->m_tolerance_bracketing};
        Real tolerance_function{this->m_tolerance_bracketing};
 
        // Check for trivial solution
        this->m_converged = this->m_fa == 0; if (this->m_converged) {return this->m_a;}
        this->m_converged = this->m_fb == 0; if (this->m_converged) {return this->m_b;}
 
        // Initialize to dumb values
        this->m_e  = QUIET_NAN;
        this->m_fe = QUIET_NAN;
 
        // While f(left) or f(right) are infinite perform bisection
        while (!(std::isfinite(this->m_fa) && std::isfinite(this->m_fb))) {
          ++this->m_iterations;
          this->m_c  = (this->m_a + this->m_b)/2.0;
          success = this->evaluate_function(std::forward<FunctionLambda>(function), this->m_c, this->m_fc);
          OPTIMIST_ASSERT_WARNING(success,
            CMD "function evaluation failed at iteration " << this->m_iterations << ".");
          this->m_converged = this->m_fc == 0;
          if (this->m_converged) {return this->m_c;}
          if (this->m_fa*this->m_fc < 0.0) {
            // -> [a, c]
            this->m_b = this->m_c; this->m_fb = this->m_fc;
          } else {
            // -> [c, b]
            this->m_a = this->m_c; this->m_fa = this->m_fc;
          }
 
          // Check for convergence
          Real abs_fa{std::abs(this->m_fa)};
          Real abs_fb{std::abs(this->m_fb)};
          this->m_converged = (this->m_b - this->m_a) < tolerance_step ||
                               abs_fa < tolerance_function ||abs_fb < tolerance_function;
          if (this->m_converged) {return abs_fb < abs_fa ? this->m_b : this->m_a;}
        }
 
        {
          // Impede the step c to be too close to the bounds a or b.
          Real b_a{this->m_b  - this->m_a};
          Real r{b_a/(this->m_fb - this->m_fa)};
          this->m_c = std::abs(this->m_fb) < std::abs(this->m_fa) ?
            this->m_b + this->m_fb*r : this->m_a - this->m_fa*r;
          Real delta{this->m_interval_shink*b_a}, a_min, b_max;
          if (this->m_c < (a_min = this->m_a + delta)) {this->m_c = a_min;}
          else if (this->m_c > (b_max = this->m_b - delta)) {this->m_c = b_max;}
        }
 
        // Call "bracketing" to get a shrinked enclosing interval and to update the termination criterion
        this->m_converged = this->bracketing(std::forward<FunctionLambda>(function));
        if (this->m_converged ) {return this->m_a;}
        this->m_converged = false;
 
        // The enclosing interval is recorded as [a0, b0] before executing the iteration steps
        while (++this->m_iterations < this->m_max_iterations) {
          ++this->m_iterations;
          Real a0_b0{this->m_b - this->m_a};
 
          // Check the termination criterion
          {
            Real abs_fa {std::abs(this->m_fa)};
            Real abs_fb {std::abs(this->m_fb)};
            this->m_converged = abs_fa < tolerance_function || abs_fb < tolerance_function;
            if (this->m_converged) {return abs_fa < abs_fb ? this->m_a : this->m_b;}
          }
 
          // Starting with the second iteration: newton quadratic or cubic interpolation
          bool do_newton_quadratic{false};
          if (!std::isfinite(this->m_fe)) {
            do_newton_quadratic = true;
          } else if (!this->all_different( this->m_fa, this->m_fb, this->m_fd, this->m_fe)) {
            do_newton_quadratic = true;
          } else {
            this->m_c = this->cubic_interpolation();
            do_newton_quadratic = (this->m_c-this->m_a)*(this->m_c-this->m_b) >= 0.0;
          }
          if (do_newton_quadratic) {this->m_c = this->newton_quadratic(2);}
 
          this->m_e  = this->m_d;
          this->m_fe = this->m_fd;
 
          // Call "bracketing" to get a shrinked enclosing interval and to update the termination criterion
          this->m_converged = this->bracketing(std::forward<FunctionLambda>(function)) || (this->m_b-this->m_a) <= this->m_tolerance;
          if (this->m_converged) {
            return std::abs(this->m_fa) < std::abs(this->m_fb) ? this->m_a : this->m_b;
          }
          if (!this->all_different(this->m_fa, this->m_fb, this->m_fd, this->m_fe)) {
            do_newton_quadratic = true;
          } else {
            this->m_c = this->cubic_interpolation();
            do_newton_quadratic = (this->m_c - this->m_a)*(this->m_c - this->m_b) >= 0.0;
          }
          if (do_newton_quadratic) {this->m_c = this->newton_quadratic(3);}
 
          // Call "bracketing" to get a shrinked enclosing interval and to update the termination criterion
          {
            this->m_converged = this->bracketing(std::forward<FunctionLambda>(function)) || (this->m_b-this->m_a) <= this->m_tolerance;
            Real abs_fa{std::abs(this->m_fa)};
            Real abs_fb{std::abs(this->m_fb)};
            if (this->m_converged) {return abs_fa < abs_fb ? this->m_a : this->m_b;}
 
            this->m_e  = this->m_d;
            this->m_fe = this->m_fd;
 
            // Takes the double-size secant step
            Real u, fu;
            if (abs_fa < abs_fb) {u = this->m_a; fu = this->m_fa;}
            else {u = this->m_b; fu = this->m_fb;}
            Real hba{(this->m_b - this->m_a)/2.0};
            this->m_c = u - 4.0*(fu/(this->m_fb - this->m_fa))*hba;
            if (std::abs(this->m_c - u) > hba) {this->m_c = this->m_a + hba;}
          }
 
          // Call "bracketing" to get a shrinked enclosing interval and to update the termination criterion
          this->m_converged = this->bracketing(std::forward<FunctionLambda>(function)) || (this->m_b-this->m_a) <= this->m_tolerance_bracketing;
          if (this->m_converged) {
            return std::abs(this->m_fa) < std::abs(this->m_fb) ? this->m_a : this->m_b;
          }
 
          // Determine whether an additional bisection step is needed
          if ((this->m_b-this->m_a) < this->m_mu*a0_b0) {continue;}
 
          this->m_e  = this->m_d;
          this->m_fe = this->m_fd;
 
          // Call "bracketing" to get a shrinked enclosing interval and to update the termination criterion
          {
            Real b_a{this->m_b-this->m_a};
            this->m_c = this->m_a + b_a/2.0;
            this->m_converged = this->bracketing(std::forward<FunctionLambda>(function)) || b_a <= this->m_tolerance_bracketing;
          }
        }
        // Return the approximate root
        return std::abs(this->m_fa) < std::abs(this->m_fb) ? this->m_a : this->m_b;
 
        #undef CMD
      }
 
    }; // class Algo748
 
  } // namespace RootFinder
 
} // namespace Optimist
 
#endif // OPTIMIST_ROOTFINDER_ALGO748_HH