RK5ODEIntegrator.h

Go to the documentation of this file.

#ifndef _utl_RK5ODEIntegrator_h_
#define _utl_RK5ODEIntegrator_h_

#include <cmath>


namespace utl {

  class DefaultRK5ErrorScaling {
  public:
    template<class VectorType>
    void
    operator()(const unsigned int n, const double dx,
               const VectorType& y, const VectorType& dYdX,
               VectorType& errorScaling)
      const
    {
      for (unsigned int i = 0; i < n; ++i)
        errorScaling[i] = std::abs(y[i]) + std::abs(dx*dYdX[i]) + 1e-30;
    }
  };


  template<class DerivativeFunctor, class VectorType>
  class RK5Iterator {
  public:
    RK5Iterator(const double x, const VectorType& y,
                DerivativeFunctor& d)
      : fX(x), fDerivativeFunctor(d), fStatus(true)
    {
      Copy(d, y, fY);
      Clear(d, fYError);
    }

    double GetX() const { return fX; }

    VectorType& GetY() { return fY; }

    const VectorType& GetY() const { return fY; }

    const VectorType& GetYError() const { return fYError; }

    bool
    Advance(const double dx, const VectorType& dYdX)
    {
      if (Step(dx, fY, dYdX)) {
        fX += dx;
        return fStatus = true;
      }
      return fStatus = false;
    }

    bool
    Advance(const double dx)
    {
      VectorType dYdX;
      fDerivativeFunctor(fX, fY, dYdX);
      return Advance(dx, dYdX);
    }

    RK5Iterator& operator+=(const double dx)
    { Advance(dx); return *this; }

    RK5Iterator& operator-=(const double dx)
    { Advance(-dx); return *this; }

    DerivativeFunctor& GetDerivativeFunctor()
    { return fDerivativeFunctor; }

    explicit operator bool() const { return fStatus; }

    void ClearStatus() { fStatus = true; }

  private:
    bool
    Step(const double dx, VectorType& yNew, const VectorType& dYdX)
    {
      const unsigned int n = fDerivativeFunctor;
      VectorType y1;
      VectorType dYdXt[5];
      for (unsigned int k = 0; k < 5; ++k) {
        for (unsigned int i = 0; i < n; ++i) {
          double dy = fB[k][0] * dYdX[i];
          for (unsigned int j = 1; j <= k; ++j)
            dy += fB[k][j] * dYdXt[j-1][i];
          y1[i] = fY[i] + dx * dy;
        }
        if (!fDerivativeFunctor(fX + fA[k] * dx, y1, dYdXt[k]))
          return fStatus = false;
      }
      for (unsigned int i = 0; i < n; ++i) {
        double dy = fC[0] * dYdX[i];
        double de = fD[0] * dYdX[i];
        for (unsigned int j = 1; j < 5; ++j) {
          dy += fC[j] * dYdXt[j][i];
          de += fD[j] * dYdXt[j][i];
        }
        yNew[i] = fY[i] + dx * dy;
        fYError[i] = dx * de;
      }
      return fStatus = true;
    }

    static
    void
    Copy(const unsigned int n, const VectorType& a, VectorType& b)
    {
      for (unsigned int i = 0; i < n; ++i)
        b[i] = a[i];
    }

    static
    void
    Clear(const unsigned int n, VectorType& b)
    {
      for (unsigned int i = 0; i < n; ++i)
        b[i] = 0;
    }

    double fX;
    VectorType fY;
    VectorType fYError;
    DerivativeFunctor& fDerivativeFunctor;
    bool fStatus;

    // constants
    static const double fA[6];
    static const double fB[5][5];
    static const double fC[5];
    static const double fD[5];

    template<class A, class B, class C>
    friend class AdaptiveRK5Iterator;
  };

  template<class DerivativeFunctor, class VectorType>
  const double RK5Iterator<DerivativeFunctor, VectorType>::fA[] = {
    0.2, 0.3, 0.6, 1, 0.875
  };

