Line data    Source code

       1             : #include "utils.h"
       2             : 
       3             : namespace elsa
       4             : {
       5             : 
       6             :     template <typename data_t>
       7             :     data_t cubic_interpolation(data_t x0, data_t f0, data_t der_f0, data_t x1, data_t f1, data_t x2,
       8             :                                data_t f2)
       9         467 :     {
      10         467 :         data_t d1 = x1 - x0;
      11         467 :         data_t d2 = x2 - x0;
      12         467 :         data_t d1_2 = d1 * d1;
      13         467 :         data_t d2_2 = d2 * d2;
      14         467 :         data_t denominator = d1_2 * d2_2 * (d1 - d2);
      15         467 :         data_t r1 = f1 - f0 - der_f0 * d1;
      16         467 :         data_t r2 = f2 - f0 - der_f0 * d2;
      17             :         // TODO: check if denominator is 0 or very small
      18         467 :         data_t a = (d2_2 * r1 - d1_2 * r2) / denominator;
      19         467 :         data_t b = (-d2_2 * d2 * r1 + d1_2 * d1 * r2) / denominator;
      20         467 :         return x0 + (-b + std::sqrt(static_cast<data_t>(b * b - 3 * a * der_f0))) / (3 * a);
      21         467 :     }
      22             : 
      23             :     template float cubic_interpolation<float>(float x0, float f0, float der_f0, float x1, float f1,
      24             :                                               float x2, float f2);
      25             :     template double cubic_interpolation<double>(double x0, double f0, double der_f0, double x1,
      26             :                                                 double f1, double x2, double f2);
      27             : } // namespace elsa