1 /*
2 pitchtrack.c
3
4 kcps, kamp ptrack asig, ihopsize [, ipeaks]
5
6 Copyright (c) Victor Lazzarini, 2007
7
8 based on M Puckette's pitch tracking algorithm.
9
10 This file is part of Csound.
11
12 The Csound Library is free software; you can redistribute it
13 and/or modify it under the terms of the GNU Lesser General Public
14 License as published by the Free Software Foundation; either
15 version 2.1 of the License, or (at your option) any later version.
16
17 Csound is distributed in the hope that it will be useful,
18 but WITHOUT ANY WARRANTY; without even the implied warranty of
19 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 GNU Lesser General Public License for more details.
21
22 You should have received a copy of the GNU Lesser General Public
23 License along with Csound; if not, write to the Free Software
24 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
25 02110-1301 USA
26 */
27
28 #include "csoundCore.h"
29 #include "interlocks.h"
30 #include <math.h>
31
32 #define MINFREQINBINS 5
33 #define MAXHIST 3
34 #define MAXWINSIZ 8192
35 #define MINWINSIZ 128
36 #define DEFAULTWINSIZ 1024
37 #define NPREV 20
38 #define MAXPEAKNOS 100
39 #define DEFAULTPEAKNOS 20
40 #define MINBW FL(0.03)
41 #define BINPEROCT 48
42 #define BPEROOVERLOG2 69.24936196
43 #define FACTORTOBINS FL(4/0.0145453)
44 #define BINGUARD 10
45 #define PARTIALDEVIANCE FL(0.023)
46 #define DBSCAL 3.333
47 #define DBOFFSET FL(-92.3)
48 #define MINBIN 3
49 #define MINAMPS 40
50 #define MAXAMPS 50
51
52
53 #define THRSH FL(10.)
54
55
56 static const MYFLT partialonset[] =
57 {
58 FL(0.0),
59 FL(48.0),
60 FL(76.0782000346154967102),
61 FL(96.0),
62 FL(111.45254855459339269887),
63 FL(124.07820003461549671089),
64 FL(134.75303625876499715823),
65 FL(144.0),
66 FL(152.15640006923099342109),
67 FL(159.45254855459339269887),
68 FL(166.05271769459026829915),
69 FL(172.07820003461549671088),
70 FL(177.62110647077242370064),
71 FL(182.75303625876499715892),
72 FL(187.53074858920888940907),
73 FL(192.0),
74 };
75
76 #define NPARTIALONSET ((int32_t)(sizeof(partialonset)/sizeof(MYFLT)))
77
78
79 #define COEF1 ((MYFLT)(.5 * 1.227054))
80 #define COEF2 ((MYFLT)(.5 * -0.302385))
81 #define COEF3 ((MYFLT)(.5 * 0.095326))
82 #define COEF4 ((MYFLT)(.5 * -0.022748))
83 #define COEF5 ((MYFLT)(.5 * 0.002533))
84 #define FLTLEN 5
85
86
87 typedef struct peak
88 {
89 MYFLT pfreq;
90 MYFLT pwidth;
91 MYFLT ppow;
92 MYFLT ploudness;
93 } PEAK;
94
95 typedef struct histopeak
96 {
97 MYFLT hpitch;
98 MYFLT hvalue;
99 MYFLT hloud;
100 int32_t hindex;
101 int32_t hused;
102 } HISTOPEAK;
103
104
105 typedef struct pitchtrack
106 {
107 OPDS h;
108 MYFLT *freq, *amp;
109 MYFLT *asig,*size,*peak;
110 AUXCH signal, prev, sin, spec1, spec2, peakarray;
111 int32_t numpks;
112 int32_t cnt;
113 int32_t histcnt;
114 int32_t hopsize;
115 MYFLT sr;
116 MYFLT cps;
117 MYFLT dbs[NPREV];
118 MYFLT amplo;
119 MYFLT amphi;
120 MYFLT npartial;
121 MYFLT dbfs;
122 MYFLT prevf;
123 } PITCHTRACK;
124
ptrack(CSOUND * csound,PITCHTRACK * p)125 void ptrack(CSOUND *csound,PITCHTRACK *p)
126 {
127 MYFLT *spec = (MYFLT *)p->spec1.auxp;
128 MYFLT *spectmp = (MYFLT *)p->spec2.auxp;
129 MYFLT *sig = (MYFLT *)p->signal.auxp;
130 MYFLT *sinus = (MYFLT *)p->sin.auxp;
131 MYFLT *prev = (MYFLT *)p->prev.auxp;
132 PEAK *peaklist = (PEAK *)p->peakarray.auxp;
133 HISTOPEAK histpeak;
134 int32_t i, j, k, hop = p->hopsize, n = 2*hop, npeak, logn = -1, count, tmp;
135 MYFLT totalpower, totalloudness, totaldb;
136 MYFLT maxbin, *histogram = spectmp + BINGUARD;
137 MYFLT hzperbin = p->sr / (n + n);
138 int32_t numpks = p->numpks;
139 int32_t indx, halfhop = hop>>1;
140 MYFLT best;
141 MYFLT cumpow = 0, cumstrength = 0, freqnum = 0, freqden = 0;
142 int32_t npartials = 0, nbelow8 = 0;
143 MYFLT putfreq;
144
145 count = p->histcnt + 1;
146 if (count == NPREV) count = 0;
147 p->histcnt = count;
148
149 tmp = n;
150 while (tmp) {
151 tmp >>= 1;
152 logn++;
153 }
154 maxbin = BINPEROCT * (logn-2);
155
156 for (i = 0, k = 0; i < hop; i++, k += 2) {
157 spec[k] = sig[i] * sinus[k];
158 spec[k+1] = sig[i] * sinus[k+1];
159 }
160
161 csound->ComplexFFT(csound, spec, hop);
162
163 for (i = 0, k = 2*FLTLEN; i < hop; i+=2, k += 4) {
164 spectmp[k] = spec[i];
165 spectmp[k+1] = spec[i+1];
166 }
167 for (i = n - 2, k = 2*FLTLEN+2; i >= 0; i-=2, k += 4) {
168 spectmp[k] = spec[i];
169 spectmp[k+1] = -spec[i+1];
170 }
171 for (i = (2*FLTLEN), k = (2*FLTLEN-2);i<FLTLEN*4; i+=2, k-=2) {
172 spectmp[k] = spectmp[i];
173 spectmp[k+1] = -spectmp[i+1];
174 }
175 for (i = (2*FLTLEN+n-2), k =(2*FLTLEN+n); i>=0; i-=2, k+=2) {
176 spectmp[k] = spectmp[i];
177 spectmp[k+1] = -spectmp[k+1];
178 }
179
180 for (i = j = 0, k = 2*FLTLEN; i < halfhop; i++, j+=8, k+=2) {
181 MYFLT re, im;
182
183 re= COEF1 * ( prev[k-2] - prev[k+1] + spectmp[k-2] - prev[k+1]) +
184 COEF2 * ( prev[k-3] - prev[k+2] + spectmp[k-3] - spectmp[ 2]) +
185 COEF3 * (-prev[k-6] +prev[k+5] -spectmp[k-6] +spectmp[k+5]) +
186 COEF4 * (-prev[k-7] +prev[k+6] -spectmp[k-7] +spectmp[k+6]) +
187 COEF5 * ( prev[k-10] -prev[k+9] +spectmp[k-10] -spectmp[k+9]);
188
189 im= COEF1 * ( prev[k-1] +prev[k] +spectmp[k-1] +spectmp[k]) +
190 COEF2 * (-prev[k-4] -prev[k+3] -spectmp[k-4] -spectmp[k+3]) +
191 COEF3 * (-prev[k-5] -prev[k+4] -spectmp[k-5] -spectmp[k+4]) +
192 COEF4 * ( prev[k-8] +prev[k+7] +spectmp[k-8] +spectmp[k+7]) +
193 COEF5 * ( prev[k-9] +prev[k+8] +spectmp[k-9] +spectmp[k+8]);
194
195 spec[j] = FL(0.707106781186547524400844362104849) * (re + im);
196 spec[j+1] = FL(0.707106781186547524400844362104849) * (im - re);
197 spec[j+4] = prev[k] + spectmp[k+1];
198 spec[j+5] = prev[k+1] - spectmp[k];
199
200 j += 8;
201 k += 2;
202 re= COEF1 * ( prev[k-2] -prev[k+1] -spectmp[k-2] +spectmp[k+1]) +
203 COEF2 * ( prev[k-3] -prev[k+2] -spectmp[k-3] +spectmp[k+2]) +
204 COEF3 * (-prev[k-6] +prev[k+5] +spectmp[k-6] -spectmp[k+5]) +
205 COEF4 * (-prev[k-7] +prev[k+6] +spectmp[k-7] -spectmp[k+6]) +
206 COEF5 * ( prev[k-10] -prev[k+9] -spectmp[k-10] +spectmp[k+9]);
207
208 im= COEF1 * ( prev[k-1] +prev[k] -spectmp[k-1] -spectmp[k]) +
209 COEF2 * (-prev[k-4] -prev[k+3] +spectmp[k-4] +spectmp[k+3]) +
210 COEF3 * (-prev[k-5] -prev[k+4] +spectmp[k-5] +spectmp[k+4]) +
211 COEF4 * ( prev[k-8] +prev[k+7] -spectmp[k-8] -spectmp[k+7]) +
212 COEF5 * ( prev[k-9] +prev[k+8] -spectmp[k-9] -spectmp[k+8]);
213
214 spec[j] = FL(0.707106781186547524400844362104849) * (re + im);
215 spec[j+1] = FL(0.707106781186547524400844362104849) * (im - re);
216 spec[j+4] = prev[k] - spectmp[k+1];
217 spec[j+5] = prev[k+1] + spectmp[k];
218
219 }
220
221
222 for (i = 0; i < n + 4*FLTLEN; i++) prev[i] = spectmp[i];
223
224 for (i = 0; i < MINBIN; i++) spec[4*i + 2] = spec[4*i + 3] = FL(0.0);
225
226 for (i = 4*MINBIN, totalpower = 0; i < (n-2)*4; i += 4) {
227 MYFLT re = spec[i] - FL(0.5) * (spec[i-8] + spec[i+8]);
228 MYFLT im = spec[i+1] - FL(0.5) * (spec[i-7] + spec[i+9]);
229 spec[i+3] = (totalpower += (spec[i+2] = re * re + im * im));
230 }
231
232 if (totalpower > FL(1.0e-9)) {
233 totaldb = FL(DBSCAL) * LOG(totalpower/n);
234 totalloudness = SQRT(SQRT(totalpower));
235 if (totaldb < 0) totaldb = 0;
236 }
237 else totaldb = totalloudness = FL(0.0);
238
239 p->dbs[count] = totaldb + DBOFFSET;
240
241 if (totaldb >= p->amplo) {
242
243 npeak = 0;
244
245 for (i = 4*MINBIN;i < (4*(n-2)) && npeak < numpks; i+=4) {
246 MYFLT height = spec[i+2], h1 = spec[i-2], h2 = spec[i+6];
247 MYFLT totalfreq, peakfr, tmpfr1, tmpfr2, m, var, stdev;
248
249 if (height < h1 || height < h2 ||
250 h1 < FL(0.00001)*totalpower ||
251 h2 < FL(0.00001)*totalpower) continue;
252
253 peakfr= ((spec[i-8] - spec[i+8]) * (FL(2.0) * spec[i] -
254 spec[i+8] - spec[i-8]) +
255 (spec[i-7] - spec[i+9]) * (FL(2.0) * spec[i+1] -
256 spec[i+9] - spec[i-7])) /
257 (height + height);
258 tmpfr1= ((spec[i-12] - spec[i+4]) *
259 (FL(2.0) * spec[i-4] - spec[i+4] - spec[i-12]) +
260 (spec[i-11] - spec[i+5]) * (FL(2.0) * spec[i-3] -
261 spec[i+5] - spec[i-11])) /
262 (FL(2.0) * h1) - 1;
263 tmpfr2= ((spec[i-4] - spec[i+12]) * (FL(2.0) * spec[i+4] -
264 spec[i+12] - spec[i-4]) +
265 (spec[i-3] - spec[i+13]) * (FL(2.0) * spec[i+5] -
266 spec[i+13] - spec[i-3])) /
267 (FL(2.0) * h2) + 1;
268
269
270 m = FL(0.333333333333) * (peakfr + tmpfr1 + tmpfr2);
271 var = FL(0.5) * ((peakfr-m)*(peakfr-m) +
272 (tmpfr1-m)*(tmpfr1-m) + (tmpfr2-m)*(tmpfr2-m));
273
274 totalfreq = (i>>2) + m;
275 if (var * totalpower > THRSH * height
276 || var < FL(1.0e-30)) continue;
277
278 stdev = (MYFLT)sqrt((double)var);
279 if (totalfreq < 4) totalfreq = 4;
280
281
282 peaklist[npeak].pwidth = stdev;
283 peaklist[npeak].ppow = height;
284 peaklist[npeak].ploudness = SQRT(SQRT(height));
285 peaklist[npeak].pfreq = totalfreq;
286 npeak++;
287
288 }
289
290 if (npeak > numpks) npeak = numpks;
291 for (i = 0; i < maxbin; i++) histogram[i] = 0;
292 //or memset(histogram, '\0', maxbin*sizeof(MYFLT));
293 for (i = 0; i < npeak; i++) {
294 MYFLT pit = (MYFLT)(BPEROOVERLOG2 * LOG(peaklist[i].pfreq) - 96.0);
295 MYFLT binbandwidth = FACTORTOBINS * peaklist[i].pwidth/peaklist[i].pfreq;
296 MYFLT putbandwidth = (binbandwidth < FL(2.0) ? FL(2.0) : binbandwidth);
297 MYFLT weightbandwidth = (binbandwidth < FL(1.0) ? FL(1.0) : binbandwidth);
298 MYFLT weightamp = FL(4.0) * peaklist[i].ploudness / totalloudness;
299 for (j = 0; j < NPARTIALONSET; j++) {
300 MYFLT bin = pit - partialonset[j];
301 if (bin < maxbin) {
302 MYFLT para, pphase, score = FL(30.0) * weightamp /
303 ((j+p->npartial) * weightbandwidth);
304 int32_t firstbin = bin + FL(0.5) - FL(0.5) * putbandwidth;
305 int32_t lastbin = bin + FL(0.5) + FL(0.5) * putbandwidth;
306 int32_t ibw = lastbin - firstbin;
307 if (firstbin < -BINGUARD) break;
308 para = FL(1.0) / (putbandwidth * putbandwidth);
309 for (k = 0, pphase = firstbin-bin; k <= ibw;
310 k++,pphase += FL(1.0))
311 histogram[k+firstbin] += score * (FL(1.0) - para * pphase * pphase);
312
313 }
314 }
315 }
316
317
318 for (best = 0, indx = -1, j=0; j < maxbin; j++)
319 if (histogram[j] > best)
320 indx = j, best = histogram[j];
321
322 histpeak.hvalue = best;
323 histpeak.hindex = indx;
324
325 putfreq = EXP((FL(1.0) / BPEROOVERLOG2) *
326 (histpeak.hindex + FL(96.0)));
327 for (j = 0; j < npeak; j++) {
328 MYFLT fpnum = peaklist[j].pfreq/putfreq;
329 int32_t pnum = (int32_t)(fpnum + FL(0.5));
330 MYFLT fipnum = pnum;
331 MYFLT deviation;
332 if (pnum > 16 || pnum < 1) continue;
333 deviation = FL(1.0) - fpnum/fipnum;
334 if (deviation > -PARTIALDEVIANCE && deviation < PARTIALDEVIANCE) {
335 MYFLT stdev, weight;
336 npartials++;
337 if (pnum < 8) nbelow8++;
338 cumpow += peaklist[j].ppow;
339 cumstrength += SQRT(SQRT(peaklist[j].ppow));
340 stdev = (peaklist[j].pwidth > MINBW ?
341 peaklist[j].pwidth : MINBW);
342 weight = FL(1.0) / ((stdev*fipnum) * (stdev*fipnum));
343 freqden += weight;
344 freqnum += weight * peaklist[j].pfreq/fipnum;
345 }
346 }
347 if ((nbelow8 < 4 || npartials < 7) && cumpow < FL(0.01) * totalpower)
348 histpeak.hvalue = 0;
349 else {
350 double pitchpow = (cumstrength * cumstrength);
351 MYFLT freqinbins = freqnum/freqden;
352 pitchpow = pitchpow * pitchpow;
353 if (freqinbins < MINFREQINBINS)
354 histpeak.hvalue = 0;
355 else {
356 p->cps = histpeak.hpitch = hzperbin * freqnum/freqden;
357 histpeak.hloud = FL(DBSCAL) * LOG(pitchpow/n);
358 }
359 }
360
361 }
362 }
363
pitchtrackinit(CSOUND * csound,PITCHTRACK * p)364 int32_t pitchtrackinit(CSOUND *csound, PITCHTRACK *p)
365 {
366
367 int32_t i, winsize = *p->size*2, powtwo, tmp;
368 MYFLT *tmpb;
369
370 if (UNLIKELY(winsize < MINWINSIZ || winsize > MAXWINSIZ)) {
371 csound->Warning(csound, Str("ptrack: FFT size out of range; using %d\n"),
372 winsize = DEFAULTWINSIZ);
373 }
374
375 tmp = winsize;
376 powtwo = -1;
377
378 while (tmp) {
379 tmp >>= 1;
380 powtwo++;
381 }
382
383 if (UNLIKELY(winsize != (1 << powtwo))) {
384 csound->Warning(csound, Str("ptrack: FFT size not a power of 2; using %d\n"),
385 winsize = (1 << powtwo));
386 }
387 p->hopsize = *p->size;
388 if (!p->signal.auxp || p->signal.size < p->hopsize*sizeof(MYFLT)) {
389 csound->AuxAlloc(csound, p->hopsize*sizeof(MYFLT), &p->signal);
390 }
391 if (!p->prev.auxp || p->prev.size < (p->hopsize*2 + 4*FLTLEN)*sizeof(MYFLT)) {
392 csound->AuxAlloc(csound, (p->hopsize*2 + 4*FLTLEN)*sizeof(MYFLT), &p->prev);
393 }
394 if (!p->sin.auxp || p->sin.size < (p->hopsize*2)*sizeof(MYFLT)) {
395 csound->AuxAlloc(csound, (p->hopsize*2)*sizeof(MYFLT), &p->sin);
396 }
397
398 if (!p->spec2.auxp || p->spec2.size < (winsize*4 + 4*FLTLEN)*sizeof(MYFLT)) {
399 csound->AuxAlloc(csound, (winsize*4 + 4*FLTLEN)*sizeof(MYFLT), &p->spec2);
400 }
401
402 if (!p->spec1.auxp || p->spec1.size < (winsize*4)*sizeof(MYFLT)) {
403 csound->AuxAlloc(csound, (winsize*4)*sizeof(MYFLT), &p->spec1);
404 }
405
406 for (i = 0, tmpb = (MYFLT *)p->signal.auxp; i < p->hopsize; i++)
407 tmpb[i] = FL(0.0);
408 for (i = 0, tmpb = (MYFLT *)p->prev.auxp; i < winsize + 4 * FLTLEN; i++)
409 tmpb[i] = FL(0.0);
410 for (i = 0, tmpb = (MYFLT *)p->sin.auxp; i < p->hopsize; i++)
411 tmpb[2*i] = (MYFLT) cos((PI*i)/(winsize)),
412 tmpb[2*i+1] = -(MYFLT)sin((PI*i)/(winsize));
413
414 p->cnt = 0;
415 if (*p->peak == 0 || *p->peak > MAXPEAKNOS)
416 p->numpks = DEFAULTPEAKNOS;
417 else
418 p->numpks = *p->peak;
419
420 if (!p->peakarray.auxp || p->peakarray.size < (p->numpks+1)*sizeof(PEAK)) {
421 csound->AuxAlloc(csound, (p->numpks+1)*sizeof(PEAK), &p->peakarray);
422 }
423
424 p->cnt = 0;
425 p->histcnt = 0;
426 p->sr = CS_ESR;
427 for (i = 0; i < NPREV; i++) p->dbs[i] = FL(-144.0);
428 p->amplo = MINAMPS;
429 p->amphi = MAXAMPS;
430 p->npartial = 7;
431 p->dbfs = FL(32768.0)/csound->e0dbfs;
432 p->prevf = p->cps = 100.0;
433 return (OK);
434 }
435
pitchtrackprocess(CSOUND * csound,PITCHTRACK * p)436 int32_t pitchtrackprocess(CSOUND *csound, PITCHTRACK *p)
437 {
438 MYFLT *sig = p->asig; int32_t i;
439 MYFLT *buf = (MYFLT *)p->signal.auxp;
440 int32_t pos = p->cnt, h = p->hopsize;
441 MYFLT scale = p->dbfs;
442 int32_t ksmps = CS_KSMPS;
443
444 for (i=0; i<ksmps; i++,pos++) {
445 if (pos == h) {
446 ptrack(csound,p);
447 pos = 0;
448 }
449 buf[pos] = sig[i]*scale;
450 }
451 //if (p->cps)
452 *p->freq = p->cps;
453 //else *p->freq = p->prevf;
454 //p->prevf = *p->freq;
455 *p->amp = p->dbs[p->histcnt];
456 p->cnt = pos;
457
458 return OK;
459 }
460
461 typedef struct _pitchaf{
462 OPDS h;
463 MYFLT *kpitch;
464 MYFLT *asig, *kfmin, *kfmax, *iflow;
465 AUXCH buff1, buff2, cor;
466 int32_t lag;
467 MYFLT pitch;
468 int32_t len,size;
469 } PITCHAF;
470
pitchafset(CSOUND * csound,PITCHAF * p)471 int32_t pitchafset(CSOUND *csound, PITCHAF *p){
472 int32_t siz = (int32_t)(CS_ESR/ (*p->iflow));
473 if (p->buff1.auxp == NULL || p->buff1.size < siz*sizeof(MYFLT))
474 csound->AuxAlloc(csound, siz*sizeof(MYFLT), &p->buff1);
475 else
476 memset(p->buff1.auxp, 0, p->buff1.size);
477 if (p->buff2.auxp == NULL ||p-> buff2.size < siz*sizeof(MYFLT))
478 csound->AuxAlloc(csound, siz*sizeof(MYFLT), &p->buff2);
479 else
480 memset(p->buff2.auxp, 0, p->buff2.size);
481 if (p->cor.auxp == NULL || p->cor.size < siz*sizeof(MYFLT))
482 csound->AuxAlloc(csound, siz*sizeof(MYFLT), &p->cor);
483 else
484 memset(p->cor.auxp, 0, p->cor.size);
485 p->lag = 0;
486 p->pitch = FL(0.0);
487 p->len = siz;
488 p->size = siz;
489 return OK;
490 }
491
pitchafproc(CSOUND * csound,PITCHAF * p)492 int32_t pitchafproc(CSOUND *csound, PITCHAF *p)
493 {
494
495 int32_t lag = p->lag,n, i, j, imax = 0, len = p->len,
496 ksmps = CS_KSMPS;
497 MYFLT *buff1 = (MYFLT *)p->buff1.auxp;
498 MYFLT *buff2 = (MYFLT *)p->buff2.auxp;
499 MYFLT *cor = (MYFLT *)p->cor.auxp;
500 MYFLT *s = p->asig, pitch;
501 //MYFLT ifmax = *p->kfmax;
502
503 for (n=0; n < ksmps; n++) {
504 for (i=0,j=lag; i < len; i++) {
505 cor[lag] += buff1[i]*buff2[j];
506 j = j != len ? j+1 : 0;
507 }
508 buff2[lag++] = s[n];
509
510 if (lag == len) {
511 float max = 0.0f;
512 for (i=0; i < len; i++) {
513 if (cor[i] > max) {
514 max = cor[i];
515 if (i) imax = i;
516 }
517 buff1[i] = buff2[i];
518 cor[i] = FL(0.0);
519 }
520 len = CS_ESR/(*p->kfmin);
521 if (len > p->size) len = p->size;
522 lag = 0;
523 }
524 }
525 p->lag = lag;
526 p->len = len;
527 if (imax) {
528 pitch = CS_ESR/imax;
529 if (pitch <= *p->kfmax) p->pitch = pitch;
530 }
531 *p->kpitch = p->pitch;
532
533 return OK;
534 }
535
536 /* PLL Pitch tracker (Zoelzer et al)
537 V Lazzarini, 2012
538 */
539
540 //#define ROOT2 (1.4142135623730950488)
541 enum {LP1=0, LP2, HP};
542
543 typedef struct biquad_ {
544 double a0, a1, a2, b1, b2;
545 double del1, del2;
546 } BIQUAD;
547
548 typedef struct plltrack_
549 {
550 OPDS h;
551 MYFLT *freq, *lock;
552 MYFLT *asig,*kd,*klpf,*klpfQ,*klf,*khf,*kthresh;
553 BIQUAD fils[6];
554 double ace, xce;
555 double cos_x, sin_x, x1, x2;
556 MYFLT klpf_o, klpfQ_o, klf_o,khf_o;
557
558 } PLLTRACK;
559
update_coefs(CSOUND * csound,double fr,double Q,BIQUAD * biquad,int32_t TYPE)560 void update_coefs(CSOUND *csound, double fr, double Q, BIQUAD *biquad, int32_t TYPE)
561 {
562 double k, ksq, div, ksqQ;
563
564 switch(TYPE){
565 case LP2:
566 k = tan(fr*csound->pidsr);
567 ksq = k*k;
568 ksqQ = ksq*Q;
569 div = ksqQ+k+Q;
570 biquad->b1 = (2*Q*(ksq-1.))/div;
571 biquad->b2 = (ksqQ-k+Q)/div;
572 biquad->a0 = ksqQ/div;
573 biquad->a1 = 2*biquad->a0;
574 biquad->a2 = biquad->a0;
575 break;
576
577 case LP1:
578 k = 1.0/tan(csound->pidsr*fr);
579 ksq = k*k;
580 biquad->a0 = 1.0 / ( 1.0 + ROOT2 * k + ksq);
581 biquad->a1 = 2.0*biquad->a0;
582 biquad->a2 = biquad->a0;
583 biquad->b1 = 2.0 * (1.0 - ksq) * biquad->a0;
584 biquad->b2 = ( 1.0 - ROOT2 * k + ksq) * biquad->a0;
585 break;
586
587 case HP:
588 k = tan(csound->pidsr*fr);
589 ksq = k*k;
590 biquad->a0 = 1.0 / ( 1.0 + ROOT2 * k + ksq);
591 biquad->a1 = -2.*biquad->a0;
592 biquad->a2 = biquad->a0;
593 biquad->b1 = 2.0 * (ksq - 1.0) * biquad->a0;
594 biquad->b2 = ( 1.0 - ROOT2 * k + ksq) * biquad->a0;
595 break;
596 }
597
598 }
599
600
plltrack_set(CSOUND * csound,PLLTRACK * p)601 int32_t plltrack_set(CSOUND *csound, PLLTRACK *p)
602 {
603 int32_t i;
604 p->x1 = p->cos_x = p->sin_x = 0.0;
605 p->x2 = 1.0;
606 p->klpf_o = p->klpfQ_o = p->klf_o = p->khf_o = 0.0;
607 update_coefs(csound,10.0, 0.0, &p->fils[4], LP1);
608 p->ace = p->xce = 0.0;
609 for (i=0; i < 6; i++)
610 p->fils[i].del1 = p->fils[i].del2 = 0.0;
611
612 return OK;
613 }
614
plltrack_perf(CSOUND * csound,PLLTRACK * p)615 int32_t plltrack_perf(CSOUND *csound, PLLTRACK *p)
616 {
617 int32_t ksmps, i, k;
618 MYFLT _0dbfs;
619 double a0[6], a1[6], a2[6], b1[6], b2[6];
620 double *mem1[6], *mem2[6];
621 double *ace, *xce;
622 double *cos_x, *sin_x, *x1, *x2;
623 double scal,esr;
624 BIQUAD *biquad = p->fils;
625 MYFLT *asig=p->asig,kd=*p->kd,klpf,klpfQ,klf,khf,kthresh;
626 MYFLT *freq=p->freq, *lock =p->lock, itmp = asig[0];
627 int32_t
628 itest = 0;
629
630 _0dbfs = csound->e0dbfs;
631 ksmps = CS_KSMPS;
632 esr = CS_ESR;
633 scal = 2.0*csound->pidsr;
634
635 /* check for muted input & bypass */
636 if (ksmps > 1){
637 for (i=0; i < ksmps; i++) {
638 if (asig[i] != 0.0 && asig[i] != itmp) {
639 itest = 1;
640 break;
641 }
642 itmp = asig[i];
643 }
644 if (!itest) return OK;
645 } else if (*asig == 0.0) return OK;
646
647
648 if (*p->klpf == 0) klpf = 20.0;
649 else klpf = *p->klpf;
650
651 if (*p->klpfQ == 0) klpfQ = 1./3.;
652 else klpfQ = *p->klpfQ;
653
654 if (*p->klf == 0) klf = 20.0;
655 else klf = *p->klf;
656
657 if (*p->khf == 0) khf = 1500.0;
658 else khf = *p->khf;
659
660 if (*p->kthresh == 0.0) kthresh= 0.001;
661 else kthresh = *p->kthresh;
662
663
664
665 if (p->khf_o != khf) {
666 update_coefs(csound, khf, 0.0, &biquad[0], LP1);
667 update_coefs(csound, khf, 0.0, &biquad[1], LP1);
668 update_coefs(csound, khf, 0.0, &biquad[2], LP1);
669 p->khf_o = khf;
670 }
671
672 if (p->klf_o != klf) {
673 update_coefs(csound, klf, 0.0, &biquad[3], HP);
674 p->klf_o = klf;
675 }
676
677 if (p->klpf_o != klpf || p->klpfQ_o != klpfQ ) {
678 update_coefs(csound, klpf, klpfQ, &biquad[5], LP2);
679 p->klpf_o = klpf; p->klpfQ_o = klpfQ;
680 }
681
682 for (k=0; k < 6; k++) {
683 a0[k] = biquad[k].a0;
684 a1[k] = biquad[k].a1;
685 a2[k] = biquad[k].a2;
686 b1[k] = biquad[k].b1;
687 b2[k] = biquad[k].b2;
688 mem1[k] = &(biquad[k].del1);
689 mem2[k] = &(biquad[k].del2);
690 }
691
692 cos_x = &p->cos_x;
693 sin_x = &p->sin_x;
694 x1 = &p->x1;
695 x2 = &p->x2;
696 xce = &p->xce;
697 ace = &p->ace;
698
699 for (i=0; i < ksmps; i++){
700 double input = (double) (asig[i]/_0dbfs), env;
701 double w, y, icef = 0.99, fosc, xd, c, s, oc;
702
703 /* input stage filters */
704 for (k=0; k < 4 ; k++){
705 w = input - *(mem1[k])*b1[k] - *(mem2[k])*b2[k];
706 y = w*a0[k] + *(mem1[k])*a1[k] + *(mem2[k])*a2[k];
707 *(mem2[k]) = *(mem1[k]);
708 *(mem1[k]) = w;
709 input = y;
710 }
711
712 /* envelope extraction */
713 w = FABS(input) - *(mem1[k])*b1[k] - *(mem2[k])*b2[k];
714 y = w*a0[k] + *(mem1[k])*a1[k] + *(mem2[k])*a2[k];
715 *(mem2[k]) = *(mem1[k]);
716 *(mem1[k]) = w;
717 env = y;
718 k++;
719
720 /* constant envelope */
721 if (env > kthresh)
722 input /= env;
723 else input = 0.0;
724
725 /*post-ce filter */
726 *ace = (1.-icef)*(input + *xce)/2. + *ace*icef;
727 *xce = input;
728
729 /* PLL */
730 xd = *cos_x * (*ace) * kd * esr;
731 w = xd - *(mem1[k])*b1[k] - *(mem2[k])*b2[k];
732 y = w*a0[k] + *(mem1[k])*a1[k] + *(mem2[k])*a2[k];
733 *(mem2[k]) = *(mem1[k]);
734 *(mem1[k]) = w;
735 freq[i] = FABS(2*y);
736 lock[i] = *ace * (*sin_x);
737 fosc = y + xd;
738
739 /* quadrature osc */
740 *sin_x = *x1;
741 *cos_x = *x2;
742 oc = fosc*scal;
743 c = COS(oc); s = SIN(oc);
744 *x1 = *sin_x*c + *cos_x*s;
745 *x2 = -*sin_x*s + *cos_x*c;
746
747 }
748 return OK;
749 }
750
751
752 #define S(x) sizeof(x)
753
754 static OENTRY pitchtrack_localops[] =
755 {
756 {"ptrack", S(PITCHTRACK), 0, 3, "kk", "aio",
757 (SUBR)pitchtrackinit, (SUBR)pitchtrackprocess},
758 {"pitchac", S(PITCHTRACK), 0, 3, "k", "akki",
759 (SUBR)pitchafset, (SUBR)pitchafproc},
760 {"plltrack", S(PLLTRACK), 0, 3, "aa", "akOOOOO",
761 (SUBR)plltrack_set, (SUBR)plltrack_perf}
762
763 };
764
765 LINKAGE_BUILTIN(pitchtrack_localops)
766