Biquad.h

Go to the documentation of this file.

// DSPark -- Professional Audio DSP Framework
// Copyright (c) 2026 Cristian Moresi -- MIT License
 
#pragma once
 
#include <algorithm>
#include <array>
#include <cmath>
#include <numbers>
#include <span>
#include <atomic>
#include <cassert>
 
#include "AudioBuffer.h"
#include "DenormalGuard.h"
 
namespace dspark {
 
// ============================================================================
// BiquadCoeffs -- Coefficient storage + factory methods
// ============================================================================
 
struct alignas(32) BiquadCoeffs
{
    double b0 = 1.0, b1 = 0.0, b2 = 0.0;
    double a1 = 0.0, a2 = 0.0;
 
    // -- Factory methods (Audio EQ Cookbook) ----------------------------------
 
    [[nodiscard]] static BiquadCoeffs makeLowPass(double sampleRate, double freq, double Q = 0.7071067811865476) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha;
        return normalise(a0,
            (1.0 - cosw0) / 2.0,
             1.0 - cosw0,
            (1.0 - cosw0) / 2.0,
            -2.0 * cosw0,
             1.0 - alpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makeHighPass(double sampleRate, double freq, double Q = 0.7071067811865476) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha;
        return normalise(a0,
             (1.0 + cosw0) / 2.0,
            -(1.0 + cosw0),
             (1.0 + cosw0) / 2.0,
            -2.0 * cosw0,
              1.0 - alpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makeBandPass(double sampleRate, double freq, double Q = 0.7071067811865476) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha;
        return normalise(a0,
            alpha,
            0.0,
            -alpha,
            -2.0 * cosw0,
             1.0 - alpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makePeak(double sampleRate, double freq, double Q, double gainDb) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double A     = std::pow(10.0, gainDb / 40.0); // dB/40 = sqrt of linear gain
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha / A;
        return normalise(a0,
            1.0 + alpha * A,
            -2.0 * cosw0,
            1.0 - alpha * A,
            -2.0 * cosw0,
            1.0 - alpha / A);
    }
 
    [[nodiscard]] static BiquadCoeffs makePeakMatched(double sampleRate, double freq,
                                                      double Q, double gainDb) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.495));
        Q = std::max(Q, 0.001);
        if (std::abs(gainDb) < 0.01)
            return { 1.0, 0.0, 0.0, 0.0, 0.0 };
 
        const double G0 = 1.0;                                  // reference gain
        const double G = std::pow(10.0, gainDb / 20.0);         // peak gain
        const double GB = std::pow(10.0, gainDb / 40.0);        // bandwidth gain (half-dB)
 
        const double w0 = 2.0 * std::numbers::pi * freq / sampleRate;
        const double Dw = w0 / Q;
 
        // Analog prototype gain at the physical Nyquist frequency:
        // |Ha(jW)|^2 = (G0^2 (W^2-W0^2)^2 + G^2 Dw^2 W^2) /
        //              (    (W^2-W0^2)^2 +      Dw^2 W^2),  W in rad/s.
        const double W0 = 2.0 * std::numbers::pi * freq;
        const double DW = W0 / Q;
        const double Wn = std::numbers::pi * sampleRate;        // 2*pi*fs/2
        const double d2 = (Wn * Wn - W0 * W0) * (Wn * Wn - W0 * W0);
        const double G1 = std::sqrt((G0 * G0 * d2 + G * G * DW * DW * Wn * Wn)
                                    / (d2 + DW * DW * Wn * Wn));
 
        // Orfanidis closed-form coefficients.
        const double G2 = G * G, G02 = G0 * G0, GB2 = GB * GB, G12 = G1 * G1;
        const double F   = std::abs(G2 - GB2);
        const double G00 = std::abs(G2 - G02);
        const double F00 = std::abs(GB2 - G02);
        const double F01 = std::abs(GB2 - G0 * G1);
        const double F11 = std::abs(GB2 - G12);
        const double G01 = std::abs(G2 - G0 * G1);
        const double G11 = std::abs(G2 - G12);
 
        const double t0 = std::tan(w0 / 2.0);
        const double W2 = std::sqrt(G11 / G00) * t0 * t0;
        const double DWd = (1.0 + std::sqrt(F00 / F11) * W2) * std::tan(Dw / 2.0);
 
        const double C = F11 * DWd * DWd - 2.0 * W2 * (F01 - std::sqrt(F00 * F11));
        const double D = 2.0 * W2 * (G01 - std::sqrt(G00 * G11));
        const double A = std::sqrt(std::max((C + D) / std::max(F, 1e-30), 0.0));
        const double B = std::sqrt(std::max((G2 * C + GB2 * D) / std::max(F, 1e-30), 0.0));
 
