1 /*
2 * Pareq
3 *
4 * This code has been extracted from the Csound opcode "pareq".
5 * It has been modified to work as a Soundpipe module.
6 *
7 * Original Author(s): Hans Mikelson, Matt Gerassimoff,
8 * Jens Groh, John ffitch, Steven Yi
9 * Year: 2001
10 * Location: Opcodes/biquad.c
11 *
12 */
13
14 #include <stdlib.h>
15 #include <math.h>
16 #include "soundpipe.h"
17
18 #ifndef M_PI
19 #define M_PI 3.14159265358979323846
20 #endif
21
sp_pareq_create(sp_pareq ** p)22 int sp_pareq_create(sp_pareq **p)
23 {
24 *p = malloc(sizeof(sp_pareq));
25 return SP_OK;
26 }
27
sp_pareq_destroy(sp_pareq ** p)28 int sp_pareq_destroy(sp_pareq **p)
29 {
30 free(*p);
31 return SP_OK;
32 }
33
sp_pareq_init(sp_data * sp,sp_pareq * p)34 int sp_pareq_init(sp_data *sp, sp_pareq *p)
35 {
36 p->q = 0.707;
37 p->v = 1;
38 p->mode = 0;
39 p->fc = 1000;
40
41 p->xnm1 = p->xnm2 = p->ynm1 = p->ynm2 = 0.0;
42 p->prv_fc = p->prv_v = p->prv_q = -1.0;
43 p->tpidsr = (2 * M_PI) / sp->sr;
44 return SP_OK;
45 }
46
sp_pareq_compute(sp_data * sp,sp_pareq * p,SPFLOAT * in,SPFLOAT * out)47 int sp_pareq_compute(sp_data *sp, sp_pareq *p, SPFLOAT *in, SPFLOAT *out)
48 {
49 SPFLOAT xn, yn;
50 SPFLOAT sq;
51
52 if (p->fc != p->prv_fc || p->v != p->prv_v || p->q != p->prv_q) {
53 SPFLOAT omega = (SPFLOAT)(p->tpidsr * p->fc), k, kk, vkk, vk, vkdq, a0;
54 p->prv_fc = p->fc; p->prv_v = p->v; p->prv_q = p->q;
55 switch ((int)p->mode) {
56 /* Low Shelf */
57 case 1:
58 sq = sqrt(2.0 * (SPFLOAT) p->prv_v);
59 k = tan(omega * 0.5);
60 kk = k * k;
61 vkk = (SPFLOAT)p->prv_v * kk;
62 p->b0 = 1.0 + sq * k + vkk;
63 p->b1 = 2.0 * (vkk - 1.0);
64 p->b2 = 1.0 - sq * k + vkk;
65 a0 = 1.0 + k / (SPFLOAT)p->prv_q + kk;
66 p->a1 = 2.0 * (kk - 1.0);
67 p->a2 = 1.0 - k / (SPFLOAT)p->prv_q + kk;
68 break;
69
70 /* High Shelf */
71 case 2:
72 sq = sqrt(2.0 * (SPFLOAT) p->prv_v);
73 k = tan((M_PI - omega) * 0.5);
74 kk = k * k;
75 vkk = (SPFLOAT)p->prv_v * kk;
76 p->b0 = 1.0 + sq * k + vkk;
77 p->b1 = -2.0 * (vkk - 1.0);
78 p->b2 = 1.0 - sq * k + vkk;
79 a0 = 1.0 + k / (SPFLOAT)p->prv_q + kk;
80 p->a1 = -2.0 * (kk - 1.0);
81 p->a2 = 1.0 - k / (SPFLOAT)p->prv_q + kk;
82 break;
83
84 /* Peaking EQ */
85 default:
86 k = tan(omega * 0.5);
87 kk = k * k;
88 vk = (SPFLOAT)p->prv_v * k;
89 vkdq = vk / (SPFLOAT)p->prv_q;
90 p->b0 = 1.0 + vkdq + kk;
91 p->b1 = 2.0 * (kk - 1.0);
92 p->b2 = 1.0 - vkdq + kk;
93 a0 = 1.0 + k / (SPFLOAT)p->prv_q + kk;
94 p->a1 = 2.0 * (kk - 1.0);
95 p->a2 = 1.0 - k / (SPFLOAT)p->prv_q + kk;
96 }
97 a0 = 1.0 / a0;
98 p->a1 *= a0; p->a2 *= a0; p->b0 *= a0; p->b1 *= a0; p->b2 *= a0;
99 }
100 {
101 SPFLOAT a1 = p->a1, a2 = p->a2;
102 SPFLOAT b0 = p->b0, b1 = p->b1, b2 = p->b2;
103 SPFLOAT xnm1 = p->xnm1, xnm2 = p->xnm2, ynm1 = p->ynm1, ynm2 = p->ynm2;
104 xn = *in;
105 yn = b0 * xn + b1 * xnm1 + b2 * xnm2 - a1 * ynm1 - a2 * ynm2;
106 xnm2 = xnm1;
107 xnm1 = xn;
108 ynm2 = ynm1;
109 ynm1 = yn;
110 *out = yn;
111 p->xnm1 = xnm1; p->xnm2 = xnm2; p->ynm1 = ynm1; p->ynm2 = ynm2;
112 }
113 return SP_OK;
114 }
115