Added FFT code
parent
7655f1bdad
commit
bc1a6f3a85
GPG Key ID:
AD62BB059C2AAEE4
|
|
@ -0,0 +1,313 @@
|
|||
#include "fft.hpp"
|
||||
|
||||
|
||||
FFT_Visualizer::FFT_Visualizer(
|
||||
size_t width,
|
||||
size_t height,
|
||||
|
||||
// Advanced options. These have default values.
|
||||
double HZ_MIN,
|
||||
double HZ_MAX,
|
||||
uint32_t DFT_TOTAL_SIZE,
|
||||
uint32_t DFT_NONZERO_SIZE,
|
||||
double GAIN
|
||||
):
|
||||
width(width),
|
||||
height(height),
|
||||
HZ_MIN(HZ_MIN),
|
||||
HZ_MAX(HZ_MAX),
|
||||
DFT_TOTAL_SIZE(DFT_TOTAL_SIZE),
|
||||
DFT_NONZERO_SIZE(DFT_NONZERO_SIZE),
|
||||
GAIN(GAIN),
|
||||
DYNAMIC_RANGE(100 - GAIN)
|
||||
{
|
||||
|
||||
fftw_results = DFT_TOTAL_SIZE/2 + 1;
|
||||
|
||||
freq_magnitudes.resize(fftw_results);
|
||||
partial_output.reserve(width);
|
||||
output.resize(width);
|
||||
GenLogspace();
|
||||
|
||||
fftw_input = static_cast<double *>(fftw_malloc(sizeof(double)*DFT_TOTAL_SIZE));
|
||||
fftw_output = static_cast<fftw_complex *>(fftw_malloc(sizeof(fftw_complex)*fftw_results));
|
||||
memset(fftw_input, 0, sizeof(double)*DFT_TOTAL_SIZE);
|
||||
|
||||
plan = fftw_plan_dft_r2c_1d(
|
||||
DFT_TOTAL_SIZE,
|
||||
fftw_input,
|
||||
fftw_output,
|
||||
FFTW_ESTIMATE
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
FFT_Visualizer::~FFT_Visualizer() {
|
||||
fftw_destroy_plan(plan);
|
||||
fftw_free(fftw_input);
|
||||
fftw_free(fftw_output);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
Returns ideal size for this visualizer's Buffer's output.
|
||||
*/
|
||||
size_t FFT_Visualizer::compute_buffer_output_size() const {
|
||||
return DFT_NONZERO_SIZE;
|
||||
}
|
||||
|
||||
|
||||
// Generate log-scaled vector of frequencies from HZ_MIN to HZ_MAX.
|
||||
void FFT_Visualizer::GenLogspace() {
|
||||
// Prepare vector
|
||||
logspace.resize(width);
|
||||
|
||||
// Calculate number of extra bins needed between 0 HZ and HZ_MIN
|
||||
//
|
||||
// In logspace, divide the region between MAX and MIN into
|
||||
// `width - 1` equal segments (fenceposts; this gives us `width` seperators)
|
||||
const double d = (
|
||||
(log10(HZ_MAX) - log10(HZ_MIN))
|
||||
/
|
||||
(width - 1)
|
||||
);
|
||||
// Count how many of these segments will fit between
|
||||
// 0 and MIN (note that we're still in logspace).
|
||||
// This is how many log-scaled intervals are outside
|
||||
// our desired range of frequencies.
|
||||
const size_t skip_bins = log10(HZ_MIN) / d;
|
||||
|
||||
// Calculate log scale size.
|
||||
// We can't use the value of d here, because d is "anchored" to both MIN and MAX.
|
||||
// The last bin should be equal to MAX, but there may not be a bin that is equal to MIN.
|
||||
//
|
||||
// So, we re-partition our logspace:
|
||||
// Divide the distance between 0 and MAX into equal partitions.
|
||||
const double log_scale = log10(HZ_MAX) / (skip_bins + width - 1);
|
||||
|
||||
// Exponential-map bins out of logspace, skipping those that are outside our range.
|
||||
// Note that the first (skipped) bin is ALWAYS 1, since 10^(0 * log_scale) = 1.
|
||||
// The last bin ALWAYS equals MAX.
|
||||
for (size_t i = skip_bins; i < width + skip_bins; ++i) {
|
||||
logspace[i - skip_bins] = pow(10, i * log_scale);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Apply the Blackman window and copy buffer data.
|
||||
|
||||
@param output Where to write output data
|
||||
@param input Raw data from buffer
|
||||
@param data_length How much data there is. The lengths
|
||||
of both input and output must be equal to this value.
|
||||
*/
|
||||
void FFT_Visualizer::ApplyWindow(
|
||||
double* output,
|
||||
const int16_t* input,
|
||||
ssize_t data_length
|
||||
) const {
|
||||
// samples = length of input = length of output
|
||||
|
||||
// Constants.
|
||||
// These give us low sidelobes and fast sidelobe rolloff.
|
||||
// https://en.wikipedia.org/wiki/Window_function#Blackman_window
|
||||
//
|
||||
// See also:
|
||||
// https://en.wikipedia.org/wiki/Spectral_leakage
|
||||
const double alpha = 0.16;
|
||||
const double a0 = (1 - alpha) / 2;
|
||||
const double a1 = 0.5;
|
||||
const double a2 = alpha / 2;
|
||||
const double pi = 3.151592653;
|
||||
|
||||
const double window_width = DFT_NONZERO_SIZE - 1;
|
||||
|
||||
for (unsigned i = 0; i < data_length; ++i) {
|
||||
double window = (
|
||||
a0 -
|
||||
a1*cos(
|
||||
2*pi*i /
|
||||
window_width
|
||||
) +
|
||||
a2*cos(
|
||||
4*pi*i /
|
||||
window_width
|
||||
)
|
||||
);
|
||||
|
||||
// Output values are between 1 and -1.
|
||||
output[i] = (window * input[i]) / INT16_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Not sure what this does yet.
|
||||
|
||||
Takes values in [0, DFT_TOTAL_SIZE/2],
|
||||
returns values in [0, 44100 / 2].
|
||||
|
||||
@param bin An index of `freq_magnitudes`
|
||||
*/
|
||||
double FFT_Visualizer::Bin2Hz(size_t bin) const {
|
||||
|
||||
return bin*44100/DFT_TOTAL_SIZE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Fill empty bins with interpolated data.
|
||||
|
||||
@param target_x The index of the bin we want to fill.
|
||||
@param last_bar_idx The index of the last non-empty and non-interpolated bin.
|
||||
*/
|
||||
double FFT_Visualizer::Interpolate(
|
||||
size_t target_x,
|
||||
size_t last_bar_idx
|
||||
) {
|
||||
const double x_next = partial_output[last_bar_idx].first;
|
||||
const double h_next = partial_output[last_bar_idx].second;
|
||||
|
||||
double dh = 0;
|
||||
if (last_bar_idx == 0) {
|
||||
// no data points on left, linear extrapolation
|
||||
if (last_bar_idx < partial_output.size()-1) {
|
||||
const double x_next2 = partial_output[last_bar_idx + 1].first;
|
||||
const double h_next2 = partial_output[last_bar_idx + 1].second;
|
||||
dh = (h_next2 - h_next) / (x_next2 - x_next);
|
||||
}
|
||||
return h_next - dh*(x_next - target_x);
|
||||
|
||||
} else if (last_bar_idx == 1) {
|
||||
// one data point on left, linear interpolation
|
||||
const double x_prev = partial_output[last_bar_idx - 1].first;
|
||||
const double h_prev = partial_output[last_bar_idx - 1].second;
|
||||
dh = (h_next - h_prev) / (x_next - x_prev);
|
||||
return h_next - dh*(x_next - target_x);
|
||||
|
||||
} else if (last_bar_idx < partial_output.size() - 1) {
|
||||
// Two data points on both sides, cubic interpolation
|
||||
// https://en.wikipedia.org/wiki/Cubic_Hermite_spline#Interpolation_on_an_arbitrary_interval
|
||||
const double x_prev2 = partial_output[last_bar_idx - 2].first;
|
||||
const double h_prev2 = partial_output[last_bar_idx - 2].second;
|
||||
const double x_prev = partial_output[last_bar_idx - 1].first;
|
||||
const double h_prev = partial_output[last_bar_idx - 1].second;
|
||||
const double x_next2 = partial_output[last_bar_idx + 1].first;
|
||||
const double h_next2 = partial_output[last_bar_idx + 1].second;
|
||||
|
||||
const double m0 = (h_prev - h_prev2) / (x_prev - x_prev2);
|
||||
const double m1 = (h_next2 - h_next) / (x_next2 - x_next);
|
||||
const double t = (target_x - x_prev) / (x_next - x_prev);
|
||||
const double h00 = 2*t*t*t - 3*t*t + 1;
|
||||
const double h10 = t*t*t - 2*t*t + t;
|
||||
const double h01 = -2*t*t*t + 3*t*t;
|
||||
const double h11 = t*t*t - t*t;
|
||||
|
||||
return h00*h_prev + h10*(x_next-x_prev)*m0 + h01*h_next + h11*(x_next-x_prev)*m1;
|
||||
}
|
||||
|
||||
// Less than two data points on right, no interpolation.
|
||||
// This should never happen unless you have a VERY low DFT size
|
||||
return h_next;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Read a buffer and update output array
|
||||
*/
|
||||
void FFT_Visualizer::update(
|
||||
const Buffer& buf
|
||||
) {
|
||||
|
||||
// Load data and execute FFT
|
||||
ApplyWindow(fftw_input, buf.get_output().data(), buf.get_output().size());
|
||||
fftw_execute(plan);
|
||||
|
||||
|
||||
// Count magnitude of each frequency and normalize
|
||||
// (fftw does not normalize)
|
||||
for (size_t i = 0; i < fftw_results; ++i) {
|
||||
freq_magnitudes[i] = sqrt(
|
||||
fftw_output[i][0]*fftw_output[i][0]
|
||||
+
|
||||
fftw_output[i][1]*fftw_output[i][1]
|
||||
) / (DFT_NONZERO_SIZE);
|
||||
}
|
||||
|
||||
|
||||
// Skip magnitudes not in log space
|
||||
size_t cur_bin = 0;
|
||||
while (cur_bin < fftw_results && Bin2Hz(cur_bin) < logspace[0]) {
|
||||
++cur_bin;
|
||||
}
|
||||
|
||||
|
||||
|
||||
partial_output.clear();
|
||||
|
||||
// Accumulate magnitudes into bins
|
||||
for (size_t x = 0; x < width; x++) {
|
||||
double bar_height = 0;
|
||||
size_t count = 0;
|
||||
|
||||
// Check right bound
|
||||
while (cur_bin < fftw_results && Bin2Hz(cur_bin) < logspace[x]) {
|
||||
// Check left bound if not first index
|
||||
if (x == 0 || Bin2Hz(cur_bin) >= logspace[x-1]) {
|
||||
bar_height += freq_magnitudes[cur_bin];
|
||||
++count;
|
||||
}
|
||||
++cur_bin;
|
||||
}
|
||||
|
||||
// If bin is empty, we're done.
|
||||
// Don't add this bin to partial_output,
|
||||
// it will be interpolated later.
|
||||
if (count == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// average bins
|
||||
bar_height /= count;
|
||||
|
||||
// log scale heights
|
||||
bar_height = (20 * log10(bar_height) + DYNAMIC_RANGE + GAIN) / DYNAMIC_RANGE;
|
||||
// Scale bar height between 0 and height
|
||||
bar_height = bar_height > 0 ? bar_height * (height-1) : 0;
|
||||
bar_height = bar_height >= height ? (height-1) : bar_height;
|
||||
|
||||
|
||||
// use emplace_back to prevent redundant copy operations.
|
||||
// faster than push_back, in this case.
|
||||
partial_output.emplace_back(x, bar_height);
|
||||
}
|
||||
|
||||
size_t bar_idx = 0;
|
||||
for (size_t x = 0; x < width; x++) {
|
||||
const size_t bar_x = partial_output[bar_idx].first;
|
||||
const double bar_height = partial_output[bar_idx].second;
|
||||
|
||||
if (x == bar_x) {
|
||||
// This data point exists, add it to output array.
|
||||
output[x] = (size_t) bar_height;
|
||||
if (bar_idx < partial_output.size() - 1) {
|
||||
bar_idx++;
|
||||
}
|
||||
} else {
|
||||
// data point does not exist, we need to interpolate
|
||||
|
||||
// This check shouldn't be necessary, but sometimes
|
||||
// Interpolate throws out a negative value.
|
||||
//
|
||||
// figure out why!
|
||||
double i = Interpolate(x, bar_idx);
|
||||
|
||||
output[x] = (size_t) (i > 0 ? i : 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdio.h>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
#include <math.h>
|
||||
#include <cstring> // memset
|
||||
#include <fftw3.h>
|
||||
|
||||
#include "utility/buffer.hpp"
|
||||
|
||||
class FFT_Visualizer {
|
||||
public:
|
||||
FFT_Visualizer(
|
||||
size_t width,
|
||||
size_t height,
|
||||
|
||||
// Advanced options you probably shouldn't touch
|
||||
double HZ_MIN = 20,
|
||||
double HZ_MAX = 20000,
|
||||
uint32_t DFT_TOTAL_SIZE = 1 << 15,
|
||||
// #define conf_spectrum_dft_size 2 (between 1 and 5, inclusive)
|
||||
uint32_t DFT_NONZERO_SIZE = (2048 * (2*2 + 4)),
|
||||
double GAIN = 10
|
||||
);
|
||||
|
||||
~FFT_Visualizer();
|
||||
|
||||
void update(
|
||||
const Buffer& buf
|
||||
);
|
||||
|
||||
size_t compute_buffer_output_size() const;
|
||||
|
||||
const std::vector<size_t>& get_output() {
|
||||
return output;
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
///
|
||||
// Visualizer parameters
|
||||
///
|
||||
|
||||
// How many bars this visualizer will generate
|
||||
const size_t width;
|
||||
// Resolution of this visualizer's bars.
|
||||
const size_t height;
|
||||
// Leftmost frequency in spectrum
|
||||
const double HZ_MIN;
|
||||
// Rightmost frequency in spectrum
|
||||
const double HZ_MAX;
|
||||
// Not sure what this does
|
||||
const uint32_t DFT_TOTAL_SIZE;
|
||||
// Not sure what this does, either
|
||||
const uint32_t DFT_NONZERO_SIZE;
|
||||
// Visualization spectrum gain
|
||||
// tune if bars are too small
|
||||
const double GAIN;
|
||||
// Not sure what this does
|
||||
const double DYNAMIC_RANGE;
|
||||
|
||||
|
||||
///
|
||||
// FFTW resources
|
||||
///
|
||||
|
||||
fftw_plan plan;
|
||||
// How many output values fftw will give us
|
||||
size_t fftw_results;
|
||||
// Input array. This is filled with buffer data
|
||||
// that has been filtered through a window.
|
||||
double* fftw_input;
|
||||
// FFT output array.
|
||||
fftw_complex *fftw_output;
|
||||
|
||||
///
|
||||
// Intermediate values
|
||||
///
|
||||
|
||||
// The magnitudes of the complex values fftw returns.
|
||||
std::vector<double> freq_magnitudes;
|
||||
// An array of frequency bins, scaled logarithmically.
|
||||
std::vector<double> logspace;
|
||||
// Nearly-complete output.
|
||||
// This is necessary because some frequency bins
|
||||
// may be empty, and need to be interpolated
|
||||
std::vector<
|
||||
std::pair<
|
||||
size_t,
|
||||
double
|
||||
>
|
||||
> partial_output;
|
||||
|
||||
|
||||
// Output vector, with empty bins interpolated.
|
||||
// The maximum possible value of an element in this
|
||||
// vector is `height`.
|
||||
std::vector<size_t> output;
|
||||
|
||||
|
||||
///
|
||||
// Helper methods
|
||||
// See definition for docs.
|
||||
///
|
||||
|
||||
void GenLogspace();
|
||||
double Bin2Hz(size_t bin) const;
|
||||
|
||||
void ApplyWindow(
|
||||
double *output,
|
||||
const int16_t *input,
|
||||
ssize_t samples
|
||||
) const;
|
||||
|
||||
double Interpolate(
|
||||
size_t x,
|
||||
size_t h_idx
|
||||
);
|
||||
};
|
||||
Loading…
Reference in New Issue