diff --git a/include/dsp/filter.hpp b/include/dsp/filter.hpp index 3a6314ec..6d93ec85 100644 --- a/include/dsp/filter.hpp +++ b/include/dsp/filter.hpp @@ -182,20 +182,108 @@ struct TExponentialSlewLimiter { typedef TExponentialSlewLimiter<> ExponentialSlewLimiter; -template -struct TBiquadFilter { - /** input state */ - T x[2]; +/** Digital IIR filter processor. +https://en.wikipedia.org/wiki/Infinite_impulse_response +*/ +template +struct IIRFilter { + /** transfer function numerator coefficients: b_0, b_1, etc. + */ + T b[B_ORDER] = {}; + /** transfer function denominator coefficients: a_1, a_2, etc. + a_0 is fixed to 1 and omitted from the `a` array, so its indices are shifted down by 1. + */ + T a[A_ORDER - 1] = {}; + /** input state + x[0] = x_{n-1} + x[1] = x_{n-2} + etc. + */ + T x[B_ORDER - 1]; /** output state */ - T y[2]; + T y[A_ORDER - 1]; + + IIRFilter() { + reset(); + } - /** transfer function numerator coefficients: b_0, b_1, b_2 */ - float b[3]; - /** transfer function denominator coefficients: a_1, a_2 - a_0 is fixed to 1. + void reset() { + for (int i = 1; i < B_ORDER; i++) { + x[i - 1] = 0.f; + } + for (int i = 1; i < A_ORDER; i++) { + y[i - 1] = 0.f; + } + } + + void setCoefficients(const T* b, const T* a) { + for (int i = 0; i < B_ORDER; i++) { + this->b[i] = b[i]; + } + for (int i = 1; i < A_ORDER; i++) { + this->a[i - 1] = a[i - 1]; + } + } + + T process(T in) { + T out = 0.f; + // Add x state + if (0 < B_ORDER) { + out = b[0] * in; + } + for (int i = 1; i < B_ORDER; i++) { + out += b[i] * x[i - 1]; + } + // Subtract y state + for (int i = 1; i < A_ORDER; i++) { + out -= a[i - 1] * y[i - 1]; + } + // Shift x state + for (int i = B_ORDER - 1; i >= 2; i--) { + x[i - 1] = x[i - 2]; + } + x[0] = in; + // Shift y state + for (int i = A_ORDER - 1; i >= 2; i--) { + y[i - 1] = y[i - 2]; + } + y[0] = out; + return out; + } + + /** Computes the complex transfer function $H(s)$ at a particular frequency + s: normalized angular frequency equal to $2 \pi f / f_{sr}$ ($\pi$ is the Nyquist frequency) */ - float a[2]; + std::complex getTransferFunction(T s) { + // Compute sum(a_k z^-k) / sum(b_k z^-k) where z = e^(i s) + std::complex bSum(b[0], 0); + std::complex aSum(1, 0); + for (int i = 1; i < std::max(B_ORDER, A_ORDER); i++) { + T p = -i * s; + std::complex z(simd::cos(p), simd::sin(p)); + if (i < B_ORDER) + bSum += b[i] * z; + if (i < A_ORDER) + aSum += a[i - 1] * z; + } + return bSum / aSum; + } + T getFrequencyResponse(T f) { + // T hReal, hImag; + // getTransferFunction(2 * M_PI * f, &hReal, &hImag); + // return simd::hypot(hReal, hImag); + return simd::abs(getTransferFunction(2 * M_PI * f)); + } + + T getFrequencyPhase(T f) { + return simd::arg(getTransferFunction(2 * M_PI * f)); + } +}; + + +template +struct TBiquadFilter : IIRFilter<3, 3, T> { enum Type { LOWPASS_1POLE, HIGHPASS_1POLE, @@ -210,28 +298,9 @@ struct TBiquadFilter { }; TBiquadFilter() { - reset(); setParameters(LOWPASS, 0.f, 0.f, 1.f); } - void reset() { - std::memset(x, 0, sizeof(x)); - std::memset(y, 0, sizeof(y)); - } - - T process(T in) { - // Advance IIR - T out = b[0] * in + b[1] * x[0] + b[2] * x[1] - - a[0] * y[0] - a[1] * y[1]; - // Push input - x[1] = x[0]; - x[0] = in; - // Push output - y[1] = y[0]; - y[0] = out; - return out; - } - /** Calculates and sets the biquad transfer function coefficients. f: normalized frequency (cutoff frequency / sample rate), must be less than 0.5 Q: quality factor @@ -241,37 +310,37 @@ struct TBiquadFilter { float K = std::tan(M_PI * f); switch (type) { case LOWPASS_1POLE: { - a[0] = -std::exp(-2.f * M_PI * f); - a[1] = 0.f; - b[0] = 1.f + a[0]; - b[1] = 0.f; - b[2] = 0.f; + this->a[0] = -std::exp(-2.f * M_PI * f); + this->a[1] = 0.f; + this->b[0] = 1.f + this->a[0]; + this->b[1] = 0.f; + this->b[2] = 0.f; } break; case HIGHPASS_1POLE: { - a[0] = std::exp(-2.f * M_PI * (0.5f - f)); - a[1] = 0.f; - b[0] = 1.f - a[0]; - b[1] = 0.f; - b[2] = 0.f; + this->a[0] = std::exp(-2.f * M_PI * (0.5f - f)); + this->a[1] = 0.f; + this->b[0] = 1.f - this->a[0]; + this->b[1] = 0.f; + this->b[2] = 0.f; } break; case LOWPASS: { float norm = 1.f / (1.f + K / Q + K * K); - b[0] = K * K * norm; - b[1] = 2.f * b[0]; - b[2] = b[0]; - a[0] = 2.f * (K * K - 1.f) * norm; - a[1] = (1.f - K / Q + K * K) * norm; + this->b[0] = K * K * norm; + this->b[1] = 2.f * this->b[0]; + this->b[2] = this->b[0]; + this->a[0] = 2.f * (K * K - 1.f) * norm; + this->a[1] = (1.f - K / Q + K * K) * norm; } break; case HIGHPASS: { float norm = 1.f / (1.f + K / Q + K * K); - b[0] = norm; - b[1] = -2.f * b[0]; - b[2] = b[0]; - a[0] = 2.f * (K * K - 1.f) * norm; - a[1] = (1.f - K / Q + K * K) * norm; + this->b[0] = norm; + this->b[1] = -2.f * this->b[0]; + this->b[2] = this->b[0]; + this->a[0] = 2.f * (K * K - 1.f) * norm; + this->a[1] = (1.f - K / Q + K * K) * norm; } break; @@ -279,19 +348,19 @@ struct TBiquadFilter { float sqrtV = std::sqrt(V); if (V >= 1.f) { float norm = 1.f / (1.f + M_SQRT2 * K + K * K); - b[0] = (1.f + M_SQRT2 * sqrtV * K + V * K * K) * norm; - b[1] = 2.f * (V * K * K - 1.f) * norm; - b[2] = (1.f - M_SQRT2 * sqrtV * K + V * K * K) * norm; - a[0] = 2.f * (K * K - 1.f) * norm; - a[1] = (1.f - M_SQRT2 * K + K * K) * norm; + this->b[0] = (1.f + M_SQRT2 * sqrtV * K + V * K * K) * norm; + this->b[1] = 2.f * (V * K * K - 1.f) * norm; + this->b[2] = (1.f - M_SQRT2 * sqrtV * K + V * K * K) * norm; + this->a[0] = 2.f * (K * K - 1.f) * norm; + this->a[1] = (1.f - M_SQRT2 * K + K * K) * norm; } else { float norm = 1.f / (1.f + M_SQRT2 / sqrtV * K + K * K / V); - b[0] = (1.f + M_SQRT2 * K + K * K) * norm; - b[1] = 2.f * (K * K - 1) * norm; - b[2] = (1.f - M_SQRT2 * K + K * K) * norm; - a[0] = 2.f * (K * K / V - 1.f) * norm; - a[1] = (1.f - M_SQRT2 / sqrtV * K + K * K / V) * norm; + this->b[0] = (1.f + M_SQRT2 * K + K * K) * norm; + this->b[1] = 2.f * (K * K - 1) * norm; + this->b[2] = (1.f - M_SQRT2 * K + K * K) * norm; + this->a[0] = 2.f * (K * K / V - 1.f) * norm; + this->a[1] = (1.f - M_SQRT2 / sqrtV * K + K * K / V) * norm; } } break; @@ -299,86 +368,62 @@ struct TBiquadFilter { float sqrtV = std::sqrt(V); if (V >= 1.f) { float norm = 1.f / (1.f + M_SQRT2 * K + K * K); - b[0] = (V + M_SQRT2 * sqrtV * K + K * K) * norm; - b[1] = 2.f * (K * K - V) * norm; - b[2] = (V - M_SQRT2 * sqrtV * K + K * K) * norm; - a[0] = 2.f * (K * K - 1.f) * norm; - a[1] = (1.f - M_SQRT2 * K + K * K) * norm; + this->b[0] = (V + M_SQRT2 * sqrtV * K + K * K) * norm; + this->b[1] = 2.f * (K * K - V) * norm; + this->b[2] = (V - M_SQRT2 * sqrtV * K + K * K) * norm; + this->a[0] = 2.f * (K * K - 1.f) * norm; + this->a[1] = (1.f - M_SQRT2 * K + K * K) * norm; } else { float norm = 1.f / (1.f / V + M_SQRT2 / sqrtV * K + K * K); - b[0] = (1.f + M_SQRT2 * K + K * K) * norm; - b[1] = 2.f * (K * K - 1.f) * norm; - b[2] = (1.f - M_SQRT2 * K + K * K) * norm; - a[0] = 2.f * (K * K - 1.f / V) * norm; - a[1] = (1.f / V - M_SQRT2 / sqrtV * K + K * K) * norm; + this->b[0] = (1.f + M_SQRT2 * K + K * K) * norm; + this->b[1] = 2.f * (K * K - 1.f) * norm; + this->b[2] = (1.f - M_SQRT2 * K + K * K) * norm; + this->a[0] = 2.f * (K * K - 1.f / V) * norm; + this->a[1] = (1.f / V - M_SQRT2 / sqrtV * K + K * K) * norm; } } break; case BANDPASS: { float norm = 1.f / (1.f + K / Q + K * K); - b[0] = K / Q * norm; - b[1] = 0.f; - b[2] = -b[0]; - a[0] = 2.f * (K * K - 1.f) * norm; - a[1] = (1.f - K / Q + K * K) * norm; + this->b[0] = K / Q * norm; + this->b[1] = 0.f; + this->b[2] = -this->b[0]; + this->a[0] = 2.f * (K * K - 1.f) * norm; + this->a[1] = (1.f - K / Q + K * K) * norm; } break; case PEAK: { if (V >= 1.f) { float norm = 1.f / (1.f + K / Q + K * K); - b[0] = (1.f + K / Q * V + K * K) * norm; - b[1] = 2.f * (K * K - 1.f) * norm; - b[2] = (1.f - K / Q * V + K * K) * norm; - a[0] = b[1]; - a[1] = (1.f - K / Q + K * K) * norm; + this->b[0] = (1.f + K / Q * V + K * K) * norm; + this->b[1] = 2.f * (K * K - 1.f) * norm; + this->b[2] = (1.f - K / Q * V + K * K) * norm; + this->a[0] = this->b[1]; + this->a[1] = (1.f - K / Q + K * K) * norm; } else { float norm = 1.f / (1.f + K / Q / V + K * K); - b[0] = (1.f + K / Q + K * K) * norm; - b[1] = 2.f * (K * K - 1.f) * norm; - b[2] = (1.f - K / Q + K * K) * norm; - a[0] = b[1]; - a[1] = (1.f - K / Q / V + K * K) * norm; + this->b[0] = (1.f + K / Q + K * K) * norm; + this->b[1] = 2.f * (K * K - 1.f) * norm; + this->b[2] = (1.f - K / Q + K * K) * norm; + this->a[0] = this->b[1]; + this->a[1] = (1.f - K / Q / V + K * K) * norm; } } break; case NOTCH: { float norm = 1.f / (1.f + K / Q + K * K); - b[0] = (1.f + K * K) * norm; - b[1] = 2.f * (K * K - 1.f) * norm; - b[2] = b[0]; - a[0] = b[1]; - a[1] = (1.f - K / Q + K * K) * norm; + this->b[0] = (1.f + K * K) * norm; + this->b[1] = 2.f * (K * K - 1.f) * norm; + this->b[2] = this->b[0]; + this->a[0] = this->b[1]; + this->a[1] = (1.f - K / Q + K * K) * norm; } break; default: break; } } - - void copyParameters(const TBiquadFilter& from) { - b[0] = from.b[0]; - b[1] = from.b[1]; - b[2] = from.b[2]; - a[0] = from.a[0]; - a[1] = from.a[1]; - } - - /** Computes the gain of a particular frequency - f: normalized frequency - */ - float getFrequencyResponse(float f) { - // Compute sum(a_k e^(-i k f)) - std::complex bsum = b[0]; - std::complex asum = 1.f; - for (int i = 1; i < 3; i++) { - float p = 2 * M_PI * -i * f; - std::complex e(std::cos(p), std::sin(p)); - bsum += b[i] * e; - asum += a[i - 1] * e; - } - return std::abs(bsum / asum); - } }; typedef TBiquadFilter<> BiquadFilter; diff --git a/include/simd/functions.hpp b/include/simd/functions.hpp index 75610e40..c5e99990 100644 --- a/include/simd/functions.hpp +++ b/include/simd/functions.hpp @@ -168,6 +168,12 @@ inline float_4 fmod(float_4 a, float_4 b) { return a - trunc(a / b) * b; } +using std::hypot; + +inline float_4 hypot(float_4 a, float_4 b) { + return sqrt(a * a + b * b); +} + using std::fabs; inline float_4 fabs(float_4 a) { @@ -176,6 +182,22 @@ inline float_4 fabs(float_4 a) { return a & float_4::cast(mask); } +using std::abs; + +inline float_4 abs(float_4 a) { + return fabs(a); +} + +inline float_4 abs(std::complex a) { + return hypot(a.real(), a.imag()); +} + +using std::arg; + +inline float_4 arg(std::complex a) { + return atan2(a.imag(), a.real()); +} + using std::pow; inline float_4 pow(float_4 a, float_4 b) {