common/polyphase_resampler.cpp

   1
   2 #include "polyphase_resampler.h"
   3
   4 #include <algorithm>
   5 #include <cmath>
   6 #include <cstddef>
   7 #include <numeric>
   8 #include <tuple>
   9
  10 #include "alnumbers.h"
  11 #include "opthelpers.h"
  12
  13
  14 using uint = unsigned int;
  15
  16 namespace {
  17
  18 constexpr double Epsilon{1e-9};
  19
  20 #if __cpp_lib_math_special_functions >= 201603L
  21 using std::cyl_bessel_i;
  22
  23 #else
  24
  25 /* The zero-order modified Bessel function of the first kind, used for the
  26  * Kaiser window.
  27  *
  28  *   I_0(x) = sum_{k=0}^inf (1 / k!)^2 (x / 2)^(2 k)
  29  *          = sum_{k=0}^inf ((x / 2)^k / k!)^2
  30  *
  31  * This implementation only handles nu = 0, and isn't the most precise (it
  32  * starts with the largest value and accumulates successively smaller values,
  33  * compounding the rounding and precision error), but it's good enough.
  34  */
  35 template<typename T, typename U>
  36 U cyl_bessel_i(T nu, U x)
  37 {
  38     if(nu != T{0})
  39         throw std::runtime_error{"cyl_bessel_i: nu != 0"};
  40
  41     /* Start at k=1 since k=0 is trivial. */
  42     const double x2{x/2.0};
  43     double term{1.0};
  44     double sum{1.0};
  45     int k{1};
  46
  47     /* Let the integration converge until the term of the sum is no longer
  48      * significant.
  49      */
  50     double last_sum{};
  51     do {
  52         const double y{x2 / k};
  53         ++k;
  54         last_sum = sum;
  55         term *= y * y;
  56         sum += term;
  57     } while(sum != last_sum);
  58     return static_cast<U>(sum);
  59 }
  60 #endif
  61
  62 /* This is the normalized cardinal sine (sinc) function.
  63  *
  64  *   sinc(x) = { 1,                   x = 0
  65  *             { sin(pi x) / (pi x),  otherwise.
  66  */
  67 double Sinc(const double x)
  68 {
  69     if(std::abs(x) < Epsilon) UNLIKELY
  70         return 1.0;
  71     return std::sin(al::numbers::pi*x) / (al::numbers::pi*x);
  72 }
  73
  74 /* Calculate a Kaiser window from the given beta value and a normalized k
  75  * [-1, 1].
  76  *
  77  *   w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B),  -1 <= k <= 1
  78  *          { 0,                              elsewhere.
  79  *
  80  * Where k can be calculated as:
  81  *
  82  *   k = i / l,         where -l <= i <= l.
  83  *
  84  * or:
  85  *
  86  *   k = 2 i / M - 1,   where 0 <= i <= M.
  87  */
  88 double Kaiser(const double beta, const double k, const double besseli_0_beta)
  89 {
  90     if(!(k >= -1.0 && k <= 1.0))
  91         return 0.0;
  92     return cyl_bessel_i(0, beta * std::sqrt(1.0 - k*k)) / besseli_0_beta;
  93 }
  94
  95 /* Calculates the size (order) of the Kaiser window.  Rejection is in dB and
  96  * the transition width is normalized frequency (0.5 is nyquist).
  97  *
  98  *   M = { ceil((r - 7.95) / (2.285 2 pi f_t)),  r > 21
  99  *       { ceil(5.79 / 2 pi f_t),                r <= 21.
 100  *
 101  */
 102 constexpr uint CalcKaiserOrder(const double rejection, const double transition)
 103 {
 104     const double w_t{2.0 * al::numbers::pi * transition};
 105     if(rejection > 21.0) LIKELY
 106         return static_cast<uint>(std::ceil((rejection - 7.95) / (2.285 * w_t)));
 107     return static_cast<uint>(std::ceil(5.79 / w_t));
 108 }
 109
 110 // Calculates the beta value of the Kaiser window.  Rejection is in dB.
 111 constexpr double CalcKaiserBeta(const double rejection)
 112 {
 113     if(rejection > 50.0) LIKELY
 114         return 0.1102 * (rejection - 8.7);
 115     if(rejection >= 21.0)
 116         return (0.5842 * std::pow(rejection - 21.0, 0.4)) +
 117                (0.07886 * (rejection - 21.0));
 118     return 0.0;
 119 }
 120
 121 /* Calculates a point on the Kaiser-windowed sinc filter for the given half-
 122  * width, beta, gain, and cutoff.  The point is specified in non-normalized
 123  * samples, from 0 to M, where M = (2 l + 1).
 124  *
 125  *   w(k) 2 p f_t sinc(2 f_t x)
 126  *
 127  *   x    -- centered sample index (i - l)
 128  *   k    -- normalized and centered window index (x / l)
 129  *   w(k) -- window function (Kaiser)
 130  *   p    -- gain compensation factor when sampling
 131  *   f_t  -- normalized center frequency (or cutoff; 0.5 is nyquist)
 132  */
 133 double SincFilter(const uint l, const double beta, const double besseli_0_beta, const double gain,
 134     const double cutoff, const uint i)
 135 {
 136     const double x{static_cast<double>(i) - l};
 137     return Kaiser(beta, x/l, besseli_0_beta) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x);
 138 }
 139
 140 } // namespace
 141
 142 // Calculate the resampling metrics and build the Kaiser-windowed sinc filter
 143 // that's used to cut frequencies above the destination nyquist.
 144 void PPhaseResampler::init(const uint srcRate, const uint dstRate)
 145 {
 146     const uint gcd{std::gcd(srcRate, dstRate)};
 147     mP = dstRate / gcd;
 148     mQ = srcRate / gcd;
 149
 150     /* The cutoff is adjusted by half the transition width, so the transition
 151      * ends before the nyquist (0.5).  Both are scaled by the downsampling
 152      * factor.
 153      */
 154     const auto [cutoff, width] = (mP > mQ) ? std::make_tuple(0.475 / mP, 0.05 / mP)
 155         : std::make_tuple(0.475 / mQ, 0.05 / mQ);
 156
 157     // A rejection of -180 dB is used for the stop band. Round up when
 158     // calculating the left offset to avoid increasing the transition width.
 159     const uint l{(CalcKaiserOrder(180.0, width)+1) / 2};
 160     const double beta{CalcKaiserBeta(180.0)};
 161     const double besseli_0_beta{cyl_bessel_i(0, beta)};
 162     mM = l*2 + 1;
 163     mL = l;
 164     mF.resize(mM);
 165     for(uint i{0};i < mM;i++)
 166         mF[i] = SincFilter(l, beta, besseli_0_beta, mP, cutoff, i);
 167 }
 168
 169 // Perform the upsample-filter-downsample resampling operation using a
 170 // polyphase filter implementation.
 171 void PPhaseResampler::process(const al::span<const double> in, const al::span<double> out)
 172 {
 173     if(out.empty()) UNLIKELY
 174         return;
 175
 176     // Handle in-place operation.
 177     std::vector<double> workspace;
 178     al::span work{out};
 179     if(work.data() == in.data()) UNLIKELY
 180     {
 181         workspace.resize(out.size());
 182         work = workspace;
 183     }
 184
 185     // Resample the input.
 186     const uint p{mP}, q{mQ}, m{mM}, l{mL};
 187     const al::span<const double> f{mF};
 188     for(uint i{0};i < out.size();i++)
 189     {
 190         // Input starts at l to compensate for the filter delay.  This will
 191         // drop any build-up from the first half of the filter.
 192         std::size_t j_f{(l + q*i) % p};
 193         std::size_t j_s{(l + q*i) / p};
 194
 195         // Only take input when 0 <= j_s < in.size().
 196         double r{0.0};
 197         if(j_f < m) LIKELY
 198         {
 199             std::size_t filt_len{(m-j_f+p-1) / p};
 200             if(j_s+1 > in.size()) LIKELY
 201             {
 202                 std::size_t skip{std::min(j_s+1 - in.size(), filt_len)};
 203                 j_f += p*skip;
 204                 j_s -= skip;
 205                 filt_len -= skip;
 206             }
 207             std::size_t todo{std::min(j_s+1, filt_len)};
 208             while(todo)
 209             {
 210                 r += f[j_f] * in[j_s];
 211                 j_f += p; --j_s;
 212                 --todo;
 213             }
 214         }
 215         work[i] = r;
 216     }
 217     // Clean up after in-place operation.
 218     if(work.data() != out.data())
 219         std::copy(work.cbegin(), work.cend(), out.begin());
 220 }