1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
|
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;
/// <summary>
/// Apply a single biquad filter.
/// </summary>
/// <remarks>This is implemented with a direct form 2.</remarks>
/// <param name="parameter">The biquad filter parameter</param>
/// <param name="state">The biquad filter state</param>
/// <param name="outputBuffer">The output buffer to write the result</param>
/// <param name="inputBuffer">The input buffer to write the result</param>
/// <param name="sampleCount">The count of samples to process</param>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void ProcessBiquadFilter(ref BiquadFilterParameter parameter, ref BiquadFilterState state, Span<float> outputBuffer, ReadOnlySpan<float> 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;
}
}
/// <summary>
/// Apply multiple biquad filter.
/// </summary>
/// <remarks>This is implemented with a direct form 1.</remarks>
/// <param name="parameters">The biquad filter parameter</param>
/// <param name="states">The biquad filter state</param>
/// <param name="outputBuffer">The output buffer to write the result</param>
/// <param name="inputBuffer">The input buffer to write the result</param>
/// <param name="sampleCount">The count of samples to process</param>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static void ProcessBiquadFilter(ReadOnlySpan<BiquadFilterParameter> parameters, Span<BiquadFilterState> states, Span<float> outputBuffer, ReadOnlySpan<float> 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;
}
}
}
}
}
|
