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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
#include "config.h"
#include "AL/alc.h"
#include "AL/al.h"
#include "alMain.h"
#include "defs.h"
extern inline void BiquadFilter_clear(BiquadFilter *filter);
extern inline void BiquadFilter_copyParams(BiquadFilter *restrict dst, const BiquadFilter *restrict src);
extern inline void BiquadFilter_passthru(BiquadFilter *filter, ALsizei numsamples);
extern inline ALfloat calc_rcpQ_from_slope(ALfloat gain, ALfloat slope);
extern inline ALfloat calc_rcpQ_from_bandwidth(ALfloat f0norm, ALfloat bandwidth);
void BiquadFilter_setParams(BiquadFilter *filter, BiquadType type, ALfloat gain, ALfloat f0norm, ALfloat rcpQ)
{
ALfloat alpha, sqrtgain_alpha_2;
ALfloat w0, sin_w0, cos_w0;
ALfloat a[3] = { 1.0f, 0.0f, 0.0f };
ALfloat b[3] = { 1.0f, 0.0f, 0.0f };
// Limit gain to -100dB
assert(gain > 0.00001f);
w0 = F_TAU * f0norm;
sin_w0 = sinf(w0);
cos_w0 = cosf(w0);
alpha = sin_w0/2.0f * rcpQ;
/* Calculate filter coefficients depending on filter type */
switch(type)
{
case BiquadType_HighShelf:
sqrtgain_alpha_2 = 2.0f * sqrtf(gain) * alpha;
b[0] = gain*((gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
b[1] = -2.0f*gain*((gain-1.0f) + (gain+1.0f)*cos_w0 );
b[2] = gain*((gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
a[0] = (gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
a[1] = 2.0f* ((gain-1.0f) - (gain+1.0f)*cos_w0 );
a[2] = (gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
break;
case BiquadType_LowShelf:
sqrtgain_alpha_2 = 2.0f * sqrtf(gain) * alpha;
b[0] = gain*((gain+1.0f) - (gain-1.0f)*cos_w0 + sqrtgain_alpha_2);
b[1] = 2.0f*gain*((gain-1.0f) - (gain+1.0f)*cos_w0 );
b[2] = gain*((gain+1.0f) - (gain-1.0f)*cos_w0 - sqrtgain_alpha_2);
a[0] = (gain+1.0f) + (gain-1.0f)*cos_w0 + sqrtgain_alpha_2;
a[1] = -2.0f* ((gain-1.0f) + (gain+1.0f)*cos_w0 );
a[2] = (gain+1.0f) + (gain-1.0f)*cos_w0 - sqrtgain_alpha_2;
break;
case BiquadType_Peaking:
gain = sqrtf(gain);
b[0] = 1.0f + alpha * gain;
b[1] = -2.0f * cos_w0;
b[2] = 1.0f - alpha * gain;
a[0] = 1.0f + alpha / gain;
a[1] = -2.0f * cos_w0;
a[2] = 1.0f - alpha / gain;
break;
case BiquadType_LowPass:
b[0] = (1.0f - cos_w0) / 2.0f;
b[1] = 1.0f - cos_w0;
b[2] = (1.0f - cos_w0) / 2.0f;
a[0] = 1.0f + alpha;
a[1] = -2.0f * cos_w0;
a[2] = 1.0f - alpha;
break;
case BiquadType_HighPass:
b[0] = (1.0f + cos_w0) / 2.0f;
b[1] = -(1.0f + cos_w0);
b[2] = (1.0f + cos_w0) / 2.0f;
a[0] = 1.0f + alpha;
a[1] = -2.0f * cos_w0;
a[2] = 1.0f - alpha;
break;
case BiquadType_BandPass:
b[0] = alpha;
b[1] = 0;
b[2] = -alpha;
a[0] = 1.0f + alpha;
a[1] = -2.0f * cos_w0;
a[2] = 1.0f - alpha;
break;
}
filter->a1 = a[1] / a[0];
filter->a2 = a[2] / a[0];
filter->b0 = b[0] / a[0];
filter->b1 = b[1] / a[0];
filter->b2 = b[2] / a[0];
}
void BiquadFilter_processC(BiquadFilter *filter, ALfloat *restrict dst, const ALfloat *restrict src, ALsizei numsamples)
{
if(LIKELY(numsamples > 1))
{
const ALfloat a1 = filter->a1;
const ALfloat a2 = filter->a2;
const ALfloat b0 = filter->b0;
const ALfloat b1 = filter->b1;
const ALfloat b2 = filter->b2;
ALfloat z1 = filter->z1;
ALfloat z2 = filter->z2;
ALsizei i;
/* Processing loop is transposed direct form II. This requires less
* storage versus direct form I (only two delay components, instead of
* a four-sample history; the last two inputs and outputs), and works
* better for floating-point which favors summing similarly-sized
* values while being less bothered by overflow.
*
* See: http://www.earlevel.com/main/2003/02/28/biquads/
*/
for(i = 0;i < numsamples;i++)
{
ALfloat input = src[i];
ALfloat output = input*b0 + z1;
z1 = input*b1 - output*a1 + z2;
z2 = input*b2 - output*a2;
dst[i] = output;
}
filter->z1 = z1;
filter->z2 = z2;
}
else if(numsamples == 1)
{
ALfloat input = *src;
ALfloat output = input*filter->b0 + filter->z1;
filter->z1 = input*filter->b1 - output*filter->a1 + filter->z2;
filter->z2 = input*filter->b2 - output*filter->a2;
*dst = output;
}
}
|
