1 /* partials.c
2
3 Copyright (c) Victor Lazzarini, 2005
4
5 This file is part of Csound.
6
7 The Csound Library is free software; you can redistribute it
8 and/or modify it under the terms of the GNU Lesser General Public
9 License as published by the Free Software Foundation; either
10 version 2.1 of the License, or (at your option) any later version.
11
12 Csound is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU Lesser General Public License for more details.
16
17 You should have received a copy of the GNU Lesser General Public
18 License along with Csound; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
20 02110-1301 USA
21 */
22
23 /* PARTIALS
24
25 Streaming partial track analysis
26
27 ftrk partials ffr, fphs, kthresh, kminpts, kmaxgap, imaxtracks
28
29 ftrk - TRACKS streaming spectral signal
30
31 ffrs - AMP_FREQ input signal
32 fphs - AMP_PHASE input signal (unwrapped phase expected)
33 kthresh - analysis threshold (0 <= kthresh <= 1)
34 kminpts - minimum number of time-points for a partial track
35 kmaxgap - max gap between detected peaks in a track
36 imaxtracks - max number of tracks (<= number of analysis bins)
37
38 */
39
40 #include "pvs_ops.h"
41 #include "pstream.h"
42
43 typedef struct _parts {
44 OPDS h;
45 PVSDAT *fout;
46 PVSDAT *fin1, *fin2;
47 MYFLT *kthresh, *pts, *gap, *mtrks;
48 int32_t tracks, numbins, mtracks, prev, cur;
49 uint64_t accum;
50 uint32 lastframe, timecount;
51 AUXCH mags, lmags, index, cflag, trkid, trndx;
52 AUXCH tstart, binex, magex, oldbins, diffs, adthresh;
53 AUXCH pmags, bins, lastpk;
54 int32_t nophase;
55
56 } _PARTS;
57
partials_init(CSOUND * csound,_PARTS * p)58 static int32_t partials_init(CSOUND * csound, _PARTS * p)
59 {
60
61 int32_t N = p->fin1->N, maxtracks;
62 int32_t numbins = N / 2 + 1, i;
63 int32_t *trkid;
64 int32_t *trndx;
65
66 p->tracks = 0;
67 p->mtracks = *p->mtrks;
68 p->timecount = 0;
69 p->accum = 0;
70 p->numbins = numbins;
71
72 maxtracks = (p->mtracks < numbins ? p->mtracks : numbins);
73
74 p->prev = 0;
75 p->cur = maxtracks;
76
77 if (p->mags.auxp == NULL || p->mags.size < sizeof(double) * numbins)
78 csound->AuxAlloc(csound, sizeof(double) * numbins, &p->mags);
79 else
80 memset(p->mags.auxp, 0,sizeof(double) * numbins );
81
82 if (p->lmags.auxp == NULL || p->lmags.size < sizeof(double) * numbins)
83 csound->AuxAlloc(csound, sizeof(double) * numbins, &p->lmags);
84 else
85 memset(p->lmags.auxp, 0,sizeof(double) * numbins );
86
87 if (p->cflag.auxp == NULL || p->cflag.size < sizeof(int32_t) * maxtracks)
88 csound->AuxAlloc(csound, sizeof(int32_t) * maxtracks, &p->cflag);
89 else
90 memset(p->cflag.auxp, 0, sizeof(int32_t) * maxtracks);
91
92 if (p->trkid.auxp == NULL || p->trkid.size < sizeof(int32_t) * maxtracks * 2)
93 csound->AuxAlloc(csound, sizeof(int32_t) * maxtracks * 2, &p->trkid);
94 else
95 memset(p->trkid.auxp, 0, sizeof(int32_t) * maxtracks * 2);
96
97 if (p->trndx.auxp == NULL || p->trndx.size < sizeof(int32_t) * maxtracks)
98 csound->AuxAlloc(csound, sizeof(int32_t) * maxtracks, &p->trndx);
99 else
100 memset(p->trndx.auxp, 0, sizeof(int32_t) * maxtracks );
101
102 if (p->index.auxp == NULL || p->index.size < sizeof(int32_t) * numbins)
103 csound->AuxAlloc(csound, sizeof(int32_t) * numbins, &p->index);
104 else
105 memset(p->index.auxp, 0,sizeof(int32_t) * numbins );
106
107 if (p->tstart.auxp == NULL || p->tstart.size < sizeof(uint32) * maxtracks * 2)
108 csound->AuxAlloc(csound, sizeof(uint32) * maxtracks * 2, &p->tstart);
109 else
110 memset(p->tstart.auxp, 0, sizeof(uint32) * maxtracks * 2);
111
112 if (p->lastpk.auxp == NULL ||
113 p->lastpk.size < sizeof(uint32) * maxtracks * 2)
114 csound->AuxAlloc(csound, sizeof(uint32) * maxtracks * 2, &p->lastpk);
115 else
116 memset(p->lastpk.auxp, 0, sizeof(uint32) * maxtracks * 2);
117
118 if (p->binex.auxp == NULL || p->binex.size < sizeof(double) * numbins)
119 csound->AuxAlloc(csound, sizeof(double) * numbins, &p->binex);
120 else
121 memset(p->binex.auxp, 0,sizeof(double) * numbins );
122
123 if (p->magex.auxp == NULL || p->magex.size < sizeof(double) * numbins)
124 csound->AuxAlloc(csound, sizeof(double) * numbins, &p->magex);
125 else
126 memset(p->magex.auxp, 0,sizeof(double) * numbins );
127
128 if (p->bins.auxp == NULL || p->bins.size < sizeof(double) * maxtracks)
129 csound->AuxAlloc(csound, sizeof(double) * maxtracks, &p->bins);
130 else
131 memset(p->bins.auxp, 0, sizeof(double) * maxtracks );
132
133 if (p->oldbins.auxp == NULL ||
134 p->oldbins.size < sizeof(double) * maxtracks * 2)
135 csound->AuxAlloc(csound, sizeof(double) * maxtracks * 2, &p->oldbins);
136 else
137 memset(p->oldbins.auxp, 0, sizeof(double) * maxtracks * 2);
138
139 if (p->diffs.auxp == NULL || p->diffs.size < sizeof(double) * numbins)
140 csound->AuxAlloc(csound, sizeof(double) * numbins, &p->diffs);
141 else
142 memset(p->diffs.auxp, 0, sizeof(double) * numbins );
143
144 if (p->pmags.auxp == NULL || p->pmags.size < sizeof(double) * maxtracks * 2)
145 csound->AuxAlloc(csound, sizeof(double) * maxtracks * 2, &p->pmags);
146 else
147 memset(p->pmags.auxp, 0, sizeof(double) * maxtracks * 2);
148
149 if (p->adthresh.auxp == NULL ||
150 p->adthresh.size < sizeof(double) * maxtracks * 2)
151 csound->AuxAlloc(csound, sizeof(double) * maxtracks * 2, &p->adthresh);
152 else
153 memset(p->adthresh.auxp, 0, sizeof(double) * maxtracks * 2);
154
155 if (p->fout->frame.auxp == NULL ||
156 p->fout->frame.size < sizeof(float) * numbins * 4)
157 csound->AuxAlloc(csound, sizeof(float) * numbins * 4, &p->fout->frame);
158 else
159 memset(p->fout->frame.auxp, 0,sizeof(float) * numbins * 4);
160
161 p->fout->N = N;
162 p->fout->overlap = p->fin1->overlap;
163 p->fout->winsize = p->fin1->winsize;
164 p->fout->wintype = p->fin1->wintype;
165 p->fout->framecount = 1;
166 p->fout->format = PVS_TRACKS;
167
168 trkid = (int32_t *) p->trkid.auxp;
169 trndx = (int32_t *) p->trndx.auxp;
170 for (i = 0; i < maxtracks; i++)
171 trkid[p->cur + i] = trkid[p->prev + i] = trndx[i] = -1;
172
173 p->mtracks = maxtracks;
174
175 if (UNLIKELY(p->fin1->format != PVS_AMP_FREQ)) {
176 return
177 csound->InitError(csound,
178 Str("partials: first input not in AMP_FREQ format\n"));
179 }
180
181 if (UNLIKELY(p->fin2->format != PVS_AMP_PHASE)) {
182 csound->Warning(csound,
183 Str("partials: no phase input, tracks will contain "
184 "amp & freq only\n"));
185 p->nophase = 1;
186 }
187 else
188 p->nophase = 0;
189
190 p->lastframe = 0;
191
192 return OK;
193 }
194
Analysis(CSOUND * csound,_PARTS * p)195 static void Analysis(CSOUND * csound, _PARTS * p)
196 {
197
198 float absthresh, logthresh;
199 int32_t ndx, count = 0, i = 0, n = 0, j = 0;
200 float dbstep;
201 double y1, y2, a, b, dtmp;
202 float ftmp, ftmp2;
203 int32_t numbins = p->numbins, maxtracks = p->mtracks;
204 int32_t prev = p->prev, cur = p->cur, foundcont;
205 int32_t accum = p->accum, minpoints = (int32_t) (*p->pts > 1 ? *p->pts : 1) - 1;
206 int32_t tracks = p->tracks;
207 double *mags = (double *) p->mags.auxp;
208 double *lmags = (double *) p->lmags.auxp;
209 int32_t *cflag = (int32_t *) p->cflag.auxp;
210 int32_t *trkid = (int32_t *) p->trkid.auxp;
211 int32_t *trndx = (int32_t *) p->trndx.auxp;
212 int32_t *index = (int32_t *) p->index.auxp;
213 uint32 *tstart = (uint32 *) p->tstart.auxp;
214 double *binex = (double *) p->binex.auxp;
215 double *magex = (double *) p->magex.auxp;
216 double *oldbins = (double *) p->oldbins.auxp;
217 double *diffs = (double *) p->diffs.auxp;
218 double *adthresh = (double *) p->adthresh.auxp;
219 double *pmags = (double *) p->pmags.auxp;
220 double *bins = (double *) p->bins.auxp;
221 uint32 *lastpk = (uint32 *) p->lastpk.auxp;
222 uint32_t timecount = p->timecount,
223 maxgap = (uint32_t) (*p->gap > 0 ? *p->gap : 0);
224 int32_t test1 = 1, test2 = 0;
225
226 if(*p->kthresh >= 0) {
227 float max = 0.0f;
228 for (i = 0; i < numbins; i++)
229 if (max < mags[i]) {
230 max = mags[i];
231 }
232 absthresh = (float)(*p->kthresh * max);
233 } else absthresh = (float)(-*p->kthresh * csound->Get0dBFS(csound));
234
235 logthresh = logf(absthresh / 5.0f);
236
237 /* Quadratic Interpolation
238 obtains bin indexes and magnitudes
239 binex & magex respectively
240 */
241
242 /* take the logarithm of the magnitudes */
243 for (i = 0; i < numbins; i++)
244 lmags[i] = log((double)mags[i]);
245
246 for (i = 0; i < numbins - 1; i++) {
247
248 if (i)
249 test1 = (lmags[i] > lmags[i - 1] ? 1 : 0);
250 else
251 test1 = 0;
252 test2 = (lmags[i] >= lmags[i + 1] ? 1 : 0);
253
254 if ((lmags[i] > logthresh) && (test1 && test2)) {
255 index[n] = i;
256 n++;
257 }
258
259 }
260
261 for (i = 0; i < n; i++) {
262 int32_t rmax;
263
264 rmax = index[i];
265
266 y1 = lmags[rmax] - (dtmp =
267 (rmax ? lmags[rmax - 1] : lmags[rmax + 1])) +
268 0.000001;
269 y2 = (rmax <
270 numbins - 1 ? lmags[rmax + 1] : lmags[rmax]) - dtmp + 0.000001;
271
272 a = (y2 - 2.0 * y1) / 2.0;
273 b = 1.0 - y1 / a;
274
275 binex[i] = (double) (rmax - 1.0 + b / 2.0);
276 magex[i] = (double) exp(dtmp - a * b * b / 4.0);
277 }
278 /* Track allocation */
279
280 /* reset continuation flags */
281 for (i = 0; i < maxtracks; i++) {
282 cflag[i] = 0;
283 }
284
285 /* loop to the end of tracks (indicate by the 0'd bins)
286 find continuation tracks */
287
288 for (j = 0; j < maxtracks && oldbins[prev + j] != 0.f; j++) {
289
290 foundcont = 0;
291
292 if (n > 0) { /* check for peaks; n will be > 0 */
293
294 ftmp = oldbins[prev + j];
295
296 for (i = 0; i < numbins; i++) {
297 diffs[i] = binex[i] - ftmp; /* differences */
298 diffs[i] = (diffs[i] < 0 ? -diffs[i] : diffs[i]);
299 }
300
301 ndx = 0; /* best index */
302 for (i = 0; i < numbins; i++)
303 if (diffs[i] < diffs[ndx])
304 ndx = i;
305
306 /* if difference smaller than 1 bin */
307 ftmp2 = ftmp - binex[ndx];
308 ftmp2 = (ftmp2 < 0.0f ? -ftmp2 : ftmp2);
309 if (ftmp2 < 1.0f) {
310
311 /* if amp jump is too great */
312 if (adthresh[prev + j] <
313 (dbstep = 20.0f * LOG10(magex[ndx] / pmags[prev + j]))) {
314 /* mark for discontinuation */
315 cflag[j] = 0;
316 }
317 else {
318 oldbins[prev + j] = binex[ndx];
319 pmags[prev + j] = magex[ndx];
320 /*
321 track index keeps track history
322 so we know which ones continue
323 */
324 cflag[j] = 1;
325 binex[ndx] = magex[ndx] = FL(0.0);
326 lastpk[prev + j] = timecount;
327 foundcont = 1;
328 count++;
329
330 /* update adthresh */
331 ftmp = dbstep * 1.5f;
332 ftmp2 = adthresh[prev + j] -
333 (adthresh[prev + j] - 1.5f) * 0.048770575f;
334 adthresh[prev + j] = (ftmp > ftmp2 ? ftmp : ftmp2);
335
336 } /* else */
337 }
338 }
339 if (foundcont == 0) {
340 if ((mags[(int32_t) (oldbins[prev + j] + 0.5f)]) < (0.2 * pmags[prev + j])
341 || (timecount - lastpk[prev + j]) > maxgap)
342 cflag[j] = 0;
343 else {
344 cflag[j] = 1;
345 count++;
346 }
347 }
348
349 } /* for loop */
350
351 if (count < maxtracks) {
352
353 /* if we have not exceeded available tracks.
354 compress the arrays */
355 for (i = 0, n = 0; i < maxtracks; i++) {
356 if (cflag[i]) {
357 oldbins[cur + n] = oldbins[prev + i];
358 pmags[cur + n] = pmags[prev + i];
359 adthresh[cur + n] = adthresh[prev + i];
360 tstart[cur + n] = tstart[prev + i];
361 trkid[cur + n] = trkid[prev + i];
362 lastpk[cur + n] = lastpk[prev + i];
363 n++;
364 } /* ID == -1 means zeroed track */
365 else
366 trndx[i] = -1;
367 }
368
369 /* now current arrays are the compressed previous
370 arrays. Create new tracks */
371
372 for (j = 0; j < numbins && count < maxtracks; j++) {
373
374 if (magex[j] > absthresh) {
375 oldbins[cur + count] = binex[j];
376 pmags[cur + count] = magex[j];
377 adthresh[cur + count] = 400.f;
378 /* track ID is a positive number in the
379 range of 0 - maxtracks*4 - 1
380 it is given when the track starts
381 used to identify and match tracks
382 */
383 tstart[cur + count] = timecount;
384 trkid[cur + count] = ((accum++));// % (maxtracks * 1000));
385 lastpk[cur + count] = timecount;
386 count++;
387
388 }
389 }
390 for (i = count; i < maxtracks; i++) {
391 /* zero the right-hand size of the current arrays */
392 pmags[cur + i] = oldbins[cur + i] = adthresh[cur + i] = 0.0f;
393 trkid[cur + i] = -1;
394
395 }
396
397 } /* if count != maxtracks */
398
399 /* count is the number of continuing tracks + new tracks
400 now we check for tracks that have been there for more
401 than minpoints hop periods and output them
402 */
403
404 tracks = 0;
405 for (i = 0; i < maxtracks; i++) {
406 if (i < count && tstart[cur + i] <= timecount - minpoints) {
407 bins[i] = oldbins[cur + i];
408 mags[i] = pmags[cur + i];
409 trndx[i] = trkid[cur + i];
410 tracks++;
411 }
412 }
413 /* current arrays become previous */
414 timecount++;
415 p->timecount = timecount;
416 p->cur = prev;
417 p->prev = cur;
418 p->accum = accum;
419 p->tracks = tracks;
420
421 }
422
partials_process(CSOUND * csound,_PARTS * p)423 static int32_t partials_process(CSOUND * csound, _PARTS * p)
424 {
425
426 int32_t pos, ndx, end, fftsize = p->fin1->N;
427 int32_t numbins = fftsize / 2 + 1, i, k;
428 int32_t tracks, nophase = p->nophase;
429 float *fin1 = p->fin1->frame.auxp;
430 float *fin2 = p->fin2->frame.auxp;
431 float *fout = p->fout->frame.auxp;
432 double *mags = p->mags.auxp;
433 double *bins = p->bins.auxp;
434 int32_t *trndx = p->trndx.auxp;
435 double frac, a, b;
436 int32_t maxtracks = (p->mtracks < numbins ? p->mtracks : numbins);
437 end = numbins * 4;
438
439 if (p->lastframe < p->fin1->framecount) {
440
441 for (i = k = 0; i < fftsize + 2; i += 2, k++)
442 mags[k] = fin1[i];
443 Analysis(csound, p);
444 /* fout holds [amp, freq, pha, ID] */
445 tracks = p->tracks;
446 for (i = k = 0; i < end && k < maxtracks; i += 4, k++) {
447 if (k < tracks) {
448 /* magnitudes */
449 ndx = (int32_t) bins[k];
450 fout[i] = (float) mags[k];
451 /* fractional part of bin indexes */
452 frac = (bins[k] - ndx);
453 /* freq interpolation */
454 pos = ndx * 2 + 1;
455 a = fin1[pos];
456 b = (bins[k] < numbins - 1 ? (fin1[pos + 2] - a) : 0);
457 fout[i + 1] = (float) (a + frac * b);
458 if (!nophase){
459 float pha0 = fin2[pos];
460 float pha1 = bins[k] < numbins - 1 ? fin2[pos + 2] : fin2[pos];
461 float mag0 = fin2[pos - 1];
462 float mag1 = bins[k] < numbins - 1 ? fin2[pos + 1] : fin2[pos - 1];
463 /* while (pha >= PI_F)
464 pha -= TWOPI_F;
465 while (pha < -PI_F)
466 pha += TWOPI_F; */
467 //fout[i + 2] = pha; /* phase (truncated) */
468 MYFLT cos0 = mag0*COS(pha0);
469 MYFLT sin0 = mag0*SIN(pha0);
470 MYFLT cos1 = mag1*COS(pha1);
471 MYFLT sin1 = mag1*SIN(pha1);
472 MYFLT re = cos0 + frac*(cos1 - cos0);
473 MYFLT im = sin0 + frac*(sin1 - sin0);
474 fout[i + 2] = atan2(im,re); /* phase (interpolated) */
475 }
476 else
477 fout[i + 2] = 0.f;
478 fout[i + 3] = (float) trndx[k]; /* trk IDs */
479 }
480 else { /* empty tracks */
481 // VL: 14.07.20
482 // explicitly set it to -1. to mark it dead
483 fout[i + 3] = -1.f;//trndx[k];
484 }
485 }
486
487 p->lastframe = p->fout->framecount = p->fin1->framecount;
488 }
489 return OK;
490 }
491
492 typedef struct _partxt{
493 OPDS h;
494 STRINGDAT *fname;
495 PVSDAT *tracks;
496 FDCH fdch;
497 FILE *f;
498 uint32 lastframe;
499 } PARTXT;
500
501
part2txt_init(CSOUND * csound,PARTXT * p)502 int32_t part2txt_init(CSOUND *csound, PARTXT *p){
503
504 if (p->fdch.fd != NULL)
505 csound_fd_close(csound, &(p->fdch));
506 p->fdch.fd = csound->FileOpen2(csound, &(p->f), CSFILE_STD, p->fname->data,
507 "w", "", CSFTYPE_FLOATS_TEXT, 0);
508 if (UNLIKELY(p->fdch.fd == NULL))
509 return csound->InitError(csound, Str("Cannot open %s"), p->fname->data);
510
511 p->lastframe = 0;
512 return OK;
513 }
514
part2txt_perf(CSOUND * csound,PARTXT * p)515 int32_t part2txt_perf(CSOUND *csound, PARTXT *p){
516 IGN(csound);
517 float *tracks = (float *) p->tracks->frame.auxp;
518 int32_t i = 0;
519
520 if (p->tracks->framecount > p->lastframe){
521 for (i=0; tracks[i+3] > 0; i+=4){
522 fprintf(p->f, "%f %f %f %d\n", tracks[i],tracks[i+1],
523 tracks[i+2], (int32_t) tracks[i+3]);
524 }
525 fprintf(p->f, "-1.0 -1.0 -1.0 -1\n");
526 p->lastframe = p->tracks->framecount;
527 }
528 return OK;
529 }
530
531 static OENTRY localops[] =
532 {
533 { "partials", sizeof(_PARTS), 0, 3, "f", "ffkkki",
534 (SUBR) partials_init, (SUBR) partials_process },
535 { "part2txt", sizeof(_PARTS), 0, 3, "", "Sf",
536 (SUBR) part2txt_init, (SUBR) part2txt_perf }
537 };
538
partials_init_(CSOUND * csound)539 int32_t partials_init_(CSOUND *csound)
540 {
541 return csound->AppendOpcodes(csound, &(localops[0]),
542 (int32_t
543 ) (sizeof(localops) / sizeof(OENTRY)));
544 }
545