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