libs/ardour/dsp_filter.cc

   1 /*
   2  * Copyright (C) 2016 Robin Gareus <robin@gareus.org>
   3  *
   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.
   8  *
   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.
  13  *
  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.
  17  *
  18  */
  19
  20 #include <algorithm>
  21 #include <stdlib.h>
  22 #include <cmath>
  23 #include "ardour/dB.h"
  24 #include "ardour/dsp_filter.h"
  25
  26 #ifdef COMPILER_MSVC
  27 #include <float.h>
  28 #define isfinite_local(val) (bool)_finite((double)val)
  29 #else
  30 #define isfinite_local std::isfinite
  31 #endif
  32
  33 #ifndef M_PI
  34 #define M_PI 3.14159265358979323846
  35 #endif
  36
  37 using namespace ARDOUR::DSP;
  38
  39 void
  40 ARDOUR::DSP::memset (float *data, const float val, const uint32_t n_samples) {
  41         for (uint32_t i = 0; i < n_samples; ++i) {
  42                 data[i] = val;
  43         }
  44 }
  45
  46 void
  47 ARDOUR::DSP::mmult (float *data, float *mult, const uint32_t n_samples) {
  48         for (uint32_t i = 0; i < n_samples; ++i) {
  49                 data[i] *= mult[i];
  50         }
  51 }
  52
  53 float
  54 ARDOUR::DSP::log_meter (float power) {
  55         // compare to gtk2_ardour/logmeter.h
  56         static const float lower_db = -192.f;
  57         static const float upper_db = 0.f;
  58         static const float non_linearity = 8.0;
  59         return (power < lower_db ? 0.0 : powf ((power - lower_db) / (upper_db - lower_db), non_linearity));
  60 }
  61
  62 float
  63 ARDOUR::DSP::log_meter_coeff (float coeff) {
  64         if (coeff <= 0) return 0;
  65         return log_meter (fast_coefficient_to_dB (coeff));
  66 }
  67
  68 void
  69 ARDOUR::DSP::peaks (float *data, float &min, float &max, uint32_t n_samples) {
  70         for (uint32_t i = 0; i < n_samples; ++i) {
  71                 if (data[i] < min) min = data[i];
  72                 if (data[i] > max) max = data[i];
  73         }
  74 }
  75
  76 LowPass::LowPass (double samplerate, float freq)
  77         : _rate (samplerate)
  78         , _z (0)
  79 {
  80         set_cutoff (freq);
  81 }
  82
  83 void
  84 LowPass::set_cutoff (float freq)
  85 {
  86         _a = 1.f - expf (-2.f * M_PI * freq / _rate);
  87 }
  88
  89 void
  90 LowPass::proc (float *data, const uint32_t n_samples)
  91 {
  92         // localize variables
  93         const float a = _a;
  94         float z = _z;
  95         for (uint32_t i = 0; i < n_samples; ++i) {
  96                 data[i] += a * (data[i] - z);
  97                 z = data[i];
  98         }
  99         _z = z;
 100         if (!isfinite_local (_z)) { _z = 0; }
 101 }
 102
 103 void
 104 LowPass::ctrl (float *data, const float val, const uint32_t n_samples)
 105 {
 106         // localize variables
 107         const float a = _a;
 108         float z = _z;
 109         for (uint32_t i = 0; i < n_samples; ++i) {
 110                 data[i] += a * (val - z);
 111                 z = data[i];
 112         }
 113         _z = z;
 114 }
 115
 116 ///////////////////////////////////////////////////////////////////////////////
 117
 118 Biquad::Biquad (double samplerate)
 119         : _rate (samplerate)
 120         , _z1 (0.0)
 121         , _z2 (0.0)
 122         , _a1 (0.0)
 123         , _a2 (0.0)
 124         , _b0 (1.0)
 125         , _b1 (0.0)
 126         , _b2 (0.0)
 127 {
 128 }
 129
 130 Biquad::Biquad (const Biquad &other)
 131         : _rate (other._rate)
 132         , _z1 (0.0)
 133         , _z2 (0.0)
 134         , _a1 (other._a1)
 135         , _a2 (other._a2)
 136         , _b0 (other._b0)
 137         , _b1 (other._b1)
 138         , _b2 (other._b2)
 139 {
 140 }
 141
 142 void
 143 Biquad::run (float *data, const uint32_t n_samples)
 144 {
 145         for (uint32_t i = 0; i < n_samples; ++i) {
 146                 const float xn = data[i];
 147                 const float z = _b0 * xn + _z1;
 148                 _z1           = _b1 * xn - _a1 * z + _z2;
 149                 _z2           = _b2 * xn - _a2 * z;
 150                 data[i] = z;
 151         }
 152
 153         if (!isfinite_local (_z1)) { _z1 = 0; }
 154         if (!isfinite_local (_z2)) { _z2 = 0; }
 155 }
 156
 157 void
 158 Biquad::configure (double a1, double a2, double b0, double b1, double b2)
 159 {
 160         _a1 = a1;
 161         _a2 = a2;
 162         _b0 = b0;
 163         _b1 = b1;
 164         _b2 = b2;
 165 }
 166
 167 void
 168 Biquad::compute (Type type, double freq, double Q, double gain)
 169 {
 170         if (Q <= .001)     { Q = 0.001; }
 171         if (freq <= 1.)    { freq = 1.; }
 172         if (freq >= _rate) { freq = _rate; }
 173
 174         /* Compute biquad filter settings.
 175          * Based on 'Cookbook formulae for audio EQ biquad filter coefficents'
 176          * by Robert Bristow-Johnson
 177          */
 178         const double A = pow (10.0, (gain / 40.0));
 179         const double W0 = (2.0 * M_PI * freq) / _rate;
 180         const double sinW0 = sin (W0);
 181         const double cosW0 = cos (W0);
 182         const double alpha = sinW0 / (2.0 * Q);
 183         const double beta  = sqrt (A) / Q;
 184
 185         double _a0;
 186
 187         switch (type) {
 188                 case LowPass:
 189                         _b0 = (1.0 - cosW0) / 2.0;
 190                         _b1 =  1.0 - cosW0;
 191                         _b2 = (1.0 - cosW0) / 2.0;
 192                         _a0 =  1.0 + alpha;
 193                         _a1 = -2.0 * cosW0;
 194                         _a2 =  1.0 - alpha;
 195                         break;
 196
 197                 case HighPass:
 198                         _b0 =  (1.0 + cosW0) / 2.0;
 199                         _b1 = -(1.0 + cosW0);
 200                         _b2 =  (1.0 + cosW0) / 2.0;
 201                         _a0 =   1.0 + alpha;
 202                         _a1 =  -2.0 * cosW0;
 203                         _a2 =   1.0 - alpha;
 204                         break;
 205
 206                 case BandPassSkirt: /* Constant skirt gain, peak gain = Q */
 207                         _b0 =  sinW0 / 2.0;
 208                         _b1 =  0.0;
 209                         _b2 = -sinW0 / 2.0;
 210                         _a0 =  1.0 + alpha;
 211                         _a1 = -2.0 * cosW0;
 212                         _a2 =  1.0 - alpha;
 213                         break;
 214
 215                 case BandPass0dB: /* Constant 0 dB peak gain */
 216                         _b0 =  alpha;
 217                         _b1 =  0.0;
 218                         _b2 = -alpha;
 219                         _a0 =  1.0 + alpha;
 220                         _a1 = -2.0 * cosW0;
 221                         _a2 =  1.0 - alpha;
 222                         break;
 223
 224                 case Notch:
 225                         _b0 =  1.0;
 226                         _b1 = -2.0 * cosW0;
 227                         _b2 =  1.0;
 228                         _a0 =  1.0 + alpha;
 229                         _a1 = -2.0 * cosW0;
 230                         _a2 =  1.0 - alpha;
 231                         break;
 232
 233                 case AllPass:
 234                         _b0 =  1.0 - alpha;
 235                         _b1 = -2.0 * cosW0;
 236                         _b2 =  1.0 + alpha;
 237                         _a0 =  1.0 + alpha;
 238                         _a1 = -2.0 * cosW0;
 239                         _a2 =  1.0 - alpha;
 240                         break;
 241
 242                 case Peaking:
 243                         _b0 =  1.0 + (alpha * A);
 244                         _b1 = -2.0 * cosW0;
 245                         _b2 =  1.0 - (alpha * A);
 246                         _a0 =  1.0 + (alpha / A);
 247                         _a1 = -2.0 * cosW0;
 248                         _a2 =  1.0 - (alpha / A);
 249                         break;
 250
 251                 case LowShelf:
 252                         _b0 =         A * ((A + 1) - ((A - 1) * cosW0) + (beta * sinW0));
 253                         _b1 = (2.0 * A) * ((A - 1) - ((A + 1) * cosW0));
 254                         _b2 =         A * ((A + 1) - ((A - 1) * cosW0) - (beta * sinW0));
 255                         _a0 =              (A + 1) + ((A - 1) * cosW0) + (beta * sinW0);
 256                         _a1 =      -2.0 * ((A - 1) + ((A + 1) * cosW0));
 257                         _a2 =              (A + 1) + ((A - 1) * cosW0) - (beta * sinW0);
 258                         break;
 259
 260                 case HighShelf:
 261                         _b0 =          A * ((A + 1) + ((A - 1) * cosW0) + (beta * sinW0));
 262                         _b1 = -(2.0 * A) * ((A - 1) + ((A + 1) * cosW0));
 263                         _b2 =          A * ((A + 1) + ((A - 1) * cosW0) - (beta * sinW0));
 264                         _a0 =               (A + 1) - ((A - 1) * cosW0) + (beta * sinW0);
 265                         _a1 =        2.0 * ((A - 1) - ((A + 1) * cosW0));
 266                         _a2 =               (A + 1) - ((A - 1) * cosW0) - (beta * sinW0);
 267                         break;
 268                 default:
 269                         abort(); /*NOTREACHED*/
 270                         break;
 271         }
 272
 273         _b0 /= _a0;
 274         _b1 /= _a0;
 275         _b2 /= _a0;
 276         _a1 /= _a0;
 277         _a2 /= _a0;
 278 }
 279
 280 float
 281 Biquad::dB_at_freq (float freq) const
 282 {
 283         const double W0 = (2.0 * M_PI * freq) / _rate;
 284         const float c1 = cosf (W0);
 285         const float s1 = sinf (W0);
 286
 287         const float A = _b0 + _b2;
 288         const float B = _b0 - _b2;
 289         const float C = 1.0 + _a2;
 290         const float D = 1.0 - _a2;
 291
 292         const float a = A * c1 + _b1;
 293         const float b = B * s1;
 294         const float c = C * c1 + _a1;
 295         const float d = D * s1;
 296
 297 #define SQUARE(x) ( (x) * (x) )
 298         float rv = 20.f * log10f (sqrtf ((SQUARE(a) + SQUARE(b)) * (SQUARE(c) + SQUARE(d))) / (SQUARE(c) + SQUARE(d)));
 299         if (!isfinite_local (rv)) { rv = 0; }
 300         return std::min (120.f, std::max(-120.f, rv));
 301 }
 302
 303
 304 Glib::Threads::Mutex FFTSpectrum::fft_planner_lock;
 305
 306 FFTSpectrum::FFTSpectrum (uint32_t window_size, double rate)
 307         : hann_window (0)
 308 {
 309         init (window_size, rate);
 310 }
 311
 312 FFTSpectrum::~FFTSpectrum ()
 313 {
 314         {
 315                 Glib::Threads::Mutex::Lock lk (fft_planner_lock);
 316                 fftwf_destroy_plan (_fftplan);
 317         }
 318         fftwf_free (_fft_data_in);
 319         fftwf_free (_fft_data_out);
 320         free (_fft_power);
 321         free (hann_window);
 322 }
 323
 324 void
 325 FFTSpectrum::init (uint32_t window_size, double rate)
 326 {
 327         Glib::Threads::Mutex::Lock lk (fft_planner_lock);
 328
 329         _fft_window_size = window_size;
 330         _fft_data_size   = window_size / 2;
 331         _fft_freq_per_bin = rate / _fft_data_size / 2.f;
 332
 333         _fft_data_in  = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
 334         _fft_data_out = (float *) fftwf_malloc (sizeof(float) * _fft_window_size);
 335         _fft_power    = (float *) malloc (sizeof(float) * _fft_data_size);
 336
 337         reset ();
 338
 339         _fftplan = fftwf_plan_r2r_1d (_fft_window_size, _fft_data_in, _fft_data_out, FFTW_R2HC, FFTW_MEASURE);
 340
 341         hann_window  = (float *) malloc(sizeof(float) * window_size);
 342         double sum = 0.0;
 343
 344         for (uint32_t i = 0; i < window_size; ++i) {
 345                 hann_window[i] = 0.5f - (0.5f * (float) cos (2.0f * M_PI * (float)i / (float)(window_size)));
 346                 sum += hann_window[i];
 347         }
 348         const double isum = 2.0 / sum;
 349         for (uint32_t i = 0; i < window_size; ++i) {
 350                 hann_window[i] *= isum;
 351         }
 352 }
 353
 354 void
 355 FFTSpectrum::reset ()
 356 {
 357         for (uint32_t i = 0; i < _fft_data_size; ++i) {
 358                 _fft_power[i] = 0;
 359         }
 360         for (uint32_t i = 0; i < _fft_window_size; ++i) {
 361                 _fft_data_out[i] = 0;
 362         }
 363 }
 364
 365 void
 366 FFTSpectrum::set_data_hann (float const * const data, uint32_t n_samples, uint32_t offset)
 367 {
 368         assert(n_samples + offset <= _fft_window_size);
 369         for (uint32_t i = 0; i < n_samples; ++i) {
 370                 _fft_data_in[i + offset] = data[i] * hann_window[i + offset];
 371         }
 372 }
 373
 374 void
 375 FFTSpectrum::execute ()
 376 {
 377         fftwf_execute (_fftplan);
 378
 379         _fft_power[0] = _fft_data_out[0] * _fft_data_out[0];
 380
 381 #define FRe (_fft_data_out[i])
 382 #define FIm (_fft_data_out[_fft_window_size - i])
 383         for (uint32_t i = 1; i < _fft_data_size - 1; ++i) {
 384                 _fft_power[i] = (FRe * FRe) + (FIm * FIm);
 385                 //_fft_phase[i] = atan2f (FIm, FRe);
 386         }
 387 #undef FRe
 388 #undef FIm
 389 }
 390
 391 float
 392 FFTSpectrum::power_at_bin (const uint32_t b, const float norm) const {
 393         assert (b >= 0 && b < _fft_data_size);
 394         const float a = _fft_power[b] * norm;
 395         return a > 1e-12 ? 10.0 * fast_log10 (a) : -INFINITY;
 396 }