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