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
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
|
#include "polyphase_resampler.h"
#include <algorithm>
#include <cmath>
#include "alnumbers.h"
#include "opthelpers.h"
namespace {
constexpr double Epsilon{1e-9};
using uint = unsigned int;
/* This is the normalized cardinal sine (sinc) function.
*
* sinc(x) = { 1, x = 0
* { sin(pi x) / (pi x), otherwise.
*/
double Sinc(const double x)
{
if(std::abs(x) < Epsilon) [[unlikely]]
return 1.0;
return std::sin(al::numbers::pi*x) / (al::numbers::pi*x);
}
/* The zero-order modified Bessel function of the first kind, used for the
* Kaiser window.
*
* I_0(x) = sum_{k=0}^inf (1 / k!)^2 (x / 2)^(2 k)
* = sum_{k=0}^inf ((x / 2)^k / k!)^2
*/
constexpr double BesselI_0(const double x)
{
// Start at k=1 since k=0 is trivial.
const double x2{x/2.0};
double term{1.0};
double sum{1.0};
int k{1};
// Let the integration converge until the term of the sum is no longer
// significant.
double last_sum{};
do {
const double y{x2 / k};
++k;
last_sum = sum;
term *= y * y;
sum += term;
} while(sum != last_sum);
return sum;
}
/* Calculate a Kaiser window from the given beta value and a normalized k
* [-1, 1].
*
* w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B), -1 <= k <= 1
* { 0, elsewhere.
*
* Where k can be calculated as:
*
* k = i / l, where -l <= i <= l.
*
* or:
*
* k = 2 i / M - 1, where 0 <= i <= M.
*/
double Kaiser(const double b, const double k)
{
if(!(k >= -1.0 && k <= 1.0))
return 0.0;
return BesselI_0(b * std::sqrt(1.0 - k*k)) / BesselI_0(b);
}
// Calculates the greatest common divisor of a and b.
constexpr uint Gcd(uint x, uint y)
{
while(y > 0)
{
const uint z{y};
y = x % y;
x = z;
}
return x;
}
/* Calculates the size (order) of the Kaiser window. Rejection is in dB and
* the transition width is normalized frequency (0.5 is nyquist).
*
* M = { ceil((r - 7.95) / (2.285 2 pi f_t)), r > 21
* { ceil(5.79 / 2 pi f_t), r <= 21.
*
*/
constexpr uint CalcKaiserOrder(const double rejection, const double transition)
{
const double w_t{2.0 * al::numbers::pi * transition};
if(rejection > 21.0) [[likely]]
return static_cast<uint>(std::ceil((rejection - 7.95) / (2.285 * w_t)));
return static_cast<uint>(std::ceil(5.79 / w_t));
}
// Calculates the beta value of the Kaiser window. Rejection is in dB.
constexpr double CalcKaiserBeta(const double rejection)
{
if(rejection > 50.0) [[likely]]
return 0.1102 * (rejection - 8.7);
if(rejection >= 21.0)
return (0.5842 * std::pow(rejection - 21.0, 0.4)) +
(0.07886 * (rejection - 21.0));
return 0.0;
}
/* Calculates a point on the Kaiser-windowed sinc filter for the given half-
* width, beta, gain, and cutoff. The point is specified in non-normalized
* samples, from 0 to M, where M = (2 l + 1).
*
* w(k) 2 p f_t sinc(2 f_t x)
*
* x -- centered sample index (i - l)
* k -- normalized and centered window index (x / l)
* w(k) -- window function (Kaiser)
* p -- gain compensation factor when sampling
* f_t -- normalized center frequency (or cutoff; 0.5 is nyquist)
*/
double SincFilter(const uint l, const double b, const double gain, const double cutoff,
const uint i)
{
const double x{static_cast<double>(i) - l};
return Kaiser(b, x / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x);
}
} // namespace
// Calculate the resampling metrics and build the Kaiser-windowed sinc filter
// that's used to cut frequencies above the destination nyquist.
void PPhaseResampler::init(const uint srcRate, const uint dstRate)
{
const uint gcd{Gcd(srcRate, dstRate)};
mP = dstRate / gcd;
mQ = srcRate / gcd;
/* The cutoff is adjusted by half the transition width, so the transition
* ends before the nyquist (0.5). Both are scaled by the downsampling
* factor.
*/
double cutoff, width;
if(mP > mQ)
{
cutoff = 0.475 / mP;
width = 0.05 / mP;
}
else
{
cutoff = 0.475 / mQ;
width = 0.05 / mQ;
}
// A rejection of -180 dB is used for the stop band. Round up when
// calculating the left offset to avoid increasing the transition width.
const uint l{(CalcKaiserOrder(180.0, width)+1) / 2};
const double beta{CalcKaiserBeta(180.0)};
mM = l*2 + 1;
mL = l;
mF.resize(mM);
for(uint i{0};i < mM;i++)
mF[i] = SincFilter(l, beta, mP, cutoff, i);
}
// Perform the upsample-filter-downsample resampling operation using a
// polyphase filter implementation.
void PPhaseResampler::process(const uint inN, const double *in, const uint outN, double *out)
{
if(outN == 0) [[unlikely]]
return;
// Handle in-place operation.
std::vector<double> workspace;
double *work{out};
if(work == in) [[unlikely]]
{
workspace.resize(outN);
work = workspace.data();
}
// Resample the input.
const uint p{mP}, q{mQ}, m{mM}, l{mL};
const double *f{mF.data()};
for(uint i{0};i < outN;i++)
{
// Input starts at l to compensate for the filter delay. This will
// drop any build-up from the first half of the filter.
size_t j_f{(l + q*i) % p};
size_t j_s{(l + q*i) / p};
// Only take input when 0 <= j_s < inN.
double r{0.0};
if(j_f < m) [[likely]]
{
size_t filt_len{(m-j_f+p-1) / p};
if(j_s+1 > inN) [[likely]]
{
size_t skip{std::min<size_t>(j_s+1 - inN, filt_len)};
j_f += p*skip;
j_s -= skip;
filt_len -= skip;
}
if(size_t todo{std::min<size_t>(j_s+1, filt_len)}) [[likely]]
{
do {
r += f[j_f] * in[j_s];
j_f += p;
--j_s;
} while(--todo);
}
}
work[i] = r;
}
// Clean up after in-place operation.
if(work != out)
std::copy_n(work, outN, out);
}
|
