gtk2_ardour/fft.cc

   1 /*
   2  * Copyright (C) 2008 Sampo Savolainen <v2@iki.fi>
   3  * Copyright (C) 2019 Robin Gareus <robin@gareus.org>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License along
  16  * with this program; if not, write to the Free Software Foundation, Inc.,
  17  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18  */
  19 #include "fft.h"
  20
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 using namespace GTKArdour;
  26
  27 FFT::FFT(uint32_t windowSize)
  28         : _window_size(windowSize),
  29           _data_size(_window_size/2),
  30         _iterations(0),
  31         _hann_window(0)
  32 {
  33         _fftInput  = (float *) fftwf_malloc(sizeof(float) * _window_size);
  34
  35         _fftOutput = (float *) fftwf_malloc(sizeof(float) * _window_size);
  36
  37         _power_at_bin  = (float *) malloc(sizeof(float) * _data_size);
  38         _phase_at_bin  = (float *) malloc(sizeof(float) * _data_size);
  39
  40         _plan = fftwf_plan_r2r_1d(_window_size, _fftInput, _fftOutput, FFTW_R2HC, FFTW_ESTIMATE);
  41
  42         reset();
  43 }
  44
  45 void
  46 FFT::reset()
  47 {
  48         memset(_power_at_bin, 0, sizeof(float) * _data_size);
  49         memset(_phase_at_bin, 0, sizeof(float) * _data_size);
  50
  51         _iterations = 0;
  52 }
  53
  54 void
  55 FFT::analyze(ARDOUR::Sample *input, WindowingType windowing_type)
  56 {
  57         _iterations++;
  58
  59         memcpy(_fftInput, input, sizeof(float) * _window_size);
  60
  61         if (windowing_type == HANN) {
  62                 float *window = get_hann_window();
  63                 for (uint32_t i = 0; i < _window_size; i++) {
  64                         _fftInput[i] *= window[i];
  65                 }
  66         }
  67
  68         fftwf_execute(_plan);
  69
  70         _power_at_bin[0] += _fftOutput[0] * _fftOutput[0];
  71         _phase_at_bin[0] += 0.0;
  72
  73         float power;
  74         float phase;
  75
  76 #define Re (_fftOutput[i])
  77 #define Im (_fftOutput[_window_size-i])
  78         for (uint32_t i = 1; i < _data_size - 1; ++i) {
  79
  80                 power = (Re * Re) + (Im * Im);
  81                 if (power < 1e-16) {
  82                         phase = 0;
  83                 } else {
  84                         phase = atanf (Im / Re);
  85                         if (Re < 0.0 && Im > 0.0) {
  86                                 phase += M_PI;
  87                         } else if (Re < 0.0 && Im < 0.0) {
  88                                 phase -= M_PI;
  89                         }
  90                 }
  91
  92                 _power_at_bin[i] += power;
  93                 _phase_at_bin[i] += phase;
  94         }
  95 #undef Re
  96 #undef Im
  97 }
  98
  99 void
 100 FFT::calculate ()
 101 {
 102         if (_iterations > 1) {
 103                 for (uint32_t i=0; i < _data_size - 1; i++) {
 104                         _power_at_bin[i] /= (float)_iterations;
 105                         _phase_at_bin[i] /= (float)_iterations;
 106                 }
 107                 _iterations = 1;
 108         }
 109 }
 110
 111 float*
 112 FFT::get_hann_window ()
 113 {
 114         if (_hann_window) {
 115                 return _hann_window;
 116         }
 117
 118         _hann_window = (float*) malloc (sizeof (float) * _window_size);
 119
 120         double sum = 0.0;
 121
 122         for (uint32_t i = 0; i < _window_size; ++i) {
 123                 _hann_window[i] = 0.81f * (0.5f - (0.5f * (float) cos (2.0f * M_PI * (float)i / (float)(_window_size))));
 124                 sum += _hann_window[i];
 125         }
 126
 127         double isum = 1.0 / sum;
 128
 129         for (uint32_t i = 0; i < _window_size; ++i) {
 130                 _hann_window[i] *= isum;
 131         }
 132
 133         return _hann_window;
 134 }
 135
 136 FFT::~FFT()
 137 {
 138         if (_hann_window) {
 139                 free(_hann_window);
 140         }
 141         fftwf_destroy_plan(_plan);
 142         free(_power_at_bin);
 143         free(_phase_at_bin);
 144         fftwf_free(_fftOutput);
 145         fftwf_free(_fftInput);
 146 }