Move the polyphase resampler to the common lib

2 files changed, 259 insertions, 0 deletions

diff --git a/common/polyphase_resampler.cpp b/common/polyphase_resampler.cpp
new file mode 100644
index 00000000..4605b732
--- /dev/null
+++ b/common/polyphase_resampler.cpp
@@ -0,0 +1,214 @@
+
+#include "polyphase_resampler.h"
+
+#include <algorithm>
+#include <cmath>
+
+
+namespace {
+
+#ifndef M_PI
+#define M_PI                         3.14159265358979323846
+#endif
+
+#define EPSILON                      1e-9
+
+using uint = unsigned int;
+
+/* This is the normalized cardinal sine (sinc) function.
+ *
+ *   sinc(x) = { 1,                   x = 0
+ *             { sin(pi x) / (pi x),  otherwise.
+ */
+double Sinc(const double x)
+{
+    if(std::abs(x) < EPSILON)
+        return 1.0;
+    return std::sin(M_PI * x) / (M_PI * x);
+}
+
+/* The zero-order modified Bessel function of the first kind, used for the
+ * Kaiser window.
+ *
+ *   I_0(x) = sum_{k=0}^inf (1 / k!)^2 (x / 2)^(2 k)
+ *          = sum_{k=0}^inf ((x / 2)^k / k!)^2
+ */
+double BesselI_0(const double x)
+{
+    double term, sum, x2, y, last_sum;
+    int k;
+
+    // Start at k=1 since k=0 is trivial.
+    term = 1.0;
+    sum = 1.0;
+    x2 = x/2.0;
+    k = 1;
+
+    // Let the integration converge until the term of the sum is no longer
+    // significant.
+    do {
+        y = x2 / k;
+        k++;
+        last_sum = sum;
+        term *= y * y;
+        sum += term;
+    } while(sum != last_sum);
+    return sum;
+}
+
+/* Calculate a Kaiser window from the given beta value and a normalized k
+ * [-1, 1].
+ *
+ *   w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B),  -1 <= k <= 1
+ *          { 0,                              elsewhere.
+ *
+ * Where k can be calculated as:
+ *
+ *   k = i / l,         where -l <= i <= l.
+ *
+ * or:
+ *
+ *   k = 2 i / M - 1,   where 0 <= i <= M.
+ */
+double Kaiser(const double b, const double k)
+{
+    if(!(k >= -1.0 && k <= 1.0))
+        return 0.0;
+    return BesselI_0(b * std::sqrt(1.0 - k*k)) / BesselI_0(b);
+}
+
+// Calculates the greatest common divisor of a and b.
+uint Gcd(uint x, uint y)
+{
+    while(y > 0)
+    {
+        uint z{y};
+        y = x % y;
+        x = z;
+    }
+    return x;
+}
+
+/* Calculates the size (order) of the Kaiser window.  Rejection is in dB and
+ * the transition width is normalized frequency (0.5 is nyquist).
+ *
+ *   M = { ceil((r - 7.95) / (2.285 2 pi f_t)),  r > 21
+ *       { ceil(5.79 / 2 pi f_t),                r <= 21.
+ *
+ */
+uint CalcKaiserOrder(const double rejection, const double transition)
+{
+    double w_t = 2.0 * M_PI * transition;
+    if(rejection > 21.0)
+        return static_cast<uint>(std::ceil((rejection - 7.95) / (2.285 * w_t)));
+    return static_cast<uint>(std::ceil(5.79 / w_t));
+}
+
+// Calculates the beta value of the Kaiser window.  Rejection is in dB.
+double CalcKaiserBeta(const double rejection)
+{
+    if(rejection > 50.0)
+        return 0.1102 * (rejection - 8.7);
+    if(rejection >= 21.0)
+        return (0.5842 * std::pow(rejection - 21.0, 0.4)) +
+               (0.07886 * (rejection - 21.0));
+    return 0.0;
+}
+
+/* Calculates a point on the Kaiser-windowed sinc filter for the given half-
+ * width, beta, gain, and cutoff.  The point is specified in non-normalized
+ * samples, from 0 to M, where M = (2 l + 1).
+ *
+ *   w(k) 2 p f_t sinc(2 f_t x)
+ *
+ *   x    -- centered sample index (i - l)
+ *   k    -- normalized and centered window index (x / l)
+ *   w(k) -- window function (Kaiser)
+ *   p    -- gain compensation factor when sampling
+ *   f_t  -- normalized center frequency (or cutoff; 0.5 is nyquist)
+ */
+double SincFilter(const uint l, const double b, const double gain, const double cutoff,
+    const uint i)
+{
+    const double x{static_cast<double>(i) - l};
+    return Kaiser(b, x / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x);
+}
+
+} // namespace
+
+// Calculate the resampling metrics and build the Kaiser-windowed sinc filter
+// that's used to cut frequencies above the destination nyquist.
+void PPhaseResampler::init(const uint srcRate, const uint dstRate)
+{
+    const uint gcd{Gcd(srcRate, dstRate)};
+    mP = dstRate / gcd;
+    mQ = srcRate / gcd;
+
+    /* The cutoff is adjusted by half the transition width, so the transition
+     * ends before the nyquist (0.5).  Both are scaled by the downsampling
+     * factor.
+     */
+    double cutoff, width;
+    if(mP > mQ)
+    {
+        cutoff = 0.475 / mP;
+        width = 0.05 / mP;
+    }
+    else
+    {
+        cutoff = 0.475 / mQ;
+        width = 0.05 / mQ;
+    }
+    // A rejection of -180 dB is used for the stop band. Round up when
+    // calculating the left offset to avoid increasing the transition width.
+    const uint l{(CalcKaiserOrder(180.0, width)+1) / 2};
+    const double beta{CalcKaiserBeta(180.0)};
+    mM = l*2 + 1;
+    mL = l;
+    mF.resize(mM);
+    for(uint i{0};i < mM;i++)
+        mF[i] = SincFilter(l, beta, mP, cutoff, i);
+}
+
+// Perform the upsample-filter-downsample resampling operation using a
+// polyphase filter implementation.
+void PPhaseResampler::process(const uint inN, const double *in, const uint outN, double *out)
+{
+    if(outN == 0)
+        return;
+
+    const uint p{mP}, q{mQ}, m{mM}, l{mL};
+
+    // Handle in-place operation.
+    std::vector<double> workspace;
+    double *work{out};
+    if(work == in)
+    {
+        workspace.resize(outN);
+        work = workspace.data();
+    }
+
+    // Resample the input.
+    const double *f{mF.data()};
+    for(uint i{0};i < outN;i++)
+    {
+        double r{0.0};
+        // Input starts at l to compensate for the filter delay.  This will
+        // drop any build-up from the first half of the filter.
+        uint j_f{(l + (q * i)) % p};
+        uint j_s{(l + (q * i)) / p};
+        while(j_f < m)
+        {
+            // Only take input when 0 <= j_s < inN.  This single unsigned
+            // comparison catches both cases.
+            if(j_s < inN)
+                r += f[j_f] * in[j_s];
+            j_f += p;
+            j_s--;
+        }
+        work[i] = r;
+    }
+    // Clean up after in-place operation.
+    if(work != out)
+        std::copy_n(work, outN, out);
+}
diff --git a/common/polyphase_resampler.h b/common/polyphase_resampler.h
new file mode 100644
index 00000000..e9833d12
--- /dev/null
+++ b/common/polyphase_resampler.h
@@ -0,0 +1,45 @@
+#ifndef POLYPHASE_RESAMPLER_H
+#define POLYPHASE_RESAMPLER_H
+
+#include <vector>
+
+
+/* This is a polyphase sinc-filtered resampler. It is built for very high
+ * quality results, rather than real-time performance.
+ *
+ *              Upsample                      Downsample
+ *
+ *              p/q = 3/2                     p/q = 3/5
+ *
+ *          M-+-+-+->                     M-+-+-+->
+ *         -------------------+          ---------------------+
+ *   p  s * f f f f|f|        |    p  s * f f f f f           |
+ *   |  0 *   0 0 0|0|0       |    |  0 *   0 0 0 0|0|        |
+ *   v  0 *     0 0|0|0 0     |    v  0 *     0 0 0|0|0       |
+ *      s *       f|f|f f f   |       s *       f f|f|f f     |
+ *      0 *        |0|0 0 0 0 |       0 *         0|0|0 0 0   |
+ *         --------+=+--------+       0 *          |0|0 0 0 0 |
+ *          d . d .|d|. d . d            ----------+=+--------+
+ *                                        d . . . .|d|. . . .
+ *          q->
+ *                                        q-+-+-+->
+ *
+ *   P_f(i,j) = q i mod p + pj
+ *   P_s(i,j) = floor(q i / p) - j
+ *   d[i=0..N-1] = sum_{j=0}^{floor((M - 1) / p)} {
+ *                   { f[P_f(i,j)] s[P_s(i,j)],  P_f(i,j) < M
+ *                   { 0,                        P_f(i,j) >= M. }
+ */
+
+struct PPhaseResampler {
+    using uint = unsigned int;
+
+    void init(const uint srcRate, const uint dstRate);
+    void process(const uint inN, const double *in, const uint outN, double *out);
+
+private:
+    uint mP, mQ, mM, mL;
+    std::vector<double> mF;
+};
+
+#endif /* POLYPHASE_RESAMPLER_H */