1
2 #include "polyphase_resampler.h"
3
Deg2Rad(float x)4 #include <algorithm>
5 #include <cmath>
6
7 #include "math_defs.h"
8 #include "opthelpers.h"
9
10
11 namespace {
12
13 constexpr double Epsilon{1e-9};
14
15 using uint = unsigned int;
16
17 /* This is the normalized cardinal sine (sinc) function.
18 *
19 * sinc(x) = { 1, x = 0
20 * { sin(pi x) / (pi x), otherwise.
21 */
22 double Sinc(const double x)
23 {
24 if UNLIKELY(std::abs(x) < Epsilon)
25 return 1.0;
26 return std::sin(al::MathDefs<double>::Pi()*x) / (al::MathDefs<double>::Pi()*x);
27 }
28
29 /* The zero-order modified Bessel function of the first kind, used for the
30 * Kaiser window.
31 *
32 * I_0(x) = sum_{k=0}^inf (1 / k!)^2 (x / 2)^(2 k)
33 * = sum_{k=0}^inf ((x / 2)^k / k!)^2
34 */
35 constexpr double BesselI_0(const double x)
36 {
37 // Start at k=1 since k=0 is trivial.
38 const double x2{x/2.0};
39 double term{1.0};
40 double sum{1.0};
41 int k{1};
42
43 // Let the integration converge until the term of the sum is no longer
44 // significant.
45 double last_sum{};
46 do {
47 const double y{x2 / k};
48 ++k;
49 last_sum = sum;
50 term *= y * y;
51 sum += term;
52 } while(sum != last_sum);
53 return sum;
54 }
55
56 /* Calculate a Kaiser window from the given beta value and a normalized k
57 * [-1, 1].
58 *
59 * w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B), -1 <= k <= 1
60 * { 0, elsewhere.
61 *
62 * Where k can be calculated as:
63 *
64 * k = i / l, where -l <= i <= l.
65 *
66 * or:
67 *
68 * k = 2 i / M - 1, where 0 <= i <= M.
69 */
70 double Kaiser(const double b, const double k)
71 {
72 if(!(k >= -1.0 && k <= 1.0))
73 return 0.0;
74 return BesselI_0(b * std::sqrt(1.0 - k*k)) / BesselI_0(b);
75 }
76
77 // Calculates the greatest common divisor of a and b.
78 constexpr uint Gcd(uint x, uint y)
79 {
80 while(y > 0)
81 {
82 const uint z{y};
83 y = x % y;
84 x = z;
85 }
86 return x;
87 }
88
89 /* Calculates the size (order) of the Kaiser window. Rejection is in dB and
90 * the transition width is normalized frequency (0.5 is nyquist).
91 *
92 * M = { ceil((r - 7.95) / (2.285 2 pi f_t)), r > 21
93 * { ceil(5.79 / 2 pi f_t), r <= 21.
94 *
95 */
96 constexpr uint CalcKaiserOrder(const double rejection, const double transition)
97 {
98 const double w_t{2.0 * al::MathDefs<double>::Pi() * transition};
99 if LIKELY(rejection > 21.0)
100 return static_cast<uint>(std::ceil((rejection - 7.95) / (2.285 * w_t)));
101 return static_cast<uint>(std::ceil(5.79 / w_t));
102 }
103
104 // Calculates the beta value of the Kaiser window. Rejection is in dB.
105 constexpr double CalcKaiserBeta(const double rejection)
106 {
107 if LIKELY(rejection > 50.0)
108 return 0.1102 * (rejection - 8.7);
109 if(rejection >= 21.0)
110 return (0.5842 * std::pow(rejection - 21.0, 0.4)) +
111 (0.07886 * (rejection - 21.0));
112 return 0.0;
113 }
114
115 /* Calculates a point on the Kaiser-windowed sinc filter for the given half-
116 * width, beta, gain, and cutoff. The point is specified in non-normalized
117 * samples, from 0 to M, where M = (2 l + 1).
118 *
119 * w(k) 2 p f_t sinc(2 f_t x)
120 *
121 * x -- centered sample index (i - l)
122 * k -- normalized and centered window index (x / l)
123 * w(k) -- window function (Kaiser)
124 * p -- gain compensation factor when sampling
125 * f_t -- normalized center frequency (or cutoff; 0.5 is nyquist)
126 */
127 double SincFilter(const uint l, const double b, const double gain, const double cutoff,
128 const uint i)
129 {
130 const double x{static_cast<double>(i) - l};
131 return Kaiser(b, x / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x);
132 }
133
134 } // namespace
135
136 // Calculate the resampling metrics and build the Kaiser-windowed sinc filter
137 // that's used to cut frequencies above the destination nyquist.
138 void PPhaseResampler::init(const uint srcRate, const uint dstRate)
139 {
140 const uint gcd{Gcd(srcRate, dstRate)};
141 mP = dstRate / gcd;
142 mQ = srcRate / gcd;
143
144 /* The cutoff is adjusted by half the transition width, so the transition
145 * ends before the nyquist (0.5). Both are scaled by the downsampling
146 * factor.
147 */
148 double cutoff, width;
149 if(mP > mQ)
150 {
151 cutoff = 0.475 / mP;
152 width = 0.05 / mP;
153 }
154 else
155 {
156 cutoff = 0.475 / mQ;
157 width = 0.05 / mQ;
158 }
159 // A rejection of -180 dB is used for the stop band. Round up when
160 // calculating the left offset to avoid increasing the transition width.
161 const uint l{(CalcKaiserOrder(180.0, width)+1) / 2};
162 const double beta{CalcKaiserBeta(180.0)};
163 mM = l*2 + 1;
164 mL = l;
165 mF.resize(mM);
166 for(uint i{0};i < mM;i++)
167 mF[i] = SincFilter(l, beta, mP, cutoff, i);
168 }
169
170 // Perform the upsample-filter-downsample resampling operation using a
171 // polyphase filter implementation.
172 void PPhaseResampler::process(const uint inN, const double *in, const uint outN, double *out)
173 {
174 if UNLIKELY(outN == 0)
175 return;
176
177 // Handle in-place operation.
178 std::vector<double> workspace;
179 double *work{out};
180 if UNLIKELY(work == in)
181 {
182 workspace.resize(outN);
183 work = workspace.data();
184 }
185
186 // Resample the input.
187 const uint p{mP}, q{mQ}, m{mM}, l{mL};
188 const double *f{mF.data()};
189 for(uint i{0};i < outN;i++)
190 {
191 // Input starts at l to compensate for the filter delay. This will
192 // drop any build-up from the first half of the filter.
193 size_t j_f{(l + q*i) % p};
194 size_t j_s{(l + q*i) / p};
195
196 // Only take input when 0 <= j_s < inN.
197 double r{0.0};
198 if LIKELY(j_f < m)
199 {
200 size_t filt_len{(m-j_f+p-1) / p};
201 if LIKELY(j_s+1 > inN)
202 {
203 size_t skip{std::min<size_t>(j_s+1 - inN, filt_len)};
204 j_f += p*skip;
205 j_s -= skip;
206 filt_len -= skip;
207 }
208 if(size_t todo{std::min<size_t>(j_s+1, filt_len)})
209 {
210 do {
211 r += f[j_f] * in[j_s];
212 j_f += p;
213 --j_s;
214 } while(--todo);
215 }
216 }
217 work[i] = r;
218 }
219 // Clean up after in-place operation.
220 if(work != out)
221 std::copy_n(work, outN, out);
222 }
223