experiments.cpp (3949B)
1 2 #include <assert.h> 3 #include <math.h> 4 5 #include "filters/experiments.hpp" 6 7 using namespace bogaudio::dsp; 8 9 void ComplexBiquadFilter::setComplexParams( 10 float gain, 11 float zeroRadius, 12 float zeroTheta, 13 float poleRadius, 14 float poleTheta 15 ) { 16 if ( 17 _gain != gain || 18 _zeroRadius != zeroRadius || 19 _zeroTheta != zeroTheta || 20 _poleRadius != poleRadius || 21 _poleTheta != poleTheta 22 ) { 23 assert(gain >= 0.0f && gain <= 1.0f); 24 assert(zeroRadius >= 0.0f && zeroRadius <= 1.0f); 25 assert(zeroTheta >= 0.0f && zeroTheta <= 2.0f*M_PI); 26 assert(poleRadius >= 0.0f && poleRadius <= 1.0f); 27 assert(poleTheta >= 0.0f && poleTheta <= 2.0f*M_PI); 28 _gain = gain; 29 _zeroRadius = zeroRadius; 30 _zeroTheta = zeroTheta; 31 _poleRadius = poleRadius; 32 _poleTheta = poleTheta; 33 updateParams(); 34 } 35 } 36 37 void ComplexBiquadFilter::updateParams() { 38 setParams( 39 _gain, 40 -2.0f * _zeroRadius * cosf(_zeroTheta) * _gain, 41 _zeroRadius * _zeroRadius * _gain, 42 1.0f, 43 -2.0f * _poleRadius * cosf(_poleTheta), 44 _poleRadius * _poleRadius 45 ); 46 } 47 48 49 // Adapted from Smith 1997, "The Scientist and Engineer's Guide to DSP" 50 void MultipoleFilter::setParams( 51 Type type, 52 int poles, 53 float sampleRate, 54 float cutoff, 55 float ripple 56 ) { 57 if ( 58 _type == type && 59 _poles == poles && 60 _sampleRate == sampleRate && 61 _cutoff == cutoff && 62 _ripple == ripple 63 ) { 64 return; 65 } 66 assert(poles >= 1 && poles <= maxPoles); 67 assert(poles % 2 == 0); // relax this later? 68 assert(sampleRate >= 0.0f); 69 assert(cutoff >= 0.0f && cutoff <= sampleRate / 2.0f); 70 assert(ripple >= 0.0f && ripple <= maxRipple); 71 _type = type; 72 _poles = poles; 73 _sampleRate = sampleRate; 74 _cutoff = cutoff; 75 _ripple = ripple; 76 77 for (int p = 0, pn = _poles / 2; p < pn; ++p) { 78 double a0 = 0.0; 79 double a1 = 0.0; 80 double a2 = 0.0; 81 double b1 = 0.0; 82 double b2 = 0.0; 83 { 84 double angle = M_PI/(_poles*2.0) + p*M_PI/_poles; 85 double rp = -cos(angle); 86 double ip = sin(angle); 87 88 if (_ripple > 0.01f) { 89 double es = sqrt(pow(1.0 / (1.0 - _ripple), 2.0) - 1.0); 90 double esi = 1.0 / es; 91 double esis = esi * esi; 92 double polesi = 1.0 / (float)_poles; 93 double vx = polesi * log(esi + sqrt(esis + 1.0)); 94 double kx = polesi * log(esi + sqrt(esis - 1.0)); 95 kx = (exp(kx) + exp(-kx)) / 2.0; 96 rp *= ((exp(vx) - exp(-vx)) / 2.0) / kx; 97 ip *= ((exp(vx) + exp(-vx)) / 2.0) / kx; 98 // printf("\n\n\ncheb p=%d pn=%d rip=%f rp=%f ip=%f es=%f vx=%f kx=%f\n", p, pn, _ripple, rp, ip, es, vx, kx); 99 } 100 101 const double t = 2.0 * tan(0.5); 102 const double ts = t * t; 103 // printf("\n\nco=%f sr=%f fc=%f\n", _cutoff, _sampleRate, _cutoff / _sampleRate); 104 double m = rp*rp + ip*ip; 105 double mts = m * ts; 106 double d = 4.0 - 4.0*rp*t + mts; 107 double x0 = ts / d; 108 double x1 = 2.0 * x0; 109 double x2 = x0; 110 double y1 = (8.0 - 2.0*mts) / d; 111 double y2 = (-4.0 - 4.0*rp*t - mts) / d; 112 // printf("vs p=%d t=%f w=%f m=%f d=%f x0=%f x1=%f x2=%f y1=%f y2=%f\n", p, t, w, m, d, x0, x1, x2, y1, y2); 113 114 // FIXME: optimization: everything above here only depends on type, poles and ripple. 115 116 double w = 2.0 * M_PI * (_cutoff / _sampleRate); 117 double w2 = w / 2.0; 118 double k = 0.0; 119 switch (_type) { 120 case LP_TYPE: { 121 k = sin(0.5 - w2) / sin(0.5 + w2); 122 break; 123 } 124 case HP_TYPE: { 125 k = -cos(w2 + 0.5) / cos(w2 - 0.5); 126 break; 127 } 128 } 129 double ks = k * k; 130 d = 1.0 + y1*k - y2*ks; 131 a0 = (x0 - x1*k + x2*ks) / d; 132 a1 = (-2.0*x0*k + x1 + x1*ks - 2.0*x2*k) / d; 133 a2 = (x0*ks - x1*k + x2) / d; 134 b1 = (2.0*k + y1 + y1*ks - 2.0*y2*k) / d; 135 b2 = (-ks - y1*k + y2) / d; 136 if (_type == HP_TYPE) { 137 a1 = -a1; 138 b1 = -b1; 139 } 140 141 // printf("pole %d: rp=%f ip=%f a0=%f a1=%f a2=%f b1=%f b2=%f\n\n\n", p, rp, ip, a0, a1, a2, b1, b2); 142 _biquads[p].setParams(a0, a1, a2, 1.0, -b1, -b2); 143 } 144 } 145 } 146 147 float MultipoleFilter::next(float sample) { 148 for (int p = 0, pn = _poles / 2; p < pn; ++p) { 149 sample = _biquads[p].next(sample); 150 } 151 return sample; 152 }