Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 14:31:55

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