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