diff --git a/filters/BiQuad.cpp b/filters/BiQuad.cpp index d52374f..370388d 100644 --- a/filters/BiQuad.cpp +++ b/filters/BiQuad.cpp @@ -17,23 +17,20 @@ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ -#define _USE_MATH_DEFINES -#include -#include #include "BiQuad.h" using namespace std; BiQuad::BiQuad(Type type, double dbGain, double freq, double srate, double bandwidthOrQOrS, bool isBandwidth) { - double A; + double A; if(type == PEAKING || type == LOW_SHELF || type == HIGH_SHELF) A = pow(10, dbGain / 40); else A = pow(10, dbGain / 20); - double omega = 2 * M_PI * freq / srate; - double sn = sin(omega); - double cs = cos(omega); + double omega = 2 * M_PI * freq / srate; + double sn = sin(omega); + double cs = cos(omega); double alpha; double beta = -1; @@ -52,36 +49,36 @@ BiQuad::BiQuad(Type type, double dbGain, double freq, double srate, double bandw switch(type) { case LOW_PASS: - b0 = (1 - cs) /2; - b1 = 1 - cs; - b2 = (1 - cs) /2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = (1 - cs) /2; + b1 = 1 - cs; + b2 = (1 - cs) /2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; break; case HIGH_PASS: - b0 = (1 + cs) /2; - b1 = -(1 + cs); - b2 = (1 + cs) /2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = (1 + cs) /2; + b1 = -(1 + cs); + b2 = (1 + cs) /2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; break; case BAND_PASS: - b0 = alpha; - b1 = 0; - b2 = -alpha; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = alpha; + b1 = 0; + b2 = -alpha; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; break; case NOTCH: - b0 = 1; - b1 = -2 * cs; - b2 = 1; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + b0 = 1; + b1 = -2 * cs; + b2 = 1; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; break; case ALL_PASS: b0 = 1 - alpha; @@ -92,12 +89,12 @@ BiQuad::BiQuad(Type type, double dbGain, double freq, double srate, double bandw a2 = 1 - alpha; break; case PEAKING: - b0 = 1 + (alpha * A); - b1 = -2 * cs; - b2 = 1 - (alpha * A); - a0 = 1 + (alpha / A); - a1 = -2 * cs; - a2 = 1 - (alpha / A); + b0 = 1 + (alpha * A); + b1 = -2 * cs; + b2 = 1 - (alpha * A); + a0 = 1 + (alpha / A); + a1 = -2 * cs; + a2 = 1 - (alpha / A); break; case LOW_SHELF: b0 = A * ((A + 1) - (A - 1) * cs + beta); @@ -108,20 +105,20 @@ BiQuad::BiQuad(Type type, double dbGain, double freq, double srate, double bandw a2 = (A + 1) + (A - 1) * cs - beta; break; case HIGH_SHELF: - b0 = A * ((A + 1) + (A - 1) * cs + beta); - b1 = -2 * A * ((A - 1) + (A + 1) * cs); - b2 = A * ((A + 1) + (A - 1) * cs - beta); - a0 = (A + 1) - (A - 1) * cs + beta; - a1 = 2 * ((A - 1) - (A + 1) * cs); - a2 = (A + 1) - (A - 1) * cs - beta; + b0 = A * ((A + 1) + (A - 1) * cs + beta); + b1 = -2 * A * ((A - 1) + (A + 1) * cs); + b2 = A * ((A + 1) + (A - 1) * cs - beta); + a0 = (A + 1) - (A - 1) * cs + beta; + a1 = 2 * ((A - 1) - (A + 1) * cs); + a2 = (A + 1) - (A - 1) * cs - beta; break; } - this->a0 = float(b0 / a0); - this->a[0] = float(b1 / a0); - this->a[1] = float(b2 / a0); - this->a[2] = float(a1 / a0); - this->a[3] = float(a2 / a0); + this->a0 = b0 / a0; + this->a[0] = b1 / a0; + this->a[1] = b2 / a0; + this->a[2] = a1 / a0; + this->a[3] = a2 / a0; x1 = 0; x2 = 0; @@ -129,20 +126,20 @@ BiQuad::BiQuad(Type type, double dbGain, double freq, double srate, double bandw y2 = 0; } -float BiQuad::gainAt(float freq, float srate) +double BiQuad::gainAt(double freq, double srate) { - float omega = 2 * (float)M_PI * freq / srate; - float sn = sin(omega/2.0f); - float phi = sn * sn; - float b0 = this->a0; - float b1 = this->a[0]; - float b2 = this->a[1]; - float a0 = 1.0f; - float a1 = -this->a[2]; - float a2 = -this->a[3]; + double omega = 2 * M_PI * freq / srate; + double sn = sin(omega/2.0); + double phi = sn * sn; + double b0 = this->a0; + double b1 = this->a[0]; + double b2 = this->a[1]; + double a0 = 1.0; + double a1 = this->a[2]; + double a2 = this->a[3]; - float dbGain = 10*log10( pow(b0+b1+b2, 2) - 4*(b0*b1 + 4*b0*b2 + b1*b2)*phi + 16*b0*b2*phi*phi ) + double dbGain = 10*log10( pow(b0+b1+b2, 2) - 4*(b0*b1 + 4*b0*b2 + b1*b2)*phi + 16*b0*b2*phi*phi ) -10*log10( pow(a0+a1+a2, 2) - 4*(a0*a1 + 4*a0*a2 + a1*a2)*phi + 16*a0*a2*phi*phi ); return dbGain; -} \ No newline at end of file +} diff --git a/filters/BiQuad.h b/filters/BiQuad.h index a9fdf04..f34d2d6 100644 --- a/filters/BiQuad.h +++ b/filters/BiQuad.h @@ -19,9 +19,12 @@ #pragma once +#define _USE_MATH_DEFINES +#include +#include #include -#define IS_DENORMAL(f) (((*(unsigned int *)&(f))&0x7f800000) == 0) +#define IS_DENORMAL(d) (abs(d) < DBL_MIN) class BiQuad { @@ -38,20 +41,20 @@ public: void removeDenormals() { if(IS_DENORMAL(x1)) - x1 = 0.0f; + x1 = 0.0; if(IS_DENORMAL(x2)) - x2 = 0.0f; + x2 = 0.0; if(IS_DENORMAL(y1)) - y1 = 0.0f; + y1 = 0.0; if(IS_DENORMAL(y2)) - y2 = 0.0f; + y2 = 0.0; } __forceinline - float process(float sample) + double process(double sample) { // changed order of additions leads to better pipelining - float result = a0 * sample + a[1] * x2 + a[0] * x1 - a[3] * y2 - a[2] * y1; + double result = a0 * sample + a[1] * x2 + a[0] * x1 - a[3] * y2 - a[2] * y1; x2 = x1; x1 = sample; @@ -62,12 +65,12 @@ public: return result; } - float gainAt(float freq, float srate); + double gainAt(double freq, double srate); private: - __declspec(align(16)) float a[4]; - float a0; + __declspec(align(16)) double a[4]; + double a0; - float x1, x2; - float y1, y2; + double x1, x2; + double y1, y2; }; diff --git a/filters/BiQuadFilter.cpp b/filters/BiQuadFilter.cpp index ab83b26..b2ff76c 100644 --- a/filters/BiQuadFilter.cpp +++ b/filters/BiQuadFilter.cpp @@ -22,9 +22,11 @@ using namespace std; -BiQuadFilter::BiQuadFilter(BiQuad::Type type, double dbGain, double freq, double bandwidthOrQOrS, bool isBandwidth) - :type(type), dbGain(dbGain), freq(freq), bandwidthOrQOrS(bandwidthOrQOrS), isBandwidth(isBandwidth) +BiQuadFilter::BiQuadFilter(BiQuad::Type type, double dbGain, double freq, double bandwidthOrQOrS, bool isBandwidth, bool isCornerFreq) + :type(type), dbGain(dbGain), freq(freq), bandwidthOrQOrS(bandwidthOrQOrS), isBandwidth(isBandwidth), isCornerFreq(isCornerFreq) { + channelCount = 0; + biquads = NULL; } BiQuadFilter::~BiQuadFilter() @@ -40,10 +42,20 @@ vector BiQuadFilter::initialize(float sampleRate, unsigned maxFrameCoun { this->channelCount = channelNames.size(); biquads = (BiQuad*)MemoryHelper::alloc(channelCount * sizeof(BiQuad)); + double biquadFreq = freq; + if(isCornerFreq && (type == BiQuad::LOW_SHELF || type == BiQuad::HIGH_SHELF)) + { + // frequency adjustment for DCX2496 + double centerFreqFactor = pow(10.0, abs(dbGain) / 80.0 / bandwidthOrQOrS); + if(type == BiQuad::LOW_SHELF) + biquadFreq *= centerFreqFactor; + else + biquadFreq /= centerFreqFactor; + } for(unsigned i=0; i initialize(float sampleRate, unsigned maxFrameCount, std::vector channelNames); virtual void process(float** output, float** input, unsigned frameCount); + BiQuad::Type getType() const; + double getDbGain() const; + double getFreq() const; + double getBandwidthOrQOrS() const; + bool getIsBandwidth() const; + bool getIsCornerFreq() const; + private: BiQuad::Type type; double dbGain; double freq; double bandwidthOrQOrS; bool isBandwidth; + bool isCornerFreq; size_t channelCount; BiQuad* biquads; diff --git a/filters/BiQuadFilterFactory.cpp b/filters/BiQuadFilterFactory.cpp index 25ce8cf..0e7079f 100644 --- a/filters/BiQuadFilterFactory.cpp +++ b/filters/BiQuadFilterFactory.cpp @@ -93,6 +93,7 @@ vector BiQuadFilterFactory::createFilter(const wstring& configPath, ws double gain = 0; double bandwidthOrQOrS = 0; bool isBandwidth = false; + bool isCornerFreq = false; bool error = false; found = regex_search(parameters, match, regexFreq); @@ -194,14 +195,7 @@ vector BiQuadFilterFactory::createFilter(const wstring& configPath, ws // Maximum S is 1 for 12 dB bandwidthOrQOrS /= 12.0; if(typeString[typeString.length()-1] != L'C') - { - // frequency adjustment for DCX2496 - double centerFreqFactor = pow(10.0, abs(gain) / 80.0 / bandwidthOrQOrS); - if(type == BiQuad::LOW_SHELF) - freq *= centerFreqFactor; - else - freq /= centerFreqFactor; - } + isCornerFreq = true; } if(!error) @@ -209,7 +203,7 @@ vector BiQuadFilterFactory::createFilter(const wstring& configPath, ws TraceF(L"%s", stream.str().c_str()); void* mem = MemoryHelper::alloc(sizeof(BiQuadFilter)); - filter = new(mem) BiQuadFilter(type, gain, freq, bandwidthOrQOrS, isBandwidth); + filter = new(mem) BiQuadFilter(type, gain, freq, bandwidthOrQOrS, isBandwidth, isCornerFreq); } } else if(typeString != L"None") @@ -245,4 +239,4 @@ double BiQuadFilterFactory::getFreq(const wstring& freqString) } else return -1.0; -} \ No newline at end of file +} diff --git a/filters/IIRFilter.cpp b/filters/IIRFilter.cpp index 0a70c7d..3a2f9fb 100644 --- a/filters/IIRFilter.cpp +++ b/filters/IIRFilter.cpp @@ -22,22 +22,22 @@ using namespace std; -#define IS_DENORMAL(f) (((*(unsigned int *)&(f))&0x7f800000) == 0) +#define IS_DENORMAL(d) (abs(d) < DBL_MIN) IIRFilter::IIRFilter(const vector& coefficients) { order = (unsigned)coefficients.size() / 2 - 1; - a = (float*)MemoryHelper::alloc(order * sizeof(float)); - b = (float*)MemoryHelper::alloc(order * sizeof(float)); + a = (double*)MemoryHelper::alloc(order * sizeof(double)); + b = (double*)MemoryHelper::alloc(order * sizeof(double)); x = NULL; y = NULL; double a0 = coefficients[order+1]; - b0 = float(coefficients[0] / a0); + b0 = coefficients[0] / a0; for(unsigned i=0; i IIRFilter::initialize(float sampleRate, unsigned maxFrameCount, if(y != NULL) MemoryHelper::free(y); - x = (float*)MemoryHelper::alloc(order * channelCount * sizeof(float)); - y = (float*)MemoryHelper::alloc(order * channelCount * sizeof(float)); - memset(x, 0, order * channelCount * sizeof(float)); - memset(y, 0, order * channelCount * sizeof(float)); + x = (double*)MemoryHelper::alloc(order * channelCount * sizeof(double)); + y = (double*)MemoryHelper::alloc(order * channelCount * sizeof(double)); + memset(x, 0, order * channelCount * sizeof(double)); + memset(y, 0, order * channelCount * sizeof(double)); return channelNames; } @@ -78,16 +78,17 @@ void IIRFilter::process(float** output, float** input, unsigned frameCount) float* outputChannel = output[i]; unsigned channelOffset = i*order; - float* xo = x+channelOffset; - float* yo = y+channelOffset; + double* xo = x+channelOffset; + double* yo = y+channelOffset; for(unsigned j=0; j0; k--) { sum += b[k] * xo[k]; + xo[k] = xo[k-1]; } sum += b[0] * xo[0]; @@ -95,29 +96,24 @@ void IIRFilter::process(float** output, float** input, unsigned frameCount) for(unsigned k=order-1; k>0; k--) { sum += a[k] * yo[k]; + yo[k] = yo[k-1]; } sum += a[0] * yo[0]; - for(unsigned k=order-1; k>0; k--) - { - xo[k] = xo[k-1]; - yo[k] = yo[k-1]; - } - xo[0] = sample; yo[0] = sum; - outputChannel[j] = sum; + outputChannel[j] = (float)sum; } } for(unsigned i=0; i