Mercurial > hg > beaglert

annotate projects/d-box/Biquad.cpp @ 0:8a575ba3ab52

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Initial commit.
author andrewm
date Fri, 31 Oct 2014 19:10:17 +0100
parents
children
rev   line source
andrewm@0 1 //
andrewm@0 2 // Biquad.cpp
andrewm@0 3 //
andrewm@0 4 // Created by Nigel Redmon on 11/24/12
andrewm@0 5 // EarLevel Engineering: earlevel.com
andrewm@0 6 // Copyright 2012 Nigel Redmon
andrewm@0 7 //
andrewm@0 8 // For a complete explanation of the Biquad code:
andrewm@0 9 // http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/
andrewm@0 10 //
andrewm@0 11 // License:
andrewm@0 12 //
andrewm@0 13 // This source code is provided as is, without warranty.
andrewm@0 14 // You may copy and distribute verbatim copies of this document.
andrewm@0 15 // You may modify and use this source code to create binary code
andrewm@0 16 // for your own purposes, free or commercial.
andrewm@0 17 //
andrewm@0 18
andrewm@0 19 #include <math.h>
andrewm@0 20 #include "Biquad.h"
andrewm@0 21 #include <iostream>
andrewm@0 22
andrewm@0 23 Biquad::Biquad() {
andrewm@0 24 type = bq_type_lowpass;
andrewm@0 25 a0 = 1.0;
andrewm@0 26 a1 = a2 = b1 = b2 = 0.0;
andrewm@0 27 Fc = 0.50;
andrewm@0 28 Q = 0.707;
andrewm@0 29 peakGain = 0.0;
andrewm@0 30 z1 = z2 = 0.0;
andrewm@0 31 }
andrewm@0 32
andrewm@0 33 Biquad::Biquad(int type, double Fc, double Q, double peakGainDB) {
andrewm@0 34 setBiquad(type, Fc, Q, peakGainDB);
andrewm@0 35 z1 = z2 = 0.0;
andrewm@0 36 }
andrewm@0 37
andrewm@0 38 Biquad::~Biquad() {
andrewm@0 39 }
andrewm@0 40
andrewm@0 41 void Biquad::setType(int type) {
andrewm@0 42 this->type = type;
andrewm@0 43 calcBiquad();
andrewm@0 44 }
andrewm@0 45
andrewm@0 46 void Biquad::setQ(double Q) {
andrewm@0 47 this->Q = Q;
andrewm@0 48 calcBiquad();
andrewm@0 49 }
andrewm@0 50
andrewm@0 51 void Biquad::setFc(double Fc) {
andrewm@0 52 this->Fc = Fc;
andrewm@0 53 calcBiquad();
andrewm@0 54 }
andrewm@0 55
andrewm@0 56 void Biquad::setPeakGain(double peakGainDB) {
andrewm@0 57 this->peakGain = peakGainDB;
andrewm@0 58 calcBiquad();
andrewm@0 59 }
andrewm@0 60
andrewm@0 61 void Biquad::setBiquad(int type, double Fc, double Q, double peakGainDB) {
andrewm@0 62 this->type = type;
andrewm@0 63 this->Q = Q;
andrewm@0 64 this->Fc = Fc;
andrewm@0 65 startFc = Fc;
andrewm@0 66 startQ = Q;
andrewm@0 67 startPeakGain = peakGainDB;
andrewm@0 68 setPeakGain(peakGainDB);
andrewm@0 69 }
andrewm@0 70
andrewm@0 71 void Biquad::calcBiquad(void) {
andrewm@0 72 double norm;
andrewm@0 73 double V = pow(10, fabs(peakGain) / 20.0);
andrewm@0 74 double K = tan(M_PI * Fc);
andrewm@0 75 switch (this->type) {
andrewm@0 76 case bq_type_lowpass:
andrewm@0 77 norm = 1 / (1 + K / Q + K * K);
andrewm@0 78 a0 = K * K * norm;
andrewm@0 79 a1 = 2 * a0;
andrewm@0 80 a2 = a0;
andrewm@0 81 b1 = 2 * (K * K - 1) * norm;
andrewm@0 82 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 83 break;
andrewm@0 84
andrewm@0 85 case bq_type_highpass:
andrewm@0 86 norm = 1 / (1 + K / Q + K * K);
andrewm@0 87 a0 = 1 * norm;
andrewm@0 88 a1 = -2 * a0;
andrewm@0 89 a2 = a0;
andrewm@0 90 b1 = 2 * (K * K - 1) * norm;
andrewm@0 91 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 92 break;
andrewm@0 93
andrewm@0 94 case bq_type_bandpass:
andrewm@0 95 norm = 1 / (1 + K / Q + K * K);
andrewm@0 96 a0 = K / Q * norm;
andrewm@0 97 a1 = 0;
andrewm@0 98 a2 = -a0;
andrewm@0 99 b1 = 2 * (K * K - 1) * norm;
andrewm@0 100 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 101 break;
andrewm@0 102
andrewm@0 103 case bq_type_notch:
andrewm@0 104 norm = 1 / (1 + K / Q + K * K);
andrewm@0 105 a0 = (1 + K * K) * norm;
andrewm@0 106 a1 = 2 * (K * K - 1) * norm;
andrewm@0 107 a2 = a0;
andrewm@0 108 b1 = a1;
andrewm@0 109 b2 = (1 - K / Q + K * K) * norm;
andrewm@0 110 break;
andrewm@0 111
andrewm@0 112 case bq_type_peak:
andrewm@0 113 if (peakGain >= 0) { // boost
andrewm@0 114 norm = 1 / (1 + 1/Q * K + K * K);
andrewm@0 115 a0 = (1 + V/Q * K + K * K) * norm;
andrewm@0 116 a1 = 2 * (K * K - 1) * norm;
andrewm@0 117 a2 = (1 - V/Q * K + K * K) * norm;
andrewm@0 118 b1 = a1;
andrewm@0 119 b2 = (1 - 1/Q * K + K * K) * norm;
andrewm@0 120 }
andrewm@0 121 else { // cut
andrewm@0 122 norm = 1 / (1 + V/Q * K + K * K);
andrewm@0 123 a0 = (1 + 1/Q * K + K * K) * norm;
andrewm@0 124 a1 = 2 * (K * K - 1) * norm;
andrewm@0 125 a2 = (1 - 1/Q * K + K * K) * norm;
andrewm@0 126 b1 = a1;
andrewm@0 127 b2 = (1 - V/Q * K + K * K) * norm;
andrewm@0 128 }
andrewm@0 129 break;
andrewm@0 130 case bq_type_lowshelf:
andrewm@0 131 if (peakGain >= 0) { // boost
andrewm@0 132 norm = 1 / (1 + sqrt(2) * K + K * K);
andrewm@0 133 a0 = (1 + sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 134 a1 = 2 * (V * K * K - 1) * norm;
andrewm@0 135 a2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 136 b1 = 2 * (K * K - 1) * norm;
andrewm@0 137 b2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 138 }
andrewm@0 139 else { // cut
andrewm@0 140 norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
andrewm@0 141 a0 = (1 + sqrt(2) * K + K * K) * norm;
andrewm@0 142 a1 = 2 * (K * K - 1) * norm;
andrewm@0 143 a2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 144 b1 = 2 * (V * K * K - 1) * norm;
andrewm@0 145 b2 = (1 - sqrt(2*V) * K + V * K * K) * norm;
andrewm@0 146 }
andrewm@0 147 break;
andrewm@0 148 case bq_type_highshelf:
andrewm@0 149 if (peakGain >= 0) { // boost
andrewm@0 150 norm = 1 / (1 + sqrt(2) * K + K * K);
andrewm@0 151 a0 = (V + sqrt(2*V) * K + K * K) * norm;
andrewm@0 152 a1 = 2 * (K * K - V) * norm;
andrewm@0 153 a2 = (V - sqrt(2*V) * K + K * K) * norm;
andrewm@0 154 b1 = 2 * (K * K - 1) * norm;
andrewm@0 155 b2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 156 }
andrewm@0 157 else { // cut
andrewm@0 158 norm = 1 / (V + sqrt(2*V) * K + K * K);
andrewm@0 159 a0 = (1 + sqrt(2) * K + K * K) * norm;
andrewm@0 160 a1 = 2 * (K * K - 1) * norm;
andrewm@0 161 a2 = (1 - sqrt(2) * K + K * K) * norm;
andrewm@0 162 b1 = 2 * (K * K - V) * norm;
andrewm@0 163 b2 = (V - sqrt(2*V) * K + K * K) * norm;
andrewm@0 164 }
andrewm@0 165 break;
andrewm@0 166 }
andrewm@0 167
andrewm@0 168 return;
andrewm@0 169 }