1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
|
// FFT
#include "fft.h"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <ctime>
#include <exception>
#include <functional>
#include <iostream>
#include <memory>
#include <numeric>
#include <string>
namespace { // Helper functions
bool is_power_of_two(unsigned int n) {
return n != 0 && (n & (n - 1)) == 0;
}
}
std::vector<double> RIT::magnitudes(std::vector<std::complex<double>>& v) {
std::vector<double> result(v.size());
std::transform(std::begin(v), std::end(v), std::begin(result), std::abs<double>);
return result;
}
RIT::FFT::FFT(int size, bool halfOnly): mSize(size), order(size), expLUT(size/2), mFlagHalfOnly(halfOnly) {
if (!is_power_of_two(size))
throw std::invalid_argument("Size must be a power of two");
// reorder LUT
for (int i = 0; i < size; ++i) {
order[i] = bitreverse(i);
}
// exp LUT
for (int i = 0; i < size / 2; ++i) {
expLUT[i] = exp(std::complex<double>(0,-2.*M_PI*i/size));
}
}
std::vector<std::complex<double>> RIT::FFT::operator()(const std::vector<std::complex<double>> &v) const {
if (v.size() != mSize)
throw std::length_error("Bad input size");
std::vector<std::complex<double>> result;
reorder(v, result);
if (mFlagHalfOnly) {
fft_half(std::begin(result), mSize);
result.resize(mSize/2);
} else
fft_recursive(std::begin(result), mSize);
return result;
}
RIT::FFT& RIT::FFT::SetHalfOnly(bool enable) {
mFlagHalfOnly = enable;
return *this;
}
int RIT::FFT::bitreverse(int i) const {
int size{mSize};
int result{0};
while (size > 1) {
result <<= 1;
result |= i & 1;
i >>= 1;
size >>= 1;
}
return result;
}
void RIT::FFT::reorder(const std::vector<std::complex<double>>& src, std::vector<std::complex<double>>& dst) const {
int size = src.size();
dst.resize(size);
for (int i = 0; i < size; ++i) {
dst[order[i]] = src[i];
}
}
// N must be a power-of-2, or bad things will happen.
// Currently no check for this condition.
//
// N input samples in X[] are FFT'd and results left in X[].
// Because of Nyquist theorem, N samples means
// only first N/2 FFT results in X[] are the answer.
// (upper half of X[] is a reflection with no new information).
void RIT::FFT::fft_recursive(std::vector<std::complex<double>>::iterator X, int N) const {
if(N > 2) {
fft_recursive(X, N/2); // recurse even items
fft_recursive(X+N/2, N/2); // recurse odd items
}
// combine results of two half recursions
for(int k=0; k<N/2; k++) {
std::complex<double> e = X[k ]; // even
std::complex<double> o = X[k+N/2]; // odd
// w is the "twiddle-factor"
std::complex<double> w = expLUT[k * mSize / N];
X[k ] = e + w * o;
X[k+N/2] = e - w * o;
}
}
// Same as fft_recursive, but results in only half the result due to symmetry in real input
void RIT::FFT::fft_half(std::vector<std::complex<double>>::iterator X, int N) const {
if(N > 2) {
fft_recursive(X, N/2); // recurse even items
fft_recursive(X+N/2, N/2); // recurse odd items
}
// combine results of two half recursions
for(int k=0; k<N/2; k++) {
X[k] += expLUT[k * mSize / N] * X[k+N/2];
}
}
struct RIT::IFFT::Impl {
public:
using transform_function = std::complex<double> (*)(const std::complex<double>&);
Impl(int size): mFft(std::make_shared<RIT::FFT>(size)) {}
Impl(std::shared_ptr<RIT::FFT> fft): mFft(fft) {}
~Impl(){}
std::shared_ptr<RIT::FFT> mFft;
};
RIT::IFFT::IFFT(int size): mImpl(std::make_unique<RIT::IFFT::Impl>(size))
{
}
RIT::IFFT::IFFT(std::shared_ptr<RIT::FFT> fft): mImpl(std::make_unique<RIT::IFFT::Impl>(fft))
{
}
RIT::IFFT::~IFFT()
{
}
std::vector<std::complex<double>> RIT::IFFT::operator()(const std::vector<std::complex<double>> &v) const
{
std::vector<std::complex<double>> input(v.size());
std::transform(std::begin(v), std::end(v), std::begin(input), static_cast<Impl::transform_function>(std::conj));
auto result = (*mImpl->mFft)(input);
const double factor = 1.0 / v.size();
std::transform(std::begin(result), std::end(result), std::begin(result),
[=](const auto& x){return std::conj(x) * factor;});
return result;
}
struct RIT::AutoCorrelation::Impl {
public:
Impl(int size): mFft(std::make_shared<RIT::FFT>(size)), mIfft(mFft) {}
std::shared_ptr<RIT::FFT> mFft;
RIT::IFFT mIfft;
};
RIT::AutoCorrelation::AutoCorrelation(int size): mImpl(std::make_unique<RIT::AutoCorrelation::Impl>(size))
{
}
RIT::AutoCorrelation::~AutoCorrelation()
{
}
std::vector<std::complex<double>> RIT::AutoCorrelation::operator()(const std::vector<std::complex<double>> &v) const
{
auto result = (*mImpl->mFft)(v);
std::transform(std::begin(result), std::end(result), std::begin(result),
[=](const auto& x){return x * std::conj(x);});
return mImpl->mIfft(result);
}
|
