resampler.hpp

Go to the documentation of this file.

#pragma once

 
#include <pulp/signal/mirrored_history_buffer.hpp>
#include <pulp/signal/windowed_sinc_design.hpp>
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <vector>
 
namespace pulp::signal {
 
// ────────────────────────────────────────────────────────────────────────────
// Polyphase-FIR resampler with arbitrary ratio.
// ────────────────────────────────────────────────────────────────────────────

struct ResamplerQuality {
    double stopband_db = 96.0;          
    double transition_fraction = 0.10;  
    double cutoff_fraction = 0.90;      
    std::size_t phases = 64;            
};
 
struct ResamplerProcessResult {
    std::size_t output_frames = 0;
    std::size_t input_frames_consumed = 0;
};
 
template <typename SampleType = float>
class ResamplerT {
public:
    ResamplerT() = default;

    void prepare(double input_rate,
                 double output_rate,
                 std::size_t channels,
                 std::size_t max_block_size,
                 ResamplerQuality quality = {}) {
        input_rate_ = input_rate;
        output_rate_ = output_rate;
        channels_ = (channels == 0) ? 1u : channels;
        quality_ = quality;
        if (quality_.phases < 2u) quality_.phases = 2u;
 
        // Design the prototype LP at the polyphase sample rate
        // (= L × input_rate). When each phase is applied to the input
        // delay line, the effective filter response — in input-rate-
        // normalized terms — is the prototype's response scaled by L
        // along the frequency axis (phase p contains every Lth tap of
        // the prototype). To reject everything above min(in_nyq, out_nyq),
        // the cutoff must sit below min(in_nyq, out_nyq) in continuous Hz
        // terms; we then normalize to the polyphase rate.
        const double min_rate = std::min(input_rate_, output_rate_);
        const double min_nyq = 0.5 * min_rate;
        const double cutoff_hz = quality_.cutoff_fraction * min_nyq;
        const double transition_hz = quality_.transition_fraction * min_nyq;
 
        // Polyphase rate is L × input_rate so the L-fold spacing of
        // each phase places the effective cutoff at `fc_norm * L` in
        // input-rate-normalized units, which equals `cutoff_hz / input_rate`.
        const double poly_rate = static_cast<double>(quality_.phases) * input_rate_;
        const double fc_norm = cutoff_hz / poly_rate;
        const double df_norm = transition_hz / poly_rate;
 
        const double beta = kaiser_beta_for_stopband(quality_.stopband_db);
        std::size_t n_taps = kaiser_length_for_transition(quality_.stopband_db, df_norm);
 
        // Hard-cap the prototype length. The tap count grows with the rate ratio, so
        // an extreme input:output ratio (e.g. a hostile IR header) would otherwise
        // demand a multi-GB prototype and minutes of design work. A real audio
        // resample needs only a few hundred taps; this ceiling is orders of
        // magnitude above that, so it only ever clamps a pathological ratio —
        // trading a slightly wider transition band for a bounded allocation.
        constexpr std::size_t kMaxProtoTaps = 1u << 16;   // 65536
        if (n_taps > kMaxProtoTaps) n_taps = kMaxProtoTaps;
 
        // Force n_taps to be `L * taps_per_phase` so the polyphase
        // decomposition is rectangular.
        const std::size_t L = quality_.phases;
        const std::size_t taps_per_phase = (n_taps + L - 1u) / L;
        n_taps = taps_per_phase * L;
 
        prototype_ = design_windowed_sinc(n_taps, fc_norm, beta);
 
        // Scale by L so each phase has unit DC gain after polyphase split.
        for (auto& v : prototype_) v *= static_cast<double>(L);
 
        // Slice into L phases of `taps_per_phase` each. Phase p contains
        // prototype_[p, p+L, p+2L, ...]. Each phase is stored with its taps
        // REVERSED (index k holds tap `taps-1-k`): the convolution reads the
        // most-recent-`taps` window newest-last, so reversing the phase table
        // here lets the hot loop walk the window and the coefficients in the
        // same ascending direction — straight vector loads, no reverse shuffle.
        phases_.assign(L, std::vector<SampleType>(taps_per_phase, SampleType{0.0f}));
        for (std::size_t p = 0; p < L; ++p) {
            for (std::size_t k = 0; k < taps_per_phase; ++k) {
                phases_[p][k] =
                    static_cast<SampleType>(prototype_[(taps_per_phase - 1u - k) * L + p]);
            }
        }
        taps_per_phase_ = taps_per_phase;
 
        // Each channel owns a fixed-capacity mirrored history, so the complete
        // oldest-to-newest tap span stays contiguous at every write position.
        delays_.clear();
        delays_.resize(channels_);
        for (auto& delay : delays_) delay.prepare(taps_per_phase);
 
        // Output scratch — worst-case output count for a max-size input block.
        const double ratio = output_rate_ / input_rate_;
        const std::size_t worst_out =
            static_cast<std::size_t>(std::ceil(static_cast<double>(max_block_size) * ratio)) + 8u;
        scratch_out_.assign(channels_, std::vector<SampleType>(worst_out,
                                                              SampleType{0.0f}));
 
        // Step (input samples per output sample) in the input domain.
        step_ = input_rate_ / output_rate_;
        phase_acc_ = 0.0;
    }

