Line data    Source code

       1             : #include "Bessel.h"
       2             : #include "elsaDefines.h"
       3             : #include <unsupported/Eigen/SpecialFunctions>
       4             : 
       5             : namespace elsa::math
       6             : {
       7           0 :     double bessi0(double x)
       8             :     {
       9           0 :         double ax = std::abs(x);
      10             : 
      11             :         // polynomial fit, different polynoms for different ranges
      12           0 :         if (ax < 3.75) {
      13           0 :             const auto y = std::pow(x / 3.75, 2);
      14             : 
      15             :             // different terms
      16           0 :             const auto p0 = 0.45813e-2;
      17           0 :             const auto p1 = 0.360768e-1 + y * p0;
      18           0 :             const auto p2 = 0.2659732 + y * p1;
      19           0 :             const auto p3 = 1.2067492 + y * p2;
      20           0 :             const auto p4 = 3.0899424 + y * p3;
      21           0 :             const auto p5 = 3.5156229 + y * p4;
      22           0 :             return 1.0 + y * p5;
      23             :         } else {
      24           0 :             const auto y = 3.75 / ax;
      25             : 
      26             :             // different terms
      27           0 :             const auto p0 = 0.392377e-2;
      28           0 :             const auto p1 = -0.1647633e-1 + y * p0;
      29           0 :             const auto p2 = 0.2635537e-1 + y * p1;
      30           0 :             const auto p3 = -0.2057706e-1 + y * p2;
      31           0 :             const auto p4 = 0.916281e-2 + y * p3;
      32           0 :             const auto p5 = -0.157565e-2 + y * p4;
      33           0 :             const auto p6 = 0.225319e-2 + y * p5;
      34           0 :             const auto p7 = 0.1328592e-1 + y * p6;
      35           0 :             const auto p8 = 0.39894228 + y * p7;
      36             : 
      37           0 :             return (std::exp(ax) / std::sqrt(ax)) * p8;
      38             :         }
      39             :     }
      40             : 
      41           0 :     double bessi1(double x)
      42             :     {
      43           0 :         double result = 0;
      44           0 :         double ax = std::abs(x);
      45             : 
      46             :         // polynomial fit, different polynoms for different ranges
      47           0 :         if (ax < 3.75) {
      48           0 :             const auto y = std::pow(x / 3.75, 2);
      49             : 
      50           0 :             const auto p0 = 0.32411e-3;
      51           0 :             const auto p1 = 0.301532e-2 + y * p0;
      52           0 :             const auto p2 = 0.2658733e-1 + y * p1;
      53           0 :             const auto p3 = 0.15084934 + y * p2;
      54           0 :             const auto p4 = 0.51498869 + y * p3;
      55           0 :             const auto p5 = 0.87890594 + y * p4;
      56           0 :             const auto p6 = 0.5 + y * p5;
      57             : 
      58           0 :             result = ax * p6;
      59             :         } else {
      60           0 :             const auto y = 3.75 / ax;
      61             : 
      62           0 :             const auto p0 = 0.420059e-2;
      63           0 :             const auto p1 = 0.1787654e-1 - y * p0;
      64           0 :             const auto p2 = -0.2895312e-1 + y * p1;
      65           0 :             const auto p3 = 0.2282967e-1 + y * p2;
      66           0 :             const auto p4 = -0.1031555e-1 + y * p3;
      67           0 :             const auto p5 = 0.163801e-2 + y * p4;
      68           0 :             const auto p6 = -0.362018e-2 + y * p5;
      69           0 :             const auto p7 = -0.3988024e-1 + y * p6;
      70           0 :             const auto p8 = 0.39894228 + y * p7;
      71             : 
      72           0 :             result = p8 * (exp(ax) / sqrt(ax));
      73             :         }
      74           0 :         return x < 0.0 ? -result : result;
      75             :     }
      76             : 
      77           0 :     double bessi2(double x) { return (x == 0) ? 0 : bessi0(x) - ((2 * 1) / x) * bessi1(x); }
      78             : 
      79           0 :     double bessi3(double x) { return (x == 0) ? 0 : bessi1(x) - ((2 * 2) / x) * bessi2(x); }
      80             : 
      81           0 :     double bessi4(double x) { return (x == 0) ? 0 : bessi2(x) - ((2 * 3) / x) * bessi3(x); }
      82             : 
      83           0 :     double bessi(int m, double x)
      84             :     {
      85           0 :         if (m == 0) {
      86           0 :             return bessi0(x);
      87           0 :         } else if (m == 1) {
      88           0 :             return bessi1(x);
      89           0 :         } else if (m == 2) {
      90           0 :             return bessi2(x);
      91           0 :         } else if (m == 3) {
      92           0 :             return bessi3(x);
      93           0 :         } else if (m == 4) {
      94           0 :             return bessi4(x);
      95             :         }
      96             : 
      97           0 :         constexpr double ACC = 40.0;
      98           0 :         constexpr double BIGNO = 1.0e10;
      99           0 :         constexpr double BIGNI = 1.0e-10;
     100             : 
     101           0 :         if (x == 0.0) {
     102           0 :             return 0.0;
     103             :         } else {
     104           0 :             double tox = 2.0 / std::abs(x);
     105           0 :             double result = 0.0;
     106           0 :             double bip = 0.0;
     107           0 :             double bi = 1.0;
     108           0 :             for (int j = 2 * (m + (int) std::sqrt(ACC * m)); j > 0; --j) {
     109           0 :                 double bim = bip + j * tox * bi;
     110           0 :                 bip = bi;
     111           0 :                 bi = bim;
     112           0 :                 if (std::abs(bi) > BIGNO) {
     113           0 :                     result *= BIGNI;
     114           0 :                     bi *= BIGNI;
     115           0 :                     bip *= BIGNI;
     116             :                 }
     117             : 
     118           0 :                 if (j == m) {
     119           0 :                     result = bip;
     120             :                 }
     121             :             }
     122           0 :             result *= bessi0(x) / bi;
     123           0 :             return (((x < 0.0) && ((m % 2) == 0)) ? -result : result);
     124             :         }
     125             :     }
     126             : } // namespace elsa::math