BogaudioModules

BogaudioModules for VCV Rack
Log | Files | Refs | README | LICENSE

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