    void set_ratio(double input_rate, double output_rate) {
        if (input_rate <= 0.0 || output_rate <= 0.0) return;
        input_rate_ = input_rate;
        output_rate_ = output_rate;
        step_ = input_rate_ / output_rate_;
    }

    void reset() {
        for (auto& delay : delays_) delay.reset();
        phase_acc_ = 0.0;
    }

    ResamplerProcessResult process_block_detailed(const SampleType* const* input,
                                                  std::size_t in_count,
                                                  SampleType* const* output,
                                                  std::size_t out_capacity) {
        if (channels_ == 0 || taps_per_phase_ == 0 || phases_.empty()) return {};
        const std::size_t L = phases_.size();
        const double L_d = static_cast<double>(L);
 
        std::size_t out_n = 0;
        std::size_t input_consumed = 0;
 
        // Process: for each output sample we need, check whether the
        // phase accumulator has crossed an integer (meaning we must
        // push the next input sample into the delay line) and then
        // evaluate the polyphase filter at the fractional position.
        while (out_n < out_capacity) {
            // Push input samples until the accumulator is below 1.0.
            while (phase_acc_ >= 1.0) {
                if (input_consumed >= in_count) {
                    // Ran out of input — return what we have.
                    return {out_n, input_consumed};
                }
                for (std::size_t c = 0; c < channels_; ++c) {
                    delays_[c].push(input[c][input_consumed]);
                }
                ++input_consumed;
                phase_acc_ -= 1.0;
            }
 
            // We need at least one input sample in the line. On the
            // very first call, the delay is zeroed which still yields
            // a valid (silence) output until input arrives.
            const double phase_pos = phase_acc_ * L_d;     // in [0, L)
            std::size_t p0 = static_cast<std::size_t>(phase_pos);
            if (p0 >= L) p0 = L - 1u;
            std::size_t p1 = p0 + 1u;
            double frac = phase_pos - static_cast<double>(p0);
            if (p1 >= L) { p1 = p0; frac = 0.0; }
 
            for (std::size_t c = 0; c < channels_; ++c) {
                const auto window = delays_[c].window();
                const auto& ph0 = phases_[p0];
                const auto& ph1 = phases_[p1];
 
                // Convolve the contiguous window against the (reversed) phase
                // coefficients: y = sum_k win[k] * ph[k]. The mirror write makes
                // `win[0 .. taps-1]` the physical most-recent-`taps` span oldest→
                // newest, and the phase table is stored reversed to match, so both
                // walk ascending with straight vector loads.
                //
                // Four independent single-precision lane accumulators let the
                // compiler fold the reduction into SIMD FMAs. This reassociates the
                // sum (and drops the double accumulator), so the result differs from
                // a strict left-to-right double sum in the last few ulps — but the
                // error sits ~69 dB below the filter's stopband floor (the
                // "float reassociation stays below the stopband" test asserts it),
                // far under any audible or spec-relevant level.
                const SampleType* win = window.data();
                const std::size_t T = taps_per_phase_;
                SampleType a0 = SampleType{0.0f};
                SampleType a1 = SampleType{0.0f};
                SampleType a2 = SampleType{0.0f};
                SampleType a3 = SampleType{0.0f};
                std::size_t k = 0;
                if (frac != 0.0) {
                    SampleType b0 = SampleType{0.0f};
                    SampleType b1 = SampleType{0.0f};
                    SampleType b2 = SampleType{0.0f};
                    SampleType b3 = SampleType{0.0f};
                    for (; k + 4u <= T; k += 4u) {
                        a0 += win[k]      * ph0[k];      b0 += win[k]      * ph1[k];
                        a1 += win[k + 1u] * ph0[k + 1u]; b1 += win[k + 1u] * ph1[k + 1u];
                        a2 += win[k + 2u] * ph0[k + 2u]; b2 += win[k + 2u] * ph1[k + 2u];
                        a3 += win[k + 3u] * ph0[k + 3u]; b3 += win[k + 3u] * ph1[k + 3u];
                    }
                    SampleType acc0 = (a0 + a1) + (a2 + a3);
                    SampleType acc1 = (b0 + b1) + (b2 + b3);
                    for (; k < T; ++k) { acc0 += win[k] * ph0[k]; acc1 += win[k] * ph1[k]; }
                    output[c][out_n] = acc0 + static_cast<SampleType>(frac) * (acc1 - acc0);
                } else {
                    // Integer phase position (e.g. integer resample ratios): the
                    // second phase is weighted by frac == 0, so it never reaches the
                    // output — skip the redundant second MAC. y == acc0 exactly.
                    for (; k + 4u <= T; k += 4u) {
                        a0 += win[k]      * ph0[k];
                        a1 += win[k + 1u] * ph0[k + 1u];
                        a2 += win[k + 2u] * ph0[k + 2u];
                        a3 += win[k + 3u] * ph0[k + 3u];
                    }
                    SampleType acc0 = (a0 + a1) + (a2 + a3);
                    for (; k < T; ++k) acc0 += win[k] * ph0[k];
                    output[c][out_n] = acc0;
                }
            }
 
            ++out_n;
            phase_acc_ += step_;
        }
        return {out_n, input_consumed};
    }
 
