axmol/thirdparty/openal/common/polyphase_resampler.cpp

223 lines
6.1 KiB
C++
Raw Normal View History

#include "polyphase_resampler.h"
#include <algorithm>
#include <cmath>
2022-04-25 12:02:45 +08:00
#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)
{
2023-02-04 15:03:54 +08:00
if(std::abs(x) < Epsilon) [[unlikely]]
return 1.0;
2022-04-25 12:02:45 +08:00
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)
{
2022-04-25 12:02:45 +08:00
const double w_t{2.0 * al::numbers::pi * transition};
2023-02-04 15:03:54 +08:00
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)
{
2023-02-04 15:03:54 +08:00
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)
{
2023-02-04 15:03:54 +08:00
if(outN == 0) [[unlikely]]
return;
// Handle in-place operation.
std::vector<double> workspace;
double *work{out};
2023-02-04 15:03:54 +08:00
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};
2023-02-04 15:03:54 +08:00
if(j_f < m) [[likely]]
{
size_t filt_len{(m-j_f+p-1) / p};
2023-02-04 15:03:54 +08:00
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;
}
2023-02-04 15:03:54 +08:00
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);
}