From ec68e756cda7e413d430c0c92d38d47ec11b7a3f Mon Sep 17 00:00:00 2001 From: Roland Rossgotterer Date: Tue, 1 Oct 2019 09:46:18 +0200 Subject: [PATCH] [AFV] Port NAudio BiQuadFilter --- src/blacksound/dsp/biquadfilter.cpp | 203 ++++++++---------- src/blacksound/dsp/biquadfilter.h | 51 ++--- .../equalizersampleprovider.cpp | 12 +- 3 files changed, 108 insertions(+), 158 deletions(-) diff --git a/src/blacksound/dsp/biquadfilter.cpp b/src/blacksound/dsp/biquadfilter.cpp index dfc415c7c..35459e788 100644 --- a/src/blacksound/dsp/biquadfilter.cpp +++ b/src/blacksound/dsp/biquadfilter.cpp @@ -3,134 +3,99 @@ #include #include -// return buffer index -#define BUFFIX(n,k) ((n + k + BQN) % BQN) - -BiQuadFilter::BiQuadFilter(BiQuadFilterType type, int fs, double fc, double Q, double peakGain) : - m_fs(fs), - m_type(type), - m_fc(fc), - m_Q(Q), - m_peakGain(peakGain) +float BiQuadFilter::transform(float inSample) { - clear(); - calculate(); + // compute result + double result = a0 * inSample + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2; + + // shift x1 to x2, sample to x1 + x2 = x1; + x1 = inSample; + + // shift y1 to y2, result to y1 + y2 = y1; + y1 = (float)result; + + return y1; } -float BiQuadFilter::process(float input) +void BiQuadFilter::setCoefficients(double aa0, double aa1, double aa2, double b0, double b1, double b2) { - unsigned int n = m_index; - - // put input on to buffer - m_X[BUFFIX(n,0)] = input; - - // process input - m_Y[BUFFIX(n,0)] = - m_B[0] * m_X[BUFFIX(n, 0)] + - m_B[1] * m_X[BUFFIX(n, -1)] + - m_B[2] * m_X[BUFFIX(n, -2)] - - m_A[1] * m_Y[BUFFIX(n, -1)] - - m_A[2] * m_Y[BUFFIX(n, -2)]; - - // write output - float output = m_Y[BUFFIX(n, 0)]; - - // step through buffer - m_index = BUFFIX(n, 1); - - return output; + // precompute the coefficients + a0 = b0 / aa0; + a1 = b1 / aa0; + a2 = b2 / aa0; + a3 = aa1 / aa0; + a4 = aa2 / aa0; } -void BiQuadFilter::clear() +void BiQuadFilter::setLowPassFilter(float sampleRate, float cutoffFrequency, float q) { - for (int n = 0; n < BQN; n++) - { - m_X[n] = 0.0; - m_Y[n] = 0.0; - } + // H(s) = 1 / (s^2 + s/Q + 1) + auto w0 = 2 * M_PI * cutoffFrequency / sampleRate; + auto cosw0 = qCos(w0); + auto alpha = qSin(w0) / (2 * q); + + auto b0 = (1 - cosw0) / 2; + auto b1 = 1 - cosw0; + auto b2 = (1 - cosw0) / 2; + auto aa0 = 1 + alpha; + auto aa1 = -2 * cosw0; + auto aa2 = 1 - alpha; + setCoefficients(aa0, aa1, aa2, b0, b1, b2); } -void BiQuadFilter::calculate() +void BiQuadFilter::setPeakingEq(float sampleRate, float centreFrequency, float q, float dbGain) { - double AA = qPow(10.0, m_peakGain / 40.0); - double w0 = 2.0 * M_PI * m_fc / m_fs; - double alpha = qSin(w0) / (2.0 * m_Q); + // H(s) = (s^2 + s*(A/Q) + 1) / (s^2 + s/(A*Q) + 1) + auto w0 = 2 * M_PI * centreFrequency / sampleRate; + auto cosw0 = qCos(w0); + auto sinw0 = qSin(w0); + auto alpha = sinw0 / (2 * q); + auto a = qPow(10, dbGain / 40); // TODO: should we square root this value? - double cos_w0 = qCos(w0); - double sqrt_AA = qSqrt(AA); - - // source : http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt - - switch (m_type) { - case BiQuadFilterType::LowPass: - m_B[0] = (1.0 - cos_w0) / 2.0; - m_B[1] = 1.0 - cos_w0; - m_B[2] = (1.0 - cos_w0) / 2.0; - m_A[0] = 1 + alpha; - m_A[1] = -2.0 * cos_w0; - m_A[2] = 1.0 - alpha; - break; - case BiQuadFilterType::HighPass: - m_B[0] = (1.0 + cos_w0) / 2.0; - m_B[1] = -(1.0 + cos_w0); - m_B[2] = (1.0 + cos_w0) / 2.0; - m_A[0] = 1.0 + alpha; - m_A[1] = -2.0 * cos_w0; - m_A[2] = 1.0 - alpha; - break; - case BiQuadFilterType::BandPass: // (constant 0 dB peak gain) - m_B[0] = alpha; - m_B[1] = 0.0; - m_B[2] = -alpha; - m_A[0] = 1.0 + alpha; - m_A[1] = -2.0 * cos_w0; - m_A[2] = 1.0 - alpha; - break; - case BiQuadFilterType::Notch: - m_B[0] = 1.0; - m_B[1] = -2.0 * cos_w0; - m_B[2] = 1.0; - m_A[0] = 1.0 + alpha; - m_A[1] = -2.0 * cos_w0; - m_A[2] = 1.0 - alpha; - break; - case BiQuadFilterType::Peak: - m_B[0] = 1.0 + alpha*AA; - m_B[1] = -2.0 * cos_w0; - m_B[2] = 1.0 - alpha*AA; - m_A[0] = 1.0 + alpha/AA; - m_A[1] = -2.0 * cos_w0; - m_A[2] = 1.0 - alpha/AA; - break; - case BiQuadFilterType::LowShelf: - m_B[0] = AA*( (AA+1.0) - (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha ); - m_B[1] = 2.0*AA*( (AA-1) - (AA+1.0) * cos_w0 ); - m_B[2] = AA*( (AA+1.0) - (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha ); - m_A[0] = (AA+1.0) + (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha; - m_A[1] = -2.0*( (AA-1.0) + (AA+1.0) * cos_w0 ); - m_A[2] = (AA+1.0) + (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha; - break; - case BiQuadFilterType::HighShelf: - m_B[0] = AA*( (AA+1.0) + (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha ); - m_B[1] = -2.0*AA*( (AA-1.0) + (AA+1.0) * cos_w0 ); - m_B[2] = AA*( (AA+1.0) + (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha ); - m_A[0] = (AA+1.0) - (AA-1.0) * cos_w0 + 2.0 * sqrt_AA * alpha; - m_A[1] = 2.0*( (AA-1.0) - (AA+1.0) * cos_w0 ); - m_A[2] = (AA+1.0) - (AA-1.0) * cos_w0 - 2.0 * sqrt_AA * alpha; - break; - case BiQuadFilterType::None: - m_B[0] = 1.0; - m_B[1] = 0.0; - m_B[2] = 0.0; - m_A[0] = 1.0; - m_A[1] = 0.0; - m_A[2] = 0.0; - } - - // normalize - double norm = m_A[0]; - for (int i = 0; i < BQN; i++) { - m_A[i] /= norm; - m_B[i] /= norm; - } + auto b0 = 1 + alpha * a; + auto b1 = -2 * cosw0; + auto b2 = 1 - alpha * a; + auto aa0 = 1 + alpha / a; + auto aa1 = -2 * cosw0; + auto aa2 = 1 - alpha / a; + setCoefficients(aa0, aa1, aa2, b0, b1, b2); +} + +void BiQuadFilter::setHighPassFilter(float sampleRate, float cutoffFrequency, float q) +{ + // H(s) = s^2 / (s^2 + s/Q + 1) + auto w0 = 2 * M_PI * cutoffFrequency / sampleRate; + auto cosw0 = qCos(w0); + auto alpha = qSin(w0) / (2 * q); + + auto b0 = (1 + cosw0) / 2; + auto b1 = -(1 + cosw0); + auto b2 = (1 + cosw0) / 2; + auto aa0 = 1 + alpha; + auto aa1 = -2 * cosw0; + auto aa2 = 1 - alpha; + setCoefficients(aa0, aa1, aa2, b0, b1, b2); +} + +BiQuadFilter BiQuadFilter::lowPassFilter(float sampleRate, float cutoffFrequency, float q) +{ + BiQuadFilter filter; + filter.setLowPassFilter(sampleRate, cutoffFrequency, q); + return filter; +} + +BiQuadFilter BiQuadFilter::highPassFilter(float sampleRate, float cutoffFrequency, float q) +{ + BiQuadFilter filter; + filter.setHighPassFilter(sampleRate, cutoffFrequency, q); + return filter; +} + +BiQuadFilter BiQuadFilter::peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain) +{ + BiQuadFilter filter; + filter.setPeakingEq(sampleRate, centreFrequency, q, dbGain); + return filter; } diff --git a/src/blacksound/dsp/biquadfilter.h b/src/blacksound/dsp/biquadfilter.h index e699897d0..0bf430804 100644 --- a/src/blacksound/dsp/biquadfilter.h +++ b/src/blacksound/dsp/biquadfilter.h @@ -3,48 +3,33 @@ #include "blacksound/blacksoundexport.h" -// filter type enumeration -enum class BiQuadFilterType { - None, - LowPass, - HighPass, - BandPass, - Notch, - Peak, - LowShelf, - HighShelf -}; - -#define BQN 3 - class BiQuadFilter { public: BiQuadFilter() = default; - BiQuadFilter(BiQuadFilterType type, - int fs = 44100, - double fc = 1000, - double Q = 0.7071, - double peakGain = 0); + float transform(float inSample); + void setCoefficients(double aa0, double aa1, double aa2, double b0, double b1, double b2); + void setLowPassFilter(float sampleRate, float cutoffFrequency, float q); + void setPeakingEq(float sampleRate, float centreFrequency, float q, float dbGain); + void setHighPassFilter(float sampleRate, float cutoffFrequency, float q); - float process(float input); - - void clear(); + static BiQuadFilter lowPassFilter(float sampleRate, float cutoffFrequency, float q); + static BiQuadFilter highPassFilter(float sampleRate, float cutoffFrequency, float q); + static BiQuadFilter peakingEQ(float sampleRate, float centreFrequency, float q, float dbGain); private: - void calculate(); + double a0; + double a1; + double a2; + double a3; + double a4; - double m_A[BQN]; - double m_B[BQN]; - float m_X[BQN]; - float m_Y[BQN]; - unsigned int m_index = 0; - double m_fs = 44100.0; - BiQuadFilterType m_type = BiQuadFilterType::None; - double m_fc = 1000.0; - double m_Q = 0.7071; - double m_peakGain = 0.0; + // state + float x1 = 0.0; + float x2 = 0.0; + float y1 = 0.0; + float y2 = 0.0; }; #endif // BIQUADFILTER_H diff --git a/src/blacksound/sampleprovider/equalizersampleprovider.cpp b/src/blacksound/sampleprovider/equalizersampleprovider.cpp index 3ccd7f780..80e957e4d 100644 --- a/src/blacksound/sampleprovider/equalizersampleprovider.cpp +++ b/src/blacksound/sampleprovider/equalizersampleprovider.cpp @@ -26,7 +26,7 @@ namespace BlackSound for (int band = 0; band < m_filters.size(); band++) { - doubleSamples[n] = m_filters[band].process(doubleSamples[n]); + doubleSamples[n] = m_filters[band].transform(doubleSamples[n]); } doubleSamples[n] *= m_outputGain; } @@ -40,11 +40,11 @@ namespace BlackSound switch (preset) { case VHFEmulation: - m_filters.push_back(BiQuadFilter(BiQuadFilterType::HighPass, 44100, 310, 0.25)); - m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 450, 0.75, 17.0)); - m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 1450, 1.0, 25.0)); - m_filters.push_back(BiQuadFilter(BiQuadFilterType::Peak, 44100, 2000, 1.0, 25.0)); - m_filters.push_back(BiQuadFilter(BiQuadFilterType::LowPass, 44100, 2500, 0.25)); + m_filters.push_back(BiQuadFilter::highPassFilter(44100, 310, 0.25)); + m_filters.push_back(BiQuadFilter::peakingEQ(44100, 450, 0.75, 17.0)); + m_filters.push_back(BiQuadFilter::peakingEQ(44100, 1450, 1.0, 25.0)); + m_filters.push_back(BiQuadFilter::peakingEQ(44100, 2000, 1.0, 25.0)); + m_filters.push_back(BiQuadFilter::lowPassFilter(44100, 2500, 0.25)); break; } }