1 // Copyright 2014 Olivier Gillet. 2 // 3 // Author: Olivier Gillet (ol.gillet@gmail.com) 4 // 5 // Permission is hereby granted, free of charge, to any person obtaining a copy 6 // of this software and associated documentation files (the "Software"), to deal 7 // in the Software without restriction, including without limitation the rights 8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 // copies of the Software, and to permit persons to whom the Software is 10 // furnished to do so, subject to the following conditions: 11 // 12 // The above copyright notice and this permission notice shall be included in 13 // all copies or substantial portions of the Software. 14 // 15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 // THE SOFTWARE. 22 // 23 // See http://creativecommons.org/licenses/MIT/ for more information. 24 // 25 // ----------------------------------------------------------------------------- 26 // 27 // Zero-delay-feedback filters (one pole and SVF). 28 // Naive SVF. 29 30 #ifndef STMLIB_DSP_FILTER_H_ 31 #define STMLIB_DSP_FILTER_H_ 32 33 #include "stmlib/stmlib.h" 34 35 #include <cmath> 36 37 namespace stmlib { 38 39 enum FilterMode { 40 FILTER_MODE_LOW_PASS, 41 FILTER_MODE_BAND_PASS, 42 FILTER_MODE_BAND_PASS_NORMALIZED, 43 FILTER_MODE_HIGH_PASS 44 }; 45 46 enum FrequencyApproximation { 47 FREQUENCY_EXACT, 48 FREQUENCY_ACCURATE, 49 FREQUENCY_FAST, 50 FREQUENCY_DIRTY 51 }; 52 53 #define M_PI_F float(M_PI) 54 #define M_PI_POW_2 M_PI * M_PI 55 #define M_PI_POW_3 M_PI_POW_2 * M_PI 56 #define M_PI_POW_5 M_PI_POW_3 * M_PI_POW_2 57 #define M_PI_POW_7 M_PI_POW_5 * M_PI_POW_2 58 #define M_PI_POW_9 M_PI_POW_7 * M_PI_POW_2 59 #define M_PI_POW_11 M_PI_POW_9 * M_PI_POW_2 60 61 class DCBlocker { 62 public: DCBlocker()63 DCBlocker() { } ~DCBlocker()64 ~DCBlocker() { } 65 Init(float pole)66 void Init(float pole) { 67 x_ = 0.0f; 68 y_ = 0.0f; 69 pole_ = pole; 70 } 71 Process(float * in_out,size_t size)72 inline void Process(float* in_out, size_t size) { 73 float x = x_; 74 float y = y_; 75 const float pole = pole_; 76 while (size--) { 77 float old_x = x; 78 x = *in_out; 79 *in_out++ = y = y * pole + x - old_x; 80 } 81 x_ = x; 82 y_ = y; 83 } 84 85 private: 86 float pole_; 87 float x_; 88 float y_; 89 }; 90 91 class OnePole { 92 public: OnePole()93 OnePole() { } ~OnePole()94 ~OnePole() { } 95 Init()96 void Init() { 97 set_f<FREQUENCY_DIRTY>(0.01f); 98 Reset(); 99 } 100 Reset()101 void Reset() { 102 state_ = 0.0f; 103 } 104 105 template<FrequencyApproximation approximation> tan(float f)106 static inline float tan(float f) { 107 if (approximation == FREQUENCY_EXACT) { 108 // Clip coefficient to about 100. 109 f = f < 0.497f ? f : 0.497f; 110 return tanf(M_PI * f); 111 } else if (approximation == FREQUENCY_DIRTY) { 112 // Optimized for frequencies below 8kHz. 113 const float a = 3.736e-01 * M_PI_POW_3; 114 return f * (M_PI_F + a * f * f); 115 } else if (approximation == FREQUENCY_FAST) { 116 // The usual tangent approximation uses 3.1755e-01 and 2.033e-01, but 117 // the coefficients used here are optimized to minimize error for the 118 // 16Hz to 16kHz range, with a sample rate of 48kHz. 119 const float a = 3.260e-01 * M_PI_POW_3; 120 const float b = 1.823e-01 * M_PI_POW_5; 121 float f2 = f * f; 122 return f * (M_PI_F + f2 * (a + b * f2)); 123 } else if (approximation == FREQUENCY_ACCURATE) { 124 // These coefficients don't need to be tweaked for the audio range. 125 const float a = 3.333314036e-01 * M_PI_POW_3; 126 const float b = 1.333923995e-01 * M_PI_POW_5; 127 const float c = 5.33740603e-02 * M_PI_POW_7; 128 const float d = 2.900525e-03 * M_PI_POW_9; 129 const float e = 9.5168091e-03 * M_PI_POW_11; 130 float f2 = f * f; 131 return f * (M_PI_F + f2 * (a + f2 * (b + f2 * (c + f2 * (d + f2 * e))))); 132 } 133 } 134 135 // Set frequency and resonance from true units. Various approximations 136 // are available to avoid the cost of tanf. 137 template<FrequencyApproximation approximation> set_f(float f)138 inline void set_f(float f) { 139 g_ = tan<approximation>(f); 140 gi_ = 1.0f / (1.0f + g_); 141 } 142 143 template<FilterMode mode> Process(float in)144 inline float Process(float in) { 145 float lp; 146 lp = (g_ * in + state_) * gi_; 147 state_ = g_ * (in - lp) + lp; 148 149 if (mode == FILTER_MODE_LOW_PASS) { 150 return lp; 151 } else if (mode == FILTER_MODE_HIGH_PASS) { 152 return in - lp; 153 } else { 154 return 0.0f; 155 } 156 } 157 158 private: 159 float g_; 160 float gi_; 161 float state_; 162 163 DISALLOW_COPY_AND_ASSIGN(OnePole); 164 }; 165 166 167 168 class Svf { 169 public: Svf()170 Svf() { } ~Svf()171 ~Svf() { } 172 Init()173 void Init() { 174 set_f_q<FREQUENCY_DIRTY>(0.01f, 100.0f); 175 Reset(); 176 } 177 Reset()178 void Reset() { 179 state_1_ = state_2_ = 0.0f; 180 } 181 182 // Copy settings from another filter. set(const Svf & f)183 inline void set(const Svf& f) { 184 g_ = f.g(); 185 r_ = f.r(); 186 h_ = f.h(); 187 } 188 189 // Set all parameters from LUT. set_g_r_h(float g,float r,float h)190 inline void set_g_r_h(float g, float r, float h) { 191 g_ = g; 192 r_ = r; 193 h_ = h; 194 } 195 196 // Set frequency and resonance coefficients from LUT, adjust remaining 197 // parameter. set_g_r(float g,float r)198 inline void set_g_r(float g, float r) { 199 g_ = g; 200 r_ = r; 201 h_ = 1.0f / (1.0f + r_ * g_ + g_ * g_); 202 } 203 204 // Set frequency from LUT, resonance in true units, adjust the rest. set_g_q(float g,float resonance)205 inline void set_g_q(float g, float resonance) { 206 g_ = g; 207 r_ = 1.0f / resonance; 208 h_ = 1.0f / (1.0f + r_ * g_ + g_ * g_); 209 } 210 211 // Set frequency and resonance from true units. Various approximations 212 // are available to avoid the cost of tanf. 213 template<FrequencyApproximation approximation> set_f_q(float f,float resonance)214 inline void set_f_q(float f, float resonance) { 215 g_ = OnePole::tan<approximation>(f); 216 r_ = 1.0f / resonance; 217 h_ = 1.0f / (1.0f + r_ * g_ + g_ * g_); 218 } 219 220 template<FilterMode mode> Process(float in)221 inline float Process(float in) { 222 float hp, bp, lp; 223 hp = (in - r_ * state_1_ - g_ * state_1_ - state_2_) * h_; 224 bp = g_ * hp + state_1_; 225 state_1_ = g_ * hp + bp; 226 lp = g_ * bp + state_2_; 227 state_2_ = g_ * bp + lp; 228 229 if (mode == FILTER_MODE_LOW_PASS) { 230 return lp; 231 } else if (mode == FILTER_MODE_BAND_PASS) { 232 return bp; 233 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 234 return bp * r_; 235 } else if (mode == FILTER_MODE_HIGH_PASS) { 236 return hp; 237 } 238 } 239 240 template<FilterMode mode> Process(const float * in,float * out,size_t size)241 inline void Process(const float* in, float* out, size_t size) { 242 float hp, bp, lp; 243 float state_1 = state_1_; 244 float state_2 = state_2_; 245 246 while (size--) { 247 hp = (*in - r_ * state_1 - g_ * state_1 - state_2) * h_; 248 bp = g_ * hp + state_1; 249 state_1 = g_ * hp + bp; 250 lp = g_ * bp + state_2; 251 state_2 = g_ * bp + lp; 252 253 float value; 254 if (mode == FILTER_MODE_LOW_PASS) { 255 value = lp; 256 } else if (mode == FILTER_MODE_BAND_PASS) { 257 value = bp; 258 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 259 value = bp * r_; 260 } else if (mode == FILTER_MODE_HIGH_PASS) { 261 value = hp; 262 } 263 264 *out = value; 265 ++out; 266 ++in; 267 } 268 state_1_ = state_1; 269 state_2_ = state_2; 270 } 271 272 template<FilterMode mode> Process(const float * in,float * out,size_t size,size_t stride)273 inline void Process(const float* in, float* out, size_t size, size_t stride) { 274 float hp, bp, lp; 275 float state_1 = state_1_; 276 float state_2 = state_2_; 277 278 while (size--) { 279 hp = (*in - r_ * state_1 - g_ * state_1 - state_2) * h_; 280 bp = g_ * hp + state_1; 281 state_1 = g_ * hp + bp; 282 lp = g_ * bp + state_2; 283 state_2 = g_ * bp + lp; 284 285 float value; 286 if (mode == FILTER_MODE_LOW_PASS) { 287 value = lp; 288 } else if (mode == FILTER_MODE_BAND_PASS) { 289 value = bp; 290 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 291 value = bp * r_; 292 } else if (mode == FILTER_MODE_HIGH_PASS) { 293 value = hp; 294 } 295 296 *out = value; 297 out += stride; 298 in += stride; 299 } 300 state_1_ = state_1; 301 state_2_ = state_2; 302 } 303 ProcessMultimode(const float * in,float * out,size_t size,float mode)304 inline void ProcessMultimode( 305 const float* in, 306 float* out, 307 size_t size, 308 float mode) { 309 float hp, bp, lp; 310 float state_1 = state_1_; 311 float state_2 = state_2_; 312 313 mode *= mode; 314 315 float hp_gain = mode < 0.5f ? mode * 2.0f : 2.0f - mode * 2.0f; 316 float lp_gain = mode < 0.5f ? 1.0f - mode * 2.0f : 0.0f; 317 float bp_gain = mode < 0.5f ? 0.0f : mode * 2.0f - 1.0f; 318 319 while (size--) { 320 hp = (*in - r_ * state_1 - g_ * state_1 - state_2) * h_; 321 bp = g_ * hp + state_1; 322 state_1 = g_ * hp + bp; 323 lp = g_ * bp + state_2; 324 state_2 = g_ * bp + lp; 325 *out = hp_gain * hp + bp_gain * bp + lp_gain * lp; 326 ++in; 327 ++out; 328 } 329 state_1_ = state_1; 330 state_2_ = state_2; 331 } 332 333 template<FilterMode mode> Process(const float * in,float * out_1,float * out_2,size_t size,float gain_1,float gain_2)334 inline void Process( 335 const float* in, float* out_1, float* out_2, size_t size, 336 float gain_1, float gain_2) { 337 float hp, bp, lp; 338 float state_1 = state_1_; 339 float state_2 = state_2_; 340 341 while (size--) { 342 hp = (*in - r_ * state_1 - g_ * state_1 - state_2) * h_; 343 bp = g_ * hp + state_1; 344 state_1 = g_ * hp + bp; 345 lp = g_ * bp + state_2; 346 state_2 = g_ * bp + lp; 347 348 float value; 349 if (mode == FILTER_MODE_LOW_PASS) { 350 value = lp; 351 } else if (mode == FILTER_MODE_BAND_PASS) { 352 value = bp; 353 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 354 value = bp * r_; 355 } else if (mode == FILTER_MODE_HIGH_PASS) { 356 value = hp; 357 } 358 359 *out_1 += value * gain_1; 360 *out_2 += value * gain_2; 361 ++out_1; 362 ++out_2; 363 ++in; 364 } 365 state_1_ = state_1; 366 state_2_ = state_2; 367 } 368 g()369 inline float g() const { return g_; } r()370 inline float r() const { return r_; } h()371 inline float h() const { return h_; } 372 373 private: 374 float g_; 375 float r_; 376 float h_; 377 378 float state_1_; 379 float state_2_; 380 381 DISALLOW_COPY_AND_ASSIGN(Svf); 382 }; 383 384 385 386 // Naive Chamberlin SVF. 387 class NaiveSvf { 388 public: NaiveSvf()389 NaiveSvf() { } ~NaiveSvf()390 ~NaiveSvf() { } 391 Init()392 void Init() { 393 set_f_q<FREQUENCY_DIRTY>(0.01f, 100.0f); 394 Reset(); 395 } 396 Reset()397 void Reset() { 398 lp_ = bp_ = 0.0f; 399 } 400 401 // Set frequency and resonance from true units. Various approximations 402 // are available to avoid the cost of sinf. 403 template<FrequencyApproximation approximation> set_f_q(float f,float resonance)404 inline void set_f_q(float f, float resonance) { 405 f = f < 0.497f ? f : 0.497f; 406 if (approximation == FREQUENCY_EXACT) { 407 f_ = 2.0f * sinf(M_PI_F * f); 408 } else { 409 f_ = 2.0f * M_PI_F * f; 410 } 411 damp_ = 1.0f / resonance; 412 } 413 414 template<FilterMode mode> Process(float in)415 inline float Process(float in) { 416 float hp, notch, bp_normalized; 417 bp_normalized = bp_ * damp_; 418 notch = in - bp_normalized; 419 lp_ += f_ * bp_; 420 hp = notch - lp_; 421 bp_ += f_ * hp; 422 423 if (mode == FILTER_MODE_LOW_PASS) { 424 return lp_; 425 } else if (mode == FILTER_MODE_BAND_PASS) { 426 return bp_; 427 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 428 return bp_normalized; 429 } else if (mode == FILTER_MODE_HIGH_PASS) { 430 return hp; 431 } 432 } 433 lp()434 inline float lp() const { return lp_; } bp()435 inline float bp() const { return bp_; } 436 437 template<FilterMode mode> Process(const float * in,float * out,size_t size)438 inline void Process(const float* in, float* out, size_t size) { 439 float hp, notch, bp_normalized; 440 float lp = lp_; 441 float bp = bp_; 442 while (size--) { 443 bp_normalized = bp * damp_; 444 notch = *in++ - bp_normalized; 445 lp += f_ * bp; 446 hp = notch - lp; 447 bp += f_ * hp; 448 449 if (mode == FILTER_MODE_LOW_PASS) { 450 *out++ = lp; 451 } else if (mode == FILTER_MODE_BAND_PASS) { 452 *out++ = bp; 453 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 454 *out++ = bp_normalized; 455 } else if (mode == FILTER_MODE_HIGH_PASS) { 456 *out++ = hp; 457 } 458 } 459 lp_ = lp; 460 bp_ = bp; 461 } 462 Split(const float * in,float * low,float * high,size_t size)463 inline void Split(const float* in, float* low, float* high, size_t size) { 464 float hp, notch, bp_normalized; 465 float lp = lp_; 466 float bp = bp_; 467 while (size--) { 468 bp_normalized = bp * damp_; 469 notch = *in++ - bp_normalized; 470 lp += f_ * bp; 471 hp = notch - lp; 472 bp += f_ * hp; 473 *low++ = lp; 474 *high++ = hp; 475 } 476 lp_ = lp; 477 bp_ = bp; 478 } 479 480 template<FilterMode mode> Process(const float * in,float * out,size_t size,size_t decimate)481 inline void Process(const float* in, float* out, size_t size, size_t decimate) { 482 float hp, notch, bp_normalized; 483 float lp = lp_; 484 float bp = bp_; 485 size_t n = decimate - 1; 486 while (size--) { 487 bp_normalized = bp * damp_; 488 notch = *in++ - bp_normalized; 489 lp += f_ * bp; 490 hp = notch - lp; 491 bp += f_ * hp; 492 493 ++n; 494 if (n == decimate) { 495 if (mode == FILTER_MODE_LOW_PASS) { 496 *out++ = lp; 497 } else if (mode == FILTER_MODE_BAND_PASS) { 498 *out++ = bp; 499 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 500 *out++ = bp_normalized; 501 } else if (mode == FILTER_MODE_HIGH_PASS) { 502 *out++ = hp; 503 } 504 n = 0; 505 } 506 } 507 lp_ = lp; 508 bp_ = bp; 509 } 510 511 private: 512 float f_; 513 float damp_; 514 float lp_; 515 float bp_; 516 517 DISALLOW_COPY_AND_ASSIGN(NaiveSvf); 518 }; 519 520 521 522 // Modified Chamberlin SVF (Duane K. Wise) 523 // http://www.dafx.ca/proceedings/papers/p_053.pdf 524 class ModifiedSvf { 525 public: ModifiedSvf()526 ModifiedSvf() { } ~ModifiedSvf()527 ~ModifiedSvf() { } 528 Init()529 void Init() { 530 Reset(); 531 } 532 Reset()533 void Reset() { 534 lp_ = bp_ = 0.0f; 535 } 536 set_f_fq(float f,float fq)537 inline void set_f_fq(float f, float fq) { 538 f_ = f; 539 fq_ = fq; 540 x_ = 0.0f; 541 } 542 543 template<FilterMode mode> Process(const float * in,float * out,size_t size)544 inline void Process(const float* in, float* out, size_t size) { 545 float lp = lp_; 546 float bp = bp_; 547 float x = x_; 548 const float fq = fq_; 549 const float f = f_; 550 while (size--) { 551 lp += f * bp; 552 bp += -fq * bp -f * lp + *in; 553 if (mode == FILTER_MODE_BAND_PASS || 554 mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 555 bp += x; 556 } 557 x = *in++; 558 559 if (mode == FILTER_MODE_LOW_PASS) { 560 *out++ = lp * f; 561 } else if (mode == FILTER_MODE_BAND_PASS) { 562 *out++ = bp * f; 563 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 564 *out++ = bp * fq; 565 } else if (mode == FILTER_MODE_HIGH_PASS) { 566 *out++ = x - lp * f - bp * fq; 567 } 568 } 569 lp_ = lp; 570 bp_ = bp; 571 x_ = x; 572 } 573 574 private: 575 float f_; 576 float fq_; 577 float x_; 578 float lp_; 579 float bp_; 580 581 DISALLOW_COPY_AND_ASSIGN(ModifiedSvf); 582 }; 583 584 585 586 // Two passes of modified Chamberlin SVF with the same coefficients - 587 // to implement Linkwitz–Riley (Butterworth squared) crossover filters. 588 class CrossoverSvf { 589 public: CrossoverSvf()590 CrossoverSvf() { } ~CrossoverSvf()591 ~CrossoverSvf() { } 592 Init()593 void Init() { 594 Reset(); 595 } 596 Reset()597 void Reset() { 598 lp_[0] = bp_[0] = lp_[1] = bp_[1] = 0.0f; 599 x_[0] = 0.0f; 600 x_[1] = 0.0f; 601 } 602 set_f_fq(float f,float fq)603 inline void set_f_fq(float f, float fq) { 604 f_ = f; 605 fq_ = fq; 606 } 607 608 template<FilterMode mode> Process(const float * in,float * out,size_t size)609 inline void Process(const float* in, float* out, size_t size) { 610 float lp_1 = lp_[0]; 611 float bp_1 = bp_[0]; 612 float lp_2 = lp_[1]; 613 float bp_2 = bp_[1]; 614 float x_1 = x_[0]; 615 float x_2 = x_[1]; 616 const float fq = fq_; 617 const float f = f_; 618 while (size--) { 619 lp_1 += f * bp_1; 620 bp_1 += -fq * bp_1 -f * lp_1 + *in; 621 if (mode == FILTER_MODE_BAND_PASS || 622 mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 623 bp_1 += x_1; 624 } 625 x_1 = *in++; 626 627 float y; 628 if (mode == FILTER_MODE_LOW_PASS) { 629 y = lp_1 * f; 630 } else if (mode == FILTER_MODE_BAND_PASS) { 631 y = bp_1 * f; 632 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 633 y = bp_1 * fq; 634 } else if (mode == FILTER_MODE_HIGH_PASS) { 635 y = x_1 - lp_1 * f - bp_1 * fq; 636 } 637 638 lp_2 += f * bp_2; 639 bp_2 += -fq * bp_2 -f * lp_2 + y; 640 if (mode == FILTER_MODE_BAND_PASS || 641 mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 642 bp_2 += x_2; 643 } 644 x_2 = y; 645 646 if (mode == FILTER_MODE_LOW_PASS) { 647 *out++ = lp_2 * f; 648 } else if (mode == FILTER_MODE_BAND_PASS) { 649 *out++ = bp_2 * f; 650 } else if (mode == FILTER_MODE_BAND_PASS_NORMALIZED) { 651 *out++ = bp_2 * fq; 652 } else if (mode == FILTER_MODE_HIGH_PASS) { 653 *out++ = x_2 - lp_2 * f - bp_2 * fq; 654 } 655 } 656 lp_[0] = lp_1; 657 bp_[0] = bp_1; 658 lp_[1] = lp_2; 659 bp_[1] = bp_2; 660 x_[0] = x_1; 661 x_[1] = x_2; 662 } 663 664 private: 665 float f_; 666 float fq_; 667 float x_[2]; 668 float lp_[2]; 669 float bp_[2]; 670 671 DISALLOW_COPY_AND_ASSIGN(CrossoverSvf); 672 }; 673 674 } // namespace stmlib 675 676 #endif // STMLIB_DSP_FILTER_H_ 677