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