1 /*
2 cross2.c:
3
4 Copyright (C) 1997 Paris Smaragdis, John ffitch
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 #include "stdopcod.h"
25 #include "ptrigtbl.h"
26 #include "fhtfun.h"
27 #include "interlocks.h"
28 #include <math.h>
29
30 #define CH_THRESH 1.19209e-7
31 #define CHOP(a) (a < CH_THRESH ? CH_THRESH : a)
32
plog2(int32 x)33 static int32 plog2(int32 x)
34 {
35 int32 mask, i;
36
37 if (x == 0) return (-1);
38 x--;
39
40 for (mask = ~1 , i = 0; ; mask = mask+mask, i++) {
41 if (x == 0) return (i);
42 x = x & mask;
43 }
44 }
45
getmag(MYFLT * x,int32 size)46 static void getmag(MYFLT *x, int32 size)
47 {
48 MYFLT *i = x + 1, *j = x + size - 1, max = FL(0.0);
49 int32 n = size/2 - 1;
50
51 do {
52 MYFLT ii = *i;
53 MYFLT jj = *j;
54 ii = HYPOT(ii,jj);
55 if (ii > max)
56 max = ii;
57 *i = ii;
58 i++;
59 j--;
60 } while (--n);
61
62 if (LIKELY(max!=FL(0.0))) {
63 int32_t NN = size/2 + 1;
64 for (n=0; n<NN; n++) {
65 x[n] /= max;
66 }
67 }
68 }
69
mult(MYFLT * x,MYFLT * y,int32 size,MYFLT w)70 static void mult(MYFLT *x, MYFLT *y, int32 size, MYFLT w)
71 {
72 MYFLT *j = x + size - 1;
73
74 size = size/2 + 1;
75 do {
76 MYFLT z = w * *y++;
77 *x++ *= z;
78 *j-- *= z;
79 } while (--size);
80 }
81
lineaprox(MYFLT * x,int32 size,int32 m)82 static void lineaprox(MYFLT *x, int32 size, int32 m)
83 {
84 int32 i, c;
85 MYFLT a, f;
86 MYFLT rm = FL(1.0)/(MYFLT)m;
87
88 f = x[0];
89 for (i = 0 ; i < size ; i += m) {
90 a = FL(0.0);
91 for (c = 0 ; c < m ; c++) {
92 if (a < FABS(x[i + c]))
93 a = x[i + c];
94 }
95 x[i] = a;
96 }
97 a = (x[0] + f) * rm;
98 for (c = 0 ; c < m ; c++)
99 x[c] = a * c + f;
100 for (i = m ; i < size ; i += m) {
101 a = ( x[i] - x[i - 1]) * rm;
102 for ( c = 0 ; c < m ; c++)
103 x[i + c] = a * c + x[i - 1];
104 }
105 }
106
do_fht(MYFLT * real,int32 n)107 static void do_fht(MYFLT *real, int32 n)
108 {
109 MYFLT a, b;
110 int32 i, j, k;
111
112 pfht(real,n);
113 for (i = 1, j = n-1, k = n/2 ; i < k ; i++, j--) {
114 a = real[i];
115 b = real[j];
116 real[j] = (a-b)*FL(0.5);
117 real[i] = (a+b)*FL(0.5);
118 }
119 }
120
do_ifht(MYFLT * real,int32 n)121 static void do_ifht(MYFLT *real, int32 n)
122 {
123 MYFLT a, b;
124 int32 i, j, k;
125
126 for (i = 1, j = n-1, k = n/2 ; i < k ; i++, j--) {
127 a = real[i];
128 b = real[j];
129 real[j] = (a-b);
130 real[i] = (a+b);
131 }
132 for (i = 0 ; i < n ; i++) real[i] /= n;
133 pfht(real,n);
134 }
135
pfht(MYFLT * fz,int32 n)136 static void pfht(MYFLT *fz, int32 n)
137 {
138 int32 i, k, k1, k2, k3, k4, kx;
139 MYFLT *fi, *fn, *gi;
140 TRIG_VARS;
141
142 k1 = 1;
143 k2 = 0;
144 do {
145 MYFLT a;
146
147 for (k = n >> 1 ; !( (k2 ^= k) & k) ; k >>= 1);
148 if (k1 > k2) {
149 a = fz[k1];
150 fz[k1] = fz[k2];
151 fz[k2] = a;
152 }
153 k1++;
154 } while (k1 < n);
155
156 k = 0;
157 i = 1;
158 do {
159 k++;
160 } while ( (i << k) < n);
161
162 k &= 1;
163 if (k == 0) {
164 fi = fz;
165 fn = fz + n;
166 do {
167 MYFLT f0, f1, f2, f3;
168 f1 = fi[0] - fi[1];
169 f0 = fi[0] + fi[1];
170 f3 = fi[2] - fi[3];
171 f2 = fi[2] + fi[3];
172 fi[2] = f0 - f2;
173 fi[0] = f0 + f2;
174 fi[3] = f1 - f3;
175 fi[1] = f1 + f3;
176 fi += 4;
177 } while (fi < fn);
178 }
179 else {
180 fi = fz;
181 fn = fz + n;
182 gi = fi + 1;
183 do {
184 MYFLT s1, c1, s2, c2, s3, c3, s4, c4, g0, f0, f1, g1, f2, g2, f3, g3;
185
186 c1 = fi[0] - gi[0];
187 s1 = fi[0] + gi[0];
188 c2 = fi[2] - gi[2];
189 s2 = fi[2] + gi[2];
190 c3 = fi[4] - gi[4];
191 s3 = fi[4] + gi[4];
192 c4 = fi[6] - gi[6];
193 s4 = fi[6] + gi[6];
194 f1 = s1 - s2;
195 f0 = s1 + s2;
196 g1 = c1 - c2;
197 g0 = c1 + c2;
198 f3 = s3 - s4;
199 f2 = s3 + s4;
200 g3 = ROOT2 * c4;
201 g2 = ROOT2 * c3;
202 fi[4] = f0 - f2;
203 fi[0] = f0 + f2;
204 fi[6] = f1 - f3;
205 fi[2] = f1 + f3;
206 gi[4] = g0 - g2;
207 gi[0] = g0 + g2;
208 gi[6] = g1 - g3;
209 gi[2] = g1 + g3;
210
211 fi += 8;
212 gi += 8;
213 } while (fi < fn);
214 }
215 if (n < 16)
216 return;
217 do {
218 MYFLT s1, c1;
219
220 k += 2;
221 k1 = 1L << k;
222 k2 = k1 << 1;
223 k4 = k2 << 1;
224 k3 = k2 + k1;
225 kx = k1 >> 1;
226 fi = fz;
227 gi = fi + kx;
228 fn = fz + n;
229 do {
230 MYFLT g0, f0, f1, g1, f2, g2, f3, g3;
231
232 f1 = fi[0] - fi[k1];
233 f0 = fi[0] + fi[k1];
234 f3 = fi[k2] - fi[k3];
235 f2 = fi[k2] + fi[k3];
236
237 fi[k2] = f0 - f2;
238 fi[0] = f0 + f2;
239 fi[k3] = f1 - f3;
240 fi[k1] = f1 + f3;
241
242 g1 = gi[0] - gi[k1];
243 g0 = gi[0] + gi[k1];
244 g3 = ROOT2 * gi[k3];
245 g2 = ROOT2 * gi[k2];
246
247 gi[k2] = g0 - g2;
248 gi[0] = g0 + g2;
249 gi[k3] = g1 - g3;
250 gi[k1] = g1 + g3;
251
252 gi += k4;
253 fi += k4;
254 } while (fi < fn);
255
256 TRIG_INIT(k, c1, s1);
257
258 i = 1;
259 do {
260 MYFLT c2, s2;
261 TRIG_NEXT(k, c1, s1);
262
263 c2 = c1 * c1 - s1 * s1;
264 s2 = 2 * c1 * s1;
265 fn = fz + n;
266 fi = fz + i;
267 gi = fz + k1 - i;
268
269 do {
270 MYFLT a, b, g0, f0, f1, g1, f2, g2, f3, g3;
271
272 b = s2 * fi[k1] - c2 * gi[k1];
273 a = c2 * fi[k1] + s2 * gi[k1];
274 f1 = fi[0] - a;
275 f0 = fi[0] + a;
276 g1 = gi[0] - b;
277 g0 = gi[0] + b;
278
279 b = s2 * fi[k3] - c2 * gi[k3];
280 a = c2 * fi[k3] + s2 * gi[k3];
281 f3 = fi[k2] - a;
282 f2 = fi[k2] + a;
283 g3 = gi[k2] - b;
284 g2 = gi[k2] + b;
285
286 b = s1 * f2 - c1 * g3;
287 a = c1 * f2 + s1 * g3;
288 fi[k2] = f0 - a;
289 fi[0] = f0 + a;
290 gi[k3] = g1 - b;
291 gi[k1] = g1 + b;
292
293 b = c1 * g2 - s1 * f3;
294 a = s1 * g2 + c1 * f3;
295 gi[k2] = g0 - a;
296 gi[0] = g0 + a;
297 fi[k3] = f1 - b;
298 fi[k1] = f1 + b;
299
300 gi += k4;
301 fi += k4;
302 } while (fi < fn);
303
304 i++;
305 } while (i < kx);
306 } while (k4 < n);
307 }
308
Xsynthset(CSOUND * csound,CON * p)309 static int32_t Xsynthset(CSOUND *csound, CON *p)
310 {
311 uint32_t flen, bufsize;
312 MYFLT *b;
313 FUNC *ftp;
314 MYFLT ovlp = *p->ovlp;
315
316 flen = (int32)*p->len;
317 if (UNLIKELY(flen<1))
318 return csound->InitError(csound, Str("cross2: length must be at least 1"));
319 p->m = plog2(flen);
320 flen = 1L << p->m;
321
322 if (ovlp < FL(2.0)) ovlp = FL(2.0);
323 else if (ovlp > (MYFLT)(flen+flen)) ovlp = (MYFLT)(flen+flen);
324 ovlp = (MYFLT)(1 << (int32_t)plog2((int32)ovlp));
325
326 bufsize = 10 * flen * sizeof(MYFLT);
327
328 if (p->mem.auxp == NULL || bufsize > p->mem.size)
329 csound->AuxAlloc(csound, bufsize, &p->mem);
330 else
331 memset(p->mem.auxp, 0, (size_t)bufsize); /* Replaces loop */
332
333 b = (MYFLT*)p->mem.auxp;
334 p->buffer_in1 = b; b += 2 * flen;
335 p->buffer_in2 = b; b += 2 * flen;
336 p->buffer_out = b; b += 2 * flen;
337 p->in1 = b; b += 2 * flen;
338 p->in2 = b; b += 2 * flen;
339
340 if ((ftp = csound->FTnp2Finde(csound, p->iwin)) != NULL)
341 p->win = ftp;
342 else return NOTOK;
343
344 p->count = 0;
345 p->s_ovlp = ovlp;
346 return OK;
347 }
348
Xsynth(CSOUND * csound,CON * p)349 static int32_t Xsynth(CSOUND *csound, CON *p)
350 {
351 IGN(csound);
352 MYFLT *s, *f, *out, *buf1, *buf2, *outbuf, rfn;
353 int32 size, div;
354 int32 n, m;
355 uint32_t offset = p->h.insdshead->ksmps_offset;
356 uint32_t early = p->h.insdshead->ksmps_no_end;
357 uint32_t nn, nsmps = CS_KSMPS;
358
359 s = p->as;
360 f = p->af;
361 out = p->out;
362
363 outbuf = p->buffer_out;
364 buf1 = p->buffer_in1;
365 buf2 = p->buffer_in2;
366
367 size = (int32)*p->len;
368 div = size / (int32)p->s_ovlp;
369 rfn = (MYFLT)p->win->flen / (MYFLT)size; /* Moved here for efficiency */
370
371 n = p->count;
372 m = n % div;
373 if (UNLIKELY(offset)) memset(out, '\0', offset*sizeof(MYFLT));
374 if (UNLIKELY(early)) {
375 nsmps -= early;
376 memset(&out[nsmps], '\0', early*sizeof(MYFLT));
377 }
378 for (nn = offset; nn < nsmps; nn++) {
379 buf1[n] = s[nn];
380 buf2[n] = f[nn];
381
382 out[nn] = outbuf[n];
383 n++; m++;
384 if (n == size) n = 0; /* Moved to here from inside loop */
385 if (m == div) { /* wrap */
386 int32 i, mask, index;
387 MYFLT window;
388 MYFLT *x, *y, *win;
389 m = 0;
390 mask = size - 1;
391 win = p->win->ftable;
392 x = p->in1;
393 y = p->in2;
394
395 for (i = 0 ; i < size ; i++) {
396
397 window = win[(int32)(i * rfn)];
398 index = (i + n) & mask;
399
400 x[i] = buf1[index] * window;
401 y[i] = buf2[index] * window;
402 }
403
404 memset(&x[size], 0, sizeof(MYFLT)*size);
405 memset(&y[size], 0, sizeof(MYFLT)*size);
406 /* for (; i < 2 * size ; i++) { */
407 /* x[i] = FL(0.0); */
408 /* y[i] = FL(0.0); */
409 /* } */
410
411 if (*p->bias != FL(0.0)) {
412 int32_t nsize = (int32_t)(size+size);
413
414 do_fht( x, nsize);
415 do_fht( y, nsize);
416 getmag( y, nsize);
417
418 lineaprox( y, nsize, 16);
419 mult( x, y, nsize, *p->bias);
420
421 do_ifht( x, nsize);
422
423 }
424
425 for (i = n + size - div ; i < n + size ; i++)
426 outbuf[i&mask] = FL(0.0);
427
428 window = FL(5.0) / p->s_ovlp;
429
430 for (i = 0 ; i < size ; i++)
431 outbuf[(i+n)&mask] += x[i] * window;
432
433 }
434 if (n == size) n = 0; /* Moved to here from inside loop */
435 }
436
437 p->count = n;
438 return OK;
439 }
440
441 #define S(x) sizeof(x)
442
443 static OENTRY localops[] = {
444 { "cross2", S(CON), TR, 3, "a", "aaiiik",(SUBR)Xsynthset, (SUBR)Xsynth}
445 };
446
cross2_init_(CSOUND * csound)447 int32_t cross2_init_(CSOUND *csound)
448 {
449 return csound->AppendOpcodes(csound, &(localops[0]),
450 (int32_t) (sizeof(localops) / sizeof(OENTRY)));
451 }
452
453