Debian Codegraph @

plugins/LadspaEffect/swh/util/biquad.h

raw
1 #ifndef BIQUAD_H
2 #define BIQUAD_H
3
4 #define LN_2_2 0.34657359f // ln(2)/2
5
6 #include "ladspa-util.h"
7
8 #ifndef LIMIT
9 #define LIMIT(v,l,u) (v<l?l:(v>u?u:v))
10 #endif
11
12 #ifndef BIQUAD_TYPE
13 #define BIQUAD_TYPE float
14 #endif
15
16 typedef BIQUAD_TYPE bq_t;
17
18 /* Biquad filter (adapted from lisp code by Eli Brandt,
19 http://www.cs.cmu.edu/~eli/) */
20
21 typedef struct {
22 bq_t a1;
23 bq_t a2;
24 bq_t b0;
25 bq_t b1;
26 bq_t b2;
27 bq_t x1;
28 bq_t x2;
29 bq_t y1;
30 bq_t y2;
31 } biquad;
32
33 static inline void biquad_init(biquad *f) {
34 f->x1 = 0.0f;
35 f->x2 = 0.0f;
36 f->y1 = 0.0f;
37 f->y2 = 0.0f;
38 }
39
40 static inline void eq_set_params(biquad *f, bq_t fc, bq_t gain, bq_t bw,
41 bq_t fs);
42 static inline void eq_set_params(biquad *f, bq_t fc, bq_t gain, bq_t bw,
43 bq_t fs)
44 {
45 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0f, fs/2.0f) / fs;
46 bq_t cw = cosf(w);
47 bq_t sw = sinf(w);
48 bq_t J = pow(10.0f, gain * 0.025f);
49 bq_t g = sw * sinhf(LN_2_2 * LIMIT(bw, 0.0001f, 4.0f) * w / sw);
50 bq_t a0r = 1.0f / (1.0f + (g / J));
51
52 f->b0 = (1.0f + (g * J)) * a0r;
53 f->b1 = (-2.0f * cw) * a0r;
54 f->b2 = (1.0f - (g * J)) * a0r;
55 f->a1 = -(f->b1);
56 f->a2 = ((g / J) - 1.0f) * a0r;
57 }
58
59 static inline void ls_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope,
60 bq_t fs);
61 static inline void ls_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope,
62 bq_t fs)
63 {
64 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0, fs/2.0) / fs;
65 bq_t cw = cosf(w);
66 bq_t sw = sinf(w);
67 bq_t A = powf(10.0f, gain * 0.025f);
68 bq_t b = sqrt(((1.0f + A * A) / LIMIT(slope, 0.0001f, 1.0f)) - ((A -
69 1.0f) * (A - 1.0)));
70 bq_t apc = cw * (A + 1.0f);
71 bq_t amc = cw * (A - 1.0f);
72 bq_t bs = b * sw;
73 bq_t a0r = 1.0f / (A + 1.0f + amc + bs);
74
75 f->b0 = a0r * A * (A + 1.0f - amc + bs);
76 f->b1 = a0r * 2.0f * A * (A - 1.0f - apc);
77 f->b2 = a0r * A * (A + 1.0f - amc - bs);
78 f->a1 = a0r * 2.0f * (A - 1.0f + apc);
79 f->a2 = a0r * (-A - 1.0f - amc + bs);
80 }
81
82 static inline void hs_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope,
83 bq_t fs);
84 static inline void hs_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope,
85 bq_t fs)
86 {
87 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0, fs/2.0) / fs;
88 bq_t cw = cosf(w);
89 bq_t sw = sinf(w);
90 bq_t A = powf(10.0f, gain * 0.025f);
91 bq_t b = sqrt(((1.0f + A * A) / LIMIT(slope, 0.0001f, 1.0f)) - ((A -
92 1.0f) * (A - 1.0f)));
93 bq_t apc = cw * (A + 1.0f);
94 bq_t amc = cw * (A - 1.0f);
95 bq_t bs = b * sw;
96 bq_t a0r = 1.0f / (A + 1.0f - amc + bs);
97
98 f->b0 = a0r * A * (A + 1.0f + amc + bs);
99 f->b1 = a0r * -2.0f * A * (A - 1.0f + apc);
100 f->b2 = a0r * A * (A + 1.0f + amc - bs);
101 f->a1 = a0r * -2.0f * (A - 1.0f - apc);
102 f->a2 = a0r * (-A - 1.0f + amc + bs);
103 }
104
105 static inline void lp_set_params(biquad *f, bq_t fc, bq_t bw, bq_t fs)
106 {
107 bq_t omega = 2.0 * M_PI * fc/fs;
108 bq_t sn = sin(omega);
109 bq_t cs = cos(omega);
110 bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
111
112 const float a0r = 1.0 / (1.0 + alpha);
113 f->b0 = a0r * (1.0 - cs) * 0.5;
114 f->b1 = a0r * (1.0 - cs);
115 f->b2 = a0r * (1.0 - cs) * 0.5;
116 f->a1 = a0r * (2.0 * cs);
117 f->a2 = a0r * (alpha - 1.0);
118 }
119
120 static inline void hp_set_params(biquad *f, bq_t fc, bq_t bw, bq_t fs)
121 {
122 bq_t omega = 2.0 * M_PI * fc/fs;
123 bq_t sn = sin(omega);
124 bq_t cs = cos(omega);
125 bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
126
127 const float a0r = 1.0 / (1.0 + alpha);
128 f->b0 = a0r * (1.0 + cs) * 0.5;
129 f->b1 = a0r * -(1.0 + cs);
130 f->b2 = a0r * (1.0 + cs) * 0.5;
131 f->a1 = a0r * (2.0 * cs);
132 f->a2 = a0r * (alpha - 1.0);
133 }
134
135 static inline void bp_set_params(biquad *f, bq_t fc, bq_t bw, bq_t fs)
136 {
137 bq_t omega = 2.0 * M_PI * fc/fs;
138 bq_t sn = sin(omega);
139 bq_t cs = cos(omega);
140 bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
141
142 const float a0r = 1.0 / (1.0 + alpha);
143 f->b0 = a0r * alpha;
144 f->b1 = 0.0;
145 f->b2 = a0r * -alpha;
146 f->a1 = a0r * (2.0 * cs);
147 f->a2 = a0r * (alpha - 1.0);
148 }
149
150 static inline bq_t biquad_run(biquad *f, const bq_t x) {
151 bq_t y;
152
153 y = f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2
154 + f->a1 * f->y1 + f->a2 * f->y2;
155 y = flush_to_zero(y);
156 f->x2 = f->x1;
157 f->x1 = x;
158 f->y2 = f->y1;
159 f->y1 = y;
160
161 return y;
162 }
163
164 static inline bq_t biquad_run_fb(biquad *f, bq_t x, const bq_t fb) {
165 bq_t y;
166
167 x += f->y1 * fb * 0.98;
168 y = f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2
169 + f->a1 * f->y1 + f->a2 * f->y2;
170 y = flush_to_zero(y);
171 f->x2 = f->x1;
172 f->x1 = x;
173 f->y2 = f->y1;
174 f->y1 = y;
175
176 return y;
177 }
178
179 #endif