    std::size_t process_block(const SampleType* const* input,
                              std::size_t in_count,
                              SampleType* const* output,
                              std::size_t out_capacity) {
        return process_block_detailed(input, in_count, output, out_capacity).output_frames;
    }
 
    ResamplerProcessResult process_block_mono_detailed(const SampleType* input,
                                                       std::size_t in_count,
                                                       SampleType* output,
                                                       std::size_t out_capacity) {
        const SampleType* in_ptrs[1] = { input };
        SampleType* out_ptrs[1] = { output };
        return process_block_detailed(in_ptrs, in_count, out_ptrs, out_capacity);
    }

    std::size_t process_block_mono(const SampleType* input,
                                   std::size_t in_count,
                                   SampleType* output,
                                   std::size_t out_capacity) {
        return process_block_mono_detailed(input, in_count, output, out_capacity).output_frames;
    }

    std::size_t max_output_for(std::size_t in_count) const {
        const double ratio = output_rate_ / input_rate_;
        return static_cast<std::size_t>(std::ceil(static_cast<double>(in_count) * ratio)) + 8u;
    }

    std::size_t taps_per_phase() const { return taps_per_phase_; }

    std::size_t phases() const { return phases_.size(); }

    std::size_t prototype_length() const { return prototype_.size(); }
 
    double input_rate() const { return input_rate_; }
    double output_rate() const { return output_rate_; }
    std::size_t channels() const { return channels_; }
 
private:
    double input_rate_ = 0.0;
    double output_rate_ = 0.0;
    std::size_t channels_ = 0;
    ResamplerQuality quality_{};
 
    // Prototype LP (length L * taps_per_phase). Kept around so tests
    // can introspect it. Phases are the runtime representation.
    std::vector<double> prototype_;
    std::vector<std::vector<SampleType>> phases_;
    std::size_t taps_per_phase_ = 0;
 
    // Per-channel contiguous oldest-to-newest sample history.
    std::vector<MirroredHistoryBuffer<SampleType>> delays_;
 
    // Per-channel scratch — not used directly by process_block but
    // kept available so future block-process variants can drain into
    // it without allocating.
    std::vector<std::vector<SampleType>> scratch_out_;
 
    // Phase accumulator in input samples per output sample.
    double step_ = 1.0;
    double phase_acc_ = 0.0;
};
 
using Resampler = ResamplerT<float>;
using Resampler64 = ResamplerT<double>;
 
} // namespace pulp::signal