using Ryujinx.Audio.Renderer.Dsp.State;
using Ryujinx.Audio.Renderer.Parameter;
using System;
using System.Runtime.CompilerServices;
namespace Ryujinx.Audio.Renderer.Dsp
{
public static class BiquadFilterHelper
{
private const int FixedPointPrecisionForParameter = 14;
///
/// Apply a single biquad filter.
///
/// This is implemented with a direct form 2.
/// The biquad filter parameter
/// The biquad filter state
/// The output buffer to write the result
/// The input buffer to write the result
/// The count of samples to process
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void ProcessBiquadFilter(ref BiquadFilterParameter parameter, ref BiquadFilterState state, Span outputBuffer, ReadOnlySpan inputBuffer, uint sampleCount)
{
float a0 = FixedPointHelper.ToFloat(parameter.Numerator[0], FixedPointPrecisionForParameter);
float a1 = FixedPointHelper.ToFloat(parameter.Numerator[1], FixedPointPrecisionForParameter);
float a2 = FixedPointHelper.ToFloat(parameter.Numerator[2], FixedPointPrecisionForParameter);
float b1 = FixedPointHelper.ToFloat(parameter.Denominator[0], FixedPointPrecisionForParameter);
float b2 = FixedPointHelper.ToFloat(parameter.Denominator[1], FixedPointPrecisionForParameter);
for (int i = 0; i < sampleCount; i++)
{
float input = inputBuffer[i];
float output = input * a0 + state.State0;
state.State0 = input * a1 + output * b1 + state.State1;
state.State1 = input * a2 + output * b2;
outputBuffer[i] = output;
}
}
///
/// Apply multiple biquad filter.
///
/// This is implemented with a direct form 1.
/// The biquad filter parameter
/// The biquad filter state
/// The output buffer to write the result
/// The input buffer to write the result
/// The count of samples to process
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void ProcessBiquadFilter(ReadOnlySpan parameters, Span states, Span outputBuffer, ReadOnlySpan inputBuffer, uint sampleCount)
{
for (int stageIndex = 0; stageIndex < parameters.Length; stageIndex++)
{
BiquadFilterParameter parameter = parameters[stageIndex];
ref BiquadFilterState state = ref states[stageIndex];
float a0 = FixedPointHelper.ToFloat(parameter.Numerator[0], FixedPointPrecisionForParameter);
float a1 = FixedPointHelper.ToFloat(parameter.Numerator[1], FixedPointPrecisionForParameter);
float a2 = FixedPointHelper.ToFloat(parameter.Numerator[2], FixedPointPrecisionForParameter);
float b1 = FixedPointHelper.ToFloat(parameter.Denominator[0], FixedPointPrecisionForParameter);
float b2 = FixedPointHelper.ToFloat(parameter.Denominator[1], FixedPointPrecisionForParameter);
for (int i = 0; i < sampleCount; i++)
{
float input = inputBuffer[i];
float output = input * a0 + state.State0 * a1 + state.State1 * a2 + state.State2 * b1 + state.State3 * b2;
state.State1 = state.State0;
state.State0 = input;
state.State3 = state.State2;
state.State2 = output;
outputBuffer[i] = output;
}
}
}
}
}