common/polyphase_resampler.cpp

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