1 /*
2 ugens5.c:
3
4 Copyright (C) 1991 Barry Vercoe, John ffitch, Gabriel Maldonado
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 "csoundCore.h" /* UGENS5.C */
25 #include "ugens5.h"
26 #include <math.h>
27 #include <inttypes.h>
28
29 /*
30 * LPC storage slots
31 */
32
33 #define MAX_LPC_SLOT 20
34
35
porset(CSOUND * csound,PORT * p)36 int32_t porset(CSOUND *csound, PORT *p)
37 {
38 IGN(csound);
39 p->c2 = pow(0.5, (double)CS_ONEDKR / *p->ihtim);
40 p->c1 = 1.0 - p->c2;
41 if (LIKELY(*p->isig >= FL(0.0)))
42 p->yt1 = (double)(*p->isig);
43 p->ihtim_old = *p->ihtim;
44 return OK;
45 }
kporset(CSOUND * csound,PORT * p)46 int32_t kporset(CSOUND *csound, PORT *p) { return porset(csound, p); }
47
port(CSOUND * csound,PORT * p)48 int32_t port(CSOUND *csound, PORT *p)
49 {
50 IGN(csound);
51 p->yt1 = p->c1 * (double)*p->ksig + p->c2 * p->yt1;
52 *p->kr = (MYFLT)p->yt1;
53 return OK;
54 }
55
kport(CSOUND * csound,KPORT * p)56 int32_t kport(CSOUND *csound, KPORT *p)
57 {
58 IGN(csound);
59 if (p->ihtim_old != *p->ihtim) {
60 p->c2 = pow(0.5, (double)CS_ONEDKR / *p->ihtim);
61 p->c1 = 1.0 - p->c2;
62 p->ihtim_old = *p->ihtim;
63 }
64 p->yt1 = p->c1 * (double)*p->ksig + p->c2 * p->yt1;
65 *p->kr = (MYFLT)p->yt1;
66 return OK;
67 }
68
tonset(CSOUND * csound,TONE * p)69 int32_t tonset(CSOUND *csound, TONE *p)
70 {
71 double b;
72 p->prvhp = (double)*p->khp;
73 b = 2.0 - cos((double)(p->prvhp * csound->tpidsr));
74 p->c2 = b - sqrt(b * b - 1.0);
75 p->c1 = 1.0 - p->c2;
76
77 if (LIKELY(!(*p->istor)))
78 p->yt1 = 0.0;
79 return OK;
80 }
81
ktonset(CSOUND * csound,TONE * p)82 int32_t ktonset(CSOUND *csound, TONE *p) {
83 IGN(csound);
84 double b;
85 p->prvhp = (double)*p->khp;
86 b = 2.0 - cos((double)(p->prvhp * CS_ONEDKR *TWOPI));
87 p->c2 = b - sqrt(b * b - 1.0);
88 p->c1 = 1.0 - p->c2;
89
90 if (LIKELY(!(*p->istor)))
91 p->yt1 = 0.0;
92 return OK;
93 }
94
ktone(CSOUND * csound,TONE * p)95 int32_t ktone(CSOUND *csound, TONE *p)
96 {
97 IGN(csound);
98 double c1 = p->c1, c2 = p->c2;
99 double yt1 = p->yt1;
100
101 if (*p->khp != (MYFLT)p->prvhp) {
102 double b;
103 p->prvhp = (double)*p->khp;
104 b = 2.0 - cos((double)(p->prvhp * CS_ONEDKR *TWOPI));
105 p->c2 = c2 = b - sqrt(b * b - 1.0);
106 p->c1 = c1 = 1.0 - c2;
107 }
108 yt1 = c1 * (double)(*p->asig) + c2 * yt1;
109 *p->ar = (MYFLT)yt1;
110 p->yt1 = yt1;
111 return OK;
112 }
113
tone(CSOUND * csound,TONE * p)114 int32_t tone(CSOUND *csound, TONE *p)
115 {
116 IGN(csound);
117 MYFLT *ar, *asig;
118 uint32_t offset = p->h.insdshead->ksmps_offset;
119 uint32_t early = p->h.insdshead->ksmps_no_end;
120 uint32_t n, nsmps = CS_KSMPS;
121 double c1 = p->c1, c2 = p->c2;
122 double yt1 = p->yt1;
123
124 if (*p->khp != (MYFLT)p->prvhp) {
125 double b;
126 p->prvhp = (double)*p->khp;
127 b = 2.0 - cos((double)(p->prvhp * csound->tpidsr));
128 p->c2 = c2 = b - sqrt(b * b - 1.0);
129 p->c1 = c1 = 1.0 - c2;
130 }
131 ar = p->ar;
132 asig = p->asig;
133 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
134 if (UNLIKELY(early)) {
135 nsmps -= early;
136 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
137 }
138 for (n=offset; n<nsmps; n++) {
139 yt1 = c1 * (double)(asig[n]) + c2 * yt1;
140 ar[n] = (MYFLT)yt1;
141 }
142 p->yt1 = yt1;
143 return OK;
144 }
145
tonsetx(CSOUND * csound,TONEX * p)146 int32_t tonsetx(CSOUND *csound, TONEX *p)
147 { /* From Gabriel Maldonado, modified for arbitrary order */
148 {
149 double b;
150 p->prvhp = *p->khp;
151 b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
152 p->c2 = b - sqrt(b * b - 1.0);
153 p->c1 = 1.0 - p->c2;
154 }
155 if (UNLIKELY((p->loop = (int32_t) (*p->ord + FL(0.5))) < 1)) p->loop = 4;
156 if (!*p->istor && (p->aux.auxp == NULL ||
157 (uint32_t)(p->loop*sizeof(double)) > p->aux.size))
158 csound->AuxAlloc(csound, (int32_t)(p->loop*sizeof(double)), &p->aux);
159 p->yt1 = (double*)p->aux.auxp;
160 if (LIKELY(!(*p->istor))) {
161 memset(p->yt1, 0, p->loop*sizeof(double)); /* Punning zero and 0.0 */
162 }
163 return OK;
164 }
165
tonex(CSOUND * csound,TONEX * p)166 int32_t tonex(CSOUND *csound, TONEX *p) /* From Gabriel Maldonado, modified */
167 {
168 MYFLT *ar = p->ar;
169 double c2 = p->c2, *yt1 = p->yt1,c1 = p->c1;
170 uint32_t offset = p->h.insdshead->ksmps_offset;
171 uint32_t early = p->h.insdshead->ksmps_no_end;
172 uint32_t n, nsmps = CS_KSMPS;
173 int32_t j, lp = p->loop;
174
175 if (*p->khp != p->prvhp) {
176 double b;
177 p->prvhp = (double)*p->khp;
178 b = 2.0 - cos(p->prvhp * (double)csound->tpidsr);
179 p->c2 = b - sqrt(b * b - 1.0);
180 p->c1 = 1.0 - p->c2;
181 }
182
183 memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
184 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
185 if (UNLIKELY(early)) {
186 nsmps -= early;
187 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
188 }
189 for (j=0; j< lp; j++) {
190 /* Should *yt1 be reset to something?? */
191 for (n=0; n<nsmps; n++) {
192 double x = c1 * ar[n] + c2 * yt1[j];
193 yt1[j] = x;
194 ar[n] = (MYFLT)x;
195 }
196 }
197 return OK;
198 }
199
katone(CSOUND * csound,TONE * p)200 int32_t katone(CSOUND *csound, TONE *p)
201 {
202 IGN(csound);
203 double sig, x;
204 double c2 = p->c2, yt1 = p->yt1;
205
206 if (*p->khp != p->prvhp) {
207 double b;
208 p->prvhp = *p->khp;
209 b = 2.0 - cos((double)(*p->khp * CS_ONEDKR *TWOPI));
210 p->c2 = c2 = b - sqrt(b * b - 1.0);
211 /* p->c1 = c1 = 1.0 - c2; */
212 }
213 sig = *p->asig;
214 x = yt1 = c2 * (yt1 + sig);
215 *p->ar = (MYFLT)x;
216 yt1 -= sig; /* yt1 contains yt1-xt1 */
217
218 p->yt1 = yt1;
219 return OK;
220 }
221
222
atone(CSOUND * csound,TONE * p)223 int32_t atone(CSOUND *csound, TONE *p)
224 {
225 MYFLT *ar, *asig;
226 uint32_t offset = p->h.insdshead->ksmps_offset;
227 uint32_t early = p->h.insdshead->ksmps_no_end;
228 uint32_t n, nsmps = CS_KSMPS;
229 double c2 = p->c2, yt1 = p->yt1;
230
231 if (*p->khp != p->prvhp) {
232 double b;
233 p->prvhp = *p->khp;
234 b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
235 p->c2 = c2 = b - sqrt(b * b - 1.0);
236 /* p->c1 = c1 = 1.0 - c2; */
237 }
238 ar = p->ar;
239 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
240 if (UNLIKELY(early)) {
241 nsmps -= early;
242 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
243 }
244 asig = p->asig;
245 for (n=offset; n<nsmps; n++) {
246 double sig = (double)asig[n];
247 double x = yt1 = c2 * (yt1 + sig);
248 ar[n] = (MYFLT)x;
249 yt1 -= sig; /* yt1 contains yt1-xt1 */
250 }
251 p->yt1 = yt1;
252 return OK;
253 }
254
atonex(CSOUND * csound,TONEX * p)255 int32_t atonex(CSOUND *csound, TONEX *p) /* Gabriel Maldonado, modified */
256 {
257 MYFLT *ar = p->ar;
258 double c2 = p->c2, *yt1 = p->yt1;
259 uint32_t offset = p->h.insdshead->ksmps_offset;
260 uint32_t early = p->h.insdshead->ksmps_no_end;
261 uint32_t n, nsmps = CS_KSMPS;
262 int32_t j, lp = p->loop;
263
264 if (*p->khp != p->prvhp) {
265 double b;
266 p->prvhp = *p->khp;
267 b = 2.0 - cos((double)(*p->khp * csound->tpidsr));
268 p->c2 = b - sqrt(b * b - 1.0);
269 /*p->c1 = 1. - p->c2;*/
270 }
271
272 memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
273 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
274 if (UNLIKELY(early)) {
275 nsmps -= early;
276 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
277 }
278 for (j=1; j<lp; j++) {
279 for (n=offset; n<nsmps; n++) {
280 double sig = (double)ar[n];
281 double x = c2 * (yt1[j] + sig);
282 yt1[j] = x - sig; /* yt1 contains yt1-xt1 */
283 ar[n] = (MYFLT)x;
284 }
285 }
286 return OK;
287 }
288
rsnset(CSOUND * csound,RESON * p)289 int32_t rsnset(CSOUND *csound, RESON *p)
290 {
291 int32_t scale;
292 p->scale = scale = (int32_t)*p->iscl;
293 if (UNLIKELY(scale && scale != 1 && scale != 2)) {
294 return csound->InitError(csound, Str("illegal reson iscl value, %f"),
295 *p->iscl);
296 }
297 p->prvcf = p->prvbw = -100.0;
298 if (!(*p->istor))
299 p->yt1 = p->yt2 = 0.0;
300 p->asigf = IS_ASIG_ARG(p->kcf);
301 p->asigw = IS_ASIG_ARG(p->kbw);
302
303 return OK;
304 }
305
krsnset(CSOUND * csound,RESON * p)306 int32_t krsnset(CSOUND *csound, RESON *p){ return rsnset(csound,p); }
307
kreson(CSOUND * csound,RESON * p)308 int32_t kreson(CSOUND *csound, RESON *p)
309 {
310 uint32_t flag = 0;
311 double c3p1, c3t4, omc3, c2sqr;
312 double yt0, yt1, yt2, c1 = p->c1, c2 = p->c2, c3 = p->c3;
313 IGN(csound);
314
315 if (*p->kcf != (MYFLT)p->prvcf) {
316 p->prvcf = (double)*p->kcf;
317 p->cosf = cos(p->prvcf * (double)(CS_ONEDKR *TWOPI));
318 flag = 1; /* Mark as changed */
319 }
320 if (*p->kbw != (MYFLT)p->prvbw) {
321 p->prvbw = (double)*p->kbw;
322 c3 = p->c3 = exp(p->prvbw * (double)(-CS_ONEDKR *TWOPI));
323 flag = 1; /* Mark as changed */
324 }
325 if (flag) {
326 c3p1 = c3 + 1.0;
327 c3t4 = c3 * 4.0;
328 omc3 = 1.0 - c3;
329 c2 = p->c2 = c3t4 * p->cosf / c3p1; /* -B, so + below */
330 c2sqr = c2 * c2;
331 if (p->scale == 1)
332 c1 = p->c1 = omc3 * sqrt(1.0 - c2sqr / c3t4);
333 else if (p->scale == 2)
334 c1 = p->c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
335 else c1 = p->c1 = 1.0;
336 }
337
338 yt1 = p->yt1; yt2 = p->yt2;
339 yt0 = c1 * ((double)*p->asig) + c2 * yt1 - c3 * yt2;
340 *p->ar = (MYFLT)yt0;
341 p->yt1 = yt0; p->yt2 = yt1; /* Write back for next cycle */
342 return OK;
343 }
344
345
reson(CSOUND * csound,RESON * p)346 int32_t reson(CSOUND *csound, RESON *p)
347 {
348 uint32_t offset = p->h.insdshead->ksmps_offset;
349 uint32_t early = p->h.insdshead->ksmps_no_end;
350 uint32_t flag = 0, n, nsmps = CS_KSMPS;
351 MYFLT *ar, *asig;
352 double c3p1, c3t4, omc3, c2sqr;
353 double yt1, yt2, c1 = p->c1, c2 = p->c2, c3 = p->c3;
354 int32_t asigf = p->asigf;
355 int32_t asigw = p->asigw;
356
357 asig = p->asig;
358 ar = p->ar;
359 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
360 if (UNLIKELY(early)) {
361 nsmps -= early;
362 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
363 }
364 yt1 = p->yt1; yt2 = p->yt2;
365 for (n=offset; n<nsmps; n++) {
366 double yt0;
367 MYFLT cf = asigf ? p->kcf[n] : *p->kcf;
368 MYFLT bw = asigw ? p->kbw[n] : *p->kbw;
369 if (cf != (MYFLT)p->prvcf) {
370 p->prvcf = (double)cf;
371 p->cosf = cos(cf * (double)(csound->tpidsr));
372 flag = 1; /* Mark as changed */
373 }
374 if (bw != (MYFLT)p->prvbw) {
375 p->prvbw = (double)bw;
376 c3 = p->c3 = exp(bw * (double)(csound->mtpdsr));
377 flag = 1; /* Mark as changed */
378 }
379 if (flag) {
380 c3p1 = c3 + 1.0;
381 c3t4 = c3 * 4.0;
382 omc3 = 1.0 - c3;
383 c2 = p->c2 = c3t4 * p->cosf / c3p1; /* -B, so + below */
384 c2sqr = c2 * c2;
385 if (p->scale == 1)
386 c1 = p->c1 = omc3 * sqrt(1.0 - c2sqr / c3t4);
387 else if (p->scale == 2)
388 c1 = p->c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
389 else c1 = p->c1 = 1.0;
390 flag = 0;
391 }
392 yt0 = c1 * ((double)asig[n]) + c2 * yt1 - c3 * yt2;
393 ar[n] = (MYFLT)yt0;
394 yt2 = yt1;
395 yt1 = yt0;
396 }
397 p->yt1 = yt1; p->yt2 = yt2; /* Write back for next cycle */
398 return OK;
399 }
400
rsnsetx(CSOUND * csound,RESONX * p)401 int32_t rsnsetx(CSOUND *csound, RESONX *p)
402 { /* Gabriel Maldonado, modifies for arb order */
403 int32_t scale;
404 p->scale = scale = (int32_t) *p->iscl;
405 if ((p->loop = (int32_t) (*p->ord + FL(0.5))) < 1)
406 p->loop = 4; /* default value */
407 if (!*p->istor && (p->aux.auxp == NULL ||
408 (uint32_t)(p->loop*2*sizeof(double)) > p->aux.size))
409 csound->AuxAlloc(csound, (int32_t)(p->loop*2*sizeof(double)), &p->aux);
410 p->yt1 = (double*)p->aux.auxp; p->yt2 = (double*)p->aux.auxp + p->loop;
411 if (UNLIKELY(scale && scale != 1 && scale != 2)) {
412 return csound->InitError(csound, Str("illegal reson iscl value, %f"),
413 *p->iscl);
414 }
415 p->prvcf = p->prvbw = -100.0;
416
417 if (!(*p->istor)) {
418 memset(p->yt1, 0, p->loop*sizeof(double));
419 memset(p->yt2, 0, p->loop*sizeof(double));
420 }
421 return OK;
422 }
423
resonx(CSOUND * csound,RESONX * p)424 int32_t resonx(CSOUND *csound, RESONX *p) /* Gabriel Maldonado, modified */
425 {
426 uint32_t offset = p->h.insdshead->ksmps_offset;
427 uint32_t early = p->h.insdshead->ksmps_no_end;
428 uint32_t flag = 0, n, nsmps = CS_KSMPS;
429 int32_t j;
430 MYFLT *ar;
431 double c3p1, c3t4, omc3, c2sqr;
432 double *yt1, *yt2, c1,c2,c3;
433 int32_t asgf = IS_ASIG_ARG(p->kcf);
434 int32_t asgw = IS_ASIG_ARG(p->kbw);
435
436 ar = p->ar;
437 c1 = p->c1;
438 c2 = p->c2;
439 c3 = p->c3;
440 yt1 = p->yt1;
441 yt2 = p->yt2;
442 ar = p->ar;
443 memmove(ar,p->asig,sizeof(MYFLT)*nsmps);
444 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
445 if (UNLIKELY(early)) {
446 nsmps -= early;
447 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
448 }
449 for (j=0; j< p->loop; j++) {
450 for (n=offset; n<nsmps; n++) {
451 double x;
452 MYFLT cf = asgf ? p->kcf[n] : *p->kcf;
453 MYFLT bw = asgw ? p->kbw[n] : *p->kbw;
454 if (cf != (MYFLT)p->prvcf) {
455 p->prvcf = (double)cf;
456 p->cosf = cos(cf * (double)(csound->tpidsr));
457 flag = 1;
458 }
459 if (bw != (MYFLT)p->prvbw) {
460 p->prvbw = (double)bw;
461 c3 = exp(bw * (double)(csound->mtpdsr));
462 flag = 1;
463 }
464 if (flag) {
465 c3p1 = c3 + 1.0;
466 c3t4 = c3 * 4.0;
467 omc3 = 1.0 - c3;
468 c2 = c3t4 * p->cosf / c3p1; /* -B, so + below */
469 c2sqr = c2 * c2;
470 if (p->scale == 1)
471 c1 = omc3 * sqrt(1.0 - (c2sqr / c3t4));
472 else if (p->scale == 2)
473 c1 = sqrt((c3p1*c3p1-c2sqr) * omc3/c3p1);
474 else c1 = 1.0;
475 flag =0;
476 }
477 x = c1 * ((double)ar[n]) + c2 * yt1[j] - c3 * yt2[j];
478 yt2[j] = yt1[j];
479 ar[n] = (MYFLT)x;
480 yt1[j] = x;
481 }
482 }
483 p->c1 = c1; p->c2 = c2; p->c3 = c3;
484 return OK;
485 }
486
kareson(CSOUND * csound,RESON * p)487 int32_t kareson(CSOUND *csound, RESON *p)
488 {
489 uint32_t flag = 0;
490 double c3p1, c3t4, omc3, c2sqr; //, D = 2.0; /* 1/RMS = root2 (rand) */
491 /* or 1/.5 (sine) */
492 double yt1, yt2, c1, c2, c3;
493 IGN(csound);
494
495 if (*p->kcf != (MYFLT)p->prvcf) {
496 p->prvcf = (double)*p->kcf;
497 p->cosf = cos(p->prvcf * (double)(CS_ONEDKR *TWOPI));
498 flag = 1;
499 }
500 if (*p->kbw != (MYFLT)p->prvbw) {
501 p->prvbw = (double)*p->kbw;
502 p->c3 = exp(p->prvbw * (double)(-CS_ONEDKR *TWOPI));
503 flag = 1;
504 }
505 if (flag) {
506 c3p1 = p->c3 + 1.0;
507 c3t4 = p->c3 * 4.0;
508 omc3 = 1.0 - p->c3;
509 p->c2 = c3t4 * p->cosf / c3p1;
510 c2sqr = p->c2 * p->c2;
511 if (p->scale == 1) /* i.e. 1 - A(reson) */
512 p->c1 = 1.0 - omc3 * sqrt(1.0 - c2sqr / c3t4);
513 else if (p->scale == 2) /* i.e. D - A(reson) */
514 p->c1 = 2.0 - sqrt((c3p1*c3p1-c2sqr)*omc3/c3p1);
515 else p->c1 = 0.0; /* cannot tell */
516 }
517
518 c1 = p->c1; c2 = p->c2; c3 = p->c3; yt1 = p->yt1; yt2 = p->yt2;
519 if (p->scale == 1 || p->scale == 0) {
520 double sig = (double) *p->asig;
521 double ans = c1 * sig + c2 * yt1 - c3 * yt2;
522 yt2 = yt1;
523 yt1 = ans - sig; /* yt1 contains yt1-xt1 */
524 *p->ar = (MYFLT)ans;
525 }
526 else if (p->scale == 2) {
527 double sig = (double) *p->asig;
528 double ans = c1 * sig + c2 * yt1 - c3 * yt2;
529 yt2 = yt1;
530 yt1 = ans - 2.0 * sig; /* yt1 contains yt1-D*xt1 */
531 *p->ar = (MYFLT)ans;
532 }
533 p->yt1 = yt1; p->yt2 = yt2;
534 return OK;
535 }
536
areson(CSOUND * csound,RESON * p)537 int32_t areson(CSOUND *csound, RESON *p)
538 {
539 uint32_t offset = p->h.insdshead->ksmps_offset;
540 uint32_t early = p->h.insdshead->ksmps_no_end;
541 uint32_t flag = 0, n, nsmps = CS_KSMPS;
542 MYFLT *ar, *asig;
543 double c3p1, c3t4, omc3, c2sqr;//, D = 2.0; /* 1/RMS = root2 (rand) */
544 /* or 1/.5 (sine) */
545 double yt1, yt2, c1, c2, c3;
546
547 if (*p->kcf != (MYFLT)p->prvcf) {
548 p->prvcf = (double)*p->kcf;
549 p->cosf = cos(p->prvcf * (double)(csound->tpidsr));
550 flag = 1;
551 }
552 if (*p->kbw != (MYFLT)p->prvbw) {
553 p->prvbw = (double)*p->kbw;
554 p->c3 = exp(p->prvbw * (double)(csound->mtpdsr));
555 flag = 1;
556 }
557 if (flag) {
558 c3p1 = p->c3 + 1.0;
559 c3t4 = p->c3 * 4.0;
560 omc3 = 1.0 - p->c3;
561 p->c2 = c3t4 * p->cosf / c3p1;
562 c2sqr = p->c2 * p->c2;
563 if (p->scale == 1) /* i.e. 1 - A(reson) */
564 p->c1 = 1.0 - omc3 * sqrt(1.0 - c2sqr / c3t4);
565 else if (p->scale == 2) /* i.e. D - A(reson) */
566 p->c1 = 2.0 - sqrt((c3p1*c3p1-c2sqr)*omc3/c3p1);
567 else p->c1 = 0.0; /* cannot tell */
568 }
569 asig = p->asig;
570 ar = p->ar;
571 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
572 if (UNLIKELY(early)) {
573 nsmps -= early;
574 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
575 }
576 c1 = p->c1; c2 = p->c2; c3 = p->c3; yt1 = p->yt1; yt2 = p->yt2;
577 if (p->scale == 1 || p->scale == 0) {
578 for (n=offset; n<nsmps; n++) {
579 double sig = (double)asig[n];
580 double ans = c1 * sig + c2 * yt1 - c3 * yt2;
581 yt2 = yt1;
582 yt1 = ans - sig; /* yt1 contains yt1-xt1 */
583 ar[n] = (MYFLT)ans;
584 }
585 }
586 else if (p->scale == 2) {
587 for (n=offset; n<nsmps; n++) {
588 double sig = (double)asig[n];
589 double ans = c1 * sig + c2 * yt1 - c3 * yt2;
590 yt2 = yt1;
591 yt1 = ans - 2.0 * sig; /* yt1 contains yt1-D*xt1 */
592 ar[n] = (MYFLT)ans;
593 }
594 }
595 p->yt1 = yt1; p->yt2 = yt2;
596 return OK;
597 }
598
599 /*
600 *
601 * LPREAD opcode : initialisation phase
602 *
603 *
604 */
605
lprdset_(CSOUND * csound,LPREAD * p,int32_t stringname)606 int32_t lprdset_(CSOUND *csound, LPREAD *p, int32_t stringname)
607 {
608 LPHEADER *lph;
609 MEMFIL *mfp;
610 int32_t magic;
611 int32_t totvals;
612 char lpfilname[MAXNAME];
613
614 /* Store adress of opcode for other lpXXXX init to point to */
615 if (csound->lprdaddr == NULL ||
616 csound->currentLPCSlot >= csound->max_lpc_slot) {
617 csound->max_lpc_slot = csound->currentLPCSlot + MAX_LPC_SLOT;
618 csound->lprdaddr = csound->ReAlloc(csound,
619 csound->lprdaddr,
620 csound->max_lpc_slot * sizeof(LPREAD*));
621 }
622 ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot] = p;
623
624 /* Build file name */
625 if (stringname) strNcpy(lpfilname, ((STRINGDAT*)p->ifilcod)->data, MAXNAME-1);
626 else if (csound->ISSTRCOD(*p->ifilcod))
627 strNcpy(lpfilname, get_arg_string(csound, *p->ifilcod), MAXNAME-1);
628 else csound->strarg2name(csound, lpfilname, p->ifilcod, "lp.", 0);
629
630 /* Do not reload existing file ? */
631 if (UNLIKELY((mfp = p->mfp) != NULL && strcmp(mfp->filename, lpfilname) == 0))
632 goto lpend; /* rtn if file prv known */
633 /* Load analysis in memory file */
634 /* else read file */
635 if (UNLIKELY((mfp = ldmemfile2withCB(csound, lpfilname, CSFTYPE_LPC, NULL))
636 == NULL)) {
637 return csound->InitError(csound, Str("LPREAD cannot load %s"), lpfilname);
638 }
639 /* Store memory file location in opcode */
640 p->mfp = mfp; /* & record facts */
641 /* Take a peek to the header if exisiting. Else take input arguments */
642 lph = (LPHEADER *) mfp->beginp;
643
644 magic=lph->lpmagic;
645 if (LIKELY((magic==LP_MAGIC)||(magic==LP_MAGIC2))) {
646 p->storePoles = (magic==LP_MAGIC2);
647
648 csound->Warning(csound, Str("Using %s type of file.\n"),
649 p->storePoles?Str("pole"):Str("filter coefficient"));
650 /* Store header length */
651 p->headlen = lph->headersize;
652 /* Check if input values where available */
653 if (*p->inpoles || *p->ifrmrate) {
654 csound->Warning(csound, Str("lpheader overriding inputs"));
655 }
656 /* Check orc/analysis sample rate compatibility */
657 if (lph->srate != csound->esr) {
658 csound->Warning(csound, Str("lpfile srate != orch sr"));
659 }
660 p->npoles = lph->npoles; /* note npoles, etc. */
661 /* Store header info in opcode */
662 p->nvals = lph->nvals;
663 p->framrat16 = lph->framrate * FL(65536.0);/* scaled framno cvt */
664 }
665 else if (UNLIKELY(BYTREVL(lph->lpmagic) == LP_MAGIC)) { /* Header reversed: */
666 return csound->InitError(csound, Str("file %s bytes are in wrong order"),
667 lpfilname);
668 }
669 else { /* No Header on file:*/
670 p->headlen = 0;
671 p->npoles = (int32_t)*p->inpoles; /* data from inargs */
672 p->nvals = p->npoles + 4;
673 p->framrat16 = *p->ifrmrate * FL(65536.0);
674 if (UNLIKELY(!p->npoles || !p->framrat16)) {
675 return csound->InitError(csound,
676 Str("insufficient args and no file header"));
677 }
678 }
679 /* Check pole number */
680 csound->AuxAlloc(csound, (int32_t)(p->npoles*8*sizeof(MYFLT)), &p->aux);
681 p->kcoefs = (MYFLT*)p->aux.auxp;
682 /* if (UNLIKELY(p->npoles > MAXPOLES)) { */
683 /* return csound->InitError(csound, Str("npoles > MAXPOLES")); */
684 /* } */
685 /* Look for total frame data size (file size - header) */
686 totvals = (mfp->length - p->headlen)/sizeof(MYFLT);
687 /* Store the size of a frame in integer */
688 p->lastfram16 = (((totvals - p->nvals) / p->nvals) << 16) - 1;
689 if (UNLIKELY(csound->oparms->odebug))
690 csound->Message(csound, Str(
691 "npoles %"PRIi32", nvals %"PRIi32", totvals %"PRIi32
692 ", lastfram16 = %"PRIi32"x\n"),
693 p->npoles, p->nvals, totvals, p->lastfram16);
694 lpend:
695 p->lastmsg = 0;
696 return OK;
697 }
698
lprdset(CSOUND * csound,LPREAD * p)699 int32_t lprdset(CSOUND *csound, LPREAD *p){
700 return lprdset_(csound, p, 0);
701 }
702
lprdset_S(CSOUND * csound,LPREAD * p)703 int32_t lprdset_S(CSOUND *csound, LPREAD *p){
704 return lprdset_(csound, p, 1);
705 }
706
707
708 #ifdef TRACE_POLES
709 static void
DumpPolesF(int32_t poleCount,MYFLT * part1,MYFLT * part2,int32_t isMagn,char * where)710 DumpPolesF(int32_t poleCount, MYFLT *part1, MYFLT *part2,
711 int32_t isMagn, char *where)
712 {
713 int32_t i;
714
715 csound->Message(csound, "%s\n", where);
716 for (i=0; i<poleCount; i++) {
717 if (isMagn)
718 csound->Message(csound, Str("magnitude: %f Phase: %f\n"),
719 part1[i], part2[i]);
720 else
721 csound->Message(csound, Str("Real: %f Imag: %f\n"),
722 part1[i], part2[i]);
723 }
724 }
725 #endif
726
SortPoles(int32_t poleCount,MYFLT * poleMagn,MYFLT * polePhas)727 static void SortPoles(int32_t poleCount, MYFLT *poleMagn, MYFLT *polePhas)
728 {
729 int32_t i, j;
730 MYFLT diff, fTemp;
731 int32_t shouldSwap;
732
733 /* DumpPolesF(poleCount, poleMagn, polePhas, 1, "Before sort"); */
734
735 for (i=1; i<poleCount; i++) {
736 for (j=0; j<i; j++) {
737
738 shouldSwap = 0;
739
740 diff = FABS(polePhas[j])-FABS(polePhas[i]);
741 if (diff>FL(1.0e-10))
742 shouldSwap = 1;
743 else if (diff>-FL(1.0e-10)) {
744 diff = poleMagn[j]-poleMagn[i];
745
746 if (diff>FL(1.0e-10))
747 shouldSwap = 1;
748 else if (diff>-FL(1.0e-10))
749 {
750 if (polePhas[j]>polePhas[i])
751 shouldSwap = 1;
752 }
753 }
754 if (shouldSwap) {
755 fTemp = poleMagn[i];
756 poleMagn[i] = poleMagn[j];
757 poleMagn[j] = fTemp;
758
759 fTemp = polePhas[i];
760 polePhas[i] = polePhas[j];
761 polePhas[j] = fTemp;
762 }
763 }
764 }
765 /* DumpPolesF(poleCount, poleMagn, polePhas, 1, "After sort"); */
766 }
767
DoPoleInterpolation(int poleCount,MYFLT * pm1,MYFLT * pp1,MYFLT * pm2,MYFLT * pp2,MYFLT factor,MYFLT * outMagn,MYFLT * outPhas)768 static int32_t DoPoleInterpolation(int poleCount,
769 MYFLT *pm1, MYFLT *pp1,
770 MYFLT *pm2, MYFLT *pp2,
771 MYFLT factor, MYFLT *outMagn, MYFLT *outPhas)
772 {
773 int32_t i;
774
775 if (UNLIKELY(poleCount%2!=0)) {
776 printf (Str("Cannot handle uneven pole count yet\n"));
777 return (0);
778 }
779
780 for (i=0; i<poleCount; i++) {
781 if (FABS(FL(FABS(pp1[i])-PI))<FL(1.0e-5)) {
782 pm1[i] = -pm1[i];
783 pp1[i] = FL(0.0);
784 }
785
786 if (FABS(FL(FABS(pp2[i])-PI))<FL(1.0e-5)) {
787 pm2[i] = -pm2[i];
788 pp2[i] = FL(0.0);
789 }
790 }
791
792 /* Sort poles according to abs(phase) */
793
794 SortPoles(poleCount, pm1, pp1);
795 SortPoles(poleCount, pm2, pp2);
796
797 /* DumpPolesF(poleCount, pm1, pp1, 1, "Sorted poles 1"); */
798 /* DumpPolesF(poleCount, pm2, pp2, 1, "Sorted poles 2"); */
799
800 /* printf ("factor=%f\n", factor); */
801
802 for (i=0; i<poleCount; i++) {
803 outMagn[i] = pm1[i]+(pm2[i]-pm1[i])*factor;
804 outPhas[i] = pp1[i]+(pp2[i]-pp1[i])*factor;
805 }
806
807 /* DumpPolesF(poleCount, outMagn, outPhas, 1, "Interpolated poles"); */
808
809 return(1);
810 }
811
InvertPoles(int32_t count,double * real,double * imag)812 static inline void InvertPoles(int32_t count, double *real, double *imag)
813 {
814 int32_t i;
815 double pr,pi,mag;
816
817 for (i=0; i<count; i++) {
818 pr = real[i];
819 pi = imag[i];
820 mag = pr*pr+pi*pi;
821 real[i] = pr/mag;
822 imag[i] = -pi/mag;
823 }
824 }
825
826 /*
827 *
828 * Resynthetize filter coefficients from poles values
829 *
830 */
831
832 static inline void
synthetize(int32_t poleCount,double * poleReal,double * poleImag,double * polyReal,double * polyImag)833 synthetize(int32_t poleCount,
834 double *poleReal, double *poleImag,
835 double *polyReal, double *polyImag)
836 {
837 int32_t j, k;
838 double pr, pi, cr, ci;
839
840 polyReal[0] = 1;
841 polyImag[0] = 0;
842
843 for (j=0; j<poleCount; j++) {
844 polyReal[j+1] = 1;
845 polyImag[j+1] = 0;
846
847 pr = poleReal[j];
848 pi = poleImag[j];
849
850 for (k=j; k>=0; k--) {
851 cr = polyReal[k];
852 ci = polyImag[k];
853
854 polyReal[k] = -(cr*pr-ci*pi);
855 polyImag[k] = -(ci*pr+cr*pi);
856
857 if (LIKELY(k>0)) {
858 polyReal[k] += polyReal[k-1];
859 polyImag[k] += polyImag[k-1];
860 }
861 }
862 }
863
864 /* Makes it 1+a1.x+...+anXn */
865
866 pr = polyReal[0];
867 for (j=0; j<=poleCount; j++)
868 polyReal[j] /= pr;
869 }
870
871 /*
872 *
873 * LPREAD k/a time access. This will setup current pole values
874 *
875 */
876
lpread(CSOUND * csound,LPREAD * p)877 int32_t lpread(CSOUND *csound, LPREAD *p)
878 {
879 MYFLT *bp, *np, *cp;
880 int32_t nn, framphase;
881 MYFLT fract;
882 int32_t i, status;
883 MYFLT *poleMagn1 = p->kcoefs + 2*p->npoles;
884 MYFLT *polePhas1 = poleMagn1 + p->npoles;
885 MYFLT *poleMagn2 = polePhas1 + p->npoles;
886 MYFLT *polePhas2 = poleMagn2 + p->npoles;
887 MYFLT *interMagn = polePhas2 + p->npoles;
888 MYFLT *interPhas = interMagn + p->npoles;
889
890
891 if (UNLIKELY(p->mfp==NULL)) {
892 return csound->PerfError(csound, &(p->h),
893 Str("lpread: not initialised"));
894 }
895 /* Locate frame position range */
896 if (UNLIKELY((framphase = (int32)(*p->ktimpt*p->framrat16)) < 0)) {
897 /* for kfram reqd*/
898 return csound->PerfError(csound, &(p->h),
899 Str("lpread timpnt < 0"));
900 }
901 if (framphase > p->lastfram16) { /* not past last one */
902 framphase = p->lastfram16;
903 if (UNLIKELY(!p->lastmsg)) {
904 p->lastmsg = 1;
905 csound->Warning(csound, Str("lpread ktimpnt truncated to last frame"));
906 }
907 }
908 /* Locate frames bounding current time */
909 bp = (MYFLT *)(p->mfp->beginp + p->headlen); /* locate begin frame data */
910 nn = (framphase >> 16) * p->nvals;
911 bp = bp + nn; /* locate begin this frame */
912 np = bp + p->nvals; /* & interp betw adj frams */
913 fract = (framphase & 0x0FFFFL) / FL(65536.0);
914 /* Interpolate freq/amplpitude and store in opcode */
915 *p->krmr = *bp + (*np - *bp) * fract; bp++; np++; /* for 4 rslts */
916 *p->krmo = *bp + (*np - *bp) * fract; bp++; np++;
917 *p->kerr = *bp + (*np - *bp) * fract; bp++; np++;
918 *p->kcps = *bp + (*np - *bp) * fract; bp++; np++;
919
920 /* Interpolate filter or poles coef values and store in opcode */
921
922 cp = p->kcoefs; /* This is where the coefs get stored */
923 if (p->storePoles) {
924 for (i=0; i<p->npoles; i++) {
925 poleMagn1[i] = *bp++;
926 polePhas1[i] = *bp++;
927 poleMagn2[i] = *np++;
928 polePhas2[i] = *np++;
929 }
930
931 status =
932 DoPoleInterpolation(p->npoles,poleMagn1,polePhas1,poleMagn2,
933 polePhas2,fract,interMagn,interPhas);
934 if (UNLIKELY(!status)) {
935 return csound->PerfError(csound, &(p->h),
936 Str("Interpolation failed"));
937 }
938 for (i=0; i<p->npoles; i++) {
939 *cp++ = interMagn[i];
940 *cp++ = interPhas[i];
941 }
942 }
943 else {
944 for (nn = 0; nn< p->npoles; nn++) {
945 cp[nn] = bp[nn] + (np[nn] - bp[nn]) * fract;
946 }
947 }
948 /* if (csound->oparms->odebug) {
949 csound->Message(csound,
950 "phase:%lx fract:%6.2f rmsr:%6.2f rmso:%6.2f kerr:%6.2f kcps:%6.2f\n",
951 framphase,fract,*p->krmr,*p->krmo,*p->kerr,*p->kcps);
952 cp = p->kcoefs;
953 nn = p->npoles;
954 do {
955 csound->Message(csound, " %6.2f",*cp++);
956 } while (--nn);
957 csound->Message(csound, "\n");
958 } */
959 return OK;
960 }
961
962
lpformantset(CSOUND * csound,LPFORM * p)963 int32_t lpformantset(CSOUND *csound, LPFORM *p)
964 {
965 LPREAD *q;
966
967 /* connect to previously loaded lpc analysis */
968 /* get adr lpread struct */
969 p->lpread = q = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
970 csound->AuxAlloc(csound, p->lpread->npoles*sizeof(MYFLT), &p->aux);
971 return OK;
972 }
973
lpformant(CSOUND * csound,LPFORM * p)974 int32_t lpformant(CSOUND *csound, LPFORM *p)
975 {
976 LPREAD *q = p->lpread;
977 MYFLT *coefp, sr = csound->esr;
978 MYFLT *cfs = (MYFLT*)p->aux.auxp;
979 MYFLT *bws = cfs+p->lpread->npoles/2;
980 int32_t i, j, ndx = *p->kfor;
981 double pm,pp;
982
983 if (q->storePoles) {
984 coefp = q->kcoefs;
985 for (i=2,j=0; i<q->npoles*2; i+=4, j++) {
986 pm = coefp[i];
987 pp = coefp[i+1];
988 cfs[j] = pp*sr/TWOPI;
989 /* if (pm > 1.0) csound->Message(csound,
990 Str("warning unstable pole %f\n"), pm); */
991 bws[j] = -LOG(pm)*sr/PI;
992 }
993 }
994 else {
995 csound->PerfError(csound, &(p->h),
996 Str("this opcode only works with LPC "
997 "pole analysis type (-a)\n"));
998 return NOTOK;
999 }
1000
1001 j = (ndx < 1 ? 1 :
1002 (ndx >= p->lpread->npoles/2 ? p->lpread->npoles/2 : ndx)) - 1;
1003 if (bws[j] > sr/2 || isnan(bws[j])) bws[j] = sr/2;
1004 if (bws[j] < 1.0) bws[j] = 1.0;
1005 if (cfs[j] > sr/2 || isnan(cfs[j])) cfs[j] = sr/2;
1006 cfs[j] = FABS(cfs[j]);
1007 *p->kcf = cfs[j];
1008 *p->kbw = bws[j];
1009
1010 return OK;
1011 }
1012
1013
1014 /*
1015 *
1016 * LPRESON: initialisation time
1017 *
1018 *
1019 */
lprsnset(CSOUND * csound,LPRESON * p)1020 int32_t lprsnset(CSOUND *csound, LPRESON *p)
1021 {
1022 LPREAD *q;
1023
1024 /* connect to previously loaded lpc analysis */
1025 /* get adr lpread struct */
1026 p->lpread = q = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
1027
1028 csound->AuxAlloc(csound, (int32)((q->npoles<<1)*sizeof(MYFLT)), &p->aux);
1029 /* Initialize pointer to circulat buffer (for filtering) */
1030 p->circjp = p->circbuf = (MYFLT*)p->aux.auxp;
1031 p->jp2lim = p->circbuf + (q->npoles << 1); /* npoles det circbuflim */
1032 return OK;
1033 }
1034
1035 /*
1036 *
1037 * LPRESON: k & a time access. Will actually filter the signal
1038 * Uses a circular buffer to store previous signal values.
1039 */
1040
lpreson(CSOUND * csound,LPRESON * p)1041 int32_t lpreson(CSOUND *csound, LPRESON *p)
1042 {
1043 IGN(csound);
1044 LPREAD *q = p->lpread;
1045 uint32_t offset = p->h.insdshead->ksmps_offset;
1046 uint32_t early = p->h.insdshead->ksmps_no_end;
1047 uint32_t n, nsmps = CS_KSMPS;
1048 MYFLT *coefp, *pastp, *jp, *jp2, *rslt = p->ar, *asig = p->asig;
1049 MYFLT x;
1050 double poleReal[MAXPOLES], poleImag[MAXPOLES];
1051 double polyReal[MAXPOLES+1], polyImag[MAXPOLES+1];
1052 int32_t i, nn;
1053 double pm,pp;
1054
1055 jp = p->circjp;
1056 jp2 = jp + q->npoles;
1057
1058 /* If we where using poles, we have to compute filter coefs now */
1059 if (q->storePoles) {
1060 coefp = q->kcoefs;
1061 for (i=0; i<q->npoles; i++) {
1062 pm = *coefp++;
1063 pp = *coefp++;
1064 /* csound->Message(csound, "pole %d, fr=%.2f, BW=%.2f\n", i,
1065 pp*(csound->esr)/6.28, -csound->esr*log(pm)/3.14);
1066 */
1067 if (fabs(pm)>0.999999)
1068 pm = 1/pm;
1069 poleReal[i] = pm*cos(pp);
1070 poleImag[i] = pm*sin(pp);
1071 }
1072 /* DumpPoles(q->npoles,poleReal,poleImag,0,"About to filter"); */
1073 InvertPoles(q->npoles,poleReal,poleImag);
1074 synthetize(q->npoles,poleReal,poleImag,polyReal,polyImag);
1075 coefp = q->kcoefs;
1076 for (i=0; i<q->npoles; i++) {
1077 /* MR_WHY - somthing with the atan2 ? */
1078 coefp[i] = -(MYFLT)polyReal[q->npoles-i];
1079 }
1080 }
1081
1082 if (UNLIKELY(offset)) memset(rslt, '\0', offset*sizeof(MYFLT));
1083 if (UNLIKELY(early)) {
1084 nsmps -= early;
1085 memset(&rslt[nsmps], '\0', early*sizeof(MYFLT));
1086 }
1087 /* For each sample */
1088 for (n=offset; n<nsmps; n++) {
1089 /* Compute Xn = Yn + CkXn-k */
1090
1091 #ifdef TRACE_FILTER
1092 csound->Message(csound, "Asig=%f\n", *asig);
1093 #endif
1094 x = asig[n];
1095 coefp = q->kcoefs; /* using lpread interp coefs */
1096 pastp = jp;
1097 nn = q->npoles;
1098 do {
1099 #ifdef TRACE_FILTER
1100 csound->Message(csound, "\t%f,%f\n", *coefp, *pastp);
1101 #endif
1102 x += *coefp++ * *pastp++;
1103 } while (--nn);
1104 #ifdef TRACE_FILTER
1105 csound->Message(csound, "result=%f\n", x);
1106 #endif
1107 /* Store result signal in circular and output buffers */
1108
1109 *jp++ = *jp2++ = x;
1110 rslt[n] = x;
1111
1112 /* Check if end of buffer reached */
1113 if (jp2 >= p->jp2lim) {
1114 jp2 = jp;
1115 jp = p->circbuf;
1116 }
1117 }
1118 p->circjp = jp;
1119 return OK;
1120 }
1121
1122 /*
1123 *
1124 * LPFRESON : Initialisation time
1125 *
1126 */
lpfrsnset(CSOUND * csound,LPFRESON * p)1127 int32_t lpfrsnset(CSOUND *csound, LPFRESON *p)
1128 {
1129
1130 /* Connect to previously loaded analysis file */
1131
1132 if (((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot]->storePoles) {
1133 return csound->InitError(csound, Str("Pole file not supported "
1134 "for this opcode !"));
1135 }
1136
1137
1138 p->lpread = ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot];
1139 if(p->lpread->npoles < 2) {
1140 return csound->InitError(csound, Str("Too few poles (> 2)"));
1141 }
1142
1143 p->prvratio = FL(1.0);
1144 p->d = FL(0.0);
1145 p->prvout = FL(0.0);
1146 csound->AuxAlloc(csound, (int32)(p->lpread->npoles*sizeof(MYFLT)), &p->aux);
1147 p->past = (MYFLT*)p->aux.auxp;
1148
1149 return OK;
1150 }
1151
1152 /*
1153 *
1154 * LPFRESON : k & a time : actually filters the data
1155 *
1156 */
lpfreson(CSOUND * csound,LPFRESON * p)1157 int32_t lpfreson(CSOUND *csound, LPFRESON *p)
1158 {
1159 LPREAD *q = p->lpread;
1160 uint32_t offset = p->h.insdshead->ksmps_offset;
1161 uint32_t early = p->h.insdshead->ksmps_no_end;
1162 uint32_t nn, n, nsmps = CS_KSMPS;
1163 MYFLT *coefp, *pastp, *pastp1, *rslt = p->ar, *asig = p->asig;
1164 MYFLT x, temp1, temp2, ampscale, cq;
1165
1166 if (*p->kfrqratio != p->prvratio) { /* for new freqratio */
1167 if (*p->kfrqratio <= FL(0.0)) {
1168 return csound->PerfError(csound, &(p->h),
1169 Str("illegal frqratio, %5.2f"),
1170 *p->kfrqratio);
1171 } /* calculate d */
1172 p->d = (*p->kfrqratio - FL(1.0)) / (*p->kfrqratio + FL(1.0));
1173 p->prvratio = *p->kfrqratio;
1174 }
1175 if (p->d != FL(0.0)) { /* for non-zero d, */
1176 coefp = q->kcoefs;
1177 nn = q->npoles - 1;
1178 do {
1179 temp1 = p->d * *coefp++; /* shift formants */
1180 *coefp += temp1;
1181 }
1182 while (--nn);
1183 ampscale = FL(1.0) / (FL(1.0) - p->d * *coefp); /* & reset scales */
1184 cq = (FL(1.0) - p->d * p->d) * ampscale;
1185 }
1186 else {
1187 cq = FL(1.0);
1188 ampscale = FL(1.0);
1189 }
1190 x = p->prvout;
1191 if (UNLIKELY(offset)) memset(rslt, '\0', offset*sizeof(MYFLT));
1192 if (UNLIKELY(early)) {
1193 nsmps -= early;
1194 memset(&rslt[nsmps], '\0', early*sizeof(MYFLT));
1195 }
1196 for (n=offset; n<nsmps; n++) {
1197 nn = q->npoles - 1;
1198 pastp = pastp1 = p->past + nn;
1199 temp1 = *pastp;
1200 *pastp = cq * x - p->d * *pastp;
1201 pastp--;
1202 do {
1203 temp2 = *pastp;
1204 *pastp = (*pastp1 - *pastp) * p->d + temp1;
1205 pastp--; pastp1--;
1206 temp1 = temp2;
1207 } while (--nn);
1208 x = asig[n];
1209 pastp = p->past;
1210 coefp = q->kcoefs;
1211 nn = q->npoles;
1212 do {
1213 x += *coefp++ * *pastp++;
1214 } while (--nn);
1215 rslt[n] = x * ampscale;
1216 }
1217 p->prvout = x;
1218 return OK;
1219 }
1220
rmsset(CSOUND * csound,RMS * p)1221 int32_t rmsset(CSOUND *csound, RMS *p)
1222 {
1223 double b;
1224
1225 b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1226 p->c2 = b - sqrt(b*b - 1.0);
1227 p->c1 = 1.0 - p->c2;
1228 if (!*p->istor)
1229 p->prvq = 0.0;
1230 return OK;
1231 }
1232
gainset(CSOUND * csound,GAIN * p)1233 int32_t gainset(CSOUND *csound, GAIN *p)
1234 {
1235 double b;
1236
1237 b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1238 p->c2 = b - sqrt(b*b - 1.0);
1239 p->c1 = 1.0 - p->c2;
1240 if (!*p->istor)
1241 p->prvq = p->prva = 0.0;
1242 return OK;
1243 }
1244
balnset(CSOUND * csound,BALANCE * p)1245 int32_t balnset(CSOUND *csound, BALANCE *p)
1246 {
1247 double b;
1248
1249 b = 2.0 - cos((double)(*p->ihp * csound->tpidsr));
1250 p->c2 = b - sqrt(b*b - 1.0);
1251 p->c1 = 1.0 - p->c2;
1252 if (!*p->istor)
1253 p->prvq = p->prvr = p->prva = 0.0;
1254 return OK;
1255 }
1256
rms(CSOUND * csound,RMS * p)1257 int32_t rms(CSOUND *csound, RMS *p)
1258 {
1259 IGN(csound);
1260 uint32_t offset = p->h.insdshead->ksmps_offset;
1261 uint32_t early = p->h.insdshead->ksmps_no_end;
1262 uint32_t n, nsmps = CS_KSMPS;
1263 MYFLT *asig;
1264 double q;
1265 double c1 = p->c1, c2 = p->c2;
1266
1267 q = p->prvq;
1268 asig = p->asig;
1269 if (UNLIKELY(early)) nsmps -= early;
1270 for (n=offset; n<nsmps; n++) {
1271 double as = (double)asig[n];
1272 q = c1 * as * as + c2 * q;
1273 }
1274 p->prvq = q;
1275 *p->kr = (MYFLT) sqrt(q);
1276 return OK;
1277 }
1278
gain(CSOUND * csound,GAIN * p)1279 int32_t gain(CSOUND *csound, GAIN *p)
1280 {
1281 IGN(csound);
1282 uint32_t offset = p->h.insdshead->ksmps_offset;
1283 uint32_t early = p->h.insdshead->ksmps_no_end;
1284 uint32_t n, nsmps = CS_KSMPS;
1285 MYFLT *ar, *asig;
1286 double q, a, m, diff, inc;
1287 double c1 = p->c1, c2 = p->c2;
1288
1289 q = p->prvq;
1290 asig = p->asig;
1291 if (UNLIKELY(early)) nsmps -= early;
1292 for (n = offset; n < nsmps-early; n++) {
1293 double as = (double)asig[n];
1294 q = c1 * as * as + c2 * q;
1295 }
1296 p->prvq = q;
1297 if (q > 0.0)
1298 a = *p->krms / sqrt(q);
1299 else
1300 a = *p->krms;
1301 ar = p->ar;
1302 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1303 if (UNLIKELY(early)) {
1304 nsmps -= early;
1305 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1306 }
1307 if ((diff = a - p->prva) != 0.0) {
1308 m = p->prva;
1309 inc = diff / (double)(nsmps-offset);
1310 for (n = offset; n < nsmps; n++) {
1311 ar[n] = asig[n] * m;
1312 m += inc;
1313 }
1314 p->prva = a;
1315 }
1316 else {
1317 for (n = offset; n < nsmps; n++) {
1318 ar[n] = asig[n] * a;
1319 }
1320 }
1321 return OK;
1322 }
1323
balance(CSOUND * csound,BALANCE * p)1324 int32_t balance(CSOUND *csound, BALANCE *p)
1325 {
1326 IGN(csound);
1327 uint32_t offset = p->h.insdshead->ksmps_offset;
1328 uint32_t early = p->h.insdshead->ksmps_no_end;
1329 uint32_t n, nsmps = CS_KSMPS;
1330 MYFLT *ar, *asig, *csig;
1331 double q, r, a, m, diff, inc;
1332 double c1 = p->c1, c2 = p->c2;
1333
1334 q = p->prvq;
1335 r = p->prvr;
1336 asig = p->asig;
1337 csig = p->csig;
1338 ar = p->ar;
1339 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1340 if (UNLIKELY(early)) {
1341 nsmps -= early;
1342 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1343 }
1344 for (n = offset; n < nsmps; n++) {
1345 double as = (double)asig[n];
1346 double cs = (double)csig[n];
1347 q = c1 * as * as + c2 * q;
1348 r = c1 * cs * cs + c2 * r;
1349 }
1350 p->prvq = q;
1351 p->prvr = r;
1352 if (LIKELY(q != 0.0))
1353 a = sqrt(r/q);
1354 else
1355 a = sqrt(r);
1356 if ((diff = a - p->prva) != 0.0) {
1357 m = p->prva;
1358 inc = diff / (double)(nsmps-offset);
1359 for (n = offset; n < nsmps; n++) {
1360 ar[n] = asig[n] * m;
1361 m += inc;
1362 }
1363 p->prva = a;
1364 }
1365 else {
1366 for (n = offset; n < nsmps; n++) {
1367 ar[n] = asig[n] * a;
1368 }
1369 }
1370 return OK;
1371 }
1372
1373
balance2(CSOUND * csound,BALANCE * p)1374 int32_t balance2(CSOUND *csound, BALANCE *p)
1375 {
1376 IGN(csound);
1377 uint32_t offset = p->h.insdshead->ksmps_offset;
1378 uint32_t early = p->h.insdshead->ksmps_no_end;
1379 uint32_t n, nsmps = CS_KSMPS;
1380 MYFLT *ar, *asig, *csig;
1381 double q, r, a;
1382 double c1 = p->c1, c2 = p->c2;
1383
1384 q = p->prvq;
1385 r = p->prvr;
1386 asig = p->asig;
1387 csig = p->csig;
1388 ar = p->ar;
1389 if (UNLIKELY(offset)) memset(ar, '\0', offset*sizeof(MYFLT));
1390 if (UNLIKELY(early)) {
1391 nsmps -= early;
1392 memset(&ar[nsmps], '\0', early*sizeof(MYFLT));
1393 }
1394 for (n = offset; n < nsmps; n++) {
1395 double as = (double)asig[n];
1396 double cs = (double)csig[n];
1397 q = c1 * as * as + c2 * q;
1398 r = c1 * cs * cs + c2 * r;
1399 if (LIKELY(q != 0.0))
1400 a = sqrt(r/q);
1401 else
1402 a = sqrt(r);
1403 ar[n] = asig[n] * a;
1404 }
1405 p->prvq = q;
1406 p->prvr = r;
1407 return OK;
1408 }
1409
1410 /*
1411 * Set current lpc slot
1412 */
lpslotset(CSOUND * csound,LPSLOT * p)1413 int32_t lpslotset(CSOUND *csound, LPSLOT *p)
1414 {
1415 int32_t n;
1416
1417 n = (int32_t) *(p->islotnum);
1418 if (UNLIKELY(n < 0))
1419 return csound->InitError(csound, Str("lpslot number should be positive"));
1420 else {
1421 if (n >= csound->max_lpc_slot) {
1422 csound->max_lpc_slot = n + MAX_LPC_SLOT;
1423 csound->lprdaddr = csound->ReAlloc(csound,
1424 csound->lprdaddr,
1425 csound->max_lpc_slot * sizeof(LPREAD**));
1426 }
1427 csound->currentLPCSlot = n;
1428 }
1429 return OK;
1430 }
1431
lpitpset(CSOUND * csound,LPINTERPOL * p)1432 int32_t lpitpset(CSOUND *csound, LPINTERPOL *p)
1433 {
1434
1435 if (UNLIKELY((uint32_t) ((int32_t) *(p->islot1))
1436 >= (uint32_t) csound->max_lpc_slot ||
1437 (uint32_t) ((int32_t) *(p->islot2))
1438 >= (uint32_t) csound->max_lpc_slot))
1439 return csound->InitError(csound, Str("LPC slot is not allocated"));
1440 /* Get lpread pointers */
1441 p->lp1 = ((LPREAD**) csound->lprdaddr)[(int32_t) *(p->islot1)];
1442 p->lp2 = ((LPREAD**) csound->lprdaddr)[(int32_t) *(p->islot2)];
1443
1444 /* Check if workable */
1445
1446 if (UNLIKELY((!p->lp1->storePoles) || (!p->lp2->storePoles))) {
1447 return csound->InitError(csound, Str("lpinterpol works only "
1448 "with poles files.."));
1449 }
1450 if (UNLIKELY(p->lp1->npoles != p->lp2->npoles)) {
1451 return csound->InitError(csound, Str("The poles files "
1452 "have different pole count"));
1453 }
1454
1455 #if 0 /* This is incorrect C */
1456 if (&p->kcoefs-p != &p->lp1->kcoefs-p->lp1)
1457 return csound->InitError(csound, Str("padding error"));
1458 #endif
1459
1460 p->npoles = p->lp1->npoles;
1461 csound->AuxAlloc(csound, (int32)(p->npoles*8*sizeof(MYFLT)), &p->aux);
1462 p->kcoefs = (MYFLT*)p->aux.auxp;
1463 p->storePoles = 1;
1464 ((LPREAD**) csound->lprdaddr)[csound->currentLPCSlot] = (LPREAD*) p;
1465 return OK;
1466 }
1467
lpinterpol(CSOUND * csound,LPINTERPOL * p)1468 int32_t lpinterpol(CSOUND *csound, LPINTERPOL *p)
1469 {
1470 int32_t i,status;
1471 MYFLT *cp,*cp1,*cp2;
1472 MYFLT *poleMagn1 = p->kcoefs + 2*p->npoles;
1473 MYFLT *polePhas1 = poleMagn1 + p->npoles;
1474 MYFLT *poleMagn2 = polePhas1 + p->npoles;
1475 MYFLT *polePhas2 = poleMagn2 + p->npoles;
1476 MYFLT *interMagn = polePhas2 + p->npoles;
1477 MYFLT *interPhas = interMagn + p->npoles;
1478
1479 /* RWD: guessing this... */
1480 if (UNLIKELY(p->lp1==NULL || p->lp2==NULL)) {
1481 return csound->PerfError(csound, &(p->h),
1482 Str("lpinterpol: not initialised"));
1483 }
1484 cp1 = p->lp1->kcoefs;
1485 cp2 = p->lp2->kcoefs;
1486
1487 for (i=0; i<p->npoles; i++) {
1488 poleMagn1[i] = *cp1++;
1489 polePhas1[i] = *cp1++;
1490 poleMagn2[i] = *cp2++;
1491 polePhas2[i] = *cp2++;
1492 }
1493
1494 status = DoPoleInterpolation(p->npoles,poleMagn1,polePhas1,poleMagn2,
1495 polePhas2,*p->kmix,interMagn,interPhas);
1496 if (UNLIKELY(!status)) {
1497 return csound->PerfError(csound, &(p->h),
1498 Str("Interpolation failed"));
1499 }
1500
1501 cp = p->kcoefs; /* This is where the coefs get stored */
1502 for (i=0; i<p->npoles; i++) {
1503 *cp++ = interMagn[i];
1504 *cp++ = interPhas[i];
1505 }
1506 return OK;
1507 }
1508
1509
klimit(CSOUND * csound,LIMIT * p)1510 int32_t klimit(CSOUND *csound, LIMIT *p)
1511 {
1512 IGN(csound);
1513 MYFLT sig=*p->sig, min=*p->min, max=*p->max;
1514 if (LIKELY((sig <= max) && (sig >= min))) {
1515 *p->ans = sig;
1516 }
1517 else {
1518 if ( min >= max) {
1519 *p->ans = FL(0.5) * (min + max);
1520 }
1521 else {
1522 if (sig > max)
1523 *p->ans = max;
1524 else
1525 *p->ans = min;
1526 }
1527 }
1528 return OK;
1529 }
1530
limit(CSOUND * csound,LIMIT * p)1531 int32_t limit(CSOUND *csound, LIMIT *p)
1532 {
1533 IGN(csound);
1534 MYFLT *ans, *asig;
1535 MYFLT min=*p->min, max=*p->max, aver;
1536 uint32_t offset = p->h.insdshead->ksmps_offset;
1537 uint32_t early = p->h.insdshead->ksmps_no_end;
1538 uint32_t n, nsmps = CS_KSMPS;
1539 ans = p->ans;
1540 asig = p->sig;
1541
1542 if (UNLIKELY(offset)) memset(ans, '\0', offset*sizeof(MYFLT));
1543 if (UNLIKELY(early)) {
1544 nsmps -= early;
1545 memset(&ans[nsmps], '\0', early*sizeof(MYFLT));
1546 }
1547 if (min >= max) {
1548 aver = (min + max) * FL(0.5);
1549 for (n=offset; n<nsmps; n++) {
1550 ans[n] = aver;
1551 }
1552 }
1553 else
1554 for (n=offset; n<nsmps; n++) {
1555 if ((asig[n] <= max) && (asig[n] >= min)) {
1556 ans[n] = asig[n];
1557 }
1558 else {
1559 if (asig[n] > max)
1560 ans[n] = max;
1561 else
1562 ans[n] = min;
1563 }
1564 }
1565 return OK;
1566 }
1567