        const double a0 = 1.0 + W2 + A;
        return normalise(a0,
            G1 + G0 * W2 + B,
            -2.0 * (G1 - G0 * W2),
            G1 + G0 * W2 - B,
            -2.0 * (1.0 - W2),
            1.0 + W2 - A);
    }
 
    [[nodiscard]] static BiquadCoeffs makeLowShelf(double sampleRate, double freq, double gainDb, double slope = 1.0) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        // Shelf slope S must stay in (0, 1]: for S > 1 the radicand below goes
        // negative and std::sqrt yields NaN coefficients that poison the audio.
        slope = std::clamp(slope, 0.0001, 1.0);
        const double A     = std::pow(10.0, gainDb / 40.0);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double radicand = (A + 1.0 / A) * (1.0 / slope - 1.0) + 2.0;
        const double alpha = sinw0 / 2.0 * std::sqrt(std::max(0.0, radicand));
        const double twoSqrtAAlpha = 2.0 * std::sqrt(A) * alpha;
 
        const double a0 = (A + 1.0) + (A - 1.0) * cosw0 + twoSqrtAAlpha;
        return normalise(a0,
            A * ((A + 1.0) - (A - 1.0) * cosw0 + twoSqrtAAlpha),
            2.0 * A * ((A - 1.0) - (A + 1.0) * cosw0),
            A * ((A + 1.0) - (A - 1.0) * cosw0 - twoSqrtAAlpha),
            -2.0 * ((A - 1.0) + (A + 1.0) * cosw0),
            (A + 1.0) + (A - 1.0) * cosw0 - twoSqrtAAlpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makeHighShelf(double sampleRate, double freq, double gainDb, double slope = 1.0) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        // Shelf slope S must stay in (0, 1]: for S > 1 the radicand below goes
        // negative and std::sqrt yields NaN coefficients that poison the audio.
        slope = std::clamp(slope, 0.0001, 1.0);
        const double A     = std::pow(10.0, gainDb / 40.0);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double radicand = (A + 1.0 / A) * (1.0 / slope - 1.0) + 2.0;
        const double alpha = sinw0 / 2.0 * std::sqrt(std::max(0.0, radicand));
        const double twoSqrtAAlpha = 2.0 * std::sqrt(A) * alpha;
 
        const double a0 = (A + 1.0) - (A - 1.0) * cosw0 + twoSqrtAAlpha;
        return normalise(a0,
            A * ((A + 1.0) + (A - 1.0) * cosw0 + twoSqrtAAlpha),
            -2.0 * A * ((A - 1.0) + (A + 1.0) * cosw0),
            A * ((A + 1.0) + (A - 1.0) * cosw0 - twoSqrtAAlpha),
            2.0 * ((A - 1.0) - (A + 1.0) * cosw0),
            (A + 1.0) - (A - 1.0) * cosw0 - twoSqrtAAlpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makeNotch(double sampleRate, double freq, double Q = 0.7071067811865476) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha;
        return normalise(a0,
            1.0,
            -2.0 * cosw0,
            1.0,
            -2.0 * cosw0,
            1.0 - alpha);
    }
 
    [[nodiscard]] static BiquadCoeffs makeAllPass(double sampleRate, double freq, double Q = 0.7071067811865476) noexcept
    {
        freq = std::clamp(freq, 1.0, std::max(1.0, sampleRate * 0.499));
        Q = std::max(Q, 0.001);
        const double w0    = 2.0 * std::numbers::pi * freq / sampleRate;
        const double cosw0 = std::cos(w0);
        const double sinw0 = std::sin(w0);
        const double alpha = sinw0 / (2.0 * Q);
 
        const double a0 = 1.0 + alpha;
        return normalise(a0,
            1.0 - alpha,
            -2.0 * cosw0,
            1.0 + alpha,
            -2.0 * cosw0,
            1.0 - alpha);
    }
 
    // -- First-order filter factory methods ------------------------------------
 
    [[nodiscard]] static BiquadCoeffs makeFirstOrderLowPass(double sampleRate, double frequency) noexcept
    {
        frequency = std::clamp(frequency, 1.0, std::max(1.0, sampleRate * 0.499));
        const double w = std::tan(std::numbers::pi * frequency / sampleRate);
        const double n = 1.0 / (1.0 + w);
        BiquadCoeffs c;
        c.b0 = w * n;
        c.b1 = w * n;
        c.b2 = 0.0;
        c.a1 = (w - 1.0) * n;
        c.a2 = 0.0;
        return c;
    }
 
    [[nodiscard]] static BiquadCoeffs makeFirstOrderHighPass(double sampleRate, double frequency) noexcept
    {
        frequency = std::clamp(frequency, 1.0, std::max(1.0, sampleRate * 0.499));
        const double w = std::tan(std::numbers::pi * frequency / sampleRate);
        const double n = 1.0 / (1.0 + w);
        BiquadCoeffs c;
        c.b0 = n;
        c.b1 = -n;
        c.b2 = 0.0;
        c.a1 = (w - 1.0) * n;
        c.a2 = 0.0;
        return c;
    }
 
    [[nodiscard]] static BiquadCoeffs makeTilt(double sampleRate, double pivotFreq, double gainDb) noexcept
    {
        pivotFreq = std::clamp(pivotFreq, 1.0, std::max(1.0, sampleRate * 0.499));
        const double g = std::pow(10.0, gainDb / 20.0);
        const double sqrtG = std::sqrt(g);
        const double c = std::tan(std::numbers::pi * pivotFreq / sampleRate);
 
        // First-order tilt shelf via bilinear transform:
        //   DC gain = 1/sqrt(g), pivot gain = 1, Nyquist gain = sqrt(g)
        //   Total swing = gainDb (half below pivot, half above).
        const double norm = 1.0 / (1.0 + sqrtG * c);
 
        BiquadCoeffs coeffs;
        coeffs.b0 = (sqrtG + c) * norm;
        coeffs.b1 = (c - sqrtG) * norm;
        coeffs.b2 = 0.0;
        coeffs.a1 = (sqrtG * c - 1.0) * norm;
        coeffs.a2 = 0.0;
        return coeffs;
    }
 
    // -- Frequency response analysis -------------------------------------------
 
    [[nodiscard]] double getMagnitude(double frequency, double sampleRate) const noexcept
    {
        const double w = 2.0 * std::numbers::pi * frequency / sampleRate;
        const double cosW  = std::cos(w);
        const double cos2W = std::cos(2.0 * w);
        const double sinW  = std::sin(w);
        const double sin2W = std::sin(2.0 * w);
 
        const double nRe = b0 + b1 * cosW + b2 * cos2W;
        const double nIm = -b1 * sinW - b2 * sin2W;
        const double dRe = 1.0 + a1 * cosW + a2 * cos2W;
        const double dIm = -a1 * sinW - a2 * sin2W;
 
        const double numMag2 = nRe * nRe + nIm * nIm;
        const double denMag2 = dRe * dRe + dIm * dIm;
 
        return (denMag2 > 1e-30) ? std::sqrt(numMag2 / denMag2) : 0.0;
    }
 
    template <typename U>
    void getMagnitudeForFrequencyArray(std::span<const U> frequencies,
                                       std::span<U> magnitudes,
                                       double sampleRate) const noexcept
    {
        assert(magnitudes.size() >= frequencies.size());
        for (size_t i = 0; i < frequencies.size(); ++i)
            magnitudes[i] = static_cast<U>(getMagnitude(static_cast<double>(frequencies[i]), sampleRate));
    }
 
private:
    [[nodiscard]] static BiquadCoeffs normalise(double a0, double b0r, double b1r, double b2r,
                                                double a1r, double a2r) noexcept
    {
        const double invA0 = 1.0 / a0;
        return { b0r * invA0, b1r * invA0, b2r * invA0, a1r * invA0, a2r * invA0 };
    }
};
 
// ============================================================================
// Biquad -- Filter processor with per-channel state
// ============================================================================
 
template <typename T, int MaxChannels = 8>
class alignas(32) Biquad
{
public:
    Biquad() noexcept = default;
 
