Shinya Kitaoka 810553
#pragma once
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#ifndef TZEROFINDER_H
Toshihiro Shimizu 890ddd
#define TZEROFINDER_H
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include <cmath></cmath>
Toshihiro Shimizu 890ddd
#include <assert.h></assert.h>
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
enum { FZ_OK = 0, FZ_FTOL = 1, FZ_XTOL = 2, FZ_MAX_ITER = 3, FZ_NOT_ZERO = 4 };
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//Daniele
Toshihiro Shimizu 890ddd
//  Use the one at the end instead. This may overshoot - plus, it does not
Toshihiro Shimizu 890ddd
//  bisecate the interval... (I suppose it has never been used much :) )
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template< class  unaryOp        ,
Toshihiro Shimizu 890ddd
          class  unaryOpDerivate>
Toshihiro Shimizu 890ddd
bool  findZero_Newton ( double          x0      ,
Toshihiro Shimizu 890ddd
                        double          x1      ,
Toshihiro Shimizu 890ddd
                        unaryOp         f       ,
Toshihiro Shimizu 890ddd
                        unaryOpDerivate fd1     ,
Toshihiro Shimizu 890ddd
                        double          xtol    ,
Toshihiro Shimizu 890ddd
                        double          ftol    ,
Toshihiro Shimizu 890ddd
                        int             maxIter ,
Toshihiro Shimizu 890ddd
                        double          &x      ,
Toshihiro Shimizu 890ddd
                        int             &err    )
