common/polyphase_resampler.cpp

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