2 #include "polyphase_resampler.h"
8 #include "opthelpers.h"
13 constexpr double Epsilon{1e-9};
15 using uint = unsigned int;
17 /* This is the normalized cardinal sine (sinc) function.
19 * sinc(x) = { 1, x = 0
20 * { sin(pi x) / (pi x), otherwise.
22 double Sinc(const double x)
24 if(std::abs(x) < Epsilon) UNLIKELY
26 return std::sin(al::numbers::pi*x) / (al::numbers::pi*x);
29 /* The zero-order modified Bessel function of the first kind, used for the
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
35 constexpr double BesselI_0(const double x)
37 // Start at k=1 since k=0 is trivial.
38 const double x2{x/2.0};
43 // Let the integration converge until the term of the sum is no longer
47 const double y{x2 / k};
52 } while(sum != last_sum);
56 /* Calculate a Kaiser window from the given beta value and a normalized k
59 * w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B), -1 <= k <= 1
62 * Where k can be calculated as:
64 * k = i / l, where -l <= i <= l.
68 * k = 2 i / M - 1, where 0 <= i <= M.
70 double Kaiser(const double b, const double k)
72 if(!(k >= -1.0 && k <= 1.0))
74 return BesselI_0(b * std::sqrt(1.0 - k*k)) / BesselI_0(b);
77 // Calculates the greatest common divisor of a and b.
78 constexpr uint Gcd(uint x, uint y)
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).
92 * M = { ceil((r - 7.95) / (2.285 2 pi f_t)), r > 21
93 * { ceil(5.79 / 2 pi f_t), r <= 21.
96 constexpr uint CalcKaiserOrder(const double rejection, const double transition)
98 const double w_t{2.0 * al::numbers::pi * transition};
99 if(rejection > 21.0) LIKELY
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));
104 // Calculates the beta value of the Kaiser window. Rejection is in dB.
105 constexpr double CalcKaiserBeta(const double rejection)
107 if(rejection > 50.0) LIKELY
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));
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).
119 * w(k) 2 p f_t sinc(2 f_t x)
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)
127 double SincFilter(const uint l, const double b, const double gain, const double cutoff,
130 const double x{static_cast<double>(i) - l};
131 return Kaiser(b, x / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x);
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)
140 const uint gcd{Gcd(srcRate, dstRate)};
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
148 double cutoff, width;
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)};
166 for(uint i{0};i < mM;i++)
167 mF[i] = SincFilter(l, beta, mP, cutoff, i);
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)
174 if(outN == 0) UNLIKELY
177 // Handle in-place operation.
178 std::vector<double> workspace;
180 if(work == in) UNLIKELY
182 workspace.resize(outN);
183 work = workspace.data();
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++)
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};
196 // Only take input when 0 <= j_s < inN.
200 size_t filt_len{(m-j_f+p-1) / p};
201 if(j_s+1 > inN) LIKELY
203 size_t skip{std::min<size_t>(j_s+1 - inN, filt_len)};
208 if(size_t todo{std::min<size_t>(j_s+1, filt_len)}) LIKELY
211 r += f[j_f] * in[j_s];
219 // Clean up after in-place operation.
221 std::copy_n(work, outN, out);