sinc_resampler.hpp

Go to the documentation of this file.

#pragma once

 
#include <pulp/signal/checked_allocation.hpp>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <vector>
 
namespace pulp::signal {
 
template <typename SampleType = float> class SincResamplerT {
  public:
    static bool checked_retained_bytes(int half_width, int phases,
                                       std::uint64_t target_max_bytes,
                                       std::uint64_t& bytes) noexcept {
        std::uint64_t elements = 0;
        if (!checked_capacity_product(static_cast<std::uint64_t>(std::max(1, phases) + 1),
                                      static_cast<std::uint64_t>(2 * std::max(1, half_width)),
                                      UINT64_MAX, elements))
            return false;
        CheckedRetainedByteCharge charge(target_max_bytes);
        if (!charge.add<SampleType>(elements)) return false;
        bytes = charge.total();
        return true;
    }

    void build(int half_width = 16, int phases = 512, double beta = 9.0, double cutoff = 1.0) {
        begin_build(half_width, phases, beta, cutoff);
        while (!build_step()) {
        }
    }

    void begin_build(int half_width = 16, int phases = 512, double beta = 9.0,
                     double cutoff = 1.0) {
        half_ = std::max(1, half_width);
        phases_ = std::max(1, phases);
        cutoff_ = std::isfinite(cutoff) ? std::clamp(cutoff, 1.0e-6, 1.0) : 1.0;
        const int taps = 2 * half_;
        // table_[phase][tap]; phase in [0, phases_] inclusive (the extra row
        // lets read() linearly interpolate up to phase == phases_).
        table_.assign(static_cast<size_t>(phases_ + 1) * taps, SampleType{0.0f});
        build_beta_ = beta;
        build_i0_beta_ = bessel_i0(beta);
        next_build_phase_ = 0;
    }
 
    bool build_step() {
        if (next_build_phase_ > phases_)
            return true;
        const int ph = next_build_phase_;
        const int taps = 2 * half_;
        const double frac = static_cast<double>(ph) / phases_;
        double sum = 0.0;
        for (int t = 0; t < taps; ++t) {
            const double x = static_cast<double>(t - half_ + 1) - frac;
            const double s = cutoff_ * sinc(cutoff_ * x);
            const double wn = x / half_;
            const double w =
                (wn > -1.0 && wn < 1.0)
                    ? bessel_i0(build_beta_ * std::sqrt(1.0 - wn * wn)) / build_i0_beta_
                    : 0.0;
            table_[static_cast<size_t>(ph) * taps + t] = static_cast<SampleType>(s * w);
            sum += s * w;
        }
        if (std::abs(sum) > 1.0e-12)
            for (int t = 0; t < taps; ++t)
                table_[static_cast<size_t>(ph) * taps + t] /= static_cast<SampleType>(sum);
        ++next_build_phase_;
        return next_build_phase_ > phases_;
    }
 
    int half_width() const {
        return half_;
    }
    int taps() const {
        return 2 * half_;
    }
    bool ready() const {
        return !table_.empty() && next_build_phase_ > phases_;
    }
    double cutoff() const {
        return cutoff_;
    }
    std::size_t prepared_bytes() const noexcept {
        return table_.capacity() * sizeof(SampleType);
    }

    SampleType apply(const SampleType* samples, double frac) const {
        const int taps = 2 * half_;
        const double ph = frac * phases_;
        const int p0 = static_cast<int>(ph);
        const SampleType a = static_cast<SampleType>(ph - p0);
        const SampleType* row0 = table_.data() + static_cast<size_t>(p0) * taps;
        const SampleType* row1 = table_.data() + static_cast<size_t>(p0 + 1) * taps;
        SampleType acc = SampleType{0.0f};
        for (int t = 0; t < taps; ++t)
            acc += (row0[t] + a * (row1[t] - row0[t])) * samples[t];
        return acc;
    }

    SampleType read(const SampleType* src, int len, double pos) const {
        if (src == nullptr || len <= 0 || !ready() || !std::isfinite(pos))
            return SampleType{0};
        pos = std::clamp(pos, 0.0, static_cast<double>(len - 1));
        const int taps = 2 * half_;
        const long i0 = static_cast<long>(std::floor(pos));
        const double frac = pos - static_cast<double>(i0);
        // Phase table lookup with linear interpolation between rows.
        const double ph = frac * phases_;
        const int p0 = static_cast<int>(ph);
        const SampleType a = static_cast<SampleType>(ph - p0);
        const SampleType* row0 = table_.data() + static_cast<size_t>(p0) * taps;
        const SampleType* row1 = table_.data() + static_cast<size_t>(p0 + 1) * taps;
        SampleType acc = SampleType{0.0f};
        for (int t = 0; t < taps; ++t) {
            long idx = i0 + t - half_ + 1;
            if (idx < 0)
                idx = 0;
            else if (idx >= len)
                idx = len - 1;
            const SampleType k = row0[t] + a * (row1[t] - row0[t]);
            acc += k * src[idx];
        }
        return acc;
    }
 
  private:
    static double sinc(double x) {
        if (std::abs(x) < 1e-9)
            return 1.0;
        const double px = 3.14159265358979323846 * x;
        return std::sin(px) / px;
    }
    // Modified Bessel function of the first kind, order 0 (series).
    static double bessel_i0(double x) {
        double sum = 1.0, term = 1.0;
        const double half_x = x * 0.5;
        for (int k = 1; k < 40; ++k) {
            term *= (half_x / k) * (half_x / k);
            sum += term;
            if (term < 1e-12 * sum)
                break;
        }
        return sum;
    }
 
    int half_ = 0;
    int phases_ = 0;
    double cutoff_ = 1.0;
    std::vector<SampleType> table_;
    double build_beta_ = 9.0;
    double build_i0_beta_ = 1.0;
    int next_build_phase_ = 1;
};
 
using SincResampler = SincResamplerT<float>;
using SincResampler64 = SincResamplerT<double>;
 
} // namespace pulp::signal