    // std::atomic<bool> makes the default copy/move ops vanish. Provide manual
    // move semantics so this class can live inside std::array / std::vector
    // (e.g. CrossoverFilter::AllPassChain). Copying is left deleted on
    // purpose: it would imply two threads observing the same staged coefficient
    // dirty flag, which is semantically ambiguous and not needed in practice.
    // Moves only happen during setup (single-threaded relocation into a
    // container), so the seqlock counter is simply reset to an even value.
    Biquad(Biquad&& other) noexcept
        : activeCoeffs_(other.activeCoeffs_),
          stagedCoeffs_(other.stagedCoeffs_),
          coeffsDirty_(other.coeffsDirty_.load(std::memory_order_relaxed)),
          coeffsSeq_(0),
          state_(other.state_)
    {}
 
    Biquad& operator=(Biquad&& other) noexcept
    {
        if (this == &other) return *this;
        activeCoeffs_ = other.activeCoeffs_;
        stagedCoeffs_ = other.stagedCoeffs_;
        coeffsDirty_.store(other.coeffsDirty_.load(std::memory_order_relaxed),
                           std::memory_order_relaxed);
        coeffsSeq_.store(0, std::memory_order_relaxed);
        state_ = other.state_;
        return *this;
    }
 
    Biquad(const Biquad&)            = delete;
    Biquad& operator=(const Biquad&) = delete;
 
