1 // ----------------------------------------------------------------------------
2 //
3 // Copyright (C) 2005-2021 Fons Adriaensen <fons@linuxaudio.org>
4 //
5 // This program is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU General Public License as published by
7 // the Free Software Foundation; either version 3 of the License, or
8 // (at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // ----------------------------------------------------------------------------
19
20
21 #include "nffilt.h"
22
23
NFfiltbase(int degree,int nchan)24 NFfiltbase::NFfiltbase (int degree, int nchan):
25 _degree (degree),
26 _nchan (nchan)
27 {
28 _d = new float [_degree];
29 _z = new float [_degree * _nchan];
30 }
31
~NFfiltbase(void)32 NFfiltbase::~NFfiltbase (void)
33 {
34 delete[] _d;
35 delete[] _z;
36 }
37
38
39 // Add a first order section.
init1(int i,float a)40 void NFfiltbase::init1 (int i, float a)
41 {
42 float t;
43
44 t = 1 / (1 + a);
45 _d [i] = t * (2 * a);
46 if (i) _g *= t;
47 else _g = t;
48 }
49
50 // Add a second order section.
init2(int i,float a,float b)51 void NFfiltbase::init2 (int i, float a, float b)
52 {
53 float t;
54
55 t = 1 / (1 + a + b);
56 _d [i] = t * (2 * a + 4 * b);
57 _d [i+1] = t * (4 * b);
58 if (i) _g *= t;
59 else _g = t;
60 }
61
62
63 // ----------------------------------------------------------------------------
64
65
66 // Denormal protection.
67 #define EPS 1e-30f
68
69 // First order section.
70 #define PROC1(d0, z0) \
71 x -= d0 * z0 + EPS; \
72 z0 += x;
73
74 // Second order section.
75 #define PROC2(d0, d1, z0, z1) \
76 x -= d0 * z0 + d1 * z1 + EPS; \
77 z1 += z0; \
78 z0 += x;
79
80 // ----------------------------------------------------------------------------
81
82
NFfilt1(int nchan)83 NFfilt1::NFfilt1 (int nchan):
84 NFfiltbase (1, nchan)
85 {
86 }
87
init(float w)88 void NFfilt1::init (float w)
89 {
90 float r1;
91
92 r1 = 0.5f * w;
93 init1 (0, r1);
94 reset ();
95 }
96
process(int nsam,float * inp[],float * out[],float gain)97 void NFfilt1::process (int nsam, float *inp [], float *out [], float gain)
98 {
99 int i, n;
100 float g, x;
101 float *p, *q, *z;
102
103 g = _g * gain;
104 z = _z;
105 for (i = 0; i < _nchan; i++)
106 {
107 p = inp [i];
108 q = out [i];
109 n = nsam;
110 while (n--)
111 {
112 x = g * *p++;
113 PROC1(_d [0], z [0]);
114 *q++ = x;
115 }
116 z += 1;
117 }
118 }
119
120
121 // ----------------------------------------------------------------------------
122
123
NFfilt2(int nchan)124 NFfilt2::NFfilt2 (int nchan):
125 NFfiltbase (2, nchan)
126 {
127 }
128
init(float w)129 void NFfilt2::init (float w)
130 {
131 float r1, r2;
132
133 r1 = 0.5f * w;
134 r2 = r1 * r1;
135 init2 (0, r1 * 3.0f, r2 * 3.0f);
136 reset ();
137 }
138
process(int nsam,float * inp[],float * out[],float gain)139 void NFfilt2::process (int nsam, float *inp [], float *out [], float gain)
140 {
141 int i, n;
142 float g, x;
143 float *p, *q, *z;
144
145 g = _g * gain;
146 z = _z;
147 for (i = 0; i < _nchan; i++)
148 {
149 p = inp [i];
150 q = out [i];
151 n = nsam;
152 while (n--)
153 {
154 x = g * *p++;
155 PROC2(_d [0], _d [1], z [0], z [1]);
156 *q++ = x;
157 }
158 z += 2;
159 }
160 }
161
162
163 // ----------------------------------------------------------------------------
164
165
NFfilt3(int nchan)166 NFfilt3::NFfilt3 (int nchan):
167 NFfiltbase (3, nchan)
168 {
169 }
170
init(float w)171 void NFfilt3::init (float w)
172 {
173 float r1, r2;
174
175 r1 = 0.5f * w;
176 r2 = r1 * r1;
177 init2 (0, r1 * 3.6778f, r2 * 6.4594f);
178 init1 (2, r1 * 2.3222f);
179 reset ();
180 }
181
process(int nsam,float * inp[],float * out[],float gain)182 void NFfilt3::process (int nsam, float *inp [], float *out [], float gain)
183 {
184 int i, n;
185 float g, x;
186 float *p, *q, *z;
187
188 g = _g * gain;
189 z = _z;
190 for (i = 0; i < _nchan; i++)
191 {
192 p = inp [i];
193 q = out [i];
194 n = nsam;
195 while (n--)
196 {
197 x = g * *p++;
198 PROC2(_d [0], _d [1], z [0], z [1]);
199 PROC1(_d [2], z [2]);
200 *q++ = x;
201 }
202 z += 3;
203 }
204 }
205
206
207 // ----------------------------------------------------------------------------
208
209
NFfilt4(int nchan)210 NFfilt4::NFfilt4 (int nchan):
211 NFfiltbase (4, nchan)
212 {
213 }
214
init(float w)215 void NFfilt4::init (float w)
216 {
217 float r1, r2;
218
219 r1 = 0.5f * w;
220 r2 = r1 * r1;
221 init2 (0, r1 * 4.2076f, r2 * 11.4878f);
222 init2 (2, r1 * 5.7924f, r2 * 9.1401f);
223 reset ();
224 }
225
process(int nsam,float * inp[],float * out[],float gain)226 void NFfilt4::process (int nsam, float *inp [], float *out [], float gain)
227 {
228 int i, n;
229 float g, x;
230 float *p, *q, *z;
231
232 g = _g * gain;
233 z = _z;
234 for (i = 0; i < _nchan; i++)
235 {
236 p = inp [i];
237 q = out [i];
238 n = nsam;
239 while (n--)
240 {
241 x = g * *p++;
242 PROC2(_d [0], _d [1], z [0], z [1]);
243 PROC2(_d [2], _d [3], z [2], z [3]);
244 *q++ = x;
245 }
246 z += 4;
247 }
248 }
249
250
251 // ----------------------------------------------------------------------------
252
253
NFfilt5(int nchan)254 NFfilt5::NFfilt5 (int nchan):
255 NFfiltbase (5, nchan)
256 {
257 }
258
init(float w)259 void NFfilt5::init (float w)
260 {
261 float r1, r2;
262
263 r1 = 0.5f * w;
264 r2 = r1 * r1;
265 init2 (0, r1 * 4.6493f, r2 * 18.1563f);
266 init2 (2, r1 * 6.7093f, r2 * 14.2725f);
267 init1 (4, r1 * 3.6467f);
268 reset ();
269 }
270
process(int nsam,float * inp[],float * out[],float gain)271 void NFfilt5::process (int nsam, float *inp [], float *out [], float gain)
272 {
273 int i, n;
274 float g, x;
275 float *p, *q, *z;
276
277 g = _g * gain;
278 z = _z;
279 for (i = 0; i < _nchan; i++)
280 {
281 p = inp [i];
282 q = out [i];
283 n = nsam;
284 while (n--)
285 {
286 x = g * *p++;
287 PROC2(_d [0], _d [1], z [0], z [1]);
288 PROC2(_d [2], _d [3], z [2], z [3]);
289 PROC1(_d [4], z [4]);
290 *q++ = x;
291 }
292 z += 5;
293 }
294 }
295
296
297 // ----------------------------------------------------------------------------
298
299
NFfilt6(int nchan)300 NFfilt6::NFfilt6 (int nchan):
301 NFfiltbase (6, nchan)
302 {
303 }
304
init(float w)305 void NFfilt6::init (float w)
306 {
307 float r1, r2;
308
309 r1 = 0.5f * w;
310 r2 = r1 * r1;
311 init2 (0, r1 * 5.0319f, r2 * 26.5140f);
312 init2 (2, r1 * 7.4614f, r2 * 20.8528f);
313 init2 (4, r1 * 8.4967f, r2 * 18.8021f);
314 reset ();
315 }
316
process(int nsam,float * inp[],float * out[],float gain)317 void NFfilt6::process (int nsam, float *inp [], float *out [], float gain)
318 {
319 int i, n;
320 float g, x;
321 float *p, *q, *z;
322
323 g = _g * gain;
324 z = _z;
325 for (i = 0; i < _nchan; i++)
326 {
327 p = inp [i];
328 q = out [i];
329 n = nsam;
330 while (n--)
331 {
332 x = g * *p++;
333 PROC2(_d [0], _d [1], z [0], z [1]);
334 PROC2(_d [2], _d [3], z [2], z [3]);
335 PROC2(_d [4], _d [5], z [4], z [5]);
336 *q++ = x;
337 }
338 z += 6;
339 }
340 }
341
342
343 // ----------------------------------------------------------------------------
344
345
NFfilt7(int nchan)346 NFfilt7::NFfilt7 (int nchan):
347 NFfiltbase (7, nchan)
348 {
349 }
350
init(float w)351 void NFfilt7::init (float w)
352 {
353 float r1, r2;
354
355 r1 = 0.5f * w;
356 r2 = r1 * r1;
357 init2 (0, r1 * 5.3714f, r2 * 36.5968f);
358 init2 (2, r1 * 8.1403f, r2 * 28.9365f);
359 init2 (4, r1 * 9.5166f, r2 * 25.6664f);
360 init1 (6, r1 * 4.9718f);
361 reset ();
362 }
363
process(int nsam,float * inp[],float * out[],float gain)364 void NFfilt7::process (int nsam, float *inp [], float *out [], float gain)
365 {
366 int i, n;
367 float g, x;
368 float *p, *q, *z;
369
370 g = _g * gain;
371 z = _z;
372 for (i = 0; i < _nchan; i++)
373 {
374 p = inp [i];
375 q = out [i];
376 n = nsam;
377 while (n--)
378 {
379 x = g * *p++;
380 PROC2(_d [0], _d [1], z [0], z [1]);
381 PROC2(_d [2], _d [3], z [2], z [3]);
382 PROC2(_d [4], _d [5], z [4], z [5]);
383 PROC1(_d [6], z [6]);
384 *q++ = x;
385 }
386 z += 7;
387 }
388 }
389
390
391 // ----------------------------------------------------------------------------
392
393
394
NFfilt8(int nchan)395 NFfilt8::NFfilt8 (int nchan):
396 NFfiltbase (8, nchan)
397 {
398 }
399
init(float w)400 void NFfilt8::init (float w)
401 {
402 float r1, r2;
403
404 r1 = 0.5f * w;
405 r2 = r1 * r1;
406 init2 (0, r1 * 5.6780f, r2 * 48.4320f);
407 init2 (2, r1 * 8.7366f, r2 * 38.5693f);
408 init2 (4, r1 * 10.4097f, r2 * 33.9347f);
409 init2 (6, r1 * 11.1758f, r2 * 31.9772f);
410 reset ();
411 }
412
process(int nsam,float * inp[],float * out[],float gain)413 void NFfilt8::process (int nsam, float *inp [], float *out [], float gain)
414 {
415 int i, n;
416 float g, x;
417 float *p, *q, *z;
418
419 g = _g * gain;
420 z = _z;
421 for (i = 0; i < _nchan; i++)
422 {
423 p = inp [i];
424 q = out [i];
425 n = nsam;
426 while (n--)
427 {
428 x = g * *p++;
429 PROC2(_d [0], _d [1], z [0], z [1]);
430 PROC2(_d [2], _d [3], z [2], z [3]);
431 PROC2(_d [4], _d [5], z [4], z [5]);
432 PROC2(_d [6], _d [7], z [6], z [7]);
433 *q++ = x;
434 }
435 z += 8;
436 }
437 }
438
439
440 // ----------------------------------------------------------------------------
441