multimode.cpp (13664B)
1 2 #include <assert.h> 3 #include <math.h> 4 #include <algorithm> 5 6 #include "filters/multimode.hpp" 7 8 // using namespace bogaudio::dsp; 9 namespace bogaudio { // gcc < 7 wants the namespaces this way with explicit template instantiations. 10 namespace dsp { 11 12 #ifdef RACK_SIMD 13 14 void Biquad4::setParams(int i, float a0, float a1, float a2, float b0, float b1, float b2) { 15 assert(i >= 0 && i < 4); 16 float ib0 = 1.0 / b0; 17 _a0[i] = a0 * ib0; 18 _a1[i] = a1 * ib0; 19 _a2[i] = a2 * ib0; 20 _b1[i] = b1 * ib0; 21 _b2[i] = b2 * ib0; 22 } 23 24 void Biquad4::reset() { 25 _x[0] = _x[1] = _x[2] = 0.0; 26 _y[0] = _y[1] = _y[2] = 0.0; 27 } 28 29 void Biquad4::setN(int n, bool minDelay) { 30 assert(n > 0 && n <= 4); 31 for (int i = n; i < 4; ++i) { 32 setParams(i, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); 33 } 34 _outputIndex = minDelay ? n - 1 : 3; 35 } 36 37 float Biquad4::next(float sample) { 38 if (_disable) { 39 return sample; 40 } 41 42 _x[2] = _x[1]; 43 _x[1] = _x[0]; 44 45 // slower: _x[0] = _mm_shuffle_ps(_y[0].v, _y[0].v, _MM_SHUFFLE(2, 1, 0, 0)); _x[0][0] = sample; 46 _x[0] = float_4(sample, _y[0][0], _y[0][1], _y[0][2]); 47 48 _y[2] = _y[1]; 49 _y[1] = _y[0]; 50 _y[0] = ((_a0 * _x[0]) + (_a1 * _x[1]) + (_a2 * _x[2])) - ((_b1 * _y[1]) + (_b2 * _y[2])); 51 return _y[0][_outputIndex]; 52 } 53 54 template<> void BiquadBank<MultimodeTypes::T, 4>::setParams(int i, MultimodeTypes::T a0, MultimodeTypes::T a1, MultimodeTypes::T a2, MultimodeTypes::T b0, MultimodeTypes::T b1, MultimodeTypes::T b2) { 55 assert(i >= 0 && i < 4); 56 _biquads->setParams(i, a0, a1, a2, b0, b1, b2); 57 } 58 59 template<> void BiquadBank<MultimodeTypes::T, 4>::reset() { 60 _biquads->reset(); 61 } 62 63 template<> void BiquadBank<MultimodeTypes::T, 4>::setN(int n, bool minDelay) { 64 _biquads->setN(n, minDelay); 65 } 66 67 template<> float BiquadBank<MultimodeTypes::T, 4>::next(float sample) { 68 return _biquads->next(sample); 69 } 70 71 72 template<> void BiquadBank<MultimodeTypes::T, 8>::setParams(int i, MultimodeTypes::T a0, MultimodeTypes::T a1, MultimodeTypes::T a2, MultimodeTypes::T b0, MultimodeTypes::T b1, MultimodeTypes::T b2) { 73 assert(i >= 0 && i < 8); 74 _biquads[i / 4].setParams(i % 4, a0, a1, a2, b0, b1, b2); 75 } 76 77 template<> void BiquadBank<MultimodeTypes::T, 8>::reset() { 78 _biquads[0].reset(); 79 _biquads[1].reset(); 80 } 81 82 template<> void BiquadBank<MultimodeTypes::T, 8>::setN(int n, bool minDelay) { 83 assert(n > 0 && n <= 8); 84 for (int i = 0, nn = n / 4; i < nn; ++i) { 85 _biquads[i].setN(4, false); 86 } 87 if (n % 4 != 0) { 88 _biquads[n / 4].setN(n % 4, minDelay); 89 } 90 for (int i = 0; i < 2; ++i) { 91 _biquads[i].disable(4 * i >= n); 92 } 93 } 94 95 template<> float BiquadBank<MultimodeTypes::T, 8>::next(float sample) { 96 sample = _biquads[0].next(sample); 97 return _biquads[1].next(sample); 98 } 99 100 101 template<> void BiquadBank<MultimodeTypes::T, 16>::setParams(int i, MultimodeTypes::T a0, MultimodeTypes::T a1, MultimodeTypes::T a2, MultimodeTypes::T b0, MultimodeTypes::T b1, MultimodeTypes::T b2) { 102 assert(i >= 0 && i < 16); 103 _biquads[i / 4].setParams(i % 4, a0, a1, a2, b0, b1, b2); 104 } 105 106 template<> void BiquadBank<MultimodeTypes::T, 16>::reset() { 107 _biquads[0].reset(); 108 _biquads[1].reset(); 109 _biquads[2].reset(); 110 _biquads[3].reset(); 111 } 112 113 template<> void BiquadBank<MultimodeTypes::T, 16>::setN(int n, bool minDelay) { 114 assert(n > 0 && n <= 16); 115 for (int i = 0, nn = n / 4; i < nn; ++i) { 116 _biquads[i].setN(4, false); 117 } 118 if (n % 4 != 0) { 119 _biquads[n / 4].setN(n % 4, minDelay); 120 } 121 for (int i = 0; i < 4; ++i) { 122 _biquads[i].disable(4 * i >= n); 123 } 124 } 125 126 template<> float BiquadBank<MultimodeTypes::T, 16>::next(float sample) { 127 sample = _biquads[0].next(sample); 128 sample = _biquads[1].next(sample); 129 sample = _biquads[2].next(sample); 130 return _biquads[3].next(sample); 131 } 132 133 #else 134 135 template<typename T, int N> void BiquadBank<T, N>::setParams(int i, T a0, T a1, T a2, T b0, T b1, T b2) { 136 assert(i >= 0 && i < N); 137 _biquads[i].setParams(a0, a1, a2, b0, b1, b2); 138 } 139 140 template<typename T, int N> void BiquadBank<T, N>::reset() { 141 for (int i = 0; i < N; ++i) { 142 _biquads[i].reset(); 143 } 144 } 145 146 template<typename T, int N> void BiquadBank<T, N>::setN(int n, bool _minDelay) { 147 assert(n <= N); 148 _n = n; 149 for (; n < N; ++n) { 150 _biquads[n].reset(); 151 } 152 } 153 154 template<typename T, int N> float BiquadBank<T, N>::next(float sample) { 155 for (int i = 0; i < _n; ++i) { 156 sample = _biquads[i].next(sample); 157 } 158 return sample; 159 } 160 161 #endif 162 163 template struct BiquadBank<MultimodeTypes::T, 4>; 164 template struct BiquadBank<MultimodeTypes::T, 8>; 165 template struct BiquadBank<MultimodeTypes::T, 16>; 166 167 168 constexpr int MultimodeTypes::minPoles; 169 constexpr int MultimodeTypes::maxPoles; 170 constexpr int MultimodeTypes::modPoles; 171 constexpr float MultimodeTypes::minFrequency; 172 constexpr float MultimodeTypes::maxFrequency; 173 constexpr float MultimodeTypes::minQbw; 174 constexpr float MultimodeTypes::maxQbw; 175 constexpr float MultimodeTypes::minBWLinear; 176 constexpr float MultimodeTypes::maxBWLinear; 177 constexpr float MultimodeTypes::minBWPitch; 178 constexpr float MultimodeTypes::maxBWPitch; 179 180 181 template<int N> float MultimodeDesigner<N>::effectiveMinimumFrequency() { 182 return minFrequency * std::max(1.0f, roundf(_sampleRate / 44100.0f)); 183 } 184 185 template<int N> void MultimodeDesigner<N>::setParams( 186 BiquadBank<T, N>& biquads, 187 float& outGain, 188 float sampleRate, 189 Type type, 190 int poles, 191 Mode mode, 192 float frequency, 193 float qbw, 194 BandwidthMode bwm, 195 DelayMode dm 196 ) { 197 assert(N >= minPoles && N <= maxPoles); 198 assert(poles >= minPoles && (poles <= N || (poles <= 2*N && (mode == LOWPASS_MODE || mode == HIGHPASS_MODE)))); 199 assert(poles % modPoles == 0); 200 assert(frequency >= minFrequency - 0.00001f && frequency <= maxFrequency); 201 frequency = std::max(frequency, effectiveMinimumFrequency()); 202 frequency = std::min(frequency, 0.49f * sampleRate); 203 assert(qbw >= minQbw && qbw <= maxQbw); 204 205 bool repole = _type != type || _mode != mode || _nPoles != poles || (type == CHEBYSHEV_TYPE && (mode == LOWPASS_MODE || mode == HIGHPASS_MODE) && _qbw != qbw); 206 bool redesign = repole || _frequency != frequency || _qbw != qbw || _sampleRate != sampleRate || _bandwidthMode != bwm || _delayMode != dm; 207 _sampleRate = sampleRate; 208 _half2PiST = M_PI * (1.0f / sampleRate); 209 _type = type; 210 _nPoles = poles; 211 _mode = mode; 212 _frequency = frequency; 213 _qbw = qbw; 214 _bandwidthMode = bwm; 215 _delayMode = dm; 216 217 if (repole) { 218 switch (_type) { 219 case BUTTERWORTH_TYPE: { 220 int np = _nPoles / 2 + (_nPoles % 2 == 1); 221 for (int k = 1, j = np - 1; k <= np; ++k, --j) { 222 T a = (T)(2 * k + _nPoles - 1) * M_PI / (T)(2 * _nPoles); 223 T re = std::cos(a); 224 T im = std::sin(a); 225 _poles[j] = Pole(-re, im, re + re, re * re + im * im); 226 } 227 228 outGain = 1.0f; 229 break; 230 } 231 232 case CHEBYSHEV_TYPE: { 233 T ripple = 3.0; 234 if (mode == LOWPASS_MODE || mode == HIGHPASS_MODE) { 235 ripple += std::max(0.0f, 6.0f * qbw); 236 } 237 T e = ripple / (T)10.0; 238 e = std::pow((T)10.0, e); 239 e -= (T)1.0; 240 e = std::sqrt(e); 241 T ef = std::asinh((T)1.0 / e) / (float)_nPoles; 242 T efr = -std::sinh(ef); 243 T efi = std::cosh(ef); 244 245 int np = _nPoles / 2 + (_nPoles % 2 == 1); 246 for (int k = 1, j = np - 1; k <= np; ++k, --j) { 247 T a = (T)(2 * k - 1) * M_PI / (T)(2 * _nPoles); 248 T re = efr * std::sin(a); 249 T im = efi * std::cos(a); 250 _poles[j] = Pole(-re, im, re + re, re * re + im * im); 251 } 252 253 outGain = 1.0 / (e * std::pow(2.0, (T)(_nPoles - 1))); 254 // outGain = 1.0f / std::pow(2.0f, (T)(_nPoles - 1)); 255 break; 256 } 257 258 default: { 259 assert(false); 260 } 261 } 262 } 263 264 if (redesign) { 265 switch (_mode) { 266 case LOWPASS_MODE: 267 case HIGHPASS_MODE: { 268 _nBiquads = _nPoles / 2 + _nPoles % 2; 269 biquads.setN(_nBiquads, _delayMode == MINIMUM_DELAY_MODE); 270 271 // T iq = (1.0 / std::sqrt(2.0)) - 0.65 * _qbw; 272 T iq = (T)0.8 - (T)0.6 * _qbw; 273 T wa = std::tan(_frequency * _half2PiST); 274 T wa2 = wa * wa; 275 276 if (_mode == LOWPASS_MODE) { 277 int ni = 0; 278 int nb = _nBiquads; 279 if (_nPoles % 2 == 1) { 280 ++ni; 281 --nb; 282 T wap = wa * std::real(_poles[0].p); 283 biquads.setParams(0, wa, wa, 0.0, wap + (T)1.0, wap - (T)1.0, (T)0.0); 284 } 285 T a0 = wa2; 286 T a1 = wa2 + wa2; 287 T a2 = wa2; 288 for (int i = 0; i < nb; ++i) { 289 Pole& pole = _poles[ni + i]; 290 T ywa2 = pole.y * wa2; 291 T ywa21 = ywa2 + (T)1.0; 292 T x = (((T)(i == nb / 2) * (iq - (T)1.0)) + (T)1.0) * pole.x; 293 T xwa = x * wa; 294 T b0 = ywa21 - xwa; 295 T b1 = (T)-2.0 + (ywa2 + ywa2); 296 T b2 = ywa21 + xwa; 297 biquads.setParams(ni + i, a0, a1, a2, b0, b1, b2); 298 } 299 } 300 else { 301 int ni = 0; 302 int nb = _nBiquads; 303 if (_nPoles % 2 == 1) { 304 ++ni; 305 --nb; 306 T rp = std::real(_poles[0].p); 307 biquads.setParams(0, 1.0, -1.0, 0.0, wa + rp, wa - rp, 0.0); 308 } 309 T a0 = 1.0; 310 T a1 = -2.0f; 311 T a2 = 1.0; 312 for (int i = 0; i < nb; ++i) { 313 Pole& pole = _poles[ni + i]; 314 T wa2y = wa2 + pole.y; 315 T x = (((T)(i == nb / 2) * (iq - (T)1.0)) + (T)1.0) * pole.x; 316 T xwa = x * wa; 317 T b0 = wa2y - xwa; 318 T b1 = (wa2 + wa2) - (pole.y + pole.y); 319 T b2 = wa2y + xwa; 320 biquads.setParams(ni + i, a0, a1, a2, b0, b1, b2); 321 } 322 } 323 break; 324 } 325 326 case BANDPASS_MODE: 327 case BANDREJECT_MODE: { 328 _nBiquads = ((_nPoles / 2) * 2) + (_nPoles % 2); 329 biquads.setN(_nBiquads, _delayMode == MINIMUM_DELAY_MODE); 330 331 T wdl = 0.0; 332 T wdh = 0.0; 333 switch (_bandwidthMode) { 334 case LINEAR_BANDWIDTH_MODE: { 335 float bandwidth = std::max(minBWLinear, maxBWLinear * _qbw); 336 wdl = std::max(minFrequency, _frequency - 0.5f * bandwidth); 337 wdh = std::min(maxFrequency, std::max((float)wdl + 10.0f, _frequency + 0.5f * bandwidth)); 338 break; 339 } 340 case PITCH_BANDWIDTH_MODE: { 341 float bandwidth = std::max(minBWPitch, maxBWPitch * _qbw); 342 wdl = std::max(minFrequency, powf(2.0f, -bandwidth) * _frequency); 343 wdh = std::min(maxFrequency, std::max((float)wdl + 10.0f, powf(2.0f, bandwidth) * _frequency)); 344 break; 345 } 346 default: { 347 assert(false); 348 } 349 } 350 T wal = std::tan(wdl * _half2PiST); 351 T wah = std::tan(wdh * _half2PiST); 352 T w = wah - wal; 353 T w2 = w * w; 354 T w02 = wah * wal; 355 356 if (_mode == BANDPASS_MODE) { 357 T a0 = w; 358 T a1 = 0.0; 359 T a2 = -w; 360 361 int ni = 0; 362 int nb = _nBiquads; 363 if (_nPoles % 2 == 1) { 364 ++ni; 365 --nb; 366 T wp = w * std::real(_poles[0].p); 367 biquads.setParams( 368 0, 369 a0, 370 a1, 371 a2, 372 (T)1.0 + wp + w02, 373 (T)-2.0 + (w02 + w02), 374 (T)1.0 - wp + w02 375 ); 376 } 377 for (int i = 0; i < nb; i += 2) { 378 Pole& pole = _poles[ni + i / 2]; 379 TC x = pole.p2; 380 x *= w2; 381 x -= (T)4.0 * w02; 382 x = std::sqrt(x); 383 TC xc = std::conj(x); 384 TC wp = w * pole.p; 385 TC wpc = w * pole.pc; 386 TC y1 = (x - wp) * (T)0.5; 387 TC y1c = (xc - wpc) * (T)0.5; 388 TC y2 = (-x - wp) * (T)0.5; 389 TC y2c = (-xc - wpc) * (T)0.5; 390 TC cf1a = -(y1 + y1c); 391 TC cf2a = y1 * y1c; 392 TC cf1b = -(y2 + y2c); 393 TC cf2b = y2 * y2c; 394 T f1a = std::real(cf1a); 395 T f1b = std::real(cf1b); 396 T f2a = std::real(cf2a); 397 T f2b = std::real(cf2b); 398 399 { 400 T b0 = (T)1.0 + f1a + f2a; 401 T b1 = (T)-2.0 + (f2a + f2a); 402 T b2 = (T)1.0 - f1a + f2a; 403 biquads.setParams(ni + i, a0, a1, a2, b0, b1, b2); 404 } 405 { 406 T b0 = (T)1.0 + f1b + f2b; 407 T b1 = (T)-2.0 + (f2b + f2b); 408 T b2 = (T)1.0 - f1b + f2b; 409 biquads.setParams(ni + i + 1, a0, a1, a2, b0, b1, b2); 410 } 411 } 412 } 413 else { 414 T a0 = (T)1.0 + w02; 415 T a1 = (T)-2.0 + (w02 + w02); 416 T a2 = a0; 417 418 int ni = 0; 419 int nb = _nBiquads; 420 if (_nPoles % 2 == 1) { 421 ++ni; 422 --nb; 423 T rp = std::real(_poles[0].p); 424 T rpw02 = rp * w02; 425 biquads.setParams( 426 0, 427 a0, 428 a1, 429 a2, 430 rp + w + rpw02, 431 (T)-2.0 * rp + (rpw02 + rpw02), 432 rp - w + rpw02 433 ); 434 } 435 for (int i = 0; i < nb; i += 2) { 436 Pole& pole = _poles[ni + i / 2]; 437 TC x = pole.p2; 438 x *= (T)-4.0 * w02; 439 x += w2; 440 x = std::sqrt(x); 441 TC xc = std::conj(x); 442 TC y1 = (x - w) * pole.i2p; 443 TC y1c = (xc - w) * pole.i2pc; 444 TC y2 = (-x - w) * pole.i2p; 445 TC y2c = (-xc - w) * pole.i2pc; 446 TC cf1a = -pole.r * (y1 + y1c); 447 TC cf2a = pole.r * y1 * y1c; 448 TC cf1b = -pole.r * (y2 + y2c); 449 TC cf2b = pole.r * y2 * y2c; 450 T f1a = std::real(cf1a); 451 T f1b = std::real(cf1b); 452 T f2a = std::real(cf2a); 453 T f2b = std::real(cf2b); 454 455 { 456 T b0 = pole.r + f1a + f2a; 457 T b1 = (T)-2.0 * pole.r + (f2a + f2a); 458 T b2 = pole.r - f1a + f2a; 459 biquads.setParams(ni + i, a0, a1, a2, b0, b1, b2); 460 } 461 { 462 T b0 = pole.r + f1b + f2b; 463 T b1 = (T)-2.0 * pole.r + (f2b + f2b); 464 T b2 = pole.r - f1b + f2b; 465 biquads.setParams(ni + i + 1, a0, a1, a2, b0, b1, b2); 466 } 467 } 468 } 469 break; 470 } 471 472 default: { 473 assert(false); 474 } 475 } 476 } 477 } 478 479 template struct MultimodeDesigner<4>; 480 template struct MultimodeDesigner<8>; 481 template struct MultimodeDesigner<16>; 482 483 484 template<int N> void MultimodeBase<N>::setParams( 485 float sampleRate, 486 Type type, 487 int poles, 488 Mode mode, 489 float frequency, 490 float qbw, 491 BandwidthMode bwm, 492 DelayMode dm 493 ) { 494 _designer.setParams( 495 _biquads, 496 _outGain, 497 sampleRate, 498 type, 499 poles, 500 mode, 501 frequency, 502 qbw, 503 bwm, 504 dm 505 ); 506 } 507 508 template<int N> float MultimodeBase<N>::next(float sample) { 509 return _outGain * _biquads.next(sample); 510 } 511 512 template<int N> void MultimodeBase<N>::reset() { 513 _biquads.reset(); 514 } 515 516 template struct MultimodeBase<4>; 517 template struct MultimodeBase<8>; 518 template struct MultimodeBase<16>; 519 520 } // namespace dsp 521 } // namespace bogaudio