robert@464: // robert@464: // Biquad.cpp robert@464: // robert@464: // Created by Nigel Redmon on 11/24/12 robert@464: // EarLevel Engineering: earlevel.com robert@464: // Copyright 2012 Nigel Redmon robert@464: // robert@464: // For a complete explanation of the Biquad code: robert@464: // http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/ robert@464: // robert@464: // License: robert@464: // robert@464: // This source code is provided as is, without warranty. robert@464: // You may copy and distribute verbatim copies of this document. robert@464: // You may modify and use this source code to create binary code robert@464: // for your own purposes, free or commercial. robert@464: // robert@464: robert@464: #include robert@464: #include "Biquad.h" robert@464: #include robert@464: robert@464: Biquad::Biquad() { robert@464: type = bq_type_lowpass; robert@464: a0 = 1.0; robert@464: a1 = a2 = b1 = b2 = 0.0; robert@464: Fc = 0.50; robert@464: Q = 0.707; robert@464: peakGain = 0.0; robert@464: z1 = z2 = 0.0; robert@464: } robert@464: robert@464: Biquad::Biquad(int type, double Fc, double Q, double peakGainDB) { robert@464: setBiquad(type, Fc, Q, peakGainDB); robert@464: z1 = z2 = 0.0; robert@464: } robert@464: robert@464: Biquad::~Biquad() { robert@464: } robert@464: robert@464: void Biquad::setType(int type) { robert@464: this->type = type; robert@464: calcBiquad(); robert@464: } robert@464: robert@464: void Biquad::setQ(double Q) { robert@464: this->Q = Q; robert@464: calcBiquad(); robert@464: } robert@464: robert@464: void Biquad::setFc(double Fc) { robert@464: this->Fc = Fc; robert@464: calcBiquad(); robert@464: } robert@464: robert@464: void Biquad::setPeakGain(double peakGainDB) { robert@464: this->peakGain = peakGainDB; robert@464: calcBiquad(); robert@464: } robert@464: robert@464: void Biquad::setBiquad(int type, double Fc, double Q, double peakGainDB) { robert@464: this->type = type; robert@464: this->Q = Q; robert@464: this->Fc = Fc; robert@464: startFc = Fc; robert@464: startQ = Q; robert@464: startPeakGain = peakGainDB; robert@464: setPeakGain(peakGainDB); robert@464: } robert@464: robert@464: void Biquad::calcBiquad(void) { robert@464: double norm; robert@464: double V = pow(10, fabs(peakGain) / 20.0); robert@464: double K = tan(M_PI * Fc); robert@464: switch (this->type) { robert@464: case bq_type_lowpass: robert@464: norm = 1 / (1 + K / Q + K * K); robert@464: a0 = K * K * norm; robert@464: a1 = 2 * a0; robert@464: a2 = a0; robert@464: b1 = 2 * (K * K - 1) * norm; robert@464: b2 = (1 - K / Q + K * K) * norm; robert@464: break; robert@464: robert@464: case bq_type_highpass: robert@464: norm = 1 / (1 + K / Q + K * K); robert@464: a0 = 1 * norm; robert@464: a1 = -2 * a0; robert@464: a2 = a0; robert@464: b1 = 2 * (K * K - 1) * norm; robert@464: b2 = (1 - K / Q + K * K) * norm; robert@464: break; robert@464: robert@464: case bq_type_bandpass: robert@464: norm = 1 / (1 + K / Q + K * K); robert@464: a0 = K / Q * norm; robert@464: a1 = 0; robert@464: a2 = -a0; robert@464: b1 = 2 * (K * K - 1) * norm; robert@464: b2 = (1 - K / Q + K * K) * norm; robert@464: break; robert@464: robert@464: case bq_type_notch: robert@464: norm = 1 / (1 + K / Q + K * K); robert@464: a0 = (1 + K * K) * norm; robert@464: a1 = 2 * (K * K - 1) * norm; robert@464: a2 = a0; robert@464: b1 = a1; robert@464: b2 = (1 - K / Q + K * K) * norm; robert@464: break; robert@464: robert@464: case bq_type_peak: robert@464: if (peakGain >= 0) { // boost robert@464: norm = 1 / (1 + 1/Q * K + K * K); robert@464: a0 = (1 + V/Q * K + K * K) * norm; robert@464: a1 = 2 * (K * K - 1) * norm; robert@464: a2 = (1 - V/Q * K + K * K) * norm; robert@464: b1 = a1; robert@464: b2 = (1 - 1/Q * K + K * K) * norm; robert@464: } robert@464: else { // cut robert@464: norm = 1 / (1 + V/Q * K + K * K); robert@464: a0 = (1 + 1/Q * K + K * K) * norm; robert@464: a1 = 2 * (K * K - 1) * norm; robert@464: a2 = (1 - 1/Q * K + K * K) * norm; robert@464: b1 = a1; robert@464: b2 = (1 - V/Q * K + K * K) * norm; robert@464: } robert@464: break; robert@464: case bq_type_lowshelf: robert@464: if (peakGain >= 0) { // boost robert@464: norm = 1 / (1 + sqrt(2) * K + K * K); robert@464: a0 = (1 + sqrt(2*V) * K + V * K * K) * norm; robert@464: a1 = 2 * (V * K * K - 1) * norm; robert@464: a2 = (1 - sqrt(2*V) * K + V * K * K) * norm; robert@464: b1 = 2 * (K * K - 1) * norm; robert@464: b2 = (1 - sqrt(2) * K + K * K) * norm; robert@464: } robert@464: else { // cut robert@464: norm = 1 / (1 + sqrt(2*V) * K + V * K * K); robert@464: a0 = (1 + sqrt(2) * K + K * K) * norm; robert@464: a1 = 2 * (K * K - 1) * norm; robert@464: a2 = (1 - sqrt(2) * K + K * K) * norm; robert@464: b1 = 2 * (V * K * K - 1) * norm; robert@464: b2 = (1 - sqrt(2*V) * K + V * K * K) * norm; robert@464: } robert@464: break; robert@464: case bq_type_highshelf: robert@464: if (peakGain >= 0) { // boost robert@464: norm = 1 / (1 + sqrt(2) * K + K * K); robert@464: a0 = (V + sqrt(2*V) * K + K * K) * norm; robert@464: a1 = 2 * (K * K - V) * norm; robert@464: a2 = (V - sqrt(2*V) * K + K * K) * norm; robert@464: b1 = 2 * (K * K - 1) * norm; robert@464: b2 = (1 - sqrt(2) * K + K * K) * norm; robert@464: } robert@464: else { // cut robert@464: norm = 1 / (V + sqrt(2*V) * K + K * K); robert@464: a0 = (1 + sqrt(2) * K + K * K) * norm; robert@464: a1 = 2 * (K * K - 1) * norm; robert@464: a2 = (1 - sqrt(2) * K + K * K) * norm; robert@464: b1 = 2 * (K * K - V) * norm; robert@464: b2 = (V - sqrt(2*V) * K + K * K) * norm; robert@464: } robert@464: break; robert@464: } robert@464: robert@464: return; robert@464: }