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