2 * Copyright (C) 2016 Robin Gareus <robin@gareus.org>
4 * This program is free software; you can redistribute it and/or
5 * modify it under the terms of the GNU General Public License
6 * as published by the Free Software Foundation; either version 2
7 * of the License, or (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
23 #include "ardour/dB.h"
24 #include "ardour/buffer.h"
25 #include "ardour/dsp_filter.h"
29 #define isfinite_local(val) (bool)_finite((double)val)
31 #define isfinite_local std::isfinite
35 #define M_PI 3.14159265358979323846
38 using namespace ARDOUR::DSP;
41 ARDOUR::DSP::memset (float *data, const float val, const uint32_t n_samples) {
42 for (uint32_t i = 0; i < n_samples; ++i) {
48 ARDOUR::DSP::mmult (float *data, float *mult, const uint32_t n_samples) {
49 for (uint32_t i = 0; i < n_samples; ++i) {
55 ARDOUR::DSP::log_meter (float power) {
56 // compare to libs/ardour/log_meter.h
57 static const float lower_db = -192.f;
58 static const float upper_db = 0.f;
59 static const float non_linearity = 8.0;
60 return (power < lower_db ? 0.0 : powf ((power - lower_db) / (upper_db - lower_db), non_linearity));
64 ARDOUR::DSP::log_meter_coeff (float coeff) {
65 if (coeff <= 0) return 0;
66 return log_meter (fast_coefficient_to_dB (coeff));
70 ARDOUR::DSP::peaks (const float *data, float &min, float &max, uint32_t n_samples) {
71 for (uint32_t i = 0; i < n_samples; ++i) {
72 if (data[i] < min) min = data[i];
73 if (data[i] > max) max = data[i];
78 ARDOUR::DSP::process_map (BufferSet* bufs, const ChanMapping& in, const ChanMapping& out, pframes_t nframes, samplecnt_t offset, const DataType& dt)
80 const ChanMapping::Mappings& im (in.mappings());
82 for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
83 if (tm->first != dt) { continue; }
84 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
86 const uint32_t idx = out.get (dt, i->second, &valid);
87 if (valid && idx != i->first) {
88 bufs->get (dt, idx).read_from (bufs->get (dt, i->first), nframes, offset, offset);
92 for (ChanMapping::Mappings::const_iterator tm = im.begin(); tm != im.end(); ++tm) {
93 if (tm->first != dt) { continue; }
94 for (ChanMapping::TypeMapping::const_iterator i = tm->second.begin(); i != tm->second.end(); ++i) {
96 in.get_src (dt, i->first, &valid);
98 bufs->get (dt, i->second).silence (nframes, offset);
105 LowPass::LowPass (double samplerate, float freq)
113 LowPass::set_cutoff (float freq)
115 _a = 1.f - expf (-2.f * M_PI * freq / _rate);
119 LowPass::proc (float *data, const uint32_t n_samples)
121 // localize variables
124 for (uint32_t i = 0; i < n_samples; ++i) {
125 data[i] += a * (data[i] - z);
129 if (!isfinite_local (_z)) { _z = 0; }
133 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
135 // localize variables
138 for (uint32_t i = 0; i < n_samples; ++i) {
139 data[i] += a * (val - z);
145 ///////////////////////////////////////////////////////////////////////////////
147 Biquad::Biquad (double samplerate)
159 Biquad::Biquad (const Biquad &other)
160 : _rate (other._rate)
172 Biquad::run (float *data, const uint32_t n_samples)
174 for (uint32_t i = 0; i < n_samples; ++i) {
175 const float xn = data[i];
176 const float z = _b0 * xn + _z1;
177 _z1 = _b1 * xn - _a1 * z + _z2;
178 _z2 = _b2 * xn - _a2 * z;
182 if (!isfinite_local (_z1)) { _z1 = 0; }
183 if (!isfinite_local (_z2)) { _z2 = 0; }
187 Biquad::configure (double a1, double a2, double b0, double b1, double b2)
197 Biquad::compute (Type type, double freq, double Q, double gain)
199 if (Q <= .001) { Q = 0.001; }
200 if (freq <= 1.) { freq = 1.; }
201 if (freq >= 0.4998 * _rate) { freq = 0.4998 * _rate; }
203 /* Compute biquad filter settings.
204 * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
205 * by Robert Bristow-Johnson
207 const double A = pow (10.0, (gain / 40.0));
208 const double W0 = (2.0 * M_PI * freq) / _rate;
209 const double sinW0 = sin (W0);
210 const double cosW0 = cos (W0);
211 const double alpha = sinW0 / (2.0 * Q);
212 const double beta = sqrt (A) / Q;
218 _b0 = (1.0 - cosW0) / 2.0;
220 _b2 = (1.0 - cosW0) / 2.0;
227 _b0 = (1.0 + cosW0) / 2.0;
228 _b1 = -(1.0 + cosW0);
229 _b2 = (1.0 + cosW0) / 2.0;
235 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
244 case BandPass0dB: /* Constant 0 dB peak gain */
272 _b0 = 1.0 + (alpha * A);
274 _b2 = 1.0 - (alpha * A);
275 _a0 = 1.0 + (alpha / A);
277 _a2 = 1.0 - (alpha / A);
281 _b0 = A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
282 _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
283 _b2 = A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
284 _a0 = (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
285 _a1 = -2.0 * ((A - 1) + ((A + 1) * cosW0));
286 _a2 = (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
290 _b0 = A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
291 _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
292 _b2 = A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
293 _a0 = (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
294 _a1 = 2.0 * ((A - 1) - ((A + 1) * cosW0));
295 _a2 = (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
298 abort(); /*NOTREACHED*/
310 Biquad::dB_at_freq (float freq) const
312 const double W0 = (2.0 * M_PI * freq) / _rate;
313 const float c1 = cosf (W0);
314 const float s1 = sinf (W0);
316 const float A = _b0 + _b2;
317 const float B = _b0 - _b2;
318 const float C = 1.0 + _a2;
319 const float D = 1.0 - _a2;
321 const float a = A * c1 + _b1;
322 const float b = B * s1;
323 const float c = C * c1 + _a1;
324 const float d = D * s1;
326 #define SQUARE(x) ( (x) * (x) )
327 float rv = 20.f * log10f (sqrtf ((SQUARE(a) + SQUARE(b)) * (SQUARE(c) + SQUARE(d))) / (SQUARE(c) + SQUARE(d)));
328 if (!isfinite_local (rv)) { rv = 0; }
329 return std::min (120.f, std::max(-120.f, rv));
333 Glib::Threads::Mutex FFTSpectrum::fft_planner_lock;
335 FFTSpectrum::FFTSpectrum (uint32_t window_size, double rate)
338 init (window_size, rate);
341 FFTSpectrum::~FFTSpectrum ()
344 Glib::Threads::Mutex::Lock lk (fft_planner_lock);
345 fftwf_destroy_plan (_fftplan);
347 fftwf_free (_fft_data_in);
348 fftwf_free (_fft_data_out);
354 FFTSpectrum::init (uint32_t window_size, double rate)
356 assert (window_size > 0);
357 Glib::Threads::Mutex::Lock lk (fft_planner_lock);
359 _fft_window_size = window_size;
360 _fft_data_size = window_size / 2;
361 _fft_freq_per_bin = rate / _fft_data_size / 2.f;
363 _fft_data_in = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
364 _fft_data_out = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
365 _fft_power = (float *) malloc (sizeof(float) * _fft_data_size);
369 _fftplan = fftwf_plan_r2r_1d (_fft_window_size, _fft_data_in, _fft_data_out, FFTW_R2HC, FFTW_MEASURE);
371 hann_window = (float *) malloc(sizeof(float) * window_size);
374 for (uint32_t i = 0; i < window_size; ++i) {
375 hann_window[i] = 0.5f - (0.5f * (float) cos (2.0f * M_PI * (float)i / (float)(window_size)));
376 sum += hann_window[i];
378 const double isum = 2.0 / sum;
379 for (uint32_t i = 0; i < window_size; ++i) {
380 hann_window[i] *= isum;
385 FFTSpectrum::reset ()
387 for (uint32_t i = 0; i < _fft_data_size; ++i) {
390 for (uint32_t i = 0; i < _fft_window_size; ++i) {
391 _fft_data_out[i] = 0;
396 FFTSpectrum::set_data_hann (float const * const data, uint32_t n_samples, uint32_t offset)
398 assert(n_samples + offset <= _fft_window_size);
399 for (uint32_t i = 0; i < n_samples; ++i) {
400 _fft_data_in[i + offset] = data[i] * hann_window[i + offset];
405 FFTSpectrum::execute ()
407 fftwf_execute (_fftplan);
409 _fft_power[0] = _fft_data_out[0] * _fft_data_out[0];
411 #define FRe (_fft_data_out[i])
412 #define FIm (_fft_data_out[_fft_window_size - i])
413 for (uint32_t i = 1; i < _fft_data_size - 1; ++i) {
414 _fft_power[i] = (FRe * FRe) + (FIm * FIm);
415 //_fft_phase[i] = atan2f (FIm, FRe);
422 FFTSpectrum::power_at_bin (const uint32_t b, const float norm) const {
423 assert (b < _fft_data_size);
424 const float a = _fft_power[b] * norm;
425 return a > 1e-12 ? 10.0 * fast_log10 (a) : -INFINITY;
428 Generator::Generator ()
429 : _type (UniformWhiteNoise)
432 set_type (UniformWhiteNoise);
436 Generator::set_type (Generator::Type t) {
438 _b0 = _b1 = _b2 = _b3 = _b4 = _b5 = _b6 = 0;
444 Generator::run (float *data, const uint32_t n_samples)
448 case UniformWhiteNoise:
449 for (uint32_t i = 0; i < n_samples; ++i) {
453 case GaussianWhiteNoise:
454 for (uint32_t i = 0 ; i < n_samples; ++i) {
455 data[i] = 0.7079f * grandf();
459 for (uint32_t i = 0 ; i < n_samples; ++i) {
460 const float white = .39572f * randf ();
461 _b0 = .99886f * _b0 + white * .0555179f;
462 _b1 = .99332f * _b1 + white * .0750759f;
463 _b2 = .96900f * _b2 + white * .1538520f;
464 _b3 = .86650f * _b3 + white * .3104856f;
465 _b4 = .55000f * _b4 + white * .5329522f;
466 _b5 = -.7616f * _b5 - white * .0168980f;
467 data[i] = _b0 + _b1 + _b2 + _b3 + _b4 + _b5 + _b6 + white * 0.5362f;
468 _b6 = white * 0.115926f;
477 // 31bit Park-Miller-Carta Pseudo-Random Number Generator
479 lo = 16807 * (_rseed & 0xffff);
480 hi = 16807 * (_rseed >> 16);
481 lo += (hi & 0x7fff) << 16;
483 lo = (lo & 0x7fffffff) + (lo >> 31);
484 return (_rseed = lo);
500 r = x1 * x1 + x2 * x2;
501 } while ((r >= 1.0f) || (r < 1e-22f));
503 r = sqrtf (-2.f * logf (r) / r);