feat: add iir high-pass design routine

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;
+}
+
 }