353 lines
9.3 KiB
C++
353 lines
9.3 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef IIR_BIQUAD
|
||
#define IIR_BIQUAD
|
||
|
||
#include <string.h>
|
||
#include "../../../Assertions.h"
|
||
|
||
namespace IIR {
|
||
|
||
|
||
/** frequency limits */
|
||
#define BFG_MIN 0.0001
|
||
#define BFG_MAX 0.4999
|
||
|
||
|
||
/**
|
||
* a simple biquad filter that can be used
|
||
* for low- or high-pass filtering
|
||
* http://www.musicdsp.org/files/Audio-EQ-Cookbook.txt
|
||
*/
|
||
template <typename Scalar> class BiQuad {
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
BiQuad() : in(), out() {
|
||
reset();
|
||
}
|
||
|
||
/** filter the given amplitude of the given channel (history) */
|
||
Scalar filter( const Scalar aIn ) {
|
||
|
||
Scalar aOut = Scalar(); // zero
|
||
aOut += aIn *(b0a0);
|
||
aOut += in[0] *(b1a0);
|
||
aOut += in[1] *(b2a0);
|
||
aOut -= out[0]*(a1a0);
|
||
aOut -= out[1]*(a2a0);
|
||
|
||
in[1] = in[0];
|
||
in[0] = aIn;
|
||
|
||
out[1] = out[0];
|
||
out[0] = aOut;
|
||
|
||
return aOut;
|
||
|
||
}
|
||
|
||
float getA0() {return 1;}
|
||
float getA1() {return a1a0;}
|
||
float getA2() {return a2a0;}
|
||
|
||
float getB0() {return b0a0;}
|
||
float getB1() {return b1a0;}
|
||
float getB2() {return b2a0;}
|
||
|
||
void preFill(const Scalar s) {
|
||
for (int i = 0; i < 100; ++i) {
|
||
filter(s);
|
||
}
|
||
}
|
||
|
||
/** reset (disable) the filter */
|
||
void reset() {
|
||
|
||
b0a0 = 1.0;
|
||
b1a0 = 0.0;
|
||
b2a0 = 0.0;
|
||
a1a0 = 0.0;
|
||
a2a0 = 0.0;
|
||
|
||
memset(in, 0, sizeof(in));
|
||
memset(out, 0, sizeof(out));
|
||
|
||
}
|
||
|
||
/** configure the filter as low-pass. freqFact between ]0;0.5[ */
|
||
//void setLowPass( double freqFact, const float octaves ) {
|
||
void setLowPass( double freqFact, const float Q ) {
|
||
|
||
sanityCheck(freqFact);
|
||
|
||
double w0 = 2.0 * M_PI * freqFact;
|
||
//double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
double alpha = sin(w0)/(2*Q);
|
||
|
||
double b0 = (1.0 - cos(w0))/2.0;
|
||
double b1 = 1.0 - cos(w0);
|
||
double b2 = (1.0 - cos(w0))/2.0;
|
||
double a0 = 1.0 + alpha;
|
||
double a1 = -2.0*cos(w0);
|
||
double a2 = 1.0 - alpha;
|
||
|
||
setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
}
|
||
|
||
|
||
/** configure the filter as low-pass */
|
||
void setLowPass( const float freq, const float Q, const float sRate ) {
|
||
double freqFact = double(freq) / double(sRate);
|
||
setLowPass(freqFact, Q);
|
||
}
|
||
|
||
|
||
|
||
// //http://dspwiki.com/index.php?title=Lowpass_Resonant_Biquad_Filter
|
||
// //http://www.opensource.apple.com/source/WebCore/WebCore-7536.26.14/platform/audio/Biquad.cpp
|
||
// /**
|
||
// * configure as low-pass filter with resonance
|
||
// * @param freqFact the frequency factor between ]0;0.5[
|
||
// * @param res
|
||
// */
|
||
// void setLowPassResonance( double freqFact, float res ) {
|
||
|
||
// sanityCheck(freqFact);
|
||
|
||
// res *= 10;
|
||
|
||
// double g = pow(10.0, 0.05 * res);
|
||
// double d = sqrt((4 - sqrt(16 - 16 / (g * g))) / 2);
|
||
|
||
// double theta = M_PI * freqFact;
|
||
// double sn = 0.5 * d * sin(theta);
|
||
// double beta = 0.5 * (1 - sn) / (1 + sn);
|
||
// double gamma = (0.5 + beta) * cos(theta);
|
||
// double alpha = 0.25 * (0.5 + beta - gamma);
|
||
|
||
// double a0 = 1.0;
|
||
// double b0 = 2.0 * alpha;
|
||
// double b1 = 2.0 * 2.0 * alpha;
|
||
// double b2 = 2.0 * alpha;
|
||
// double a1 = 2.0 * -gamma;
|
||
// double a2 = 2.0 * beta;
|
||
|
||
// setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
// }
|
||
|
||
/** configure the filter as high-pass. freqFact between ]0;0.5[ */
|
||
//void setHighPass( double freqFact, const float octaves ) {
|
||
void setHighPass( double freqFact, const float Q ) {
|
||
|
||
sanityCheck(freqFact);
|
||
|
||
double w0 = 2.0 * M_PI * freqFact;
|
||
//double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
double alpha = sin(w0)/(2*Q);
|
||
|
||
double b0 = (1.0 + cos(w0))/2.0;
|
||
double b1 = -(1.0 + cos(w0));
|
||
double b2 = (1.0 + cos(w0))/2.0;
|
||
double a0 = 1.0 + alpha;
|
||
double a1 = -2.0*cos(w0);
|
||
double a2 = 1.0 - alpha;
|
||
|
||
setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
}
|
||
|
||
/** configure the filter as high-pass */
|
||
void setHighPass( const float freq, const float Q, const float sRate ) {
|
||
double freqFact = double(freq) / double(sRate);
|
||
setHighPass(freqFact, Q);
|
||
}
|
||
|
||
/** configure the filter as band-pass. freqFact between ]0;0.5[ */
|
||
//void setBandPass( double freqFact, const float octaves ) {
|
||
void setBandPass( double freqFact, const float Q ) {
|
||
|
||
sanityCheck(freqFact);
|
||
|
||
//double w0 = 2 * K_PI * / 2 / freqFact;
|
||
double w0 = 2.0 * M_PI * freqFact;
|
||
//double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
double alpha = sin(w0)/(2*Q);
|
||
|
||
|
||
// constant 0dB peak gain
|
||
double b0 = alpha;
|
||
double b1 = 0.0;
|
||
double b2 = -alpha;
|
||
double a0 = 1.0 + alpha;
|
||
double a1 = -2.0*cos(w0);
|
||
double a2 = 1.0 - alpha;
|
||
|
||
setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
}
|
||
|
||
/** configure the filter as band-pass */
|
||
void setBandPass( const float freq, const float Q, float sRate ) {
|
||
double freqFact = double(freq) / double(sRate);
|
||
setBandPass(freqFact, Q);
|
||
}
|
||
|
||
|
||
// /** configure the filter as all-pass. freqFact between ]0;0.5[ */
|
||
// void setAllPass( double freqFact, const float octaves ) {
|
||
|
||
// sanityCheck(freqFact);
|
||
|
||
// double w0 = 2.0 * M_PI * freqFact;
|
||
// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
|
||
// double b0 = 1 - alpha;
|
||
// double b1 = -2*cos(w0);
|
||
// double b2 = 1 + alpha;
|
||
// double a0 = 1 + alpha;
|
||
// double a1 = -2*cos(w0);
|
||
// double a2 = 1 - alpha;
|
||
|
||
// setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
// }
|
||
|
||
// /** configure the filter as all-pass */
|
||
// void setAllPass( const float freq, const float octaves, const float sRate ) {
|
||
// double freqFact = double(freq) / double(sRate);
|
||
// setAllPass(freqFact, octaves);
|
||
// }
|
||
|
||
|
||
|
||
|
||
// /** configure as notch filter. freqFact between ]0;0.5[ */
|
||
// //void setNotch( double freqFact, const float octaves ) {
|
||
// void setNotch( double freqFact, const float Q ) {
|
||
|
||
// sanityCheck(freqFact);
|
||
|
||
// double w0 = 2.0 * M_PI * freqFact;
|
||
// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
|
||
// double b0 = 1.0;
|
||
// double b1 = -2.0*cos(w0);
|
||
// double b2 = 1.0;
|
||
// double a0 = 1.0 + alpha;
|
||
// double a1 = -2.0*cos(w0);
|
||
// double a2 = 1.0 - alpha;
|
||
|
||
// setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
// }
|
||
|
||
// /** configure as notch filter */
|
||
// void setNotch( const float freq, const float octaves, const float sRate ) {
|
||
// double freqFact = double(freq) / double(sRate);
|
||
// setNotch(freqFact, octaves);
|
||
// }
|
||
|
||
// /** configure the filter as low-shelf. increase all aplitudes below freq? freqFact between ]0;0.5[ */
|
||
// void setLowShelf( double freqFact, const float octaves, const float gain ) {
|
||
|
||
// sanityCheck(freqFact);
|
||
|
||
// double A = sqrt( pow(10, (gain/20.0)) );
|
||
// double w0 = 2.0 * M_PI * freqFact;
|
||
// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
|
||
// double b0 = A*( (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha );
|
||
// double b1 = 2.0*A*( (A-1.0) - (A+1.0)*cos(w0) );
|
||
// double b2 = A*( (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha );
|
||
// double a0 = (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha;
|
||
// double a1 = -2.0*( (A-1.0) + (A+1.0)*cos(w0) );
|
||
// double a2 = (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha;
|
||
|
||
// setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
// }
|
||
|
||
// /** configure the filter as low-shelf. increase all aplitudes below freq? */
|
||
// void setLowShelf( const float freq, const float octaves, const float gain, const float sRate ) {
|
||
// double freqFact = double(freq) / double(sRate);
|
||
// setLowShelf(freqFact, octaves, gain);
|
||
// }
|
||
|
||
// /** configure the filter as high-shelf. increase all amplitues above freq? freqFact between ]0;0.5[ */
|
||
// void setHighShelf( double freqFact, const float octaves, const float gain ) {
|
||
|
||
// sanityCheck(freqFact);
|
||
|
||
// double A = sqrt( pow(10, (gain/20.0)) );
|
||
// double w0 = 2.0 * M_PI * freqFact;
|
||
// double alpha = sin(w0)*sinh( log(2)/2 * octaves * w0/sin(w0) );
|
||
|
||
// double b0 = A*( (A+1.0) + (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha );
|
||
// double b1 = -2.0*A*( (A-1.0) + (A+1.0)*cos(w0) );
|
||
// double b2 = A*( (A+1.0) + (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha );
|
||
// double a0 = (A+1.0) - (A-1.0)*cos(w0) + 2.0*sqrt(A)*alpha;
|
||
// double a1 = 2.0*( (A-1.0) - (A+1.0)*cos(w0) );
|
||
// double a2 = (A+1.0) - (A-1.0)*cos(w0) - 2.0*sqrt(A)*alpha;
|
||
|
||
// setValues(a0, a1, a2, b0, b1, b2);
|
||
|
||
// }
|
||
|
||
|
||
// /** configure the filter as high-shelf. increase all amplitues above freq? */
|
||
// void setHighShelf( const float freq, const float octaves, const float gain, const float sRate ) {
|
||
// double freqFact = double(freq) / double(sRate);
|
||
// setHighShelf(freqFact, octaves, gain);
|
||
// }
|
||
|
||
|
||
protected:
|
||
|
||
/** pre-calculate the quotients for the filtering */
|
||
void setValues(double a0, double a1, double a2, double b0, double b1, double b2) {
|
||
b0a0 = float(b0/a0);
|
||
b1a0 = float(b1/a0);
|
||
b2a0 = float(b2/a0);
|
||
a2a0 = float(a2/a0);
|
||
a1a0 = float(a1/a0);
|
||
}
|
||
|
||
/** the bi-quad filter params */
|
||
float b0a0;
|
||
float b1a0;
|
||
float b2a0;
|
||
|
||
float a1a0;
|
||
float a2a0;
|
||
|
||
/** history for input values, per channel */
|
||
Scalar in[2];
|
||
|
||
/** history for ouput values, per channel */
|
||
Scalar out[2];
|
||
|
||
void sanityCheck(const float freqFact) const {
|
||
Assert::isTrue(freqFact >= BFG_MIN, "frequency out of bounds");
|
||
Assert::isTrue(freqFact <= BFG_MAX, "frequency out of bounds");
|
||
}
|
||
|
||
};
|
||
|
||
|
||
}
|
||
|
||
|
||
#endif // IIR_BIQUAD
|