1 /*
2 wpfilters.c:
3
4 Copyright (C) 2017 Steven Yi
5
6 This file is part of Csound.
7
8 The Csound Library is free software; you can redistribute it
9 and/or modify it under the terms of the GNU Lesser General Public
10 License as published by the Free Software Foundation; either
11 version 2.1 of the License, or (at your option) any later version.
12
13 Csound is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU Lesser General Public License for more details.
17
18 You should have received a copy of the GNU Lesser General Public
19 License along with Csound; if not, write to the Free Software
20 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
21 02110-1301 USA
22 */
23
24 /*
25 Zero Delay Feedback Filters
26
27 Based on code by Will Pirkle, presented in:
28
29 http://www.willpirkle.com/Downloads/AN-4VirtualAnalogFilters.2.0.pdf
30 http://www.willpirkle.com/Downloads/AN-5Korg35_V3.pdf
31 http://www.willpirkle.com/Downloads/AN-6DiodeLadderFilter.pdf
32 http://www.willpirkle.com/Downloads/AN-7Korg35HPF_V2.pdf
33
34 and in his book "Designing software synthesizer plug-ins in C++ : for
35 RackAFX, VST3, and Audio Units"
36
37 ZDF using Trapezoidal integrator by Vadim Zavalishin, presented in "The Art
38 of VA Filter Design" (https://www.native-instruments.com/fileadmin/ni_media/
39 downloads/pdf/VAFilterDesign_1.1.1.pdf)
40
41 Csound C versions by Steven Yi
42 */
43
44 #include "wpfilters.h"
45
zdf_1pole_mode_init(CSOUND * csound,ZDF_1POLE_MODE * p)46 static int32_t zdf_1pole_mode_init(CSOUND* csound, ZDF_1POLE_MODE* p) {
47 IGN(csound);
48 if (*p->skip == 0) {
49 p->z1 = 0.0;
50 p->last_cut = -1.0;
51 }
52 return OK;
53 }
54
55
zdf_1pole_mode_perf(CSOUND * csound,ZDF_1POLE_MODE * p)56 static int32_t zdf_1pole_mode_perf(CSOUND* csound, ZDF_1POLE_MODE* p) {
57 IGN(csound);
58 double z1 = p->z1;
59 double last_cut = p->last_cut;
60 double G = p->G;
61
62 uint32_t offset = p->h.insdshead->ksmps_offset;
63 uint32_t early = p->h.insdshead->ksmps_no_end;
64 uint32_t n, nsmps = CS_KSMPS;
65
66 double T = csound->onedsr;
67 double Tdiv2 = T / 2.0;
68 double two_div_T = 2.0 / T;
69
70 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
71
72 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
73
74 if (UNLIKELY(offset)) {
75 memset(p->outlp, '\0', offset*sizeof(MYFLT));
76 memset(p->outhp, '\0', offset*sizeof(MYFLT));
77 }
78 if (UNLIKELY(early)) {
79 nsmps -= early;
80 memset(&p->outlp[nsmps], '\0', early*sizeof(MYFLT));
81 memset(&p->outhp[nsmps], '\0', early*sizeof(MYFLT));
82 }
83
84 for (n = offset; n < nsmps; n++) {
85
86 if (cutoff_arate) {
87 cutoff = p->cutoff[n];
88 }
89
90 if (cutoff != last_cut) {
91 last_cut = cutoff;
92
93 double wd = TWOPI * cutoff;
94 double wa = two_div_T * tan(wd * Tdiv2);
95 double g = wa * Tdiv2;
96 G = g / (1.0 + g);
97 }
98
99 // do the filter, see VA book p. 46
100 // form sub-node value v(n)
101
102 double in = p->in[n];
103 double v = (in - z1) * G;
104
105 // form output of node + register
106 double lp = v + z1;
107 double hp = in - lp;
108
109 // z1 register update
110 z1 = lp + v;
111
112 p->outlp[n] = lp;
113 p->outhp[n] = hp;
114 }
115
116 p->z1 = z1;
117 p->last_cut = last_cut;
118 p->G = G;
119
120 return OK;
121 }
122
zdf_1pole_init(CSOUND * csound,ZDF_1POLE * p)123 static int32_t zdf_1pole_init(CSOUND* csound, ZDF_1POLE* p) {
124 IGN(csound);
125 if (*p->skip == 0) {
126 p->z1 = 0.0;
127 p->last_cut = -1.0;
128 }
129 return OK;
130 }
131
zdf_1pole_perf(CSOUND * csound,ZDF_1POLE * p)132 static int32_t zdf_1pole_perf(CSOUND* csound, ZDF_1POLE* p) {
133
134 double z1 = p->z1;
135 double last_cut = p->last_cut;
136 double G = p->G;
137
138 uint32_t offset = p->h.insdshead->ksmps_offset;
139 uint32_t early = p->h.insdshead->ksmps_no_end;
140 uint32_t n, nsmps = CS_KSMPS;
141
142 double T = csound->onedsr;
143 double Tdiv2 = T / 2.0;
144 double two_div_T = 2.0 / T;
145 int32_t mode = MYFLT2LONG(*p->mode);
146
147 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
148
149 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
150
151 if (UNLIKELY(offset)) {
152 memset(p->out, '\0', offset*sizeof(MYFLT));
153 }
154 if (UNLIKELY(early)) {
155 nsmps -= early;
156 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
157 }
158 for (n = offset; n < nsmps; n++) {
159
160 if (cutoff_arate) {
161 cutoff = p->cutoff[n];
162 }
163
164 if (cutoff != last_cut) {
165 last_cut = cutoff;
166
167 double wd = TWOPI * cutoff;
168 double wa = two_div_T * tan(wd * Tdiv2);
169 double g = wa * Tdiv2;
170 G = g / (1.0 + g);
171 }
172
173 // do the filter, see VA book p. 46
174 // form sub-node value v(n)
175
176 double in = p->in[n];
177 double v = (in - z1) * G;
178
179 // form output of node + register
180 double lp = v + z1;
181
182 if (mode == 0) { // low-pass
183 p->out[n] = lp;
184 }
185 else if (mode == 1) { // high-pass
186 double hp = in - lp;
187 p->out[n] = hp;
188 }
189 else if (mode == 2) { // allpass
190 double hp = in - lp;
191 p->out[n] = lp - hp;
192 }
193 // TODO Implement low-shelf and high-shelf
194 //else if (mode == 3) { // low-shelf
195 //}
196 //else if (mode == 4) { // high-shelf
197 //}
198
199 // z1 register update
200 z1 = lp + v;
201 }
202
203 p->z1 = z1;
204 p->last_cut = last_cut;
205 p->G = G;
206
207 return OK;
208 }
209
210
zdf_2pole_mode_init(CSOUND * csound,ZDF_2POLE_MODE * p)211 static int32_t zdf_2pole_mode_init(CSOUND* csound, ZDF_2POLE_MODE* p) {
212 IGN(csound);
213 if (*p->skip == 0) {
214 p->z1 = 0.0;
215 p->z2 = 0.0;
216 p->last_cut = -1.0;
217 p->last_q = -1.0;
218 p->g = 0.0;
219 p->R = 0.0;
220 }
221 return OK;
222 }
223
224
zdf_2pole_mode_perf(CSOUND * csound,ZDF_2POLE_MODE * p)225 static int32_t zdf_2pole_mode_perf(CSOUND* csound, ZDF_2POLE_MODE* p) {
226 double z1 = p->z1;
227 double z2 = p->z2;
228 double last_cut = p->last_cut;
229 double last_q = p->last_q;
230 double g = p->g;
231 double R = p->R;
232 double g2 = g * g;
233
234 uint32_t offset = p->h.insdshead->ksmps_offset;
235 uint32_t early = p->h.insdshead->ksmps_no_end;
236 uint32_t n, nsmps = CS_KSMPS;
237
238 double T = csound->onedsr;
239 double Tdiv2 = T / 2.0;
240 double two_div_T = 2.0 / T;
241
242 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
243 int32_t q_arate = IS_ASIG_ARG(p->q);
244
245 MYFLT cutoff = *p->cutoff;
246 MYFLT q = *p->q;
247
248 if (UNLIKELY(offset)) {
249 memset(p->outlp, '\0', offset*sizeof(MYFLT));
250 memset(p->outhp, '\0', offset*sizeof(MYFLT));
251 memset(p->outbp, '\0', offset*sizeof(MYFLT));
252 }
253 if (UNLIKELY(early)) {
254 nsmps -= early;
255 memset(&p->outlp[nsmps], '\0', early*sizeof(MYFLT));
256 memset(&p->outhp[nsmps], '\0', early*sizeof(MYFLT));
257 memset(&p->outbp[nsmps], '\0', early*sizeof(MYFLT));
258 }
259 for (n = offset; n < nsmps; n++) {
260
261 if (cutoff_arate) {
262 cutoff = p->cutoff[n];
263 }
264 if (q_arate) {
265 q = p->q[n];
266 }
267
268 if (cutoff != last_cut) {
269 last_cut = cutoff;
270
271 double wd = TWOPI * cutoff;
272 double wa = two_div_T * tan(wd * Tdiv2);
273 g = wa * Tdiv2;
274 g2 = g * g;
275 }
276
277 if (q != last_q) {
278 last_q = q;
279 R = 1.0 / (2.0 * q);
280 }
281
282 double in = p->in[n];
283 double hp = (in - (2.0 * R + g) * z1 - z2) / (1.0 + (2.0 * R * g) + g2);
284 double bp = g * hp + z1;
285 double lp = g * bp + z2;
286 // double notch = in - (2.0 * R * bp);
287
288 // register updates
289 z1 = g * hp + bp;
290 z2 = g * bp + lp;
291
292 p->outlp[n] = lp;
293 p->outhp[n] = hp;
294 p->outbp[n] = bp;
295 // p->outnotch[n] = notch;
296 }
297
298 p->z1 = z1;
299 p->z2 = z2;
300 p->last_cut = last_cut;
301 p->last_q = last_q;
302 p->g = g;
303 p->R = R;
304
305 return OK;
306 }
307
308
zdf_2pole_init(CSOUND * csound,ZDF_2POLE * p)309 static int32_t zdf_2pole_init(CSOUND* csound, ZDF_2POLE* p) {
310 IGN(csound);
311 if (*p->skip == 0) {
312 p->z1 = 0.0;
313 p->z2 = 0.0;
314 p->last_cut = -1.0;
315 p->last_q = -1.0;
316 p->g = 0.0;
317 p->R = 0.0;
318 }
319 return OK;
320 }
321
zdf_2pole_perf(CSOUND * csound,ZDF_2POLE * p)322 static int32_t zdf_2pole_perf(CSOUND* csound, ZDF_2POLE* p) {
323 double z1 = p->z1;
324 double z2 = p->z2;
325 double last_cut = p->last_cut;
326 double last_q = p->last_q;
327 int32_t mode = MYFLT2LONG(*p->mode);
328 double g = p->g;
329 double R = p->R;
330 double g2 = g * g;
331
332 uint32_t offset = p->h.insdshead->ksmps_offset;
333 uint32_t early = p->h.insdshead->ksmps_no_end;
334 uint32_t n, nsmps = CS_KSMPS;
335
336 double T = csound->onedsr;
337 double Tdiv2 = T / 2.0;
338 double two_div_T = 2.0 / T;
339
340 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
341 int32_t q_arate = IS_ASIG_ARG(p->q);
342
343 MYFLT cutoff = *p->cutoff;
344 MYFLT q = *p->q;
345
346 if (UNLIKELY(offset)) {
347 memset(p->out, '\0', offset*sizeof(MYFLT));
348 }
349 if (UNLIKELY(early)) {
350 nsmps -= early;
351 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
352 }
353 for (n = offset; n < nsmps; n++) {
354
355 if (cutoff_arate) {
356 cutoff = p->cutoff[n];
357 }
358 if (q_arate) {
359 q = p->q[n];
360 }
361
362 if (cutoff != last_cut) {
363 last_cut = cutoff;
364
365 double wd = TWOPI * cutoff;
366 double wa = two_div_T * tan(wd * Tdiv2);
367 g = wa * Tdiv2;
368 g2 = g * g;
369 }
370
371 if (q != last_q) {
372 last_q = q;
373 R = 1.0 / (2.0 * q);
374 }
375
376 double in = p->in[n];
377 double hp = (in - (2.0 * R + g) * z1 - z2) / (1.0 + (2.0 * R * g) + g2);
378 double bp = g * hp + z1;
379 double lp = g * bp + z2;
380
381 if (mode == 0) { // low-pass
382 p->out[n] = lp;
383 }
384 else if (mode == 1) { // high-pass
385 p->out[n] = hp;
386 }
387 else if (mode == 2) { // band-pass
388 p->out[n] = bp;
389 }
390 else if (mode == 3) { // unity-gain band-pass
391 p->out[n] = 2.0 * R * bp;
392 }
393 else if (mode == 4) { // notch
394 p->out[n] = in - 2.0 * R * bp;
395 }
396 else if (mode == 5) { // all-pass filter
397 p->out[n] = in - 4.0 * R * bp;
398 }
399 else if (mode == 6) { // peak filter
400 p->out[n] = lp - hp;
401 }
402 //else if (mode == 7) { // band shelf - not implemented
403 // p->out[n] = in + 2.0 * K * R * bp;
404 //}
405
406 // register updates
407 z1 = g * hp + bp;
408 z2 = g * bp + lp;
409
410 }
411
412 p->z1 = z1;
413 p->z2 = z2;
414 p->last_cut = last_cut;
415 p->last_q = last_q;
416 p->g = g;
417 p->R = R;
418
419 return OK;
420 }
421
zdf_ladder_init(CSOUND * csound,ZDF_LADDER * p)422 static int32_t zdf_ladder_init(CSOUND* csound, ZDF_LADDER* p) {
423 IGN(csound);
424 if (*p->skip == 0) {
425 p->z1 = 0.0;
426 p->z2 = 0.0;
427 p->z3 = 0.0;
428 p->z4 = 0.0;
429 p->last_cut = -1.0;
430 p->last_q = -1.0;
431 p->last_k = 0.0;
432 p->last_g = 0.0;
433 p->last_G = 0.0;
434 p->last_G2 = 0.0;
435 p->last_G3 = 0.0;
436 p->last_GAMMA = 0.0;
437 }
438
439 return OK;
440 }
441
zdf_ladder_perf(CSOUND * csound,ZDF_LADDER * p)442 static int32_t zdf_ladder_perf(CSOUND* csound, ZDF_LADDER* p) {
443
444 double z1 = p->z1;
445 double z2 = p->z2;
446 double z3 = p->z3;
447 double z4 = p->z4;
448 double last_cut = p->last_cut;
449 double last_q = p->last_q;
450 double k = p->last_k;
451 double g = p->last_g;
452 double G = p->last_G;
453 double G2 = p->last_G2;
454 double G3 = p->last_G3;
455 double GAMMA = p->last_GAMMA;
456
457 uint32_t offset = p->h.insdshead->ksmps_offset;
458 uint32_t early = p->h.insdshead->ksmps_no_end;
459 uint32_t n, nsmps = CS_KSMPS;
460
461 double T = csound->onedsr;
462 double Tdiv2 = T / 2.0;
463 double two_div_T = 2.0 / T;
464
465 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
466 int32_t q_arate = IS_ASIG_ARG(p->q);
467
468 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
469 MYFLT q = q_arate ? 0.0 : *p->q;
470
471 if (UNLIKELY(offset)) {
472 memset(p->out, '\0', offset*sizeof(MYFLT));
473 }
474 if (UNLIKELY(early)) {
475 nsmps -= early;
476 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
477 }
478 for (n = offset; n < nsmps; n++) {
479
480 if (cutoff_arate) {
481 cutoff = p->cutoff[n];
482 }
483 if (q_arate) {
484 q = p->q[n];
485 q = (q < 0.5) ? 0.5 : (q > 25.0) ? 25.0 : q;
486 }
487
488 if (q != last_q) {
489 last_q = q;
490 // Q [0.5,25] = k [0,4.0]
491 k = (4.0 * (q - 0.5)) / (25.0 - 0.5);
492 }
493
494 if (cutoff != last_cut) {
495 last_cut = cutoff;
496
497 double wd = TWOPI * cutoff;
498 double wa = two_div_T * tan(wd * Tdiv2);
499 g = wa * Tdiv2;
500 G = g / (1.0 + g);
501 G2 = G * G;
502 G3 = G2 * G;
503 GAMMA = G2 * G2;
504 }
505
506 double g_plus_1 = g + 1.0;
507
508 double S1 = z1 / g_plus_1;
509 double S2 = z2 / g_plus_1;
510 double S3 = z3 / g_plus_1;
511 double S4 = z4 / g_plus_1;
512
513 double S = (G3 * S1) + (G2 * S2) + (G * S3) + S4;
514 double u = (p->in[n] - k * S) / (1 + k * GAMMA);
515
516 // 1st stage
517 double v = (u - z1) * G;
518 double lp = v + z1;
519 z1 = lp + v;
520
521 // 2nd stage
522 v = (lp - z2) * G;
523 lp = v + z2;
524 z2 = lp + v;
525
526 // 3rd stage
527 v = (lp - z3) * G;
528 lp = v + z3;
529 z3 = lp + v;
530
531 // 4th stage
532 v = (lp - z4) * G;
533 lp = v + z4;
534 z4 = lp + v;
535
536 p->out[n] = lp;
537 }
538
539 p->z1 = z1;
540 p->z2 = z2;
541 p->z3 = z3;
542 p->z4 = z4;
543
544 p->last_cut = last_cut;
545 p->last_q = last_q;
546 p->last_k = k;
547 p->last_g = g;
548 p->last_G = G;
549 p->last_G2 = G2;
550 p->last_G3 = G3;
551 p->last_GAMMA = GAMMA;
552
553 return OK;
554 }
555
556
diode_ladder_init(CSOUND * csound,DIODE_LADDER * p)557 static int32_t diode_ladder_init(CSOUND* csound,
558 DIODE_LADDER* p) {
559 IGN(csound);
560 if (*p->skip == 0) {
561 int32_t i;
562 p->a[0] = 1.0;
563 p->a[1] = 0.5;
564 p->a[2] = 0.5;
565 p->a[3] = 0.5;
566
567 for (i = 0; i < 4; i++) {
568 p->z[i] = 0.0;
569 p->G[i] = 0.0;
570 p->beta[i] = 0.0;
571 p->SG[i] = 0.0;
572 }
573
574 for (i = 0; i < 3; i++) {
575 p->delta[i] = 0.0;
576 p->epsilon[i] = 0.0;
577 p->gamma[i] = 0.0;
578 }
579 p->GAMMA = 0.0;
580 p->SIGMA = 0.0;
581 p->last_cut = -1.0;
582 }
583
584 return OK;
585 }
586
diode_ladder_perf(CSOUND * csound,DIODE_LADDER * p)587 static int32_t diode_ladder_perf(CSOUND* csound,
588 DIODE_LADDER* p) {
589
590 double a1 = p->a[0];
591 double a2 = p->a[1];
592 double a3 = p->a[2];
593 double a4 = p->a[3];
594 double z1 = p->z[0];
595 double z2 = p->z[1];
596 double z3 = p->z[2];
597 double z4 = p->z[3];
598 double G1 = p->G[0];
599 double G2 = p->G[1];
600 double G3 = p->G[2];
601 double G4 = p->G[3];
602 double beta1 = p->beta[0];
603 double beta2 = p->beta[1];
604 double beta3 = p->beta[2];
605 double beta4 = p->beta[3];
606 double delta1 = p->delta[0];
607 double delta2 = p->delta[1];
608 double delta3 = p->delta[2];
609 double epsilon1 = p->epsilon[0];
610 double epsilon2 = p->epsilon[1];
611 double epsilon3 = p->epsilon[2];
612 double gamma1 = p->gamma[0];
613 double gamma2 = p->gamma[1];
614 double gamma3 = p->gamma[2];
615 double SG1 = p->SG[0];
616 double SG2 = p->SG[1];
617 double SG3 = p->SG[2];
618 double SG4 = p->SG[3];
619 double GAMMA = p->GAMMA;
620 double SIGMA = p->SIGMA;
621 double alpha = p->last_alpha;
622 double last_cut = p->last_cut;
623
624 uint32_t offset = p->h.insdshead->ksmps_offset;
625 uint32_t early = p->h.insdshead->ksmps_no_end;
626 uint32_t n, nsmps = CS_KSMPS;
627
628 double T = csound->onedsr;
629 double Tdiv2 = T / 2.0;
630 double two_div_T = 2.0 / T;
631
632 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
633 int32_t k_arate = IS_ASIG_ARG(p->kval);
634
635 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
636 MYFLT k = k_arate ? 0.0 : *p->kval;
637
638 if (UNLIKELY(offset)) {
639 memset(p->out, '\0', offset*sizeof(MYFLT));
640 }
641 if (UNLIKELY(early)) {
642 nsmps -= early;
643 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
644 }
645 for (n = offset; n < nsmps; n++) {
646 MYFLT in = p->in[n];
647
648 if (cutoff_arate) {
649 cutoff = p->cutoff[n];
650 }
651 if (k_arate) {
652 k = p->kval[n];
653 }
654
655 if (cutoff != last_cut) {
656 last_cut = cutoff;
657
658 double wd = TWOPI * cutoff;
659 double wa = two_div_T * tan(wd * Tdiv2);
660 double g = wa * Tdiv2;
661 double gp1 = 1.0 + g;
662 G4 = 0.5 * g / gp1;
663 G3 = 0.5 * g / (gp1 - 0.5 * g * G4);
664 G2 = 0.5 * g / (gp1 - 0.5 * g * G3);
665 G1 = g / (gp1 - g * G2);
666 GAMMA = G4 * G3 * G2 * G1;
667
668 SG1 = G4 * G3 * G2;
669 SG2 = G4 * G3;
670 SG3 = G4;
671 SG4 = 1.0;
672
673 alpha = g / gp1;
674
675 beta1 = 1.0 / (gp1 - g * G2);
676 beta2 = 1.0 / (gp1 - 0.5 * g * G3);
677 beta3 = 1.0 / (gp1 - 0.5 * g * G4);
678 beta4 = 1.0 / gp1;
679
680 gamma1 = 1.0 + G1 * G2;
681 gamma2 = 1.0 + G2 * G3;
682 gamma3 = 1.0 + G3 * G4;
683
684 delta1 = g;
685 delta2 = delta3 = 0.5 * g;
686
687 epsilon1 = G2;
688 epsilon2 = G3;
689 epsilon3 = G4;
690 }
691
692 //feedback inputs
693 double fb4 = beta4 * z4;
694 double fb3 = beta3 * (z3 + fb4 * delta3);
695 double fb2 = beta2 * (z2 + fb3 * delta2);
696
697 //feedback process
698 double fbo1 = (beta1 * (z1 + fb2 * delta1));
699 double fbo2 = (beta2 * (z2 + fb3 * delta2));
700 double fbo3 = (beta3 * (z3 + fb4 * delta3));
701
702 SIGMA = (SG1 * fbo1) + (SG2 * fbo2) + (SG3 * fbo3) + (SG4 * fb4);
703
704 // non-linear processing
705 if (*p->nlp == 1.0) {
706 in = (1.0 / tanh(*p->saturation)) * tanh(*p->saturation * in);
707 }
708 else if (*p->nlp == 2.0) {
709 in = tanh(*p->saturation * in);
710 }
711
712 // form input to loop
713 double un = (in - k * SIGMA) / (1.0 + k * GAMMA);
714
715 // 1st stage
716 double xin = un * gamma1 + fb2 + epsilon1 * fbo1;
717 double v = (a1 * xin - z1) * alpha;
718 double lp = v + z1;
719 z1 = lp + v;
720
721 // 2nd stage
722 xin = lp * gamma2 + fb3 + epsilon2 * fbo2;
723 v = (a2 * xin - z2) * alpha;
724 lp = v + z2;
725 z2 = lp + v;
726
727 // 3rd stage
728 xin = lp * gamma3 + fb4 + epsilon3 * fbo3;
729 v = (a3 * xin - z3) * alpha;
730 lp = v + z3;
731 z3 = lp + v;
732
733 // 4th stage
734 v = (a4 * lp - z4) * alpha;
735 lp = v + z4;
736 z4 = lp + v;
737
738 p->out[n] = lp;
739 }
740
741 p->a[0] = a1;
742 p->a[1] = a2;
743 p->a[2] = a3;
744 p->a[3] = a4;
745 p->z[0] = z1;
746 p->z[1] = z2;
747 p->z[2] = z3;
748 p->z[3] = z4;
749 p->G[0] = G1;
750 p->G[1] = G2;
751 p->G[2] = G3;
752 p->G[3] = G4;
753 p->beta[0] = beta1;
754 p->beta[1] = beta2;
755 p->beta[2] = beta3;
756 p->beta[3] = beta4;
757 p->delta[0] = delta1;
758 p->delta[1] = delta2;
759 p->delta[2] = delta3;
760 p->epsilon[0] = epsilon1;
761 p->epsilon[1] = epsilon2;
762 p->epsilon[2] = epsilon3;
763 p->gamma[0] = gamma1;
764 p->gamma[1] = gamma2;
765 p->gamma[2] = gamma3;
766 p->SG[0] = SG1;
767 p->SG[1] = SG2;
768 p->SG[2] = SG3;
769 p->SG[3] = SG4;
770 p->GAMMA = GAMMA;
771 p->SIGMA = SIGMA;
772 p->last_alpha = alpha;
773 p->last_cut = last_cut;
774
775 return OK;
776 }
777
778
k35_lpf_init(CSOUND * csound,K35_LPF * p)779 static int32_t k35_lpf_init(CSOUND* csound, K35_LPF* p) {
780 IGN(csound);
781 if (*p->skip == 0.0) {
782 p->z1 = 0.0;
783 p->z2 = 0.0;
784 p->z3 = 0.0;
785 p->last_cut = -1.0;
786 p->last_q = -1.0;
787 p->g = 0.0;
788 p->G = 0.0;
789 p->S35 = 0.0;
790 p->alpha = 0.0;
791 p->lpf2_beta = 0.0;
792 p->hpf1_beta = 0.0;
793 }
794
795 return OK;
796 }
797
798
k35_lpf_perf(CSOUND * csound,K35_LPF * p)799 static int32_t k35_lpf_perf(CSOUND* csound, K35_LPF* p) {
800
801 double z1 = p->z1;
802 double z2 = p->z2;
803 double z3 = p->z3;
804 double last_cut = p->last_cut;
805 double last_q = p->last_q;
806 double g = p->g;
807 double G = p->G;
808 double K = p->K;
809 double S35 = p->S35;
810 double alpha = p->alpha;
811 double lpf2_beta = p->lpf2_beta;
812 double hpf1_beta = p->hpf1_beta;
813
814 uint32_t offset = p->h.insdshead->ksmps_offset;
815 uint32_t early = p->h.insdshead->ksmps_no_end;
816 uint32_t n, nsmps = CS_KSMPS;
817
818 double T = csound->onedsr;
819 double Tdiv2 = T / 2.0;
820 double two_div_T = 2.0 / T;
821
822 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
823 int32_t q_arate = IS_ASIG_ARG(p->q);
824
825 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
826 MYFLT q = q_arate ? 0.0 : *p->q;
827
828 int32_t nonlinear = MYFLT2LONG(*p->nonlinear);
829 double saturation = *p->saturation;
830
831 if (UNLIKELY(offset)) {
832 memset(p->out, '\0', offset*sizeof(MYFLT));
833 }
834 if (UNLIKELY(early)) {
835 nsmps -= early;
836 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
837 }
838 for (n = offset; n < nsmps; n++) {
839 MYFLT in = p->in[n];
840
841 if (cutoff_arate) {
842 cutoff = p->cutoff[n];
843 }
844 if (q_arate) {
845 q = p->q[n];
846 // clamp from 1.0 to 10.0
847 q = (q > 10.0) ? 10.0 : (q < 1.0) ? 1.0 : q;
848 }
849
850 if (cutoff != last_cut) {
851 double wd = TWOPI * cutoff;
852 double wa = two_div_T * tan(wd * Tdiv2);
853 g = wa * Tdiv2;
854 G = g / (1.0 + g);
855 }
856
857 if (q != last_q) {
858 K = 0.01 + ((2.0 - 0.01) * (q / 10.0));
859 }
860
861 if ((cutoff != last_cut) || (q != last_q)) {
862 lpf2_beta = (K - (K * G)) / (1.0 + g);
863 hpf1_beta = -1.0 / (1.0 + g);
864 alpha = 1.0 / (1.0 - (K * G) + (K * G * G));
865 }
866
867
868 last_cut = cutoff;
869 last_q = q;
870
871 // LPF1
872 double v1 = (in - z1) * G;
873 double lp1 = v1 + z1;
874 z1 = lp1 + v1;
875
876 double u = alpha * (lp1 + S35);
877
878 if (nonlinear) {
879 u = tanh(u * saturation);
880 }
881
882 // LPF2
883 double v2 = (u - z2) * G;
884 double lp2 = v2 + z2;
885 z2 = lp2 + v2;
886 double y = K * lp2;
887
888 // HPF1
889
890 double v3 = (y - z3) * G;
891 double lp3 = v3 + z3;
892 z3 = lp3 + v3;
893 // double hp1 = y - lp3; /* FIXME: not used */
894
895 S35 = (lpf2_beta * z2) + (hpf1_beta * z3);
896 double out = (K > 0) ? (y / K) : y;
897
898 p->out[n] = out;
899 }
900
901 p->z1 = z1;
902 p->z2 = z2;
903 p->z3 = z3;
904 p->last_cut = last_cut;
905 p->last_q = last_q;
906 p->g = g;
907 p->G = G;
908 p->K = K;
909 p->S35 = S35;
910 p->alpha = alpha;
911 p->lpf2_beta = lpf2_beta;
912 p->hpf1_beta = hpf1_beta;
913
914 return OK;
915 }
916
917
k35_hpf_init(CSOUND * csound,K35_HPF * p)918 static int32_t k35_hpf_init(CSOUND* csound, K35_HPF* p) {
919 IGN(csound);
920 if (*p->skip == 0.0) {
921 p->z1 = 0.0;
922 p->z2 = 0.0;
923 p->z3 = 0.0;
924 p->last_cut = -1.0;
925 p->last_q = -1.0;
926 p->g = 0.0;
927 p->G = 0.0;
928 p->S35 = 0.0;
929 p->alpha = 0.0;
930 p->hpf2_beta = 0.0;
931 p->lpf1_beta = 0.0;
932 }
933
934 return OK;
935 }
936
937
k35_hpf_perf(CSOUND * csound,K35_HPF * p)938 static int32_t k35_hpf_perf(CSOUND* csound, K35_HPF* p) {
939
940 double z1 = p->z1;
941 double z2 = p->z2;
942 double z3 = p->z3;
943 double last_cut = p->last_cut;
944 double last_q = p->last_q;
945 double g = p->g;
946 double G = p->G;
947 double K = p->K;
948 double S35 = p->S35;
949 double alpha = p->alpha;
950 double hpf2_beta = p->hpf2_beta;
951 double lpf1_beta = p->lpf1_beta;
952
953 uint32_t offset = p->h.insdshead->ksmps_offset;
954 uint32_t early = p->h.insdshead->ksmps_no_end;
955 uint32_t n, nsmps = CS_KSMPS;
956
957 double T = csound->onedsr;
958 double Tdiv2 = T / 2.0;
959 double two_div_T = 2.0 / T;
960
961 int32_t cutoff_arate = IS_ASIG_ARG(p->cutoff);
962 int32_t q_arate = IS_ASIG_ARG(p->q);
963
964 MYFLT cutoff = cutoff_arate ? 0.0 : *p->cutoff;
965 MYFLT q = q_arate ? 0.0 : *p->q;
966
967 int32_t
968 nonlinear = MYFLT2LONG(*p->nonlinear);
969 double saturation = *p->saturation;
970
971 if (UNLIKELY(offset)) {
972 memset(p->out, '\0', offset*sizeof(MYFLT));
973 }
974 if (UNLIKELY(early)) {
975 nsmps -= early;
976 memset(&p->out[nsmps], '\0', early*sizeof(MYFLT));
977 }
978 for (n = offset; n < nsmps; n++) {
979 MYFLT in = p->in[n];
980
981 if (cutoff_arate) {
982 cutoff = p->cutoff[n];
983 }
984 if (q_arate) {
985 q = p->q[n];
986 // clamp from 1.0 to 10.0
987 q = (q > 10.0) ? 10.0 : (q < 1.0) ? 1.0 : q;
988 }
989
990 if (cutoff != last_cut) {
991 double wd = TWOPI * cutoff;
992 double wa = two_div_T * tan(wd * Tdiv2);
993 g = wa * Tdiv2;
994 G = g / (1.0 + g);
995 }
996
997 if (q != last_q) {
998 K = 0.01 + ((2.0 - 0.01) * (q / 10.0));
999 }
1000
1001 if ((cutoff != last_cut) || (q != last_q)) {
1002 hpf2_beta = -G / (1.0 + g);
1003 lpf1_beta = 1.0 / (1.0 + g);
1004 alpha = 1.0 / (1.0 - (K * G) + (K * G * G));
1005 }
1006
1007
1008 last_cut = cutoff;
1009 last_q = q;
1010
1011 // HPF1
1012 double v1 = (in - z1) * G;
1013 double lp1 = v1 + z1;
1014 z1 = lp1 + v1;
1015 double y1 = in - lp1;
1016
1017 double u = alpha * (y1 + S35);
1018 double y = K * u;
1019
1020 if (nonlinear) {
1021 y = tanh(y * saturation);
1022 }
1023
1024 // HPF2
1025 double v2 = (y - z2) * G;
1026 double lp2 = v2 + z2;
1027 z2 = lp2 + v2;
1028 double hp2 = y - lp2;
1029
1030 // LPF1
1031 double v3 = (hp2 - z3) * G;
1032 double lp3 = v3 + z3;
1033 z3 = lp3 + v3;
1034
1035 S35 = (hpf2_beta * z2) + (lpf1_beta * z3);
1036 double out = (K > 0) ? (y / K) : y;
1037
1038 p->out[n] = out;
1039 }
1040
1041 p->z1 = z1;
1042 p->z2 = z2;
1043 p->z3 = z3;
1044 p->last_cut = last_cut;
1045 p->last_q = last_q;
1046 p->g = g;
1047 p->G = G;
1048 p->K = K;
1049 p->S35 = S35;
1050 p->alpha = alpha;
1051 p->hpf2_beta = hpf2_beta;
1052 p->lpf1_beta = lpf1_beta;
1053
1054 return OK;
1055 }
1056
1057
1058 static OENTRY wpfilters_localops[] =
1059 {
1060 { "zdf_1pole", sizeof(ZDF_1POLE), 0,3,"a","axOo",
1061 (SUBR)zdf_1pole_init,(SUBR)zdf_1pole_perf},
1062 { "zdf_1pole_mode", sizeof(ZDF_1POLE_MODE), 0,3,"aa","axo",
1063 (SUBR)zdf_1pole_mode_init,(SUBR)zdf_1pole_mode_perf},
1064 { "zdf_2pole", sizeof(ZDF_2POLE), 0,3,"a","axxOo",
1065 (SUBR)zdf_2pole_init,(SUBR)zdf_2pole_perf},
1066 { "zdf_2pole_mode", sizeof(ZDF_2POLE_MODE), 0,3,"aaa","axxo",
1067 (SUBR)zdf_2pole_mode_init,(SUBR)zdf_2pole_mode_perf},
1068 { "zdf_ladder", sizeof(ZDF_LADDER), 0,3,"a","axxo",
1069 (SUBR)zdf_ladder_init,(SUBR)zdf_ladder_perf},
1070 { "diode_ladder", sizeof(DIODE_LADDER), 0,3,"a","axxOPo",
1071 (SUBR)diode_ladder_init,(SUBR)diode_ladder_perf},
1072 { "K35_lpf", sizeof(K35_LPF), 0,3,"a","axxOPo",
1073 (SUBR)k35_lpf_init,(SUBR)k35_lpf_perf},
1074 { "K35_hpf", sizeof(K35_LPF), 0,3,"a","axxOPo",(SUBR)
1075 k35_hpf_init,(SUBR)k35_hpf_perf},
1076 };
1077
1078 LINKAGE_BUILTIN(wpfilters_localops)
1079