diff --git a/pasada-lib/include/pd_signal.h b/pasada-lib/include/pd_signal.h index ff34d89..223d727 100644 --- a/pasada-lib/include/pd_signal.h +++ b/pasada-lib/include/pd_signal.h @@ -7,8 +7,11 @@ #include #include +#include namespace pd_signal { + using cplx = std::complex; + /** `num` evenly spaced numbers over interval [start,stop] */ void linspace(std::vector& 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 &out, std::deque >& m); + /** + * Convolution of two polynomials given in ASCENDING power order. + * If p = p_0 + p_1 x + ... + p_{P-1} x^{P-1} and likewise for q, + * then out = p * q in ascending power order, of length P+Q-1. + */ + void polymul(std::vector& out, + const std::vector& p, const std::vector& q); + + /** + * Build a monic polynomial from its roots: + * (x - r_0) (x - r_1) ... (x - r_{N-1}). + * Returned in DESCENDING power order, i.e. out[0]=1, ..., out[N] + * is the constant term. Length is roots.size() + 1. + */ + void poly(std::vector& out, const std::vector& 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& b, std::vector& a, + int N, double fc, double fs); } #endif //PASADASUPERPROJECT_SIGNAL_H \ No newline at end of file diff --git a/pasada-lib/pd_signal.cpp b/pasada-lib/pd_signal.cpp index f394ac5..f97ada7 100644 --- a/pasada-lib/pd_signal.cpp +++ b/pasada-lib/pd_signal.cpp @@ -9,6 +9,8 @@ namespace pd_signal { +static constexpr double kPi = 3.14159265358979323846; + static void throwIfNotAscending(std::vector& xp) { size_t N = xp.size(); for(int i = 0; i < N - 1; i++) @@ -139,4 +141,85 @@ void mean(std::vector &out, std::deque >& m) { mean_tpl(out, m); } +// Convolution of two polynomials in ascending power order. +void polymul(std::vector& out, + const std::vector& p, const std::vector& 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& out, const std::vector& 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 acc{cplx(1.0, 0.0)}; + std::vector tmp; + for (const cplx& root : roots) { + const std::vector 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& b, std::vector& 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 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 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 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; +} + }