b2numerical_differentiation.H

//------------------------------------------------------------------------
// b2numerical_differentiation.H --
//
// written by Mathias Doreille
//
// (c) 2011 SMR Engineering & Development SA
//               2502 Bienne, Switzerland
//
// All Rights Reserved.  Proprietary source code.  The contents of
// this file may not be disclosed to third parties, copied or
// duplicated in any form, in whole or in part, without the prior
// written permission of SMR.
//------------------------------------------------------------------------
 
#ifndef B2NUMERICAL_DIFFERENTIATION_H_
#define B2NUMERICAL_DIFFERENTIATION_H_
 
#include <algorithm>
#include <cmath>
#include <vector>
 
namespace b2000 {
 
void get_central_coefficient(int l, int z, int p, std::vector<double>& a);
 
void get_forward_coefficient(int l, int z, int p, std::vector<double>& a);
 
template <typename FUNCTION, typename T, typename T1>
void central_numerical_differentiation(
      FUNCTION& f, const double x, std::vector<T>& res, int derivative_order = 1,
      int approximation_order = 2, int first_nonzero_derivative_order = 0, double h = 0);
 
template <typename FUNCTION, typename T, typename T1>
void forward_numerical_differentiation(
      FUNCTION& f, const double x, std::vector<T>& res, int derivative_order = 1,
      int approximation_order = 2, int first_nonzero_derivative_order = 0, double h = 0);
 
// implementation
 
template <typename FUNCTION, typename T, typename T1>
void central_numerical_differentiation(
      FUNCTION& f, const double x, std::vector<T>& res, int derivative_order,
      int approximation_order, int first_nonzero_derivative_order, double h) {
    std::vector<std::vector<double> > a(res.size());
    for (unsigned int i = 0; i != res.size(); ++i) {
        get_central_coefficient(
              derivative_order + i, first_nonzero_derivative_order, approximation_order, a[i]);
    }
    T1 r1, r2;
    T1 r1_m_r2;
    T1 r1_p_r2;
    const double inv_h = 1 / h;
    const double inv_h_l = std::pow(inv_h, derivative_order);
    for (unsigned int j = 1; j <= a.back().size() - 1; ++j) {
        f(x + h * j, r1);
        f(x - h * j, r2);
        bool b_r1_m_r2 = false;
        bool b_r1_p_r2 = false;
        double inv_h_li = inv_h_l;
        for (unsigned int i = 0; i != res.size(); ++i, inv_h_li *= inv_h) {
            if (j >= a[i].size()) { continue; }
            if ((derivative_order + i) % 2 == 0) {
                if (!b_r1_p_r2) {
                    r1_p_r2 = r1 + r2;
                    b_r1_p_r2 = true;
                }
                res[i] += (inv_h_li * a[i][j]) * r1_p_r2;
            } else {
                if (!b_r1_m_r2) {
                    r1_m_r2 = r1 - r2;
                    b_r1_m_r2 = true;
                }
                res[i] += (inv_h_li * a[i][j]) * r1_m_r2;
            }
        }
    }
    {
        double inv_h_li = inv_h_l;
        for (unsigned int i = 0; i != res.size(); ++i, inv_h_li *= inv_h) {
            if (a[i].empty()) { continue; }
            bool isc = false;
            if (a[i][0] != 0) {
                if (!isc) {
                    f(x, r1);
                    isc = true;
                }
                res[i] += (inv_h_li * a[i][0]) * r1;
            }
        }
    }
}
 
template <typename FUNCTION, typename T, typename T1>
void forward_numerical_differentiation(
      FUNCTION& f, const double x, std::vector<T>& res, int derivative_order,
      int approximation_order, int first_nonzero_derivative_order, double h) {
    std::vector<std::vector<double> > a(res.size());
    for (unsigned int i = 0; i != res.size(); ++i) {
        get_forward_coefficient(
              derivative_order + i, first_nonzero_derivative_order, approximation_order, a[i]);
    }
    const int max_m =
          derivative_order + res.size() + approximation_order - first_nonzero_derivative_order;
    T1 r;
    const double inv_h = 1 / h;
    const double inv_h_l = std::pow(inv_h, derivative_order);
    for (int j = 0; j <= max_m; ++j) {
        bool isc = false;
        double inv_h_li = inv_h_l;
        for (unsigned int i = std::max(0, int(res.size()) + j - 1 - max_m); i != res.size();
             ++i, inv_h_li *= inv_h) {
            if (a[i][j] != 0) {
                if (!isc) {
                    f(x + h * j, r);
                    isc = true;
                }
                res[i] += (inv_h_li * a[i][j]) * r;
            }
        }
    }
}
 
}  // namespace b2000
 
#endif /* B2NUMERICAL_DIFFERENTIATION_H_ */