Move the polyphase resampler to the common lib

7 files changed, 272 insertions, 250 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98f9ad49..a4300f65 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -551,6 +551,8 @@ SET(COMMON_OBJS
     common/intrusive_ptr.h
     common/math_defs.h
     common/opthelpers.h
+    common/polyphase_resampler.cpp
+    common/polyphase_resampler.h
     common/pragmadefs.h
     common/strutils.cpp
     common/strutils.h
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 */
diff --git a/utils/makemhr/loaddef.cpp b/utils/makemhr/loaddef.cpp
index 619f5104..63014fe5 100644
--- a/utils/makemhr/loaddef.cpp
+++ b/utils/makemhr/loaddef.cpp
@@ -1710,9 +1710,9 @@ static double AverageHrirOnset(const uint rate, const uint n, const double *hrir
 {
     std::vector<double> upsampled(10 * n);
     {
-        ResamplerT rs;
-        ResamplerSetup(&rs, rate, 10 * rate);
-        ResamplerRun(&rs, n, hrir, 10 * n, upsampled.data());
+        PPhaseResampler rs;
+        rs.init(rate, 10 * rate);
+        rs.process(n, hrir, 10 * n, upsampled.data());
     }
 
     double mag{0.0};
diff --git a/utils/makemhr/loadsofa.cpp b/utils/makemhr/loadsofa.cpp
index c91613c8..473109fe 100644
--- a/utils/makemhr/loadsofa.cpp
+++ b/utils/makemhr/loadsofa.cpp
@@ -443,9 +443,9 @@ static double CalcHrirOnset(const uint rate, const uint n, std::vector<double> &
     const double *hrir)
 {
     {
-        ResamplerT rs;
-        ResamplerSetup(&rs, rate, 10 * rate);
-        ResamplerRun(&rs, n, hrir, 10 * n, upsampled.data());
+        PPhaseResampler rs;
+        rs.init(rate, 10 * rate);
+        rs.process(n, hrir, 10 * n, upsampled.data());
     }
 
     double mag{std::accumulate(upsampled.cbegin(), upsampled.cend(), double{0.0},
diff --git a/utils/makemhr/makemhr.cpp b/utils/makemhr/makemhr.cpp
index 1e28ca4b..42a384db 100644
--- a/utils/makemhr/makemhr.cpp
+++ b/utils/makemhr/makemhr.cpp
@@ -400,237 +400,6 @@ static void MinimumPhase(const uint n, const double *in, complex_d *out)
 
 
 /***************************
- *** Resampler functions ***
- ***************************/
-
-/* This is the normalized cardinal sine (sinc) function.
- *
- *   sinc(x) = { 1,                   x = 0
- *             { sin(pi x) / (pi x),  otherwise.
- */
-static 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
- */
-static 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.
- */
-static 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.
-static 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.
- *
- */
-static 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.
-static 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)
- */
-static double SincFilter(const uint l, const double b, const double gain, const double cutoff, const uint i)
-{
-    return Kaiser(b, static_cast<double>(i - l) / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * (i - l));
-}
-
-/* This is a polyphase sinc-filtered resampler.
- *
- *              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. }
- */
-
-// Calculate the resampling metrics and build the Kaiser-windowed sinc filter
-// that's used to cut frequencies above the destination nyquist.
-void ResamplerSetup(ResamplerT *rs, const uint srcRate, const uint dstRate)
-{
-    const uint gcd{Gcd(srcRate, dstRate)};
-    rs->mP = dstRate / gcd;
-    rs->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(rs->mP > rs->mQ)
-    {
-        cutoff = 0.475 / rs->mP;
-        width = 0.05 / rs->mP;
-    }
-    else
-    {
-        cutoff = 0.475 / rs->mQ;
-        width = 0.05 / rs->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)};
-    rs->mM = l*2 + 1;
-    rs->mL = l;
-    rs->mF.resize(rs->mM);
-    for(uint i{0};i < rs->mM;i++)
-        rs->mF[i] = SincFilter(l, beta, rs->mP, cutoff, i);
-}
-
-// Perform the upsample-filter-downsample resampling operation using a
-// polyphase filter implementation.
-void ResamplerRun(ResamplerT *rs, const uint inN, const double *in, const uint outN, double *out)
-{
-    const uint p = rs->mP, q = rs->mQ, m = rs->mM, l = rs->mL;
-    std::vector<double> workspace;
-    const double *f = rs->mF.data();
-    uint j_f, j_s;
-    double *work;
-    uint i;
-
-    if(outN == 0)
-        return;
-
-    // Handle in-place operation.
-    if(in == out)
-    {
-        workspace.resize(outN);
-        work = workspace.data();
-    }
-    else
-        work = out;
-    // Resample the input.
-    for(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.
-        j_f = (l + (q * i)) % p;
-        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)
-    {
-        for(i = 0;i < outN;i++)
-            out[i] = work[i];
-    }
-}
-
-
-/***************************
  *** File storage output ***
  ***************************/
 
@@ -1065,9 +834,9 @@ static void ResampleHrirs(const uint rate, HrirDataT *hData)
     uint channels = (hData->mChannelType == CT_STEREO) ? 2 : 1;
     uint n = hData->mIrPoints;
     uint ti, fi, ei, ai;
-    ResamplerT rs;
+    PPhaseResampler rs;
 
-    ResamplerSetup(&rs, hData->mIrRate, rate);
+    rs.init(hData->mIrRate, rate);
     for(fi = 0;fi < hData->mFdCount;fi++)
     {
         for(ei = hData->mFds[fi].mEvStart;ei < hData->mFds[fi].mEvCount;ei++)
@@ -1076,7 +845,7 @@ static void ResampleHrirs(const uint rate, HrirDataT *hData)
             {
                 HrirAzT *azd = &hData->mFds[fi].mEvs[ei].mAzs[ai];
                 for(ti = 0;ti < channels;ti++)
-                    ResamplerRun(&rs, n, azd->mIrs[ti], n, azd->mIrs[ti]);
+                    rs.process(n, azd->mIrs[ti], n, azd->mIrs[ti]);
             }
         }
     }
diff --git a/utils/makemhr/makemhr.h b/utils/makemhr/makemhr.h
index ba19efbe..89607cc0 100644
--- a/utils/makemhr/makemhr.h
+++ b/utils/makemhr/makemhr.h
@@ -4,6 +4,8 @@
 #include <vector>
 #include <complex>
 
+#include "polyphase_resampler.h"
+
 
 // The maximum path length used when processing filenames.
 #define MAX_PATH_LEN                 (256)
@@ -111,16 +113,6 @@ void FftForward(const uint n, complex_d *inout);
 void FftInverse(const uint n, complex_d *inout);
 
 
-// The resampler metrics and FIR filter.
-struct ResamplerT {
-    uint mP, mQ, mM, mL;
-    std::vector<double> mF;
-};
-
-void ResamplerSetup(ResamplerT *rs, const uint srcRate, const uint dstRate);
-void ResamplerRun(ResamplerT *rs, const uint inN, const double *in, const uint outN, double *out);
-
-
 // Performs linear interpolation.
 inline double Lerp(const double a, const double b, const double f)
 { return a + f * (b - a); }