1 #include <math.h>
2 #include "BiquadFilter.h"
3 #include "globals.h"
4 #include <complex>
5
6 using namespace std;
7
square(double x)8 inline double square(double x) { return x * x; }
9
BiquadFilter()10 BiquadFilter::BiquadFilter()
11 {
12 reg0.d[0] = 0;
13 reg1.d[0] = 0;
14 reg0.d[1] = 0;
15 reg1.d[1] = 0;
16 first_run = true;
17
18 a1.init_x87();
19 a2.init_x87();
20 b0.init_x87();
21 b1.init_x87();
22 b2.init_x87();
23 }
24
BiquadFilter(SurgeStorage * storage)25 BiquadFilter::BiquadFilter(SurgeStorage *storage)
26 {
27 this->storage = storage;
28 reg0.d[0] = 0;
29 reg1.d[0] = 0;
30 reg0.d[1] = 0;
31 reg1.d[1] = 0;
32 first_run = true;
33
34 /*if(storage->SSE2)
35 {
36 a1.init_SSE2();
37 a2.init_SSE2();
38 b0.init_SSE2();
39 b1.init_SSE2();
40 b2.init_SSE2();
41 }
42 else*/
43 {
44 a1.init_x87();
45 a2.init_x87();
46 b0.init_x87();
47 b1.init_x87();
48 b2.init_x87();
49 }
50 }
51
coeff_LP(double omega,double Q)52 void BiquadFilter::coeff_LP(double omega, double Q)
53 {
54 if (omega > M_PI)
55 set_coef(1, 0, 0, 1, 0, 0);
56 else
57 {
58 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2 * Q), b0 = (1 - cosi) * 0.5,
59 b1 = 1 - cosi, b2 = (1 - cosi) * 0.5, a0 = 1 + alpha, a1 = -2 * cosi, a2 = 1 - alpha;
60
61 set_coef(a0, a1, a2, b0, b1, b2);
62 }
63 }
64
coeff_LP2B(double omega,double Q)65 void BiquadFilter::coeff_LP2B(double omega, double Q)
66 {
67 if (omega > M_PI)
68 set_coef(1, 0, 0, 1, 0, 0);
69 else
70 {
71 double w_sq = omega * omega;
72 double den =
73 (w_sq * w_sq) + (M_PI * M_PI * M_PI * M_PI) + w_sq * (M_PI * M_PI) * (1 / Q - 2);
74 double G1 = min(1.0, sqrt((w_sq * w_sq) / den) * 0.5);
75
76 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2 * Q),
77
78 A = 2 * sqrt(G1) * sqrt(2 - G1), b0 = (1 - cosi + G1 * (1 + cosi) + A * sinu) * 0.5,
79 b1 = (1 - cosi - G1 * (1 + cosi)),
80 b2 = (1 - cosi + G1 * (1 + cosi) - A * sinu) * 0.5, a0 = (1 + alpha), a1 = -2 * cosi,
81 a2 = 1 - alpha;
82
83 set_coef(a0, a1, a2, b0, b1, b2);
84 }
85 }
86
coeff_HP(double omega,double Q)87 void BiquadFilter::coeff_HP(double omega, double Q)
88 {
89 if (omega > M_PI)
90 set_coef(1, 0, 0, 0, 0, 0);
91 else
92 {
93 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2 * Q), b0 = (1 + cosi) * 0.5,
94 b1 = -(1 + cosi), b2 = (1 + cosi) * 0.5, a0 = 1 + alpha, a1 = -2 * cosi,
95 a2 = 1 - alpha;
96
97 set_coef(a0, a1, a2, b0, b1, b2);
98 }
99 }
100
coeff_BP(double omega,double Q)101 void BiquadFilter::coeff_BP(double omega, double Q)
102 {
103 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2.0 * Q), b0 = alpha, b2 = -alpha,
104 a0 = 1 + alpha, a1 = -2 * cosi, a2 = 1 - alpha;
105
106 set_coef(a0, a1, a2, b0, 0, b2);
107 }
108
coeff_BP2A(double omega,double BW)109 void BiquadFilter::coeff_BP2A(double omega, double BW)
110 {
111 double cosi = cos(omega), sinu = sin(omega), q = 1 / (0.02 + 30 * BW * BW),
112 alpha = sinu / (2 * q), b0 = alpha, b2 = -alpha, a0 = 1 + alpha, a1 = -2 * cosi,
113 a2 = 1 - alpha;
114
115 set_coef(a0, a1, a2, b0, 0, b2);
116 }
117
coeff_PKA(double omega,double QQ)118 void BiquadFilter::coeff_PKA(double omega, double QQ)
119 {
120 double cosi = cos(omega), sinu = sin(omega), reso = limit_range(QQ, 0.0, 1.0),
121 q = 0.1 + 10 * reso * reso, alpha = sinu / (2 * q), b0 = q * alpha, b2 = -q * alpha,
122 a0 = 1 + alpha, a1 = -2 * cosi, a2 = 1 - alpha;
123
124 set_coef(a0, a1, a2, b0, 0, b2);
125 }
126
coeff_NOTCH(double omega,double QQ)127 void BiquadFilter::coeff_NOTCH(double omega, double QQ)
128 {
129 if (omega > M_PI)
130 set_coef(1, 0, 0, 1, 0, 0);
131 else
132 {
133 double cosi = cos(omega), sinu = sin(omega), reso = limit_range(QQ, 0.0, 1.0),
134 q = 1 / (0.02 + 30 * reso * reso), alpha = sinu / (2 * q), b0 = 1.0,
135 b1 = -2.0 * cosi, b2 = 1.0, a0 = 1.0 + alpha, a1 = -2.0 * cosi, a2 = 1.0 - alpha;
136
137 set_coef(a0, a1, a2, b0, b1, b2);
138 }
139 }
140
coeff_LP_with_BW(double omega,double BW)141 void BiquadFilter::coeff_LP_with_BW(double omega, double BW) { coeff_LP(omega, 1 / BW); }
142
coeff_HP_with_BW(double omega,double BW)143 void BiquadFilter::coeff_HP_with_BW(double omega, double BW) { coeff_HP(omega, 1 / BW); }
144
coeff_LPHPmorph(double omega,double Q,double morph)145 void BiquadFilter::coeff_LPHPmorph(double omega, double Q, double morph)
146 {
147 double HP = limit_range(morph, 0.0, 1.0), LP = 1 - HP, BP = LP * HP;
148 HP *= HP;
149 LP *= LP;
150
151 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2 * Q), b0 = alpha, b1 = 0,
152 b2 = -alpha, a0 = 1 + alpha, a1 = -2 * cosi, a2 = 1 - alpha;
153
154 set_coef(a0, a1, a2, b0, b1, b2);
155 }
156
coeff_APF(double omega,double Q)157 void BiquadFilter::coeff_APF(double omega, double Q)
158 {
159 if ((omega < 0.0) || (omega > M_PI))
160 set_coef(1, 0, 0, 1, 0, 0);
161 else
162 {
163 double cosi = cos(omega), sinu = sin(omega), alpha = sinu / (2 * Q), b0 = (1 - alpha),
164 b1 = -2 * cosi, b2 = (1 + alpha), a0 = (1 + alpha), a1 = -2 * cosi, a2 = (1 - alpha);
165
166 set_coef(a0, a1, a2, b0, b1, b2);
167 }
168 }
169
coeff_peakEQ(double omega,double BW,double gain)170 void BiquadFilter::coeff_peakEQ(double omega, double BW, double gain)
171 {
172 coeff_orfanidisEQ(omega, BW, db_to_linear(gain), db_to_linear(gain * 0.5), 1);
173 }
174
coeff_orfanidisEQ(double omega,double BW,double G,double GB,double G0)175 void BiquadFilter::coeff_orfanidisEQ(double omega, double BW, double G, double GB, double G0)
176 {
177 // For the curious http://eceweb1.rutgers.edu/~orfanidi/ece521/hpeq.pdf appears to be the source
178 // of this
179 double limit = 0.95;
180 double w0 = omega; // min(M_PI-0.000001,omega);
181 BW = max(minBW, BW);
182 double Dww = 2 * w0 * sinh((log(2.0) / 2.0) * BW); // sinh = (e^x - e^-x)/2
183
184 double gainscale = 1;
185 // if(omega>M_PI) gainscale = 1 / (1 + (omega-M_PI)*(4/Dw));
186
187 if (abs(G - G0) > 0.00001)
188 {
189 double F = abs(G * G - GB * GB);
190 double G00 = abs(G * G - G0 * G0);
191 double F00 = abs(GB * GB - G0 * G0);
192 double num =
193 G0 * G0 * square(w0 * w0 - (M_PI * M_PI)) + G * G * F00 * (M_PI * M_PI) * Dww * Dww / F;
194 double den = square(w0 * w0 - M_PI * M_PI) + F00 * M_PI * M_PI * Dww * Dww / F;
195 double G1 = sqrt(num / den);
196
197 if (omega > M_PI)
198 {
199 G = G1 * 0.9999;
200 w0 = M_PI - 0.00001;
201 G00 = abs(G * G - G0 * G0);
202 F00 = abs(GB * GB - G0 * G0);
203 }
204
205 double G01 = abs(G * G - G0 * G1);
206 double G11 = abs(G * G - G1 * G1);
207 double F01 = abs(GB * GB - G0 * G1);
208 double F11 = abs(GB * GB - G1 * G1); // blir wacko ?
209 // goes crazy (?)
210 double W2 = sqrt(G11 / G00) * square(tan(w0 / 2));
211 double w_lower = w0 * powf(2, -0.5 * BW);
212 double w_upper =
213 2 * atan(sqrt(F00 / F11) * sqrt(G11 / G00) * square(tan(w0 / 2)) / tan(w_lower / 2));
214 double Dw = abs(w_upper - w_lower);
215 double DW = (1 + sqrt(F00 / F11) * W2) * tan(Dw / 2);
216
217 double C = F11 * DW * DW - 2 * W2 * (F01 - sqrt(F00 * F11));
218 double D = 2 * W2 * (G01 - sqrt(G00 * G11));
219 double A = sqrt((C + D) / F);
220 double B = sqrt((G * G * C + GB * GB * D) / F);
221 double a0 = (1 + W2 + A), a1 = -2 * (1 - W2), a2 = (1 + W2 - A), b0 = (G1 + G0 * W2 + B),
222 b1 = -2 * (G1 - G0 * W2), b2 = (G1 - B + G0 * W2);
223
224 // if (c) sprintf(c,"G0: %f G: %f GB: %f G1: %f
225 // ",linear_to_dB(G0),linear_to_dB(G),linear_to_dB(GB),linear_to_dB(G1));
226
227 set_coef(a0, a1, a2, b0, b1, b2);
228 }
229 else
230 {
231 set_coef(1, 0, 0, 1, 0, 0);
232 }
233 }
234
coeff_same_as_last_time()235 void BiquadFilter::coeff_same_as_last_time()
236 {
237 // If you want to change interpolation then set dv = 0 here
238 }
239
coeff_instantize()240 void BiquadFilter::coeff_instantize()
241 {
242 a1.instantize();
243 a2.instantize();
244 b0.instantize();
245 b1.instantize();
246 b2.instantize();
247 }
248
set_coef(double a0,double a1,double a2,double b0,double b1,double b2)249 void BiquadFilter::set_coef(double a0, double a1, double a2, double b0, double b1, double b2)
250 {
251 double a0inv = 1 / a0;
252
253 b0 *= a0inv;
254 b1 *= a0inv;
255 b2 *= a0inv;
256 a1 *= a0inv;
257 a2 *= a0inv;
258 if (first_run)
259 {
260 this->a1.startValue(a1);
261 this->a2.startValue(a2);
262 this->b0.startValue(b0);
263 this->b1.startValue(b1);
264 this->b2.startValue(b2);
265 first_run = false;
266 }
267 this->a1.newValue(a1);
268 this->a2.newValue(a2);
269 this->b0.newValue(b0);
270 this->b1.newValue(b1);
271 this->b2.newValue(b2);
272 }
273
process_block(float * data)274 void BiquadFilter::process_block(float *data)
275 {
276 /*if(storage->SSE2) process_block_SSE2(data);
277 else*/
278 {
279 int k;
280 for (k = 0; k < BLOCK_SIZE; k++)
281 {
282 a1.process();
283 a2.process();
284 b0.process();
285 b1.process();
286 b2.process();
287
288 double input = data[k];
289 double op;
290
291 op = input * b0.v.d[0] + reg0.d[0];
292 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
293 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
294
295 data[k] = op;
296 }
297 flush_denormal(reg0.d[0]);
298 flush_denormal(reg1.d[0]);
299 }
300 }
301
process_block_to(float * __restrict data,float * __restrict dataout)302 void BiquadFilter::process_block_to(float *__restrict data, float *__restrict dataout)
303 {
304 /*if(storage->SSE2) process_block_SSE2(data);
305 else*/
306 {
307 int k;
308 for (k = 0; k < BLOCK_SIZE; k++)
309 {
310 a1.process();
311 a2.process();
312 b0.process();
313 b1.process();
314 b2.process();
315
316 double input = data[k];
317 double op;
318
319 op = input * b0.v.d[0] + reg0.d[0];
320 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
321 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
322
323 dataout[k] = op;
324 }
325 flush_denormal(reg0.d[0]);
326 flush_denormal(reg1.d[0]);
327 }
328 }
329
process_block_slowlag(float * __restrict dataL,float * __restrict dataR)330 void BiquadFilter::process_block_slowlag(float *__restrict dataL, float *__restrict dataR)
331 {
332 /*if(storage->SSE2) process_block_slowlag_SSE2(dataL,dataR);
333 else*/
334 {
335 a1.process();
336 a2.process();
337 b0.process();
338 b1.process();
339 b2.process();
340
341 int k;
342 for (k = 0; k < BLOCK_SIZE; k++)
343 {
344 double input = dataL[k];
345 double op;
346
347 op = input * b0.v.d[0] + reg0.d[0];
348 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
349 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
350
351 dataL[k] = op;
352
353 input = dataR[k];
354 op = input * b0.v.d[0] + reg0.d[1];
355 reg0.d[1] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[1];
356 reg1.d[1] = input * b2.v.d[0] - a2.v.d[0] * op;
357
358 dataR[k] = op;
359 }
360 flush_denormal(reg0.d[0]);
361 flush_denormal(reg1.d[0]);
362 flush_denormal(reg0.d[1]);
363 flush_denormal(reg1.d[1]);
364 }
365 }
366
process_block(float * dataL,float * dataR)367 void BiquadFilter::process_block(float *dataL, float *dataR)
368 {
369 /*if(storage->SSE2) process_block_SSE2(dataL,dataR);
370 else*/
371 {
372 int k;
373 for (k = 0; k < BLOCK_SIZE; k++)
374 {
375 a1.process();
376 a2.process();
377 b0.process();
378 b1.process();
379 b2.process();
380
381 double input = dataL[k];
382 double op;
383
384 op = input * b0.v.d[0] + reg0.d[0];
385 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
386 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
387
388 dataL[k] = op;
389
390 input = dataR[k];
391 op = input * b0.v.d[0] + reg0.d[1];
392 reg0.d[1] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[1];
393 reg1.d[1] = input * b2.v.d[0] - a2.v.d[0] * op;
394
395 dataR[k] = op;
396 }
397 flush_denormal(reg0.d[0]);
398 flush_denormal(reg1.d[0]);
399 flush_denormal(reg0.d[1]);
400 flush_denormal(reg1.d[1]);
401 }
402 }
403
process_block_to(float * dataL,float * dataR,float * dstL,float * dstR)404 void BiquadFilter::process_block_to(float *dataL, float *dataR, float *dstL, float *dstR)
405 {
406 /*if(storage->SSE2) process_block_to_SSE2(dataL,dataR,dstL,dstR);
407 else*/
408 {
409 int k;
410 for (k = 0; k < BLOCK_SIZE; k++)
411 {
412 a1.process();
413 a2.process();
414 b0.process();
415 b1.process();
416 b2.process();
417
418 double input = dataL[k];
419 double op;
420
421 op = input * b0.v.d[0] + reg0.d[0];
422 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
423 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
424
425 dstL[k] = op;
426
427 input = dataR[k];
428 op = input * b0.v.d[0] + reg0.d[1];
429 reg0.d[1] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[1];
430 reg1.d[1] = input * b2.v.d[0] - a2.v.d[0] * op;
431
432 dstR[k] = op;
433 }
434 flush_denormal(reg0.d[0]);
435 flush_denormal(reg1.d[0]);
436 flush_denormal(reg0.d[1]);
437 flush_denormal(reg1.d[1]);
438 }
439 }
440
process_block(double * data)441 void BiquadFilter::process_block(double *data)
442 {
443 /*if(storage->SSE2) process_block_SSE2(data);
444 else*/
445 {
446 int k;
447 for (k = 0; k < BLOCK_SIZE; k++)
448 {
449 a1.process();
450 a2.process();
451 b0.process();
452 b1.process();
453 b2.process();
454
455 double input = data[k];
456 double op;
457
458 op = input * b0.v.d[0] + reg0.d[0];
459 reg0.d[0] = input * b1.v.d[0] - a1.v.d[0] * op + reg1.d[0];
460 reg1.d[0] = input * b2.v.d[0] - a2.v.d[0] * op;
461
462 data[k] = op;
463 }
464 flush_denormal(reg0.d[0]);
465 flush_denormal(reg1.d[0]);
466 }
467 }
468
setBlockSize(int bs)469 void BiquadFilter::setBlockSize(int bs)
470 {
471 /* a1.setBlockSize(bs);
472 a2.setBlockSize(bs);
473 b0.setBlockSize(bs);
474 b1.setBlockSize(bs);
475 b2.setBlockSize(bs);*/
476 }
477
478 using namespace std;
479
plot_magnitude(float f)480 float BiquadFilter::plot_magnitude(float f)
481 {
482 complex<double> ca0(1, 0), ca1(a1.v.d[0], 0), ca2(a2.v.d[0], 0), cb0(b0.v.d[0], 0),
483 cb1(b1.v.d[0], 0), cb2(b2.v.d[0], 0);
484
485 complex<double> i(0, 1);
486 complex<double> z = exp(-2 * 3.1415 * f * i);
487
488 complex<double> h = (cb0 + cb1 * z + cb2 * z * z) / (ca0 + ca1 * z + ca2 * z * z);
489
490 double r = abs(h);
491 return r;
492 }
493