Toshihiro Shimizu 890ddd
{
Toshihiro Shimizu 890ddd
  double  f_x_n_plus_1  = 0.0   ;
Toshihiro Shimizu 890ddd
  double  f_x_n         = f(x1) ;
Toshihiro Shimizu 890ddd
  double  f_x_n_minus_1 = f(x0) ;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if( f_x_n*f_x_n_minus_1 > 0 )
Toshihiro Shimizu 890ddd
  {
Toshihiro Shimizu 890ddd
    err = FZ_NOT_ZERO;
Toshihiro Shimizu 890ddd
    return  false;
Toshihiro Shimizu 890ddd
  }
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
  int i;
Toshihiro Shimizu 890ddd
  err=FZ_OK;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
  if( f_x_n         == 0  ||
Shinya Kitaoka 120a6e
      f_x_n_minus_1 == 0    )
Toshihiro Shimizu 890ddd
  {
Toshihiro Shimizu 890ddd
    x   = (f_x_n == 0.0) ? x1 : x0;
Shinya Kitaoka 120a6e
    err = FZ_OK;
Toshihiro Shimizu 890ddd
    return true;
Toshihiro Shimizu 890ddd
  }
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
  double  x_n_plus_1  ;
Toshihiro Shimizu 890ddd
  double  x_n         ;
Toshihiro Shimizu 890ddd
  double  x_n_minus_1 ;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  x_n         = x1;
Toshihiro Shimizu 890ddd
  x_n_minus_1 = x0;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
  for(i=0;i
Toshihiro Shimizu 890ddd
  {
Toshihiro Shimizu 890ddd
    // the firts time x_n = x1
Toshihiro Shimizu 890ddd
    double  fd1_x_n = fd1(x_n);
Toshihiro Shimizu 890ddd
    assert( fd1_x_n != 0.0 );
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
    x_n_plus_1 = x_n - f_x_n / fd1_x_n;
Toshihiro Shimizu 890ddd
    if( fabs(x_n - x_n_plus_1) < xtol )
Shinya Kitaoka 120a6e
    {
Toshihiro Shimizu 890ddd
      err=FZ_XTOL;
Toshihiro Shimizu 890ddd
      break;
Toshihiro Shimizu 890ddd
    }
Shinya Kitaoka 120a6e
Toshihiro Shimizu 890ddd
    f_x_n_plus_1 = f(x_n_plus_1);
Toshihiro Shimizu 890ddd
    if( fabs(f_x_n_plus_1) < ftol )
Shinya Kitaoka 120a6e
    {
Toshihiro Shimizu 890ddd
      err=FZ_FTOL;
Toshihiro Shimizu 890ddd
      break;
Toshihiro Shimizu 890ddd
    }
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
    x_n_minus_1   = x_n;
Toshihiro Shimizu 890ddd
    x_n           = x_n_plus_1;
Toshihiro Shimizu 890ddd
    f_x_n         = f_x_n_plus_1;
Toshihiro Shimizu 890ddd
  }
Shinya Kitaoka 120a6e
Toshihiro Shimizu 890ddd
  x=x_n_plus_1;
Shinya Kitaoka 120a6e
  err = (i == maxIter ? FZ_MAX_ITER : err);
Toshihiro Shimizu 890ddd
  return true;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
*/
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*!
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
  Bisection algorithm.
Toshihiro Shimizu 890ddd
  Get an object and a method of object class.
Toshihiro Shimizu 890ddd
  x1 and x2 are extremes of test and xtol
Toshihiro Shimizu 890ddd
  is the accuracy.
Shinya Kitaoka 120a6e
Toshihiro Shimizu 890ddd
    Return:
Toshihiro Shimizu 890ddd
    the minimum or,
Toshihiro Shimizu 890ddd
    -2  if min it's unreacheable.
Toshihiro Shimizu 890ddd
    -1  if min does not exist
Toshihiro Shimizu 890ddd
    Remark:
Toshihiro Shimizu 890ddd
    Max iterations numbers is fixed to 100 in in maxIter.
Toshihiro Shimizu 890ddd
    This value fix the accuracy to 2e-100.
Shinya Kitaoka 120a6e
Toshihiro Shimizu 890ddd
      From Numerical Recipes.
Toshihiro Shimizu 890ddd
*/
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <class unaryop=""></class>
Shinya Kitaoka 120a6e
bool findZero_bisection(double x0, double x1, unaryOp f, double xtol,
Shinya Kitaoka 120a6e
                        int maxIter, double &x, int &err) {
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  err = FZ_OK;
Shinya Kitaoka 120a6e
  double dx, fx0, fmid, xmid, rtb;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  fx0  = f(x0);
Shinya Kitaoka 120a6e
  fmid = f(x1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fx0 * fmid > 0.0) {
Shinya Kitaoka 120a6e
    err = FZ_NOT_ZERO;
Shinya Kitaoka 120a6e
    return false;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fx0 == 0 || fmid == 0) {
Shinya Kitaoka 120a6e
    x   = (fmid == 0.0) ? x1 : x0;
Shinya Kitaoka 120a6e
    err = FZ_OK;
Shinya Kitaoka 120a6e
    return true;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  rtb = fx0 < 0.0 ? (dx = x1 - x0, x0) : (dx = x0 - x1, x1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 1; i <= maxIter; i++) {
Shinya Kitaoka 120a6e
    fmid = f(xmid        = rtb + (dx *= 0.5));
Shinya Kitaoka 120a6e
    if (fmid <= 0.0) rtb = xmid;
Shinya Kitaoka 120a6e
    if (fabs(dx) < xtol || fmid == 0.0) {
Shinya Kitaoka 120a6e
      x   = rtb;
Shinya Kitaoka 120a6e
      err = FZ_XTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  err = (i == maxIter ? FZ_MAX_ITER : err);
Shinya Kitaoka 120a6e
  return true;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <class unaryop=""></class>
Shinya Kitaoka 120a6e
bool findZero_secant(double x0, double x1, unaryOp f, double xtol, double ftol,
Shinya Kitaoka 120a6e
                     int maxIter, double &x, int &err) {
Shinya Kitaoka 120a6e
  double f_x_n_plus_1 = 0.0, f_x_n = f(x1), f_x_n_minus_1 = f(x0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (f_x_n * f_x_n_minus_1 > 0) {
Shinya Kitaoka 120a6e
    err = FZ_NOT_ZERO;
Shinya Kitaoka 120a6e
    return false;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  err = FZ_OK;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (f_x_n == 0 || f_x_n_minus_1 == 0) {
Shinya Kitaoka 120a6e
    x = (f_x_n == 0.0) ? x1 : x0;
Shinya Kitaoka 120a6e
    return true;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double x_n_plus_1 = x1, x_n = x1, x_n_minus_1 = x0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 0; i < maxIter; ++i) {
Shinya Kitaoka 120a6e
    double den = f_x_n - f_x_n_minus_1;
Shinya Kitaoka 120a6e
    assert(den != 0.0);
Shinya Kitaoka 120a6e
    if (den == 0.0) {
Shinya Kitaoka 120a6e
      x_n = x_n_plus_1;
Shinya Kitaoka 120a6e
      err = FZ_FTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    x_n_plus_1 = x_n - (x_n - x_n_minus_1) * f_x_n / den;
Shinya Kitaoka 120a6e
    if (fabs(x_n - x_n_plus_1) < xtol) {
Shinya Kitaoka 120a6e
      err = FZ_XTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    f_x_n_plus_1 = f(x_n_plus_1);
Shinya Kitaoka 120a6e
    if (fabs(f_x_n_plus_1) < ftol) {
Shinya Kitaoka 120a6e
      err = FZ_FTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    x_n_minus_1   = x_n;
Shinya Kitaoka 120a6e
    x_n           = x_n_plus_1;
Shinya Kitaoka 120a6e
    f_x_n_minus_1 = f_x_n;
Shinya Kitaoka 120a6e
    f_x_n         = f_x_n_plus_1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  x   = x_n_plus_1;
Shinya Kitaoka 120a6e
  err = (i == maxIter ? FZ_MAX_ITER : err);
Shinya Kitaoka 120a6e
  return true;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <class class="" unaryop1,="" unaryop2=""></class>
Shinya Kitaoka 120a6e
bool findZero_Newton(double x0, double x1, unaryOp1 f, unaryOp2 f1, double xtol,
Shinya Kitaoka 120a6e
                     double ftol, int maxIter, double &x, int &err) {
Shinya Kitaoka 120a6e
  double f_0   = f(x0);
Shinya Kitaoka 120a6e
  double f_1   = f(x1);
Shinya Kitaoka 120a6e
  double f_x_n = f_0, f_x_n_plus_1 = 0.0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  bool s_0 = (f_0 > 0);
Shinya Kitaoka 120a6e
  bool s_1 = (f_1 > 0);
Shinya Kitaoka 120a6e
  bool s_n;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (s_0 && s_1) {
Shinya Kitaoka 120a6e
    err = FZ_NOT_ZERO;
Shinya Kitaoka 120a6e
    return false;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  err = FZ_OK;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (f_0 == 0 || f_1 == 0) {
Shinya Kitaoka 120a6e
    x = (f_1 == 0.0) ? x1 : x0;
Shinya Kitaoka 120a6e
    return true;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double x_n = x1, x_n_plus_1;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 0; i < maxIter; ++i) {
Shinya Kitaoka 120a6e
    double den                 = f1(x_n);
Shinya Kitaoka 120a6e
    if (den > 1e-2) x_n_plus_1 = x_n - f_x_n / den;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (den <= 1e-2 || x0 > x_n_plus_1 || x1 < x_n_plus_1)
Shinya Kitaoka 120a6e
      x_n_plus_1 = 0.5 * (x0 + x1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (fabs(x_n - x_n_plus_1) < xtol) {
Shinya Kitaoka 120a6e
      err = FZ_XTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    f_x_n_plus_1 = f(x_n_plus_1);
Shinya Kitaoka 120a6e
    if (fabs(f_x_n_plus_1) < ftol) {
Shinya Kitaoka 120a6e
      err = FZ_FTOL;
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    x_n   = x_n_plus_1;
Shinya Kitaoka 120a6e
    f_x_n = f_x_n_plus_1;
Shinya Kitaoka 120a6e
    s_n   = (f_x_n > 0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (s_n == s_0)
Shinya Kitaoka 120a6e
      x0 = x_n;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      x1 = x_n;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  x   = x_n_plus_1;
Shinya Kitaoka 120a6e
  err = (i == maxIter ? FZ_MAX_ITER : err);
Shinya Kitaoka 120a6e
  return true;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#endif