    void setCoeffs(const BiquadCoeffs& c) noexcept
    {
        // Seqlock publish (single producer). An odd sequence number marks a
        // write in progress; the audio thread retries its read while odd or if
        // the counter moved, so a concurrent update can never be observed as a
        // torn coefficient set (mixing a1 of one filter with a2 of another).
        //
        // Standards note: the reader's struct copy races with this write in the
        // strict C++ memory model (classic seqlock caveat; the Linux kernel and
        // mainstream audio frameworks rely on the same pattern). The retry loop
        // discards any value read during a write, and the acquire/release fences
        // order the accesses on every supported compiler/architecture.
        coeffsSeq_.fetch_add(1, std::memory_order_acq_rel);   // -> odd
        stagedCoeffs_ = c;
        coeffsSeq_.fetch_add(1, std::memory_order_release);   // -> even
        coeffsDirty_.store(true, std::memory_order_release);
    }
 
    bool applyPendingCoeffs() noexcept
    {
        if (coeffsDirty_.exchange(false, std::memory_order_acquire))
        {
            // Seqlock read: retry on a torn/in-progress publish. The writer's
            // critical section is a 5-double copy, so this converges in at most
            // a couple of iterations even under a busy GUI thread.
            BiquadCoeffs tmp;
            unsigned s0, s1;
            do {
                s0  = coeffsSeq_.load(std::memory_order_acquire);
                tmp = stagedCoeffs_;
                // The fence keeps the copy above from sinking below the
                // re-read of the counter. A plain acquire load only orders
                // LATER accesses; without the fence, both the compiler and
                // weakly-ordered CPUs (ARM) may complete the copy after s1
                // is read, and a torn copy would pass the s0 == s1 check.
                std::atomic_thread_fence(std::memory_order_acquire);
                s1  = coeffsSeq_.load(std::memory_order_relaxed);
            } while ((s0 & 1u) != 0u || s0 != s1);
            activeCoeffs_ = tmp;
            return true;
        }
        return false;
    }
 
    [[nodiscard]] const BiquadCoeffs& getCoeffs() const noexcept { return activeCoeffs_; }
 
    void reset() noexcept
    {
        for (auto& s : state_)
            s = {};
    }
 
    T processSample(T input, int channel) noexcept
    {
        return static_cast<T>(processSampleCore(static_cast<double>(input), channel));
    }
 
    double processSampleCore(double input, int channel) noexcept
    {
        assert(channel >= 0 && channel < MaxChannels && "Channel index out of bounds");
 
        // Lock-free fast path: relaxed load is free on every modern CPU.
        // Branch is marked unlikely so the compiler keeps the hot DSP path
        // straight-line; the dirty flag is only true on the first sample
        // of a block where setCoeffs() landed since last process call.
        if (coeffsDirty_.load(std::memory_order_relaxed)) [[unlikely]]
            applyPendingCoeffs();
 
        auto& s = state_[channel];
 
        const double output = activeCoeffs_.b0 * input + s.z1;
        s.z1 = activeCoeffs_.b1 * input - activeCoeffs_.a1 * output + s.z2;
        s.z2 = activeCoeffs_.b2 * input - activeCoeffs_.a2 * output;
 
        return output;
    }
 
    void processBlock(AudioBufferView<T> buffer) noexcept
    {
        DenormalGuard guard;
 
        // Pick up any pending lock-free coefficient update from the GUI thread.
        applyPendingCoeffs();
 
        const int numChannels = std::min(buffer.getNumChannels(), MaxChannels);
        const int numSamples  = buffer.getNumSamples();
 
        // Block-local coefficient copy: stable for the whole block by design
        // (updates land at the next block boundary), and register-friendly.
        const BiquadCoeffs c = activeCoeffs_;
 
        for (int ch = 0; ch < numChannels; ++ch)
        {
            T* data = buffer.getChannel(ch);
            auto& s = state_[ch];
            double z1 = s.z1;
            double z2 = s.z2;
 
            for (int i = 0; i < numSamples; ++i)
            {
                const double input  = static_cast<double>(data[i]);
                const double output = c.b0 * input + z1;
                z1 = c.b1 * input - c.a1 * output + z2;
                z2 = c.b2 * input - c.a2 * output;
                data[i] = static_cast<T>(output);
            }
 
            s.z1 = z1;
            s.z2 = z2;
        }
    }
 
private:
    // Kept compact on purpose (no over-alignment): the states are only ever
    // touched by the processing thread, sequentially per channel, so packing
    // adjacent channels into the same cache line is strictly better than
    // padding each one out to its own.
    struct State
    {
        double z1 = 0.0;
        double z2 = 0.0;
    };
 
    BiquadCoeffs activeCoeffs_ {};
    BiquadCoeffs stagedCoeffs_ {};
    std::atomic<bool> coeffsDirty_{false};
    std::atomic<unsigned> coeffsSeq_{0}; 
 
    std::array<State, MaxChannels> state_ {};
};
 
} // namespace dspark