1 #include "filter.h"
2 #include <math.h>
3 #include <stdlib.h>
4 #include <stdio.h>
5 #include <string.h>
6
7
8 #include "SC_PlugIn.h"
9 extern InterfaceTable *ft;
10 extern World * gWorld;
11
choose(long n,long k)12 long choose(long n, long k) {
13 long divisor = 1;
14 long multiplier = n;
15 long answer = 1;
16 k = std::min(k,n-k);
17 while(divisor <= k)
18 {
19 answer = (answer * multiplier) / divisor;
20 multiplier--;
21 divisor++;
22 }
23 return answer;
24 }
25
Db(float B,float f,int M)26 float Db(float B, float f, int M) {
27 float C1,C2,k1,k2,k3;
28 if(M==4) {
29 C1 = .069618;
30 C2 = 2.0427;
31 k1 = -.00050469;
32 k2 = -.0064264;
33 k3 = -2.8743;
34 } else {
35 C1 = .071089;
36 C2 = 2.1074;
37 k1 = -.0026580;
38 k2 = -.014811;
39 k3 = -2.9018;
40 }
41
42 float logB = log(B);
43 float kd = exp(k1*logB*logB + k2*logB + k3);
44 float Cd = exp(C1*logB+C2);
45 float halfstep = pow(2.0,1.0/12.0);
46 float Ikey = log(f*halfstep/27.5) / log(halfstep);
47 float D = exp(Cd - Ikey*kd);
48 return D;
49 }
50 void complex_divide(float Hn[2], float Hd[2], float H[2]) ;
51 /*
52 void complex_divide(float Hn[2], float Hd[2], float H[2])
53 {
54 float magn = sqrt(Hn[0]*Hn[0] + Hn[1]*Hn[1]);
55 float argn = atan2(Hn[1],Hn[0]);
56 float magd = sqrt(Hd[0]*Hd[0] + Hd[1]*Hd[1]);
57 float argd = atan2(Hd[1],Hd[0]);
58 float mag = magn/magd;
59 float arg = argn - argd;
60 H[0] = mag*cos(arg);
61 H[1] = mag*sin(arg);
62 }
63 */
groupdelay(Filter * c,float f,float Fs)64 float groupdelay(Filter *c, float f, float Fs)
65 {
66 float df = 5;
67 float f2 = f + df;
68 float f1 = f - df;
69 float omega2 = 2*PI*f2/Fs;
70 float omega1 = 2*PI*f1/Fs;
71 return (omega2*phasedelay(c,f2,Fs) - omega1*phasedelay(c,f1,Fs))/(omega2-omega1);
72 }
73
phasedelay(Filter * c,float f,float Fs)74 float phasedelay(Filter *c, float f, float Fs)
75 {
76 float Hn[2];
77 float Hd[2];
78 float H[2];
79
80 Hn[0] = 0.0; Hn[1] = 0.0;
81 Hd[0] = 0.0; Hd[1] = 0.0;
82
83 float omega = 2*PI*f/Fs;
84 int N = c->n;
85 for(int k=0;k<=N;k++) {
86 Hn[0] += cos(k*omega)*c->b[k];
87 Hn[1] += sin(k*omega)*c->b[k];
88 }
89 for(int k=0;k<=N;k++) {
90 Hd[0] += cos(k*omega)*c->a[k];
91 Hd[1] += sin(k*omega)*c->a[k];
92 }
93 complex_divide(Hn,Hd,H);
94 float arg = atan2(H[1],H[0]);
95 if(arg<0) arg = arg + 2*PI;
96
97 return arg/omega;
98 }
99
differentiator(Filter * c)100 void differentiator(Filter *c)
101 {
102 c->x = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];//(float *)RTAlloc(gWorld,sizeof(float[2]));//
103 c->y = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
104 c->a = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
105 c->b = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
106 memset(c->x,0,2*sizeof(float));
107 memset(c->y,0,2*sizeof(float));
108
109 c->a[0] = 1;
110 c->a[1] = 0;
111 c->b[0] = 1;
112 c->b[1] = -1;
113
114 c->n = 1;
115 }
116
117
resonator(float f,float Fs,float tau,Filter * c)118 void resonator(float f, float Fs, float tau, Filter *c) {
119 c->x = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
120 c->y = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
121 c->a = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
122 c->b = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
123 memset(c->x,0,3*sizeof(float));
124 memset(c->y,0,3*sizeof(float));
125
126 float rp = exp(-1/(tau*Fs));
127 float omega = 2*PI*f/Fs;
128 c->a[0] = 1;
129 c->a[1] = -2*rp*cos(omega);
130 c->a[2] = rp*rp;
131 c->b[0] = 0;
132 c->b[1] = sin(omega);
133 c->b[2] = 0;
134 c->n = 2;
135 }
136
loss(float f0,float fs,float c1,float c3,Filter * c)137 void loss(float f0, float fs, float c1, float c3, Filter *c)
138 {
139 c->x = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
140 c->y = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
141 c->a = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
142 c->b = (float *)RTAlloc(gWorld,sizeof(float[2]));//new float[2];
143 memset(c->x,0,2*sizeof(float));
144 memset(c->y,0,2*sizeof(float));
145
146 float g = 1.0 - c1/f0;
147 float b = 4.0*c3+f0;
148 float a1 = (-b+sqrt(b*b-16.0*c3*c3))/(4.0*c3);
149 c->b[0] = g*(1+a1);
150 c->b[1] = 0;
151 c->a[0] = 1;
152 c->a[1] = a1;
153
154 c->n = 1;
155 }
156 /*/
157 void biquad(float f0, float fs, float Q, int type, Filter *c)
158 {
159 c->x = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
160 c->y = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
161 c->a = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
162 c->b = (float *)RTAlloc(gWorld,sizeof(float[3]));//new float[3];
163 memset(c->x,0,3*sizeof(float));
164 memset(c->y,0,3*sizeof(float));
165
166 float a = 1/(2*tan(PI*f0/fs));
167 float a2 = a*a;
168 float aoQ = a/Q;
169 float d = (4*a2+2*aoQ+1);
170
171 c->a[0] = 1;
172 c->a[1] = -(8*a2-2) / d;
173 c->a[2] = (4*a2 - 2*aoQ + 1) / d;
174
175 switch(type) {
176 case pass:
177 c->b[0] = 2*aoQ/d;
178 c->b[1] = 0;
179 c->b[2] = -2*aoQ/d;
180 break;
181 case low:
182 c->b[0] = 1/d;
183 c->b[1] = 2/d;
184 c->b[2] = 1/d;
185 break;
186 case high:
187 c->b[0] = 4*a2/d;
188 c->b[1] = -8*a2/d;
189 c->b[2] = 4*a2/d;
190 break;
191 case notch:
192 c->b[0] = (1+4*a2)/d;
193 c->b[1] = (2-8*a2)/d;
194 c->b[2] = (1+4*a2)/d;
195 break;
196 }
197
198 c->n = 2;
199 }
200 */
thirian(float D,int N,Filter * c)201 void thirian(float D, int N, Filter *c)
202 {
203 c->x =(float *)RTAlloc(gWorld, sizeof(float) * (N + 1));// new float[N+1];
204 c->y = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
205 c->a = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
206 c->b = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
207 memset(c->x,0,(N+1)*sizeof(float));
208 memset(c->y,0,(N+1)*sizeof(float));
209
210 for(int k=0;k<=N;k++) {
211 double ak = (float)choose((long)N,(long)k);
212 if(k%2==1)
213 ak = -ak;
214 for(int n=0;n<=N;n++) {
215 ak *= ((double)D-(double)(N-n));
216 ak /= ((double)D-(double)(N-k-n));
217 }
218 c->a[k] = (float)ak;
219 c->b[N-k] = (float)ak;
220 }
221 c->n = N;
222 }
223
thiriandispersion(float B,float f,int M,Filter * c)224 void thiriandispersion(float B, float f, int M, Filter *c)
225 {
226 int N = 2;
227 float D;
228 D = Db(B,f,M);
229
230 if(D<=1.0) {
231 c->x = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
232 c->y = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
233 c->a = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
234 c->b = (float *)RTAlloc(gWorld, sizeof(float) * (N + 1));//new float[N+1];
235 memset(c->x,0,(N+1)*sizeof(float));
236 memset(c->y,0,(N+1)*sizeof(float));
237 c->a[0] = 1;
238 c->a[1] = 0;
239 c->a[2] = 0;
240 c->b[0] = 1;
241 c->b[1] = 0;
242 c->b[2] = 0;
243 } else {
244 thirian(D,N,c);
245 }
246 }
247
destroy_filter(Filter * c)248 void destroy_filter(Filter *c) {
249 //delete c->a;
250 //delete c->b;
251 //delete c->x;
252 //delete c->y;
253 RTFree(gWorld,c->a);
254 RTFree(gWorld,c->b);
255 RTFree(gWorld,c->x);
256 RTFree(gWorld,c->y);
257 }
258
filter(float in,Filter * c)259 float filter(float in, Filter *c)
260 {
261 float *a = c->a;
262 float *b = c->b;
263 int n = c->n;
264 float *x = c->x + n;
265 float *y = c->y + n;
266 float *xend = c->x;
267
268 while(x != xend) {
269 *(x) = *(x-1);
270 *(y) = *(y-1);
271 x--;
272 y--;
273 }
274 *x = in;
275 float out = *(b) * *(x);
276 b++;
277 a++;
278 x++;
279 y++;
280 xend = x+c->n;
281
282 while(x != xend) {
283 out += *(b) * *(x);
284 out -= *(a) * *(y);
285 b++;
286 a++;
287 x++;
288 y++;
289 }
290 *(c->y) = out;
291
292 return out;
293 }
294
295
init_delay(Delay * c,int di)296 void init_delay(Delay *c, int di)
297 {
298 // turn size into a mask for quick modding
299 //c->size = 2*di;
300 /*
301 int p = 0;
302 while(c->size) {
303 c->size /= 2;
304 p++;
305 }
306 c->size = 1;
307 while(p) {
308 c->size *= 2;
309 p--;
310 }
311 c->mask = c->size - 1;
312 */
313 c->size = NEXTPOWEROFTWO(2*di);
314 c->mask = c->size - 1;
315 c->x = (float *)RTAlloc(gWorld,sizeof(float) * c->size);//new float[c->size];
316 //c->y = new float[c->size];
317 memset(c->x,0,c->size*sizeof(float));
318 // memset(c->y,0,c->size*sizeof(float));
319
320 c->cursor = 0;
321 c->di = di;
322 c->d1 = (c->size-di)%c->size;
323 }
324
325 //float probe_delay(Delay *c, int pos) {
326 // return c->y[(c->cursor-pos+c->size)%(c->size)];
327 //}
328
destroy_delay(Delay * c)329 void destroy_delay(Delay *c)
330 {
331 RTFree(gWorld,c->x);//delete c->x;
332
333 //delete c->y;
334 }
335
delay(float in,Delay * c)336 float delay(float in, Delay *c)
337 {
338 int cursor = c->cursor;
339 int d1 = c->d1;
340 float y0 = c->x[d1];
341 //c->y[cursor] = y0;
342 c->x[cursor] = in;
343 c->d1++;
344 c->d1 &= c->mask;
345 c->cursor++;
346 c->cursor &= c->mask;
347 return y0;
348 }
349