BiQuad audio filter implementation in C#

BiQuad.cs

using System.Runtime.Intrinsics;

/// <summary>
/// Implements a BiQuad function for use in audio signal EQ processing.
/// </summary>
/// <remarks>
/// Adapted from:
/// https://webaudio.github.io/Audio-EQ-Cookbook/Audio-EQ-Cookbook.txt
/// </remarks>
public class BiQuad
{
    /// <summary>
    /// Contains preset setups for various audio filter types.
    /// </summary>
    /// <param name="Mult">
    /// A 128-bit register that contains four float values representing the coefficients
    /// of the filter function in this order: { b1, b2, -a1, -a2 }
    /// </param>
    /// <param name="Bi">
    /// Precalculated result of (b0 / a0).
    /// </param>
    private readonly record struct FilterCoefficients(
        Vector128<float> Mult,
        float Bi
    )
    {
        /// <summary>
        /// Contains setup values used to further calculate filter coefficients.
        /// See the referenced paper above for more information about what these
        /// represent.
        /// </summary>
        private struct FilterSetup
        {
            public float A;
            public float W0;
            public float SinW0;
            public float CosW0;
            public float Alpha;
            public float Shelf;
        }

        /// <summary>
        /// Setup function for all filter types except shelving EQ.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="dbGain">
        /// dB of gain to apply for shelving or peaking EQ filter types.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        private static FilterSetup Setup(float fs, float f0, float dbGain, float q)
        {
            var result = new FilterSetup
            {
                A = MathF.Sqrt(MathF.Pow(10, dbGain / 20)),
                W0 = 2f * MathF.PI * f0 / fs
            };

            (result.SinW0, result.CosW0) = MathF.SinCos(result.W0);
            result.Alpha = result.SinW0 / (2f * q);

            return result;
        }

        /// <summary>
        /// Setup function for all filter types except shelving EQ.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="dbGain">
        /// dB of gain to apply for shelving or peaking EQ filter types.
        /// </param>
        /// <param name="s">
        /// Slope steepness around the cutoff frequency.
        /// </param>
        private static FilterSetup SetupShelf(float fs, float f0, float dbGain, float s)
        {
            var result = new FilterSetup
            {
                A = MathF.Sqrt(MathF.Pow(10, dbGain / 40)),
                W0 = 2f * MathF.PI * f0 / fs
            };

            (result.SinW0, result.CosW0) = MathF.SinCos(result.W0);
            result.Alpha = result.SinW0 / 2f *
                           MathF.Sqrt((result.A + 1f / result.A) * (1f / s - 1f) + 2f);
            result.Shelf = result.SinW0 *
                           MathF.Sqrt((MathF.Pow(result.A, 2) + 1f) * (1f / s - 1f) + 2f * result.A);

            return result;
        }

        /// <summary>
        /// Calculates coefficients for a low-pass filter.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Unused for this filter type.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateLowPass(float fs, float f0, float gain, float q)
        {
            var setup = Setup(fs, f0, gain, q);
            var cosW0 = setup.CosW0;
            var alpha = setup.Alpha;

            var b0 = (1f - cosW0) / 2f;
            var b1 = 1f - cosW0;
            var b2 = (1f - cosW0) / 2f;
            var a0 = 1f + alpha;
            var a1 = -2f * cosW0;
            var a2 = 1f - alpha;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for a high-pass filter.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Unused for this filter type.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateHighPass(float fs, float f0, float gain, float q)
        {
            var setup = Setup(fs, f0, gain, q);
            var cosW0 = setup.CosW0;
            var alpha = setup.Alpha;

            var b0 = (1f + cosW0) / 2f;
            var b1 = -(1f + cosW0);
            var b2 = (1f + cosW0) / 2f;
            var a0 = 1f + alpha;
            var a1 = -2f * cosW0;
            var a2 = 1f - alpha;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for a band-pass filter.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Unused for this filter type.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateBandPass(float fs, float f0, float gain, float q)
        {
            var setup = Setup(fs, f0, gain, q);
            var sinW0 = setup.SinW0;
            var alpha = setup.Alpha;
            var cosW0 = setup.CosW0;

            var b0 = sinW0 / 2f;
            const int b1 = 0;
            var b2 = -sinW0 / 2f;
            var a0 = 1 + alpha;
            var a1 = -2 * cosW0;
            var a2 = 1 - alpha;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for an all-pass filter.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Unused for this filter type.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateAllPass(float fs, float f0, float gain, float q)
        {
            var setup = Setup(fs, f0, gain, q);
            var alpha = setup.Alpha;
            var cosW0 = setup.CosW0;

            var b0 = 1f - alpha;
            var b1 = -2f * cosW0;
            var b2 = 1f + alpha;
            var a0 = 1f + alpha;
            var a1 = -2f * cosW0;
            var a2 = 1f - alpha;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for a peaking EQ.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// dB of gain to apply at the peak.
        /// </param>
        /// <param name="q">
        /// Resonance factor around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreatePeakingEq(float fs, float f0, float gain, float q)
        {
            var setup = SetupShelf(fs, f0, gain / 2f, q);
            var alpha = setup.Alpha;
            var a = setup.A;
            var cosW0 = setup.CosW0;

            var b0 = 1f + alpha * a;
            var b1 = -2f * cosW0;
            var b2 = 1f - alpha * a;
            var a0 = 1f + alpha / a;
            var a1 = -2f * cosW0;
            var a2 = 1f - alpha / a;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for a low-shelf EQ.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Maximum dB of gain to apply at the shelf.
        /// </param>
        /// <param name="s">
        /// Slope steepness around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateLowShelf(float fs, float f0, float gain, float s)
        {
            var setup = SetupShelf(fs, f0, gain, s);
            var a = setup.A;
            var cosW0 = setup.CosW0;
            var shelf = setup.Shelf;

            var b0 = a * (a + 1f - (a - 1f) * cosW0 + shelf);
            var b1 = 2f * a * (a - 1f - (a + 1f) * cosW0);
            var b2 = a * (a + 1f - (a - 1f) * cosW0 - shelf);
            var a0 = a + 1f + (a - 1f) * cosW0 + shelf;
            var a1 = -2f * (a - 1f + (a + 1f) * cosW0);
            var a2 = a + 1f + (a - 1f) * cosW0 - shelf;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }

        /// <summary>
        /// Calculates coefficients for a high-shelf EQ.
        /// </summary>
        /// <param name="fs">
        /// Sampling frequency in Hz.
        /// </param>
        /// <param name="f0">
        /// Cutoff frequency in Hz.
        /// </param>
        /// <param name="gain">
        /// Maximum dB of gain to apply at the shelf.
        /// </param>
        /// <param name="s">
        /// Slope steepness around the cutoff frequency.
        /// </param>
        public static FilterCoefficients CreateHighShelf(float fs, float f0, float gain, float s)
        {
            var setup = SetupShelf(fs, f0, gain, s);
            var a = setup.A;
            var cosW0 = setup.CosW0;
            var shelf = setup.Shelf;

            var b0 = a * (a + 1f + (a - 1f) * cosW0 + shelf);
            var b1 = -2f * a * (a - 1f + (a + 1f) * cosW0);
            var b2 = a * (a + 1f + (a - 1f) * cosW0 - shelf);
            var a0 = a + 1f - (a - 1f) * cosW0 + shelf;
            var a1 = 2f * (a - 1f - (a + 1f) * cosW0);
            var a2 = a + 1f - (a - 1f) * cosW0 - shelf;

            return new FilterCoefficients(
                Mult: Vector128.Create(b1, b2, -a1, -a2) / a0,
                Bi: b0 / a0
            );
        }
    }

    /// <summary>
    /// Stores calculated { b1, b2, -a1, -a2 } values.
    /// </summary>
    private Vector128<float> _filter;

    /// <summary>
    /// Stores calculated (b0 / a0) value.
    /// </summary>
    private float _inputCoefficient;

    private FilterType _filterType;

    /// <summary>
    /// Gets or sets which predefined filter function to use.
    /// </summary>
    public FilterType FilterType
    {
        get => _filterType;
        set
        {
            _filterType = value;
            Recalc();
        }
    }

    private float _q;

    /// <summary>
    /// For peaking and shelf EQs, determines the steepness of the slope at the cutoff frequency; lower values
    /// are more gradual across the frequency spectrum.
    /// For all other filter types, determines the resonance factor around the cutoff frequency;
    /// lower values have less of a narrow peak at the cutoff frequency.
    /// Values greater than 1 will exaggerate the signal at the cutoff frequency particularly.
    /// </summary>
    public float Q
    {
        get => _q;
        set
        {
            _q = value;
            Recalc();
        }
    }

    private float _cutoff;

    /// <summary>
    /// Determines the frequency at which the filter becomes effective.
    /// </summary>
    public float Cutoff
    {
        get => _cutoff;
        set
        {
            _cutoff = value;
            Recalc();
        }
    }

    private float _gain;

    /// <summary>
    /// For peaking and shelf EQs, determines the maximum amount of gain at the effective
    /// frequency range of the filter.
    /// </summary>
    public float Gain
    {
        get => _gain;
        set
        {
            _gain = value;
            Recalc();
        }
    }

    private int _samplingFrequency;

    /// <summary>
    /// Determines the sampling frequency of the input data.
    /// </summary>
    public int SamplingFrequency
    {
        get => _samplingFrequency;
        set
        {
            _samplingFrequency = value;
            Recalc();
        }
    }

    /// <summary>
    /// Recalculates the coefficients based on the configured properties of the filter.
    /// </summary>
    private void Recalc()
    {
        if (_samplingFrequency <= 0)
            return;
        
        var coefficients = _filterType switch
        {
            FilterType.None => default,
            FilterType.LowPass => FilterCoefficients.CreateLowPass(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.HighPass => FilterCoefficients.CreateHighPass(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.BandPass => FilterCoefficients.CreateBandPass(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.AllPass => FilterCoefficients.CreateAllPass(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.Peak => FilterCoefficients.CreatePeakingEq(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.LowShelf => FilterCoefficients.CreateLowShelf(_samplingFrequency, _cutoff, _gain, _q),
            FilterType.HighShelf => FilterCoefficients.CreateHighShelf(_samplingFrequency, _cutoff, _gain, _q),
            _ => throw new ArgumentOutOfRangeException()
        };

        _inputCoefficient = coefficients.Bi;
        _filter = coefficients.Mult;
    }

    /// <summary>
    /// Determines the size of the histogram array based on the number of audio channels.
    /// </summary>
    public static int GetHistSize(int channelCount) =>
        channelCount * 2;

    /// <summary>
    /// Apply the filter function to the specified buffer, using the specified histogram.
    /// </summary>
    /// <param name="source">
    /// Source audio data.
    /// </param>
    /// <param name="hist">
    /// The histogram data. This will be modified to reflect the most recent samples.
    /// </param>
    /// <param name="dest">
    /// Buffer to store the output audio data.
    /// </param>
    public void Process(
        ReadOnlySpan<float> source,
        Span<Vector128<float>> hist,
        Span<float> dest
    )
    {
        if (_filterType == FilterType.None)
            return;

        // Hack: infer the number of audio channels based on the size of the histogram
        
        var channelCount = hist.Length / 2;

        for (var channel = 0; channel < channelCount; channel++)
        {
            var prev = hist[channel];
            var input = source[channel..];
            var output = dest[channel..];

            while (input.Length > 0)
            {
                // per sample formula:
                // y[n] = (b0/a0)*x[n] + (b1/a0)*x[n-1] + (b2/a0)*x[n-2]
                //                     - (a1/a0)*y[n-1] - (a2/a0)*y[n-2]
                // precalculated:
                // _filter = { b1, b2, -a1, -a2 }
                // _inputCoefficient = b0 / a0
                // hist = { x[n-1], y[n-1], x[n-2], y[n-2] }

                var sample = _inputCoefficient * input[0] + Vector128.Sum(_filter * prev);
                prev = Vector128.Create(input[0], prev[0], sample, prev[2]);

                output[0] = sample;
                if (input.Length < channelCount)
                    break;
                input = input[channelCount..];
                output = output[channelCount..];
            }

            hist[channel] = prev;
        }
    }
}

public enum FilterType
{
    None,
    LowPass,
    HighPass,
    BandPass,
    AllPass,
    Peak,
    LowShelf,
    HighShelf,
}