feat: add iir high-pass design routine

pasada-lib/include/pd_signal.h

@@ -7,8 +7,11 @@
 
 #include <vector>
 #include <deque>
+#include <complex>
 
 namespace pd_signal {
+    using cplx = std::complex<double>;
+
     /** `num` evenly spaced numbers over interval [start,stop] */
     void linspace(std::vector<double>& data, double start, double stop, int num);
     /** `num` evenly spaced numbers over interval [start,stop] with endpoint=true or [start,stop) with endpoint=false */
@@ -37,6 +40,34 @@ namespace pd_signal {
     /** two-dimensional mean of a collection of signals */
     void mean(std::vector<double> &out, std::deque<std::vector<double> >& m);
 
+    /**
+     * Convolution of two polynomials given in ASCENDING power order.
+     * If <c>p = p_0 + p_1 x + ... + p_{P-1} x^{P-1}</c> and likewise for q,
+     * then <c>out = p * q</c> in ascending power order, of length P+Q-1.
+     */
+    void polymul(std::vector<cplx>& out,
+                 const std::vector<cplx>& p, const std::vector<cplx>& q);
+
+    /**
+     * Build a monic polynomial from its roots:
+     * <c>(x - r_0) (x - r_1) ... (x - r_{N-1})</c>.
+     * Returned in DESCENDING power order, i.e. <c>out[0]=1, ..., out[N]</c>
+     * is the constant term. Length is <c>roots.size() + 1</c>.
+     */
+    void poly(std::vector<cplx>& out, const std::vector<cplx>& roots);
+
+    /**
+     * Design an N-th order Butterworth IIR high-pass digital filter via the
+     * bilinear transform. The passband is normalized to unit gain at Nyquist.
+     *
+     * @param b  numerator coefficients in DESCENDING powers of z (length N+1)
+     * @param a  denominator coefficients in DESCENDING powers of z (length N+1)
+     * @param N  filter order (>= 1)
+     * @param fc cutoff frequency of the digital filter in Hz (0 < fc < fs/2)
+     * @param fs sampling frequency in Hz
+     */
+    void iirHighpass(std::vector<double>& b, std::vector<double>& a,
+                     int N, double fc, double fs);
 }
 
 #endif //PASADASUPERPROJECT_SIGNAL_H

pasada-lib/pd_signal.cpp

@@ -9,6 +9,8 @@
 
 namespace pd_signal {
 
+static constexpr double kPi = 3.14159265358979323846;
+
 static void throwIfNotAscending(std::vector<double>& xp) {
     size_t N = xp.size();
     for(int i = 0; i < N - 1; i++)
@@ -139,4 +141,85 @@ void mean(std::vector<double> &out, std::deque<std::vector<double> >& m) {
     mean_tpl(out, m);
 }
 
+// Convolution of two polynomials in ascending power order.
+void polymul(std::vector<cplx>& out,
+             const std::vector<cplx>& p, const std::vector<cplx>& q) {
+    if (p.empty() || q.empty()) {
+        out.clear();
+        return;
+    }
+    out.assign(p.size() + q.size() - 1, cplx(0.0, 0.0));
+    for (size_t i = 0; i < p.size(); ++i)
+        for (size_t j = 0; j < q.size(); ++j)
+            out[i + j] += p[i] * q[j];
+}
+
+// Build a monic polynomial from its roots, returned in descending power order.
+void poly(std::vector<cplx>& out, const std::vector<cplx>& roots) {
+    // Accumulate the product (x - r_0)(x - r_1)...(x - r_{N-1}) in ascending
+    // power order, then reverse to descending order to match numpy.poly().
+    std::vector<cplx> acc{cplx(1.0, 0.0)};
+    std::vector<cplx> tmp;
+    for (const cplx& root : roots) {
+        const std::vector<cplx> factor{-root, cplx(1.0, 0.0)};  // -root + 1*x
+        polymul(tmp, acc, factor);
+        acc.swap(tmp);
+    }
+    out.assign(acc.rbegin(), acc.rend());
+}
+
+void iirHighpass(std::vector<double>& b, std::vector<double>& a,
+                 int N, double fc, double fs) {
+    if (N < 1) throw std::invalid_argument("N must be >= 1");
+    if (!(fc > 0.0 && fc < 0.5 * fs))
+        throw std::invalid_argument("require 0 < fc < fs/2");
+
+    // 1) Analog Butterworth low-pass prototype poles (Omega_c = 1 rad/s):
+    //    equally spaced on the unit circle in the left-half s-plane.
+    // 2) Bilinear pre-warp so the digital cutoff lands exactly at fc:
+    //        Omega_c = 2*fs*tan(pi*fc/fs)  =>  F_c = Omega_c / (2 pi).
+    // 3) Scale prototype poles to the desired analog cutoff.
+    // 4) Bilinear transform s -> z, with T = 1/fs:
+    //        z = (1 + s T / 2) / (1 - s T / 2).
+    const double Fc = (fs / kPi) * std::tan(kPi * fc / fs);
+    const double T = 1.0 / fs;
+    const double scale = 2.0 * kPi * Fc;
+
+    std::vector<cplx> p_z(N);
+    for (int k = 1; k <= N; ++k) {
+        const double phase = kPi * (2 * k + N - 1) / (2.0 * N);
+        const cplx p_proto(std::cos(phase), std::sin(phase));
+        const cplx half_sT = (scale * T / 2.0) * p_proto;
+        p_z[k - 1] = (1.0 + half_sT) / (1.0 - half_sT);
+    }
+
+    // 5) High-pass: place all N zeros at z = +1 so |H(z=1)| = 0 (DC null).
+    const std::vector<cplx> z_z(N, cplx(1.0, 0.0));
+
+    // 6) Build numerator and denominator polynomials (highest power of z first).
+    //    Poles come in conjugate pairs, so the imaginary part is numerical noise.
+    std::vector<cplx> a_c, b_c;
+    poly(a_c, p_z);
+    poly(b_c, z_z);
+
+    a.resize(N + 1);
+    b.resize(N + 1);
+    for (int i = 0; i <= N; ++i) {
+        a[i] = a_c[i].real();
+        b[i] = b_c[i].real();
+    }
+
+    // 7) Normalize for unit gain at Nyquist (z = -1), the high-pass passband peak.
+    //    Sum_i b[i] * (-1)^(N-i)  /  Sum_i a[i] * (-1)^(N-i).
+    double num = 0.0, den = 0.0;
+    double sign = ((N & 1) == 0) ? 1.0 : -1.0;  // (-1)^N for i=0
+    for (int i = 0; i <= N; ++i) {
+        num += b[i] * sign;
+        den += a[i] * sign;
+        sign = -sign;
+    }
+    const double gain = num / den;
+    for (int i = 0; i <= N; ++i) b[i] /= gain;
+}
+
 }