1 /** 2 Basic IIR 1-pole and 2-pole filters through biquads. 3 4 Copyright: Guillaume Piolat 2015-2017. 5 License: $(LINK2 http://www.boost.org/LICENSE_1_0.txt, Boost License 1.0) 6 */ 7 module dplug.dsp.iir; 8 9 import std.math: SQRT1_2, PI, pow, sin, cos, sqrt; 10 import std.complex: Complex, 11 complexAbs = abs, 12 complexExp = exp, 13 complexsqAbs = sqAbs, 14 complexFromPolar = fromPolar; 15 import dplug.core.math; 16 import inteli.emmintrin; 17 18 // DMD with a 32-bit target uses the FPU 19 version(X86) 20 { 21 version(DigitalMars) 22 { 23 version = killDenormals; 24 } 25 } 26 27 // TODO: function to make biquads from z-plane poles and zeroes 28 29 public 30 { 31 32 /// Type which hold the biquad coefficients. 33 /// Important: Coefficients are considered always normalized by a0. 34 /// Note: coeff[0] is b0, 35 /// coeff[1] is b1, 36 /// coeff[2] is b2, 37 /// coeff[3] is a1, 38 /// coeff[4] is a2 in the litterature. 39 alias BiquadCoeff = double[5]; 40 41 /// Maintain state for a biquad state. 42 /// A biquad is a realization that can model two poles and two zeros. 43 struct BiquadDelay 44 { 45 enum order = 2; 46 47 double _x0; 48 double _x1; 49 double _y0; 50 double _y1; 51 52 void initialize() nothrow @nogc 53 { 54 _x0 = 0; 55 _x1 = 0; 56 _y0 = 0; 57 _y1 = 0; 58 } 59 60 static if (order == 2) 61 { 62 /// Process a single sample through the biquad filter. 63 /// This is rather inefficient, in general you'll prefer to use the `nextBuffer` 64 /// functions. 65 float nextSample(float input, const(BiquadCoeff) coeff) nothrow @nogc 66 { 67 double x1 = _x0, 68 x2 = _x1, 69 y1 = _y0, 70 y2 = _y1; 71 72 double a0 = coeff[0], 73 a1 = coeff[1], 74 a2 = coeff[2], 75 a3 = coeff[3], 76 a4 = coeff[4]; 77 78 double current = a0 * input + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2; 79 80 // kill denormals,and double values that would be converted 81 // to float denormals 82 version(killDenormals) 83 { 84 current += 1e-18f; 85 current -= 1e-18f; 86 } 87 88 _x0 = input; 89 _x1 = x1; 90 _y0 = current; 91 _y1 = y1; 92 return current; 93 } 94 ///ditto 95 double nextSample(double input, const(BiquadCoeff) coeff) nothrow @nogc 96 { 97 double x1 = _x0, 98 x2 = _x1, 99 y1 = _y0, 100 y2 = _y1; 101 102 double a0 = coeff[0], 103 a1 = coeff[1], 104 a2 = coeff[2], 105 a3 = coeff[3], 106 a4 = coeff[4]; 107 108 double current = a0 * input + a1 * x1 + a2 * x2 - a3 * y1 - a4 * y2; 109 110 // kill denormals, and double values that would be converted 111 // to float denormals 112 version(killDenormals) 113 { 114 current += 1e-18f; 115 current -= 1e-18f; 116 } 117 118 _x0 = input; 119 _x1 = x1; 120 _y0 = current; 121 _y1 = y1; 122 return current; 123 } 124 125 void nextBuffer(const(float)* input, float* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc 126 { 127 static if (D_InlineAsm_Any) 128 { 129 double x0 = _x0, 130 x1 = _x1, 131 y0 = _y0, 132 y1 = _y1; 133 134 double a0 = coeff[0], 135 a1 = coeff[1], 136 a2 = coeff[2], 137 a3 = coeff[3], 138 a4 = coeff[4]; 139 140 version(LDC) 141 { 142 import inteli.emmintrin; 143 144 __m128d XMM0 = _mm_set_pd(x1, x0); 145 __m128d XMM1 = _mm_set_pd(y1, y0); 146 __m128d XMM2 = _mm_set_pd(a2, a1); 147 __m128d XMM3 = _mm_set_pd(a4, a3); 148 __m128d XMM4 = _mm_set_sd(a0); 149 150 __m128d XMM6 = _mm_undefined_pd(); 151 __m128d XMM7 = _mm_undefined_pd(); 152 __m128d XMM5 = _mm_undefined_pd(); 153 for (int n = 0; n < frames; ++n) 154 { 155 __m128 INPUT = _mm_load_ss(input + n); 156 XMM5 = _mm_setzero_pd(); 157 XMM5 = _mm_cvtss_sd(XMM5,INPUT); 158 159 XMM6 = XMM0; 160 XMM7 = XMM1; 161 XMM5 = _mm_mul_pd(XMM5, XMM4); // input[i]*a0 162 XMM6 = _mm_mul_pd(XMM6, XMM2); // x1*a2 x0*a1 163 XMM7 = _mm_mul_pd(XMM7, XMM3); // y1*a4 y0*a3 164 XMM5 = _mm_add_pd(XMM5, XMM6); 165 XMM5 = _mm_sub_pd(XMM5, XMM7); // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 166 XMM6 = XMM5; 167 168 XMM0 = cast(double2) _mm_slli_si128!8(cast(__m128i) XMM0); 169 XMM6 = cast(double2) _mm_srli_si128!8(cast(__m128i) XMM6); 170 171 XMM0 = _mm_cvtss_sd(XMM0, INPUT); 172 XMM5 = _mm_add_pd(XMM5, XMM6); 173 XMM7 = cast(double2) _mm_cvtsd_ss(_mm_undefined_ps(), XMM5); 174 XMM5 = _mm_unpacklo_pd(XMM5, XMM1); 175 XMM1 = XMM5; 176 _mm_store_ss(output + n, cast(__m128) XMM7); 177 } 178 _mm_storel_pd(&x0, XMM0); 179 _mm_storeh_pd(&x1, XMM0); 180 _mm_storel_pd(&y0, XMM1); 181 _mm_storeh_pd(&y1, XMM1); 182 } 183 else version(D_InlineAsm_X86) 184 { 185 asm nothrow @nogc 186 { 187 mov EAX, input; 188 mov EDX, output; 189 mov ECX, frames; 190 191 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0 192 movhpd XMM0, qword ptr x1; 193 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0 194 movhpd XMM1, qword ptr y1; 195 196 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1 197 movhpd XMM2, qword ptr a2; 198 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3 199 movhpd XMM3, qword ptr a4; 200 201 movq XMM4, qword ptr a0; // XMM4 = 0 a0 202 203 align 16; 204 loop: 205 pxor XMM5, XMM5; 206 cvtss2sd XMM5, dword ptr [EAX]; 207 208 movapd XMM6, XMM0; 209 movapd XMM7, XMM1; 210 211 mulpd XMM5, XMM4; // input[i]*a0 212 mulpd XMM6, XMM2; // x1*a2 x0*a1 213 mulpd XMM7, XMM3; // y1*a4 y0*a3 214 215 addpd XMM5, XMM6; 216 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 217 218 movapd XMM6, XMM5; 219 pslldq XMM0, 8; 220 psrldq XMM6, 8; 221 222 cvtss2sd XMM0, dword ptr [EAX]; // XMM0 <- x0 input[i] 223 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4 224 225 cvtsd2ss XMM7, XMM5; 226 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current 227 add EAX, 4; 228 movd dword ptr [EDX], XMM7; 229 add EDX, 4; 230 movapd XMM1, XMM5; 231 232 sub ECX, 1; 233 jnz short loop; 234 235 movlpd qword ptr x0, XMM0; 236 movhpd qword ptr x1, XMM0; 237 movlpd qword ptr y0, XMM1; 238 movhpd qword ptr y1, XMM1; 239 } 240 } 241 else version(D_InlineAsm_X86_64) 242 { 243 ubyte[16] storage0; 244 ubyte[16] storage1; 245 asm nothrow @nogc 246 { 247 movups storage0, XMM6; 248 movups storage1, XMM7; 249 250 mov RAX, input; 251 mov RDX, output; 252 mov ECX, frames; 253 254 movlpd XMM0, qword ptr x0; // XMM0 = x1 x0 255 movhpd XMM0, qword ptr x1; 256 movlpd XMM1, qword ptr y0; // XMM1 = y1 y0 257 movhpd XMM1, qword ptr y1; 258 259 movlpd XMM2, qword ptr a1; // XMM2 = a2 a1 260 movhpd XMM2, qword ptr a2; 261 movlpd XMM3, qword ptr a3; // XMM3 = a4 a3 262 movhpd XMM3, qword ptr a4; 263 264 movq XMM4, qword ptr a0; // XMM4 = 0 a0 265 266 align 16; 267 loop: 268 pxor XMM5, XMM5; 269 cvtss2sd XMM5, dword ptr [RAX]; 270 271 movapd XMM6, XMM0; 272 movapd XMM7, XMM1; 273 274 mulpd XMM5, XMM4; // input[i]*a0 275 mulpd XMM6, XMM2; // x1*a2 x0*a1 276 mulpd XMM7, XMM3; // y1*a4 y0*a3 277 278 addpd XMM5, XMM6; 279 subpd XMM5, XMM7; // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 280 281 movapd XMM6, XMM5; 282 pslldq XMM0, 8; 283 psrldq XMM6, 8; 284 285 addpd XMM5, XMM6; // garbage | input[i]*a0 + x0*a1 - y0*a3 + x1*a2 - y1*a4 286 cvtss2sd XMM0, dword ptr [RAX]; // XMM0 <- x0 input[i] 287 cvtsd2ss XMM7, XMM5; 288 punpcklqdq XMM5, XMM1; // XMM5 <- y0 current 289 add RAX, 4; 290 movd dword ptr [RDX], XMM7; 291 add RDX, 4; 292 movapd XMM1, XMM5; 293 294 sub ECX, 1; 295 jnz short loop; 296 297 movlpd qword ptr x0, XMM0; 298 movhpd qword ptr x1, XMM0; 299 movlpd qword ptr y0, XMM1; 300 movhpd qword ptr y1, XMM1; 301 302 movups XMM6, storage0; // XMMx with x >= 6 registers need to be preserved 303 movups XMM7, storage1; 304 } 305 } 306 else 307 static assert(false, "Not implemented for this platform."); 308 309 // Kill small signals that can cause denormals (no precision loss was measurable) 310 version(killDenormals) 311 { 312 x0 += 1e-10; 313 x0 -= 1e-10; 314 x1 += 1e-10; 315 x1 -= 1e-10; 316 y0 += 1e-10; 317 y0 -= 1e-10; 318 y1 += 1e-10; 319 y1 -= 1e-10; 320 } 321 322 _x0 = x0; 323 _x1 = x1; 324 _y0 = y0; 325 _y1 = y1; 326 } 327 else 328 { 329 double x0 = _x0, 330 x1 = _x1, 331 y0 = _y0, 332 y1 = _y1; 333 334 double a0 = coeff[0], 335 a1 = coeff[1], 336 a2 = coeff[2], 337 a3 = coeff[3], 338 a4 = coeff[4]; 339 340 for(int i = 0; i < frames; ++i) 341 { 342 double current = a0 * input[i] + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1; 343 344 // kill denormals,and double values that would be converted 345 // to float denormals 346 version(killDenormals) 347 { 348 current += 1e-18f; 349 current -= 1e-18f; 350 } 351 352 x1 = x0; 353 x0 = input[i]; 354 y1 = y0; 355 y0 = current; 356 output[i] = current; 357 } 358 359 _x0 = x0; 360 _x1 = x1; 361 _y0 = y0; 362 _y1 = y1; 363 } 364 } 365 ///ditto 366 void nextBuffer(const(double)* input, double* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc 367 { 368 double x0 = _x0, 369 x1 = _x1, 370 y0 = _y0, 371 y1 = _y1; 372 373 double a0 = coeff[0], 374 a1 = coeff[1], 375 a2 = coeff[2], 376 a3 = coeff[3], 377 a4 = coeff[4]; 378 379 __m128d XMM0 = _mm_set_pd(x1, x0); 380 __m128d XMM1 = _mm_set_pd(y1, y0); 381 __m128d XMM2 = _mm_set_pd(a2, a1); 382 __m128d XMM3 = _mm_set_pd(a4, a3); 383 __m128d XMM4 = _mm_set_sd(a0); 384 385 __m128d XMM6 = _mm_undefined_pd(); 386 __m128d XMM7 = _mm_undefined_pd(); 387 __m128d XMM5 = _mm_undefined_pd(); 388 for (int n = 0; n < frames; ++n) 389 { 390 __m128d INPUT = _mm_load_sd(input + n); 391 XMM5 = INPUT; 392 393 XMM6 = XMM0; 394 XMM7 = XMM1; 395 XMM5 = _mm_mul_pd(XMM5, XMM4); // input[i]*a0 396 XMM6 = _mm_mul_pd(XMM6, XMM2); // x1*a2 x0*a1 397 XMM7 = _mm_mul_pd(XMM7, XMM3); // y1*a4 y0*a3 398 XMM5 = _mm_add_pd(XMM5, XMM6); 399 XMM5 = _mm_sub_pd(XMM5, XMM7); // x1*a2 - y1*a4 | input[i]*a0 + x0*a1 - y0*a3 400 XMM6 = XMM5; 401 402 XMM0 = cast(double2) _mm_slli_si128!8(cast(__m128i) XMM0); 403 XMM6 = cast(double2) _mm_srli_si128!8(cast(__m128i) XMM6); 404 XMM0.ptr[0] = INPUT.array[0]; 405 XMM5 = _mm_add_pd(XMM5, XMM6); 406 XMM7 = XMM5; 407 XMM5 = _mm_unpacklo_pd(XMM5, XMM1); 408 XMM1 = XMM5; 409 _mm_store_sd(output + n, XMM7); 410 } 411 _mm_storel_pd(&x0, XMM0); 412 _mm_storeh_pd(&x1, XMM0); 413 _mm_storel_pd(&y0, XMM1); 414 _mm_storeh_pd(&y1, XMM1); 415 } 416 417 /// Special version of biquad processing, for a constant DC input. 418 void nextBuffer(float input, float* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc 419 { 420 // Note: this naive version performs better than an intel-intrinsics one 421 double x0 = _x0, 422 x1 = _x1, 423 y0 = _y0, 424 y1 = _y1; 425 426 double a0 = coeff[0], 427 a1 = coeff[1], 428 a2 = coeff[2], 429 a3 = coeff[3], 430 a4 = coeff[4]; 431 432 for(int i = 0; i < frames; ++i) 433 { 434 double current = a0 * input + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1; 435 436 // kill denormals,and double values that would be converted 437 // to float denormals 438 version(killDenormals) 439 { 440 current += 1e-18f; 441 current -= 1e-18f; 442 } 443 444 x1 = x0; 445 x0 = input; 446 y1 = y0; 447 y0 = current; 448 output[i] = current; 449 } 450 451 _x0 = x0; 452 _x1 = x1; 453 _y0 = y0; 454 _y1 = y1; 455 } 456 ///ditto 457 void nextBuffer(double input, double* output, int frames, const(BiquadCoeff) coeff) nothrow @nogc 458 { 459 double x0 = _x0, 460 x1 = _x1, 461 y0 = _y0, 462 y1 = _y1; 463 464 double a0 = coeff[0], 465 a1 = coeff[1], 466 a2 = coeff[2], 467 a3 = coeff[3], 468 a4 = coeff[4]; 469 470 for(int i = 0; i < frames; ++i) 471 { 472 double current = a0 * input + a1 * x0 + a2 * x1 - a3 * y0 - a4 * y1; 473 474 // kill denormals,and double values that would be converted 475 // to float denormals 476 version(killDenormals) 477 { 478 current += 1e-18f; 479 current -= 1e-18f; 480 } 481 482 x1 = x0; 483 x0 = input; 484 y1 = y0; 485 y0 = current; 486 output[i] = current; 487 } 488 489 _x0 = x0; 490 _x1 = x1; 491 _y0 = y0; 492 _y1 = y1; 493 } 494 } 495 } 496 497 /// 1-pole low-pass filter. 498 /// Note: the cutoff frequency can be >= nyquist, in which case it asymptotically approaches a bypass. 499 /// the cutoff frequency can be below 0 Hz, in which case it is equal to zero. 500 /// This filter is normalized on DC. 501 /// You always have -3 dB at cutoff in the valid range. 502 /// 503 /// See_also: http://www.earlevel.com/main/2012/12/15/a-one-pole-filter/ 504 BiquadCoeff biquadOnePoleLowPass(double frequency, double sampleRate) nothrow @nogc 505 { 506 double fc = frequency / sampleRate; 507 if (fc < 0.0f) 508 fc = 0.0f; 509 double t2 = fast_exp(-2.0 * PI * fc); 510 BiquadCoeff result; 511 result[0] = 1 - t2; 512 result[1] = 0; 513 result[2] = 0; 514 result[3] = -t2; 515 result[4] = 0; 516 return result; 517 } 518 519 /// 1-pole high-pass filter. 520 /// Note: Like the corresponding one-pole lowpass, this is normalized for DC. 521 /// The cutoff frequency can be <= 0 Hz, in which case it is a bypass. 522 /// Going in very high frequency does NOT give zero. 523 /// You always have -3 dB at cutoff in the valid range. 524 /// 525 /// See_also: https://www.dspguide.com/ch19/2.html 526 BiquadCoeff biquadOnePoleHighPass(double frequency, double sampleRate) nothrow @nogc 527 { 528 double fc = frequency / sampleRate; 529 if (fc < 0.0f) 530 fc = 0.0f; 531 double t2 = fast_exp(-2.0 * PI * fc); 532 BiquadCoeff result; 533 result[0] = (1 + t2) * 0.5; 534 result[1] = -(1 + t2) * 0.5; 535 result[2] = 0; 536 result[3] = -t2; 537 result[4] = 0; 538 return result; 539 } 540 541 /// 1-pole low-pass filter, frequency mapping is not precise. 542 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised. 543 BiquadCoeff biquadOnePoleLowPassImprecise(double frequency, double samplerate) nothrow @nogc 544 { 545 double t0 = frequency / samplerate; 546 if (t0 > 0.5f) 547 t0 = 0.5f; 548 549 double t1 = (1 - 2 * t0); 550 double t2 = t1 * t1; 551 552 BiquadCoeff result; 553 result[0] = cast(float)(1 - t2); 554 result[1] = 0; 555 result[2] = 0; 556 result[3] = cast(float)(-t2); 557 result[4] = 0; 558 return result; 559 } 560 561 /// 1-pole high-pass filter, frequency mapping is not precise. 562 /// Not accurate across sample rates, but coefficient computation is cheap. Not advised. 563 BiquadCoeff biquadOnePoleHighPassImprecise(double frequency, double samplerate) nothrow @nogc 564 { 565 double t0 = frequency / samplerate; 566 if (t0 > 0.5f) 567 t0 = 0.5f; 568 569 double t1 = (2 * t0); 570 double t2 = t1 * t1; 571 572 BiquadCoeff result; 573 result[0] = cast(float)(1 - t2); 574 result[1] = 0; 575 result[2] = 0; 576 result[3] = cast(float)(t2); 577 result[4] = 0; 578 return result; 579 } 580 581 // Cookbook formulae for audio EQ biquad filter coefficients 582 // by Robert Bristow-Johnson 583 584 /// Low-pass filter 12 dB/oct as described by Robert Bristow-Johnson. 585 BiquadCoeff biquadRBJLowPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 586 { 587 return generateBiquad(BiquadType.LOW_PASS_FILTER, frequency, samplerate, 0, Q); 588 } 589 590 /// High-pass filter 12 dB/oct as described by Robert Bristow-Johnson. 591 BiquadCoeff biquadRBJHighPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 592 { 593 return generateBiquad(BiquadType.HIGH_PASS_FILTER, frequency, samplerate, 0, Q); 594 } 595 596 /// Band-pass filter as described by Robert Bristow-Johnson. 597 BiquadCoeff biquadRBJBandPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 598 { 599 return generateBiquad(BiquadType.BAND_PASS_FILTER, frequency, samplerate, 0, Q); 600 } 601 602 /// Notch filter as described by Robert Bristow-Johnson. 603 BiquadCoeff biquadRBJNotch(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 604 { 605 return generateBiquad(BiquadType.NOTCH_FILTER, frequency, samplerate, 0, Q); 606 } 607 608 /// Peak filter as described by Robert Bristow-Johnson. 609 BiquadCoeff biquadRBJPeak(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 610 { 611 return generateBiquad(BiquadType.PEAK_FILTER, frequency, samplerate, gain, Q); 612 } 613 614 /// Low-shelf filter as described by Robert Bristow-Johnson. 615 BiquadCoeff biquadRBJLowShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 616 { 617 return generateBiquad(BiquadType.LOW_SHELF, frequency, samplerate, gain, Q); 618 } 619 620 /// High-shelf filter as described by Robert Bristow-Johnson. 621 BiquadCoeff biquadRBJHighShelf(double frequency, double samplerate, double gain, double Q = SQRT1_2) nothrow @nogc 622 { 623 return generateBiquad(BiquadType.HIGH_SHELF, frequency, samplerate, gain, Q); 624 } 625 626 /// 2nd order All-pass filter as described by Robert Bristow-Johnson. 627 /// This is helpful to introduce the exact same phase response as the RBJ low-pass, but doesn't affect magnitude. 628 BiquadCoeff biquadRBJAllPass(double frequency, double samplerate, double Q = SQRT1_2) nothrow @nogc 629 { 630 return generateBiquad(BiquadType.ALL_PASS_FILTER, frequency, samplerate, 0, Q); 631 } 632 633 /// Identity biquad, pass signal unchanged. 634 BiquadCoeff biquadBypass() nothrow @nogc 635 { 636 BiquadCoeff result; 637 result[0] = 1; 638 result[1] = 0; 639 result[2] = 0; 640 result[3] = 0; 641 result[4] = 0; 642 return result; 643 } 644 645 /// Zero biquad, gives zero output. 646 BiquadCoeff biquadZero() nothrow @nogc 647 { 648 BiquadCoeff result; 649 result[0] = 0; 650 result[1] = 0; 651 result[2] = 0; 652 result[3] = 0; 653 result[4] = 0; 654 return result; 655 } 656 657 /// Bessel 2-pole lowpass. 658 BiquadCoeff biquadBesselLowPass(double frequency, double sampleRate) nothrow @nogc 659 { 660 double normalGain = 1; 661 double normalW = 0; 662 Complex!double pole0 = Complex!double(-1.5 , 0.8660); 663 Complex!double pole1 = Complex!double(-1.5 , -0.8660); 664 double fc = frequency / sampleRate; 665 double T = fc * 2 * PI; 666 pole0 = complexExp(pole0 * T); // matched Z transform 667 pole1 = complexExp(pole1 * T); 668 Complex!double zero01 = Complex!double(-1.0, 0.0); 669 BiquadCoeff coeff = biquad2Poles(pole0, zero01, pole1, zero01); 670 double scaleFactor = 1.0 / complexAbs( biquadResponse(coeff, 0 )); 671 return biquadApplyScale(coeff, scaleFactor); 672 } 673 } 674 675 private 676 { 677 enum BiquadType 678 { 679 LOW_PASS_FILTER, 680 HIGH_PASS_FILTER, 681 BAND_PASS_FILTER, 682 NOTCH_FILTER, 683 PEAK_FILTER, 684 LOW_SHELF, 685 HIGH_SHELF, 686 ALL_PASS_FILTER 687 } 688 689 // generates RBJ biquad coefficients 690 BiquadCoeff generateBiquad(BiquadType type, double frequency, double samplerate, double gaindB, double Q) nothrow @nogc 691 { 692 // regardless of the output precision, always compute coefficients in double precision 693 694 double A = pow(10.0, gaindB / 40.0); 695 double w0 = (2.0 * PI) * frequency / samplerate; 696 double sin_w0 = sin(w0); 697 double cos_w0 = cos(w0); 698 699 double alpha = sin_w0 / (2 * Q); 700 701 // = sin(w0)*sinh( ln(2)/2 * BW * w0/sin(w0) ) (case: BW) 702 // = sin(w0)/2 * sqrt( (A + 1/A)*(1/S - 1) + 2 ) (case: S) 703 704 double b0, b1, b2, a0, a1, a2; 705 706 final switch(type) 707 { 708 case BiquadType.LOW_PASS_FILTER: 709 b0 = (1 - cos_w0) / 2; 710 b1 = 1 - cos_w0; 711 b2 = (1 - cos_w0) / 2; 712 a0 = 1 + alpha; 713 a1 = -2 * cos_w0; 714 a2 = 1 - alpha; 715 break; 716 717 case BiquadType.HIGH_PASS_FILTER: 718 b0 = (1 + cos_w0) / 2; 719 b1 = -(1 + cos_w0); 720 b2 = (1 + cos_w0) / 2; 721 a0 = 1 + alpha; 722 a1 = -2 * cos_w0; 723 a2 = 1 - alpha; 724 break; 725 726 case BiquadType.BAND_PASS_FILTER: 727 b0 = alpha; 728 b1 = 0; 729 b2 = -alpha; 730 a0 = 1 + alpha; 731 a1 = -2 * cos_w0; 732 a2 = 1 - alpha; 733 break; 734 735 case BiquadType.NOTCH_FILTER: 736 b0 = 1; 737 b1 = -2*cos_w0; 738 b2 = 1; 739 a0 = 1 + alpha; 740 a1 = -2*cos(w0); 741 a2 = 1 - alpha; 742 break; 743 744 case BiquadType.PEAK_FILTER: 745 b0 = 1 + alpha * A; 746 b1 = -2 * cos_w0; 747 b2 = 1 - alpha * A; 748 a0 = 1 + alpha / A; 749 a1 = -2 * cos_w0; 750 a2 = 1 - alpha / A; 751 break; 752 753 case BiquadType.LOW_SHELF: 754 { 755 double ap1 = A + 1; 756 double am1 = A - 1; 757 double M = 2 * sqrt(A) * alpha; 758 b0 = A * (ap1 - am1 * cos_w0 + M); 759 b1 = 2 * A * (am1 - ap1 * cos_w0); 760 b2 = A * (ap1 - am1 * cos_w0 - M); 761 a0 = ap1 + am1 * cos_w0 + M; 762 a1 = -2 * (am1 + ap1 * cos_w0); 763 a2 = ap1 + am1 * cos_w0 - M; 764 } 765 break; 766 767 case BiquadType.HIGH_SHELF: 768 { 769 double ap1 = A + 1; 770 double am1 = A - 1; 771 double M = 2 * sqrt(A) * alpha; 772 b0 = A * (ap1 + am1 * cos_w0 + M); 773 b1 = -2 * A * (am1 + ap1 * cos_w0); 774 b2 = A * (ap1 + am1 * cos_w0 - M); 775 a0 = ap1 - am1 * cos_w0 + M; 776 a1 = 2 * (am1 - ap1 * cos_w0); 777 a2 = ap1 - am1 * cos_w0 - M; 778 } 779 break; 780 781 case BiquadType.ALL_PASS_FILTER: 782 { 783 b0 = 1 - alpha; 784 b1 = -2 * cos_w0; 785 b2 = 1 + alpha; 786 a0 = 1 + alpha; 787 a1 = -2 * cos_w0; 788 a2 = 1 - alpha; 789 } 790 break; 791 } 792 793 BiquadCoeff result; 794 result[0] = cast(float)(b0 / a0); // FUTURE: this sounds useless and harmful to cast to float??? 795 result[1] = cast(float)(b1 / a0); 796 result[2] = cast(float)(b2 / a0); 797 result[3] = cast(float)(a1 / a0); 798 result[4] = cast(float)(a2 / a0); 799 return result; 800 } 801 } 802 803 804 // TODO: deprecated this assembly, sounds useless vs inteli 805 version(D_InlineAsm_X86) 806 private enum D_InlineAsm_Any = true; 807 else version(D_InlineAsm_X86_64) 808 private enum D_InlineAsm_Any = true; 809 else 810 private enum D_InlineAsm_Any = false; 811 812 813 private: 814 815 816 817 BiquadCoeff biquad2Poles(Complex!double pole1, Complex!double zero1, Complex!double pole2, Complex!double zero2) nothrow @nogc 818 { 819 // Note: either it's a double pole, or two pole on the real axis. 820 // Same for zeroes 821 822 assert(complexAbs(pole1) <= 1); 823 assert(complexAbs(pole2) <= 1); 824 825 double a1; 826 double a2; 827 double epsilon = 0; 828 829 if (pole1.im != 0) 830 { 831 assert(pole1.re == pole2.re); 832 assert(pole1.im == -pole2.im); 833 a1 = -2 * pole1.re; 834 a2 = complexsqAbs(pole1); 835 } 836 else 837 { 838 assert(pole2.im == 0); 839 a1 = -(pole1.re + pole2.re); 840 a2 = pole1.re * pole2.re; 841 } 842 843 const double b0 = 1; 844 double b1; 845 double b2; 846 847 if (zero1.im != 0) 848 { 849 assert(zero2.re == zero2.re); 850 assert(zero2.im == -zero2.im); 851 b1 = -2 * zero1.re; 852 b2 = complexsqAbs(zero1); 853 } 854 else 855 { 856 assert(zero2.im == 0); 857 b1 = -(zero1.re + zero2.re); 858 b2 = zero1.re * zero2.re; 859 } 860 861 return [b0, b1, b2, a1, a2]; 862 } 863 864 BiquadCoeff biquadApplyScale(BiquadCoeff biquad, double scale) nothrow @nogc 865 { 866 biquad[0] *= scale; 867 biquad[1] *= scale; 868 biquad[2] *= scale; 869 return biquad; 870 } 871 872 // Calculate filter response at the given normalized frequency. 873 Complex!double biquadResponse(BiquadCoeff coeff, double normalizedFrequency) nothrow @nogc 874 { 875 static Complex!double addmul(Complex!double c, double v, Complex!double c1) 876 { 877 return Complex!double(c.re + v * c1.re, c.im + v * c1.im); 878 } 879 880 double w = 2 * PI * normalizedFrequency; 881 Complex!double czn1 = complexFromPolar (1., -w); 882 Complex!double czn2 = complexFromPolar (1., -2 * w); 883 Complex!double ch = 1.0; 884 Complex!double cbot = 1.0; 885 886 Complex!double cb = 1.0; 887 Complex!double ct = coeff[0]; // b0 888 ct = addmul (ct, coeff[1], czn1); // b1 889 ct = addmul (ct, coeff[2], czn2); // b2 890 cb = addmul (cb, coeff[3], czn1); // a1 891 cb = addmul (cb, coeff[4], czn2); // a2 892 ch *= ct; 893 cbot *= cb; 894 return ch / cbot; 895 } 896 897 nothrow @nogc unittest 898 { 899 BiquadCoeff c = biquadBesselLowPass(20000, 44100); 900 }