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 <math.h>
robert@464: #include "Biquad.h"
robert@464: #include <iostream>
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: }