|
#include "Biquad.h"
|
#define _USE_MATH_DEFINES
|
#include "math.h"
|
|
namespace ReverbHallRoom
|
{
|
Biquad::Biquad()
|
{
|
ClearBuffers();
|
}
|
|
Biquad::Biquad(FilterType filterType, float fs)
|
{
|
Type = filterType;
|
SetSamplerate(fs);
|
|
SetGainDb(0.0f);
|
Frequency = (float)(fs * 0.25f);
|
SetQ(0.5f);
|
ClearBuffers();
|
}
|
|
Biquad::~Biquad()
|
{
|
|
}
|
|
|
float Biquad::GetSamplerate()
|
{
|
return fs;
|
}
|
|
void Biquad::SetSamplerate(float fs)
|
{
|
this->fs = fs;
|
fsInv = 1.0f / fs;
|
Update();
|
}
|
|
float Biquad::GetGainDb()
|
{
|
return gainDB;
|
}
|
|
float Biquad::GetGain()
|
{
|
return gain;
|
}
|
|
void Biquad::SetGainDb (float value)
|
{
|
// Clamp value between -60 and 60
|
if (value < -60)
|
value = -60;
|
else if (value > 60)
|
value = 60;
|
|
gainDB = value;
|
gain = powf (10.0f, value / 20.0f);
|
}
|
|
void Biquad::SetGain (float value)
|
{
|
if (value < 0.001f)
|
value = 0.001f; // -60dB
|
else if (value > 1000.0f)
|
value = 1000.0f; // 60dB
|
|
gain = value;
|
gainDB = log10f (gain) * 20;
|
}
|
|
float Biquad::GetQ()
|
{
|
return q;
|
}
|
|
void Biquad::SetQ(float value)
|
{
|
if (value < 0.001f)
|
value = 0.001f;
|
q = value;
|
}
|
|
// this is the newer set of formulas from http://www.earlevel.com/main/2011/01/02/biquad-formulas/
|
// Note that for shelf and peak filters, I had to invert the if/else statements for boost and cut, as
|
// I was getting the inverse desired effect, very odd...
|
void Biquad::Update()
|
{
|
auto Fc = Frequency;
|
//auto Fs = fs;
|
|
auto V = powf(10, fabsf(gainDB) / 20.0f);
|
//auto K = tanf(M_PI * Fc / Fs);
|
auto K = tanf(M_PI * Fc * fsInv);
|
auto Q = q;
|
double norm = 1.0;
|
|
switch (Type)
|
{
|
case FilterType::LowPass6db:
|
a1 = -expf(-2.0 * M_PI * (Fc * fsInv));
|
b0 = 1.0 + a1;
|
b1 = b2 = a2 = 0;
|
break;
|
case FilterType::HighPass6db:
|
a1 = -expf(-2.0 * M_PI * (Fc * fsInv));
|
b0 = a1;
|
b1 = -a1;
|
b2 = a2 = 0;
|
break;
|
case FilterType::LowPass:
|
norm = 1 / (1 + K / Q + K * K);
|
b0 = K * K * norm;
|
b1 = 2 * b0;
|
b2 = b0;
|
a1 = 2 * (K * K - 1) * norm;
|
a2 = (1 - K / Q + K * K) * norm;
|
break;
|
case FilterType::HighPass:
|
norm = 1 / (1 + K / Q + K * K);
|
b0 = 1 * norm;
|
b1 = -2 * b0;
|
b2 = b0;
|
a1 = 2 * (K * K - 1) * norm;
|
a2 = (1 - K / Q + K * K) * norm;
|
break;
|
case FilterType::BandPass:
|
norm = 1 / (1 + K / Q + K * K);
|
b0 = K / Q * norm;
|
b1 = 0;
|
b2 = -b0;
|
a1 = 2 * (K * K - 1) * norm;
|
a2 = (1 - K / Q + K * K) * norm;
|
break;
|
case FilterType::Notch:
|
norm = 1 / (1 + K / Q + K * K);
|
b0 = (1 + K * K) * norm;
|
b1 = 2 * (K * K - 1) * norm;
|
b2 = b0;
|
a1 = b1;
|
a2 = (1 - K / Q + K * K) * norm;
|
break;
|
case FilterType::Peak:
|
if (gainDB >= 0)
|
{
|
norm = 1 / (1 + 1 / Q * K + K * K);
|
b0 = (1 + V / Q * K + K * K) * norm;
|
b1 = 2 * (K * K - 1) * norm;
|
b2 = (1 - V / Q * K + K * K) * norm;
|
a1 = b1;
|
a2 = (1 - 1 / Q * K + K * K) * norm;
|
}
|
else
|
{
|
norm = 1 / (1 + V / Q * K + K * K);
|
b0 = (1 + 1 / Q * K + K * K) * norm;
|
b1 = 2 * (K * K - 1) * norm;
|
b2 = (1 - 1 / Q * K + K * K) * norm;
|
a1 = b1;
|
a2 = (1 - V / Q * K + K * K) * norm;
|
}
|
break;
|
case FilterType::LowShelf:
|
if (gainDB >= 0)
|
{
|
norm = 1 / (1 + sqrtf(2) * K + K * K);
|
b0 = (1 + sqrtf(2 * V) * K + V * K * K) * norm;
|
b1 = 2 * (V * K * K - 1) * norm;
|
b2 = (1 - sqrtf(2 * V) * K + V * K * K) * norm;
|
a1 = 2 * (K * K - 1) * norm;
|
a2 = (1 - sqrtf(2) * K + K * K) * norm;
|
}
|
else
|
{
|
norm = 1 / (1 + sqrtf(2 * V) * K + V * K * K);
|
b0 = (1 + sqrtf(2) * K + K * K) * norm;
|
b1 = 2 * (K * K - 1) * norm;
|
b2 = (1 - sqrtf(2) * K + K * K) * norm;
|
a1 = 2 * (V * K * K - 1) * norm;
|
a2 = (1 - sqrtf(2 * V) * K + V * K * K) * norm;
|
}
|
break;
|
case FilterType::HighShelf:
|
if (gainDB >= 0)
|
{
|
norm = 1 / (1 + sqrtf(2) * K + K * K);
|
b0 = (V + sqrtf(2 * V) * K + K * K) * norm;
|
b1 = 2 * (K * K - V) * norm;
|
b2 = (V - sqrtf(2 * V) * K + K * K) * norm;
|
a1 = 2 * (K * K - 1) * norm;
|
a2 = (1 - sqrtf(2) * K + K * K) * norm;
|
}
|
else
|
{
|
norm = 1 / (V + sqrtf(2 * V) * K + K * K);
|
b0 = (1 + sqrtf(2) * K + K * K) * norm;
|
b1 = 2 * (K * K - 1) * norm;
|
b2 = (1 - sqrtf(2) * K + K * K) * norm;
|
a1 = 2 * (K * K - V) * norm;
|
a2 = (V - sqrtf(2 * V) * K + K * K) * norm;
|
}
|
break;
|
}
|
}
|
|
double Biquad::GetResponse(float freq) const
|
{
|
double phi = powf((sinf(2 * M_PI * freq / (2.0 * fs))), 2);
|
double y = ((powf(b0 + b1 + b2, 2.0) - 4.0 * (b0 * b1 + 4.0 * b0 * b2 + b1 * b2) * phi + 16.0 * b0 * b2 * phi * phi) / (powf(1.0 + a1 + a2, 2.0) - 4.0 * (a1 + 4.0 * a2 + a1 * a2) * phi + 16.0 * a2 * phi * phi));
|
// y gives you power gain, not voltage gain, and this a 10 * log_10(g) formula instead of 20 * log_10(g)
|
// by taking the sqrt we get a value that's more suitable for signal processing, i.e. the voltage gain
|
return sqrtf(y);
|
}
|
|
void Biquad::ClearBuffers()
|
{
|
y = 0;
|
x2 = 0;
|
y2 = 0;
|
x1 = 0;
|
y1 = 0;
|
}
|
}
|