  template<class DerivativeFunctor, class VectorType>
  const double RK5Iterator<DerivativeFunctor, VectorType>::fB[][5] = {
    { 0.2 },
    { 0.075, 0.225 },
    { 0.3, -0.9, 1.2 },
    { -11./54, 2.5, -70./27, 35./27 },
    { 1631./55296, 175./512, 575./13824, 44275./110592, 253./4096 }
  };

  template<class DerivativeFunctor, class VectorType>
  const double RK5Iterator<DerivativeFunctor, VectorType>::fC[] = {
    37./378, 250./621, 125./594, 0, 512./1771
  };

  template<class DerivativeFunctor, class VectorType>
  const double RK5Iterator<DerivativeFunctor, VectorType>::fD[] = {
    RK5Iterator<DerivativeFunctor, VectorType>::fC[0] - 2825./27648,
    RK5Iterator<DerivativeFunctor, VectorType>::fC[1] - 18575./48384,
    RK5Iterator<DerivativeFunctor, VectorType>::fC[2] - 13525./55296,
    RK5Iterator<DerivativeFunctor, VectorType>::fC[3] - 277./14336,
    RK5Iterator<DerivativeFunctor, VectorType>::fC[4] - 0.25
  };


  template<class DerivativeFunctor,
           class VectorType,
           class ErrorScalingPolicy = DefaultRK5ErrorScaling>
  class AdaptiveRK5Iterator {
  public:
    AdaptiveRK5Iterator(const double x0, const double dx0,
                        const VectorType& y0,
                        DerivativeFunctor& d,
                        const double accuracy = 1e-5,
                        const ErrorScalingPolicy& errorScaling =
                          DefaultRK5ErrorScaling()) :
      fIterator(x0, y0, d),
      fNextStepSize(dx0),
      fAccuracy(accuracy),
      fErrorScaling(errorScaling)
    { }

    AdaptiveRK5Iterator(const double dx0,
                        const RK5Iterator<DerivativeFunctor, VectorType>& rk5It,
                        const double accuracy = 1e-5,
                        const ErrorScalingPolicy& errorScaling =
                          DefaultRK5ErrorScaling()) :
      fIterator(rk5It),
      fNextStepSize(dx0),
      fAccuracy(accuracy),
      fErrorScaling(errorScaling)
    { }

    double GetX() const { return fIterator.fX; }

    VectorType& GetY() { return fIterator.fY; }

    const VectorType& GetY() const { return fIterator.fY; }

    const VectorType& GetYError() const { return fIterator.fYError; }

    bool
    Advance(const double dx)
    {
      double& x = fIterator.fX;
      VectorType& y = fIterator.fY;
      VectorType& yError = fIterator.fYError;
      const unsigned int n = fIterator.GetDerivativeFunctor();

      VectorType dYdX;
      fIterator.GetDerivativeFunctor()(x, y, dYdX);
      VectorType y1;
      double dxAttempt = dx;
      double x1 = x + dxAttempt;
      VectorType scaling;
      fErrorScaling(n, dx, y, dYdX, scaling);
      double relErr;
      while (true) {
        double stepFactor = 0.1;
        if (fIterator.Step(dxAttempt, y1, dYdX)) {
          relErr = GetMaxScaledError(n, yError, scaling) / fAccuracy;
          if (relErr < 1)
            break;
          stepFactor = std::max(0.1, fSlightlyLess * std::pow(relErr, fPowerDown));
        }
        dxAttempt *= stepFactor;
        x1 = x + dxAttempt;
        if (x1 == x) {
          fNextStepSize = dx;
          return fIterator.fStatus = false;
        }
      }
      fNextStepSize = dxAttempt *
        ((relErr > fPowerLawThreshold) ? fSlightlyLess * std::pow(relErr, fPowerUp) : 5);
      x = x1;
      fIterator.Copy(n, y1, y);
      return fIterator.fStatus = true;
    }

