AnalogFilter.cpp (13359B)
1 /* 2 ZynAddSubFX - a software synthesizer 3 4 AnalogFilter.cpp - Several analog filters (lowpass, highpass...) 5 Copyright (C) 2002-2005 Nasca Octavian Paul 6 Copyright (C) 2010-2010 Mark McCurry 7 Author: Nasca Octavian Paul 8 Mark McCurry 9 10 This program is free software; you can redistribute it and/or 11 modify it under the terms of the GNU General Public License 12 as published by the Free Software Foundation; either version 2 13 of the License, or (at your option) any later version. 14 */ 15 16 #include <cstring> //memcpy 17 #include <cmath> 18 #include <cassert> 19 20 #include "../Misc/Util.h" 21 #include "AnalogFilter.h" 22 23 24 const float MAX_FREQ = 20000.0f; 25 26 namespace zyn { 27 28 AnalogFilter::AnalogFilter(unsigned char Ftype, 29 float Ffreq, 30 float Fq, 31 unsigned char Fstages, 32 unsigned int srate, int bufsize) 33 :Filter(srate, bufsize), 34 type(Ftype), 35 stages(Fstages), 36 freq(Ffreq), 37 q(Fq), 38 newq(Fq), 39 gain(1.0), 40 recompute(true), 41 freqbufsize(bufsize/8) 42 { 43 for(int i = 0; i < 3; ++i) 44 coeff.c[i] = coeff.d[i] = oldCoeff.c[i] = oldCoeff.d[i] = 0.0f; 45 if(stages >= MAX_FILTER_STAGES) 46 stages = MAX_FILTER_STAGES; 47 cleanup(); 48 setfreq_and_q(Ffreq, Fq); 49 coeff.d[0] = 0; //this is not used 50 outgain = 1.0f; 51 freq_smoothing.sample_rate(samplerate_f/8); 52 freq_smoothing.thresh(2.0f); // 2Hz 53 beforeFirstTick=true; 54 } 55 56 AnalogFilter::~AnalogFilter() 57 {} 58 59 void AnalogFilter::cleanup() 60 { 61 for(int i = 0; i < MAX_FILTER_STAGES + 1; ++i) { 62 history[i].x1 = 0.0f; 63 history[i].x2 = 0.0f; 64 history[i].y1 = 0.0f; 65 history[i].y2 = 0.0f; 66 oldHistory[i] = history[i]; 67 } 68 } 69 70 AnalogFilter::Coeff AnalogFilter::computeCoeff(int type, float cutoff, float q, 71 int stages, float gain, float fs, int &order) 72 { 73 AnalogFilter::Coeff coeff; 74 bool zerocoefs = false; //this is used if the freq is too high 75 76 const float samplerate_f = fs; 77 const float halfsamplerate_f = fs/2; 78 79 //do not allow frequencies bigger than samplerate/2 80 float freq = cutoff; 81 if(freq > (halfsamplerate_f - 500.0f)) { 82 freq = halfsamplerate_f - 500.0f; 83 zerocoefs = true; 84 } 85 86 if(freq < 0.1f) 87 freq = 0.1f; 88 89 //do not allow bogus Q 90 if(q < 0.0f) 91 q = 0.0f; 92 93 94 float tmpq, tmpgain; 95 if(stages == 0) { 96 tmpq = q; 97 tmpgain = gain; 98 } else { 99 tmpq = (q > 1.0f) ? powf(q, 1.0f / (stages + 1)) : q; 100 tmpgain = powf(gain, 1.0f / (stages + 1)); 101 } 102 103 //Alias Terms 104 float *c = coeff.c; 105 float *d = coeff.d; 106 107 //General Constants 108 const float omega = 2 * PI * freq / samplerate_f; 109 const float sn = sinf(omega), cs = cosf(omega); 110 float alpha, beta; 111 112 //most of these are implementations of 113 //the "Cookbook formulae for audio EQ" by Robert Bristow-Johnson 114 //The original location of the Cookbook is: 115 //http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt 116 float tmp; 117 float tgp1; 118 float tgm1; 119 switch(type) { 120 case 0: //LPF 1 pole 121 if(!zerocoefs) 122 tmp = expf(-2.0f * PI * freq / samplerate_f); 123 else 124 tmp = 0.0f; 125 c[0] = 1.0f - tmp; 126 c[1] = 0.0f; 127 c[2] = 0.0f; 128 d[1] = tmp; 129 d[2] = 0.0f; 130 order = 1; 131 break; 132 case 1: //HPF 1 pole 133 if(!zerocoefs) 134 tmp = expf(-2.0f * PI * freq / samplerate_f); 135 else 136 tmp = 0.0f; 137 c[0] = (1.0f + tmp) / 2.0f; 138 c[1] = -(1.0f + tmp) / 2.0f; 139 c[2] = 0.0f; 140 d[1] = tmp; 141 d[2] = 0.0f; 142 order = 1; 143 break; 144 case 2: //LPF 2 poles 145 if(!zerocoefs) { 146 alpha = sn / (2.0f * tmpq); 147 tmp = 1 + alpha; 148 c[1] = (1.0f - cs) / tmp; 149 c[0] = c[2] = c[1] / 2.0f; 150 d[1] = -2.0f * cs / tmp * -1.0f; 151 d[2] = (1.0f - alpha) / tmp * -1.0f; 152 } 153 else { 154 c[0] = 1.0f; 155 c[1] = c[2] = d[1] = d[2] = 0.0f; 156 } 157 order = 2; 158 break; 159 case 3: //HPF 2 poles 160 if(!zerocoefs) { 161 alpha = sn / (2.0f * tmpq); 162 tmp = 1 + alpha; 163 c[0] = (1.0f + cs) / 2.0f / tmp; 164 c[1] = -(1.0f + cs) / tmp; 165 c[2] = (1.0f + cs) / 2.0f / tmp; 166 d[1] = -2.0f * cs / tmp * -1.0f; 167 d[2] = (1.0f - alpha) / tmp * -1.0f; 168 } 169 else 170 c[0] = c[1] = c[2] = d[1] = d[2] = 0.0f; 171 order = 2; 172 break; 173 case 4: //BPF 2 poles 174 if(!zerocoefs) { 175 alpha = sn / (2.0f * tmpq); 176 tmp = 1.0f + alpha; 177 c[0] = alpha / tmp *sqrtf(tmpq + 1.0f); 178 c[1] = 0.0f; 179 c[2] = -alpha / tmp *sqrtf(tmpq + 1.0f); 180 d[1] = -2.0f * cs / tmp * -1.0f; 181 d[2] = (1.0f - alpha) / tmp * -1.0f; 182 } 183 else 184 c[0] = c[1] = c[2] = d[1] = d[2] = 0.0f; 185 order = 2; 186 break; 187 case 5: //NOTCH 2 poles 188 if(!zerocoefs) { 189 alpha = sn / (2.0f * sqrtf(tmpq)); 190 tmp = 1.0f + alpha; 191 c[0] = 1.0f / tmp; 192 c[1] = -2.0f * cs / tmp; 193 c[2] = 1.0f / tmp; 194 d[1] = -2.0f * cs / tmp * -1.0f; 195 d[2] = (1.0f - alpha) / tmp * -1.0f; 196 } 197 else { 198 c[0] = 1.0f; 199 c[1] = c[2] = d[1] = d[2] = 0.0f; 200 } 201 order = 2; 202 break; 203 case 6: //PEAK (2 poles) 204 if(!zerocoefs) { 205 tmpq *= 3.0f; 206 alpha = sn / (2.0f * tmpq); 207 tmp = 1.0f + alpha / tmpgain; 208 c[0] = (1.0f + alpha * tmpgain) / tmp; 209 c[1] = (-2.0f * cs) / tmp; 210 c[2] = (1.0f - alpha * tmpgain) / tmp; 211 d[1] = -2.0f * cs / tmp * -1.0f; 212 d[2] = (1.0f - alpha / tmpgain) / tmp * -1.0f; 213 } 214 else { 215 c[0] = 1.0f; 216 c[1] = c[2] = d[1] = d[2] = 0.0f; 217 } 218 order = 2; 219 break; 220 case 7: //Low Shelf - 2 poles 221 if(!zerocoefs) { 222 tmpq = sqrtf(tmpq); 223 beta = sqrtf(tmpgain) / tmpq; 224 tgp1 = tmpgain + 1.0f; 225 tgm1 = tmpgain - 1.0f; 226 tmp = tgp1 + tgm1 * cs + beta * sn; 227 228 c[0] = tmpgain * (tgp1 - tgm1 * cs + beta * sn) / tmp; 229 c[1] = 2.0f * tmpgain * (tgm1 - tgp1 * cs) / tmp; 230 c[2] = tmpgain * (tgp1 - tgm1 * cs - beta * sn) / tmp; 231 d[1] = -2.0f * (tgm1 + tgp1 * cs) / tmp * -1.0f; 232 d[2] = (tgp1 + tgm1 * cs - beta * sn) / tmp * -1.0f; 233 } 234 else { 235 c[0] = tmpgain; 236 c[1] = c[2] = d[1] = d[2] = 0.0f; 237 } 238 order = 2; 239 break; 240 case 8: //High Shelf - 2 poles 241 if(!zerocoefs) { 242 tmpq = sqrtf(tmpq); 243 beta = sqrtf(tmpgain) / tmpq; 244 tgp1 = tmpgain + 1.0f; 245 tgm1 = tmpgain - 1.0f; 246 tmp = tgp1 - tgm1 * cs + beta * sn; 247 248 c[0] = tmpgain * (tgp1 + tgm1 * cs + beta * sn) / tmp; 249 c[1] = -2.0f * tmpgain * (tgm1 + tgp1 * cs) / tmp; 250 c[2] = tmpgain * (tgp1 + tgm1 * cs - beta * sn) / tmp; 251 d[1] = 2.0f * (tgm1 - tgp1 * cs) / tmp * -1.0f; 252 d[2] = (tgp1 - tgm1 * cs - beta * sn) / tmp * -1.0f; 253 } 254 else { 255 c[0] = 1.0f; 256 c[1] = c[2] = d[1] = d[2] = 0.0f; 257 } 258 order = 2; 259 break; 260 default: //wrong type 261 assert(false && "wrong type for a filter"); 262 break; 263 } 264 return coeff; 265 } 266 267 void AnalogFilter::computefiltercoefs(float freq, float q) 268 { 269 coeff = AnalogFilter::computeCoeff(type, freq, q, stages, gain, 270 samplerate_f, order); 271 } 272 273 274 void AnalogFilter::setfreq(float frequency) 275 { 276 if(frequency < 0.1f) 277 frequency = 0.1f; 278 else if ( frequency > MAX_FREQ ) 279 frequency = MAX_FREQ; 280 281 float rap = freq / frequency; 282 if(rap < 1.0f) 283 rap = 1.0f / rap; 284 285 frequency = ceilf(frequency);/* fractional Hz changes are not 286 * likely to be audible and waste CPU, 287 * esp since we're already smoothing 288 * changes, so round it */ 289 290 if ( fabsf( frequency - freq ) >= 1.0f ) 291 { 292 /* only perform computation if absolutely necessary */ 293 freq = frequency; 294 recompute = true; 295 } 296 if (recompute) 297 q = newq; 298 299 if (beforeFirstTick) { 300 freq_smoothing.reset( freq ); 301 beforeFirstTick=false; 302 } 303 } 304 305 void AnalogFilter::setfreq_and_q(float frequency, float q_) 306 { 307 newq = q_; 308 /* 309 * Only recompute based on Q change if change is more than 10% 310 * from current value (or the old or new Q is 0, which normally 311 * won't occur, but better to handle it than potentially 312 * fail on division by zero or assert). 313 */ 314 if (q == 0.0 || q_ == 0.0 || ((q > q_ ? q / q_ : q_ / q) > 1.1)) 315 recompute = true; 316 setfreq(frequency); 317 } 318 319 void AnalogFilter::setq(float q_) 320 { 321 newq = q = q_; 322 computefiltercoefs(freq,q); 323 } 324 325 void AnalogFilter::settype(int type_) 326 { 327 type = type_; 328 computefiltercoefs(freq,q); 329 } 330 331 void AnalogFilter::setgain(float dBgain) 332 { 333 gain = dB2rap(dBgain); 334 computefiltercoefs(freq,q); 335 } 336 337 void AnalogFilter::setstages(int stages_) 338 { 339 if(stages_ >= MAX_FILTER_STAGES) 340 stages_ = MAX_FILTER_STAGES - 1; 341 if(stages_ != stages) { 342 stages = stages_; 343 cleanup(); 344 computefiltercoefs(freq,q); 345 } 346 } 347 348 inline void AnalogBiquadFilterA(const float coeff[5], float &src, float work[4]) 349 { 350 work[3] = src*coeff[0] 351 + work[0]*coeff[1] 352 + work[1]*coeff[2] 353 + work[2]*coeff[3] 354 + work[3]*coeff[4]; 355 work[1] = src; 356 src = work[3]; 357 } 358 359 inline void AnalogBiquadFilterB(const float coeff[5], float &src, float work[4]) 360 { 361 work[2] = src*coeff[0] 362 + work[1]*coeff[1] 363 + work[0]*coeff[2] 364 + work[3]*coeff[3] 365 + work[2]*coeff[4]; 366 work[0] = src; 367 src = work[2]; 368 } 369 370 void AnalogFilter::singlefilterout(float *smp, fstage &hist, float f, unsigned int bufsize) 371 { 372 assert((buffersize % 8) == 0); 373 374 if ( recompute ) 375 { 376 computefiltercoefs(f,q); 377 recompute = false; 378 } 379 380 if(order == 1) { //First order filter 381 for(unsigned int i = 0; i < bufsize; ++i) { 382 float y0 = smp[i] * coeff.c[0] + hist.x1 * coeff.c[1] 383 + hist.y1 * coeff.d[1]; 384 hist.y1 = y0; 385 hist.x1 = smp[i]; 386 smp[i] = y0; 387 } 388 } else if(order == 2) {//Second order filter 389 const float coeff_[5] = {coeff.c[0], coeff.c[1], coeff.c[2], coeff.d[1], coeff.d[2]}; 390 float work[4] = {hist.x1, hist.x2, hist.y1, hist.y2}; 391 for(unsigned int i = 0; i < bufsize; i+=8) { 392 AnalogBiquadFilterA(coeff_, smp[i + 0], work); 393 AnalogBiquadFilterB(coeff_, smp[i + 1], work); 394 AnalogBiquadFilterA(coeff_, smp[i + 2], work); 395 AnalogBiquadFilterB(coeff_, smp[i + 3], work); 396 AnalogBiquadFilterA(coeff_, smp[i + 4], work); 397 AnalogBiquadFilterB(coeff_, smp[i + 5], work); 398 AnalogBiquadFilterA(coeff_, smp[i + 6], work); 399 AnalogBiquadFilterB(coeff_, smp[i + 7], work); 400 } 401 hist.x1 = work[0]; 402 hist.x2 = work[1]; 403 hist.y1 = work[2]; 404 hist.y2 = work[3]; 405 } 406 } 407 408 void AnalogFilter::filterout(float *smp) 409 { 410 float freqbuf[freqbufsize]; 411 412 if ( freq_smoothing.apply( freqbuf, freqbufsize, freq ) ) 413 { 414 /* in transition, need to do fine grained interpolation */ 415 for(int i = 0; i < stages + 1; ++i) 416 for(int j = 0; j < freqbufsize; ++j) 417 { 418 recompute = true; 419 singlefilterout(&smp[j*8], history[i], freqbuf[j], 8); 420 } 421 } 422 else 423 { 424 /* stable state, just use one coeff */ 425 for(int i = 0; i < stages + 1; ++i) 426 singlefilterout(smp, history[i], freq, buffersize); 427 } 428 429 for(int i = 0; i < buffersize; ++i) 430 smp[i] *= outgain; 431 } 432 433 float AnalogFilter::H(float freq) 434 { 435 float fr = freq / samplerate_f * PI * 2.0f; 436 float x = coeff.c[0], y = 0.0f; 437 for(int n = 1; n < 3; ++n) { 438 x += cosf(n * fr) * coeff.c[n]; 439 y -= sinf(n * fr) * coeff.c[n]; 440 } 441 float h = x * x + y * y; 442 x = 1.0f; 443 y = 0.0f; 444 for(int n = 1; n < 3; ++n) { 445 x -= cosf(n * fr) * coeff.d[n]; 446 y += sinf(n * fr) * coeff.d[n]; 447 } 448 h = h / (x * x + y * y); 449 return powf(h, (stages + 1.0f) / 2.0f); 450 } 451 452 }