Line data    Source code

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