    bool Advance() { return Advance(fNextStepSize); }

    AdaptiveRK5Iterator& operator+=(const double dx)
    { Advance(dx); return *this; }

    AdaptiveRK5Iterator& operator-=(const double dx)
    { Advance(-dx); return *this; }

    AdaptiveRK5Iterator& operator++()
    { Advance(); return *this; }

    double GetNextStepSize() const { return fNextStepSize; }

    void ClearStatus() { fIterator.ClearStatus(); }

    explicit operator bool() const { return fIterator; }

  private:
    static
    double
    GetMaxScaledError(const unsigned int n,
                      const VectorType& error, const VectorType& scaling)
    {
      double max = std::abs(error[0] / scaling[0]);
      for (unsigned int i = 1; i < n; ++i) {
        const double se = std::abs(error[i] / scaling[i]);
        if (se > max)
          max = se;
      }
      return max;
    }

    RK5Iterator<DerivativeFunctor, VectorType> fIterator;
    double fNextStepSize;
    double fAccuracy;
    const ErrorScalingPolicy& fErrorScaling;

    static const double fPowerUp;
    static const double fPowerDown;
    static const double fSlightlyLess;
    static const double fPowerLawThreshold;
  };

  template<
    class DerivativeFunctor,
    class VectorType,
    class ErrorScalingPolicy
  > const double AdaptiveRK5Iterator<
    DerivativeFunctor,
    VectorType,
    ErrorScalingPolicy
  >::fPowerUp = -0.2;

  template<
    class DerivativeFunctor,
    class VectorType,
    class ErrorScalingPolicy
  > const double AdaptiveRK5Iterator<
    DerivativeFunctor,
    VectorType,
    ErrorScalingPolicy
  >::fPowerDown = -0.25;

  template<
    class DerivativeFunctor,
    class VectorType,
    class ErrorScalingPolicy
  > const double AdaptiveRK5Iterator<
    DerivativeFunctor,
    VectorType,
    ErrorScalingPolicy
  >::fSlightlyLess = 0.9;

  template<
    class DerivativeFunctor,
    class VectorType,
    class ErrorScalingPolicy
  > const double AdaptiveRK5Iterator<
    DerivativeFunctor,
    VectorType,
    ErrorScalingPolicy
  >::fPowerLawThreshold = 1.889568e-4;


  template<class DerivativeFunctor>
  class RK5ODEIntegrator {
  public:
    RK5ODEIntegrator(DerivativeFunctor& d)
      : fDerivativeFunctor(d) { }

    template<class VectorType>
    RK5Iterator<DerivativeFunctor, VectorType>
    Begin(const double x, const VectorType& y)
    {
      return
        RK5Iterator
          <DerivativeFunctor, VectorType>(x, y, fDerivativeFunctor);
    }

    template<class VectorType>
    AdaptiveRK5Iterator<DerivativeFunctor, VectorType>
    AdaptiveBegin(const double x, const double dx, const VectorType& y,
                  const double accuracy = 1e-5)
    {
      return
        AdaptiveRK5Iterator<DerivativeFunctor, VectorType>(
          x, dx, y, fDerivativeFunctor, accuracy
        );
    }

    template<class VectorType,
             class ErrorScalingPolicy>
    AdaptiveRK5Iterator<DerivativeFunctor, VectorType, ErrorScalingPolicy>
    AdaptiveBegin(const double x, const double dx, const VectorType& y,
                  const double accuracy, ErrorScalingPolicy& es)
    {
      return
        AdaptiveRK5Iterator<DerivativeFunctor, VectorType>(
          x, dx, y, fDerivativeFunctor, accuracy, es
        );
    }

    DerivativeFunctor& GetDerivativeFunctor()
    { return fDerivativeFunctor; }

  private:
    DerivativeFunctor& fDerivativeFunctor;
  };

}


#endif