audacia
/
audacia
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
audacia/src/effects/Biquad.cpp

342 lines
12 KiB
C++
Raw Blame History

/**********************************************************************
Audacity: A Digital Audio Editor
Biquad.cpp
Norm C
Max Maisel
***********************************************************************/
#include "Biquad.h"
#include <cmath>
#include <wx/utils.h>
#define square(a) ((a)*(a))
#define PI M_PI
Biquad::Biquad()
{
fNumerCoeffs[B0] = 1;
fNumerCoeffs[B1] = 0;
fNumerCoeffs[B2] = 0;
fDenomCoeffs[A1] = 0;
fDenomCoeffs[A2] = 0;
Reset();
}
void Biquad::Reset()
{
fPrevIn = 0;
fPrevPrevIn = 0;
fPrevOut = 0;
fPrevPrevOut = 0;
}
void Biquad::Process(float* pfIn, float* pfOut, int iNumSamples)
{
for (int i = 0; i < iNumSamples; i++)
*pfOut++ = ProcessOne(*pfIn++);
}
const double Biquad::s_fChebyCoeffs[MAX_Order][MAX_Order + 1] =
{
// For Chebyshev polynomials of the first kind (see http://en.wikipedia.org/wiki/Chebyshev_polynomial)
// Coeffs are in the order 0, 1, 2...9
{ 0, 1}, // order 1
{-1, 0, 2}, // order 2 etc.
{ 0, -3, 0, 4},
{ 1, 0, -8, 0, 8},
{ 0, 5, 0, -20, 0, 16},
{-1, 0, 18, 0, -48, 0, 32},
{ 0, -7, 0, 56, 0, -112, 0, 64},
{ 1, 0, -32, 0, 160, 0, -256, 0, 128},
{ 0, 9, 0, -120, 0, 432, 0, -576, 0, 256},
{-1, 0, 50, 0, -400, 0, 1120, 0, -1280, 0, 512}
};
// order: filter order
// fn: nyquist frequency, i.e. half sample rate
// fc: cutoff frequency
// subtype: highpass or lowpass
ArrayOf<Biquad> Biquad::CalcButterworthFilter(int order, double fn, double fc, int subtype)
{
ArrayOf<Biquad> pBiquad(size_t((order+1) / 2), true);
// Set up the coefficients in all the biquads
double fNorm = fc / fn;
if (fNorm >= 0.9999)
fNorm = 0.9999F;
double fC = tan (PI * fNorm / 2);
double fDCPoleDistSqr = 1.0F;
double fZPoleX, fZPoleY;
if ((order & 1) == 0)
{
// Even order
for (int iPair = 0; iPair < order/2; iPair++)
{
double fSPoleX = fC * cos (PI - (iPair + 0.5) * PI / order);
double fSPoleY = fC * sin (PI - (iPair + 0.5) * PI / order);
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
pBiquad[iPair].fNumerCoeffs [B0] = 1;
if (subtype == kLowPass) // LOWPASS
pBiquad[iPair].fNumerCoeffs [B1] = 2;
else
pBiquad[iPair].fNumerCoeffs [B1] = -2;
pBiquad[iPair].fNumerCoeffs [B2] = 1;
pBiquad[iPair].fDenomCoeffs [A1] = -2 * fZPoleX;
pBiquad[iPair].fDenomCoeffs [A2] = square(fZPoleX) + square(fZPoleY);
if (subtype == kLowPass) // LOWPASS
fDCPoleDistSqr *= Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY);
else
fDCPoleDistSqr *= Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY); // distance from Nyquist
}
}
else
{
// Odd order - first do the 1st-order section
double fSPoleX = -fC;
double fSPoleY = 0;
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
pBiquad[0].fNumerCoeffs [B0] = 1;
if (subtype == kLowPass) // LOWPASS
pBiquad[0].fNumerCoeffs [B1] = 1;
else
pBiquad[0].fNumerCoeffs [B1] = -1;
pBiquad[0].fNumerCoeffs [B2] = 0;
pBiquad[0].fDenomCoeffs [A1] = -fZPoleX;
pBiquad[0].fDenomCoeffs [A2] = 0;
if (subtype == kLowPass) // LOWPASS
fDCPoleDistSqr = 1 - fZPoleX;
else
fDCPoleDistSqr = fZPoleX + 1; // dist from Nyquist
for (int iPair = 1; iPair <= order/2; iPair++)
{
double fSPoleX = fC * cos (PI - iPair * PI / order);
double fSPoleY = fC * sin (PI - iPair * PI / order);
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
pBiquad[iPair].fNumerCoeffs [B0] = 1;
if (subtype == kLowPass) // LOWPASS
pBiquad[iPair].fNumerCoeffs [B1] = 2;
else
pBiquad[iPair].fNumerCoeffs [B1] = -2;
pBiquad[iPair].fNumerCoeffs [B2] = 1;
pBiquad[iPair].fDenomCoeffs [A1] = -2 * fZPoleX;
pBiquad[iPair].fDenomCoeffs [A2] = square(fZPoleX) + square(fZPoleY);
if (subtype == kLowPass) // LOWPASS
fDCPoleDistSqr *= Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY);
else
fDCPoleDistSqr *= Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY); // distance from Nyquist
}
}
pBiquad[0].fNumerCoeffs [B0] *= fDCPoleDistSqr / (1 << order); // mult by DC dist from poles, divide by dist from zeroes
pBiquad[0].fNumerCoeffs [B1] *= fDCPoleDistSqr / (1 << order);
pBiquad[0].fNumerCoeffs [B2] *= fDCPoleDistSqr / (1 << order);
return std::move(pBiquad);
}
// order: filter order
// fn: nyquist frequency, i.e. half sample rate
// fc: cutoff frequency
// ripple: passband ripple in dB
// subtype: highpass or lowpass
ArrayOf<Biquad> Biquad::CalcChebyshevType1Filter(int order, double fn, double fc, double ripple, int subtype)
{
ArrayOf<Biquad> pBiquad(size_t((order+1) / 2), true);
// Set up the coefficients in all the biquads
double fNorm = fc / fn;
if (fNorm >= 0.9999)
fNorm = 0.9999F;
double fC = tan (PI * fNorm / 2);
double fDCPoleDistSqr = 1.0F;
double fZPoleX, fZPoleY;
double fZZeroX;
double beta = cos (fNorm*PI);
double eps; eps = sqrt (pow (10.0, wxMax(0.001, ripple) / 10.0) - 1);
double a; a = log (1 / eps + sqrt(1 / square(eps) + 1)) / order;
// Assume even order to start
for (int iPair = 0; iPair < order/2; iPair++)
{
double fSPoleX = -fC * sinh (a) * sin ((2*iPair + 1) * PI / (2 * order));
double fSPoleY = fC * cosh (a) * cos ((2*iPair + 1) * PI / (2 * order));
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
if (subtype == kLowPass) // LOWPASS
{
fZZeroX = -1;
fDCPoleDistSqr = Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY);
fDCPoleDistSqr /= 2*2; // dist from zero at Nyquist
}
else
{
// Highpass - do the digital LP->HP transform on the poles and zeroes
ComplexDiv (beta - fZPoleX, -fZPoleY, 1 - beta * fZPoleX, -beta * fZPoleY, &fZPoleX, &fZPoleY);
fZZeroX = 1;
fDCPoleDistSqr = Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY); // distance from Nyquist
fDCPoleDistSqr /= 2*2; // dist from zero at Nyquist
}
pBiquad[iPair].fNumerCoeffs [B0] = fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B1] = -2 * fZZeroX * fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B2] = fDCPoleDistSqr;
pBiquad[iPair].fDenomCoeffs [A1] = -2 * fZPoleX;
pBiquad[iPair].fDenomCoeffs [A2] = square(fZPoleX) + square(fZPoleY);
}
if ((order & 1) == 0)
{
double fTemp = DB_TO_LINEAR(-wxMax(0.001, ripple)); // at DC the response is down R dB (for even-order)
pBiquad[0].fNumerCoeffs [B0] *= fTemp;
pBiquad[0].fNumerCoeffs [B1] *= fTemp;
pBiquad[0].fNumerCoeffs [B2] *= fTemp;
}
else
{
// Odd order - now do the 1st-order section
double fSPoleX = -fC * sinh (a);
double fSPoleY = 0;
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
if (subtype == kLowPass) // LOWPASS
{
fZZeroX = -1;
fDCPoleDistSqr = sqrt(Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY));
fDCPoleDistSqr /= 2; // dist from zero at Nyquist
}
else
{
// Highpass - do the digital LP->HP transform on the poles and zeroes
ComplexDiv (beta - fZPoleX, -fZPoleY, 1 - beta * fZPoleX, -beta * fZPoleY, &fZPoleX, &fZPoleY);
fZZeroX = 1;
fDCPoleDistSqr = sqrt(Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY)); // distance from Nyquist
fDCPoleDistSqr /= 2; // dist from zero at Nyquist
}
pBiquad[(order-1)/2].fNumerCoeffs [B0] = fDCPoleDistSqr;
pBiquad[(order-1)/2].fNumerCoeffs [B1] = -fZZeroX * fDCPoleDistSqr;
pBiquad[(order-1)/2].fNumerCoeffs [B2] = 0;
pBiquad[(order-1)/2].fDenomCoeffs [A1] = -fZPoleX;
pBiquad[(order-1)/2].fDenomCoeffs [A2] = 0;
}
return std::move(pBiquad);
}
// order: filter order
// fn: nyquist frequency, i.e. half sample rate
// fc: cutoff frequency
// ripple: stopband ripple in dB
// subtype: highpass or lowpass
ArrayOf<Biquad> Biquad::CalcChebyshevType2Filter(int order, double fn, double fc, double ripple, int subtype)
{
ArrayOf<Biquad> pBiquad(size_t((order+1) / 2), true);
// Set up the coefficients in all the biquads
double fNorm = fc / fn;
if (fNorm >= 0.9999)
fNorm = 0.9999F;
double fC = tan (PI * fNorm / 2);
double fDCPoleDistSqr = 1.0F;
double fZPoleX, fZPoleY;
double fZZeroX, fZZeroY;
double beta = cos (fNorm*PI);
double fSZeroX, fSZeroY;
double fSPoleX, fSPoleY;
double eps = DB_TO_LINEAR(-wxMax(0.001, ripple));
double a = log (1 / eps + sqrt(1 / square(eps) + 1)) / order;
// Assume even order
for (int iPair = 0; iPair < order/2; iPair++)
{
ComplexDiv (fC, 0, -sinh (a) * sin ((2*iPair + 1) * PI / (2 * order)),
cosh (a) * cos ((2*iPair + 1) * PI / (2 * order)),
&fSPoleX, &fSPoleY);
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
fSZeroX = 0;
fSZeroY = fC / cos (((2 * iPair) + 1) * PI / (2 * order));
BilinTransform (fSZeroX, fSZeroY, &fZZeroX, &fZZeroY);
if (subtype == kLowPass) // LOWPASS
{
fDCPoleDistSqr = Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY);
fDCPoleDistSqr /= Calc2D_DistSqr (1, 0, fZZeroX, fZZeroY);
}
else
{
// Highpass - do the digital LP->HP transform on the poles and zeroes
ComplexDiv (beta - fZPoleX, -fZPoleY, 1 - beta * fZPoleX, -beta * fZPoleY, &fZPoleX, &fZPoleY);
ComplexDiv (beta - fZZeroX, -fZZeroY, 1 - beta * fZZeroX, -beta * fZZeroY, &fZZeroX, &fZZeroY);
fDCPoleDistSqr = Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY); // distance from Nyquist
fDCPoleDistSqr /= Calc2D_DistSqr (-1, 0, fZZeroX, fZZeroY);
}
pBiquad[iPair].fNumerCoeffs [B0] = fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B1] = -2 * fZZeroX * fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B2] = (square(fZZeroX) + square(fZZeroY)) * fDCPoleDistSqr;
pBiquad[iPair].fDenomCoeffs [A1] = -2 * fZPoleX;
pBiquad[iPair].fDenomCoeffs [A2] = square(fZPoleX) + square(fZPoleY);
}
// Now, if it's odd order, we have one more to do
if (order & 1)
{
int iPair = (order-1)/2; // we'll do it as a biquad, but it's just first-order
ComplexDiv (fC, 0, -sinh (a) * sin ((2*iPair + 1) * PI / (2 * order)),
cosh (a) * cos ((2*iPair + 1) * PI / (2 * order)),
&fSPoleX, &fSPoleY);
BilinTransform (fSPoleX, fSPoleY, &fZPoleX, &fZPoleY);
fZZeroX = -1; // in the s-plane, the zero is at infinity
fZZeroY = 0;
if (subtype == kLowPass) // LOWPASS
{
fDCPoleDistSqr = sqrt(Calc2D_DistSqr (1, 0, fZPoleX, fZPoleY));
fDCPoleDistSqr /= 2;
}
else
{
// Highpass - do the digital LP->HP transform on the poles and zeroes
ComplexDiv (beta - fZPoleX, -fZPoleY, 1 - beta * fZPoleX, -fZPoleY, &fZPoleX, &fZPoleY);
fZZeroX = 1;
fDCPoleDistSqr = sqrt(Calc2D_DistSqr (-1, 0, fZPoleX, fZPoleY)); // distance from Nyquist
fDCPoleDistSqr /= 2;
}
pBiquad[iPair].fNumerCoeffs [B0] = fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B1] = -fZZeroX * fDCPoleDistSqr;
pBiquad[iPair].fNumerCoeffs [B2] = 0;
pBiquad[iPair].fDenomCoeffs [A1] = -fZPoleX;
pBiquad[iPair].fDenomCoeffs [A2] = 0;
}
return std::move(pBiquad);
}
void Biquad::ComplexDiv (double fNumerR, double fNumerI, double fDenomR, double fDenomI,
double* pfQuotientR, double* pfQuotientI)
{
double fDenom = square(fDenomR) + square(fDenomI);
*pfQuotientR = (fNumerR * fDenomR + fNumerI * fDenomI) / fDenom;
*pfQuotientI = (fNumerI * fDenomR - fNumerR * fDenomI) / fDenom;
}
bool Biquad::BilinTransform (double fSX, double fSY, double* pfZX, double* pfZY)
{
double fDenom = square (1 - fSX) + square (fSY);
*pfZX = (1 - square (fSX) - square (fSY)) / fDenom;
*pfZY = 2 * fSY / fDenom;
return true;
}
float Biquad::Calc2D_DistSqr (double fX1, double fY1, double fX2, double fY2)
{
return square (fX1 - fX2) + square (fY1 - fY2);
}
double Biquad::ChebyPoly(int Order, double NormFreq) // NormFreq = 1 at the f0 point (where response is R dB down)
{
// Calc cosh (Order * acosh (NormFreq));
double x = 1;
double fSum = 0;
wxASSERT (Order >= MIN_Order && Order <= MAX_Order);
for (int i = 0; i <= Order; i++)
{
fSum += s_fChebyCoeffs [Order-1][i] * x;
x *= NormFreq;
}
return fSum;
}
Reference in New Issue View Git Blame Copy Permalink