RungeKutta/src/RKAdaptiveSolver.icc

0001 #include "RKAdaptiveSolver.h"
0002 #include <cmath>
0003
0004 //#define DEBUG_PRINTS
0005
0006 namespace RKDetails {
0007   // From http://martin.ankerl.com/2012/01/25/optimized-approximative-pow-in-c-and-cpp/
0008   // good at 10%
0009   inline double fastPow(double a, double b) {
0010     union {
0011       double d;
0012       int x[2];
0013     } u = {a};
0014     u.x[1] = (int)(b * (u.x[1] - 1072632447) + 1072632447);
0015     u.x[0] = 0;
0016     return u.d;
0017   }
0018
0019 }  // namespace RKDetails
0020
0021 template <typename T, template <typename, int> class StepWithPrec, int N>
0022 typename RKAdaptiveSolver<T, StepWithPrec, N>::Vector RKAdaptiveSolver<T, StepWithPrec, N>::operator()(
0023     Scalar startPar,
0024     const Vector& startState,
0025     Scalar step,
0026     const RKDerivative<T, N>& deriv,
0027     const RKDistance<T, N>& dist,
0028     float eps) {
0029   using namespace RKDetails;
0030   constexpr float Safety = 0.9;
0031   double remainigStep = step;
0032   double stepSize = step;  // attempt to solve in one step
0033   Scalar currentPar = startPar;
0034   Vector currentStart = startState;
0035 #ifdef DEBUG_PRINTS
0036   int nsteps = 0;
0037 #endif
0038   std::pair<Vector, Scalar> tryStep;
0039
0040   StepWithPrec<T, N> stepWithAccuracy;
0041
0042   do {
0043     tryStep = stepWithAccuracy(currentPar, currentStart, deriv, dist, stepSize);
0044     float acc = tryStep.second;
0045     // assert(eps>0);
0046     // assert(acc>=0);
0047 #ifdef DEBUG_PRINTS
0048     nsteps++;
0049 #endif
0050     if (acc < eps || std::abs(stepSize) < std::abs(remainigStep) * 0.1f) {
0051       if (std::abs(remainigStep - stepSize) < 0.5f * eps) {
0052 #ifdef DEBUG_PRINTS
0053         std::cout << "Accuracy reached, and full step taken in " << nsteps << " steps" << std::endl;
0054 #endif
0055         return tryStep.first;  // we are there
0056       } else {
0057         remainigStep -= stepSize;
0058         currentPar += stepSize;
0059         // increase step size
0060         // double factor =  std::min( Safety * pow( std::fabs(eps/tryStep.second),0.2), 4.); // gives division by 0 FPE
0061         const float cut = std::pow(4.f / Safety, 5.f);
0062         // float factor =  (eps < cut*acc) ? Safety * std::pow(eps/acc,0.2f) : 4.f;
0063         float factor = (eps < cut * acc) ? Safety * fastPow(eps / acc, 0.2) : 4.f;
0064         // stepSize = std::min( stepSize*factor, remainigStep);
0065         double absRemainingStep = std::abs(remainigStep);
0066         double absSize = std::min(std::abs(stepSize * factor), absRemainingStep);
0067         if (absSize < 0.05f * absRemainingStep)
0068           absSize = 0.05f * absRemainingStep;
0069         stepSize = std::copysign(absSize, stepSize);
0070         currentStart = tryStep.first;
0071 #ifdef DEBUG_PRINTS
0072         std::cout << "Accuracy reached, but " << remainigStep << " remain after " << nsteps
0073                   << " steps. Step size increased by " << factor << " to " << stepSize << std::endl;
0074 #endif
0075       }
0076     } else {
0077       // decrease step size
0078       constexpr float cut = Safety * Safety * Safety * Safety * 100 * 100;
0079       float factor = (cut * eps > acc) ?
0080                                        // Safety * std::sqrt(std::sqrt(eps/acc)) : 0.1f;
0081                          Safety * fastPow(eps / acc, 0.25)
0082                                        : 0.1f;
0083       stepSize *= factor;
0084       if (std::abs(stepSize) < 0.05f * std::abs(remainigStep))
0085         stepSize = 0.05f * remainigStep;
0086 #ifdef DEBUG_PRINTS
0087       std::cout << "Accuracy not yet reached: delta = " << tryStep.second << ", step reduced by " << factor << " to "
0088                 << stepSize << std::endl;
0089 #endif
0090     }
0091   } while (std::abs(remainigStep) > eps * 0.5f);
0092
0093   return tryStep.first;
0094 }