1 /*
2 * pitchamdf
3 *
4 * This code has been extracted from the Csound opcode "pitchamdf".
5 * It has been modified to work as a Soundpipe module.
6 *
7 * Original Author(s): Peter Neubacker
8 * Year: 1999
9 * Location: Opcodes/pitch.c
10 *
11 */
12
13 #include <stdlib.h>
14 #include <math.h>
15 #include "soundpipe.h"
16
17 /* #define lrintf(x) lrintf(x) */
18
sp_pitchamdf_create(sp_pitchamdf ** p)19 int sp_pitchamdf_create(sp_pitchamdf **p)
20 {
21 *p = malloc(sizeof(sp_pitchamdf));
22 return SP_OK;
23 }
24
sp_pitchamdf_destroy(sp_pitchamdf ** p)25 int sp_pitchamdf_destroy(sp_pitchamdf **p)
26 {
27 sp_pitchamdf *pp = *p;
28 sp_auxdata_free(&pp->median);
29 /* This mirrors the original code */
30 if(pp->rmsmedisize) {
31 sp_auxdata_free(&pp->rmsmedian);
32 }
33 sp_auxdata_free(&pp->buffer);
34 free(*p);
35 return SP_OK;
36 }
37
sp_pitchamdf_init(sp_data * sp,sp_pitchamdf * p,SPFLOAT imincps,SPFLOAT imaxcps)38 int sp_pitchamdf_init(sp_data *sp, sp_pitchamdf *p, SPFLOAT imincps, SPFLOAT imaxcps)
39 {
40 SPFLOAT srate, downs;
41 int32_t size, minperi, maxperi, downsamp, upsamp, msize, bufsize;
42 uint32_t interval;
43
44 p->imincps = imincps;
45 p->imaxcps = imaxcps;
46
47 /* TODO: should we expose these variables? */
48 p->icps = 0;
49 p->imedi = 1;
50 p->idowns = 1;
51 p->iexcps = 0;
52 p->irmsmedi = 0;
53
54 p->inerr = 0;
55 downs = p->idowns;
56
57 if (downs < (-1.9)) {
58 upsamp = (int)lrintf((-downs));
59 downsamp = 0;
60 srate = sp->sr * (SPFLOAT)upsamp;
61 } else {
62 downsamp = (int)lrintf(downs);
63 if (downsamp < 1) downsamp = 1;
64 srate = sp->sr / (SPFLOAT)downsamp;
65 upsamp = 0;
66 }
67
68 minperi = (int32_t)(srate / p->imaxcps);
69 maxperi = (int32_t)(0.5 + srate / p->imincps);
70 if (maxperi <= minperi) {
71 p->inerr = 1;
72 return SP_NOT_OK;
73 }
74
75 if (p->iexcps < 1)
76 interval = maxperi;
77 else
78 interval = (uint32_t)(srate / p->iexcps);
79
80 size = maxperi + interval;
81 bufsize = sizeof(SPFLOAT)*(size + maxperi + 2);
82
83 p->srate = srate;
84 p->downsamp = downsamp;
85 p->upsamp = upsamp;
86 p->minperi = minperi;
87 p->maxperi = maxperi;
88 p->size = size;
89 p->readp = 0;
90 p->index = 0;
91 p->lastval = 0.0;
92
93 if (p->icps < 1) {
94 p->peri = (minperi + maxperi) / 2;
95 } else {
96 p->peri = (int)(srate / p->icps);
97 }
98
99 if (p->irmsmedi < 1) {
100 p->rmsmedisize = 0;
101 } else {
102 p->rmsmedisize = ((int)lrintf(p->irmsmedi))*2+1;
103 }
104
105 p->rmsmediptr = 0;
106
107 if (p->rmsmedisize) {
108 msize = p->rmsmedisize * 3 * sizeof(SPFLOAT);
109 sp_auxdata_alloc(&p->rmsmedian, msize);
110 }
111
112 if (p->imedi < 1) {
113 p->medisize = 0;
114 } else {
115 p->medisize = (int)lrintf(p->imedi) * 2 + 1;
116 }
117
118 p->mediptr = 0;
119
120 if (p->medisize) {
121 msize = p->medisize * 3 * sizeof(SPFLOAT);
122 sp_auxdata_alloc(&p->median, msize);
123 }
124
125 sp_auxdata_alloc(&p->buffer, bufsize);
126 return SP_OK;
127 }
128
129
130 #define SWAP(a,b) temp=(a);(a)=(b);(b)=temp
131
medianvalue(uint32_t n,SPFLOAT * vals)132 static SPFLOAT medianvalue(uint32_t n, SPFLOAT *vals)
133 {
134 /* vals must point to 1 below relevant data! */
135 uint32_t i, ir, j, l, mid;
136 uint32_t k = (n + 1) / 2;
137 SPFLOAT a, temp;
138
139 l = 1;
140 ir = n;
141 while (1) {
142 if (ir <= l+1) {
143 if (ir == l+1 && vals[ir] < vals[l]) {
144 SWAP(vals[l], vals[ir]);
145 }
146 return vals[k];
147 } else {
148 mid = (l+ir) >> 1;
149 SWAP(vals[mid], vals[l+1]);
150 if (vals[l+1] > vals[ir]) {
151 SWAP(vals[l+1], vals[ir]);
152 }
153 if (vals[l] > vals[ir]) {
154 SWAP(vals[l], vals[ir]);
155 }
156 if (vals[l+1] > vals[l]) {
157 SWAP(vals[l+1], vals[l]);
158 }
159 i = l + 1;
160 j = ir;
161 a = vals[l];
162 while (1) {
163 do i++; while (vals[i] < a);
164 do j--; while (vals[j] > a);
165 if (j < i) break;
166 SWAP(vals[i], vals[j]);
167 }
168 vals[l] = vals[j];
169 vals[j] = a;
170 if (j >= k) ir = j-1;
171 if (j <= k) l = i;
172 }
173 }
174 }
175 #undef SWAP
176
sp_pitchamdf_compute(sp_data * sp,sp_pitchamdf * p,SPFLOAT * in,SPFLOAT * cps,SPFLOAT * rms_out)177 int sp_pitchamdf_compute(sp_data *sp, sp_pitchamdf *p, SPFLOAT *in,
178 SPFLOAT *cps, SPFLOAT *rms_out)
179 {
180 SPFLOAT *buffer = (SPFLOAT*)p->buffer.ptr;
181 SPFLOAT *rmsmedian = (SPFLOAT*)p->rmsmedian.ptr;
182 int32_t rmsmedisize = p->rmsmedisize;
183 int32_t rmsmediptr = p->rmsmediptr;
184 SPFLOAT *median = (SPFLOAT*)p->median.ptr;
185 int32_t medisize = p->medisize;
186 int32_t mediptr = p->mediptr;
187 int32_t size = p->size;
188 int32_t index = p->index;
189 int32_t minperi = p->minperi;
190 int32_t maxperi = p->maxperi;
191 SPFLOAT srate = p->srate;
192 int32_t peri = p->peri;
193 int32_t upsamp = p->upsamp;
194 SPFLOAT upsmp = (SPFLOAT)upsamp;
195 SPFLOAT lastval = p->lastval;
196 SPFLOAT newval, delta;
197 int32_t readp = p->readp;
198 int32_t interval = size - maxperi;
199 int i;
200 int32_t i1, i2;
201 SPFLOAT val, rms;
202 SPFLOAT sum;
203 SPFLOAT acc, accmin, diff;
204
205 if (upsamp) {
206 newval = *in;
207 delta = (newval-lastval) / upsmp;
208 lastval = newval;
209
210 for (i=0; i<upsamp; i++) {
211 newval += delta;
212 buffer[index++] = newval;
213
214 if (index == size) {
215 peri = minperi;
216 accmin = 0.0;
217 for (i2 = 0; i2 < size; ++i2) {
218 diff = buffer[i2+minperi] - buffer[i2];
219 if (diff > 0) accmin += diff;
220 else accmin -= diff;
221 }
222 for (i1 = minperi + 1; i1 <= maxperi; ++i1) {
223 acc = 0.0;
224 for (i2 = 0; i2 < size; ++i2) {
225 diff = buffer[i1+i2] - buffer[i2];
226 if (diff > 0) acc += diff;
227 else acc -= diff;
228 }
229 if (acc < accmin) {
230 accmin = acc;
231 peri = i1;
232 }
233 }
234
235 for (i1 = 0; i1 < interval; i1++) {
236 buffer[i1] = buffer[i1+interval];
237 }
238
239 index = maxperi;
240
241 if (medisize) {
242 median[mediptr] = (SPFLOAT)peri;
243 for (i1 = 0; i1 < medisize; i1++) {
244 median[medisize+i1] = median[i1];
245 }
246
247 median[medisize*2+mediptr] =
248 medianvalue(medisize, &median[medisize-1]);
249 peri = (int32_t)median[medisize*2 +
250 ((mediptr+medisize/2+1) % medisize)];
251
252 mediptr = (mediptr + 1) % medisize;
253 p->mediptr = mediptr;
254 }
255 }
256 }
257 p->lastval = lastval;
258 } else {
259 int32_t downsamp = p->downsamp;
260 buffer[index++] = *in;
261 readp += downsamp;
262
263 if (index == size) {
264 peri = minperi;
265 accmin = 0.0;
266
267 for (i2 = 0; i2 < size; ++i2) {
268 diff = buffer[i2+minperi] - buffer[i2];
269 if (diff > 0.0) accmin += diff;
270 else accmin -= diff;
271 }
272
273 for (i1 = minperi + 1; i1 <= maxperi; ++i1) {
274 acc = 0.0;
275 for (i2 = 0; i2 < size; ++i2) {
276 diff = buffer[i1+i2] - buffer[i2];
277 if (diff > 0.0) acc += diff;
278 else acc -= diff;
279 }
280 if (acc < accmin) {
281 accmin = acc;
282 peri = i1;
283 }
284 }
285
286 for (i1 = 0; i1 < interval; i1++) {
287 buffer[i1] = buffer[i1+interval];
288 }
289
290 index = maxperi;
291
292 if (medisize) {
293 median[mediptr] = (SPFLOAT)peri;
294
295 for (i1 = 0; i1 < medisize; i1++) {
296 median[medisize+i1] = median[i1];
297 }
298
299 median[medisize*2+mediptr] =
300 medianvalue(medisize, &median[medisize-1]);
301 peri = (int32_t)median[medisize*2 +
302 ((mediptr+medisize/2+1) % medisize)];
303
304 mediptr = (mediptr + 1) % medisize;
305 p->mediptr = mediptr;
306 }
307 }
308 }
309 buffer = &buffer[(index + size - peri) % size];
310 sum = 0.0;
311 for (i1=0; i1<peri; i1++) {
312 val = buffer[i1];
313 sum += (SPFLOAT)(val * val);
314 }
315 if (peri==0)
316 rms = 0.0;
317 else
318 rms = (SPFLOAT)sqrt(sum / (SPFLOAT)peri);
319 if (rmsmedisize) {
320 rmsmedian[rmsmediptr] = rms;
321 for (i1 = 0; i1 < rmsmedisize; i1++) {
322 rmsmedian[rmsmedisize+i1] = rmsmedian[i1];
323 }
324
325 rmsmedian[rmsmedisize*2+rmsmediptr] =
326 medianvalue(rmsmedisize, &rmsmedian[rmsmedisize-1]);
327 rms = rmsmedian[rmsmedisize*2 +
328 ((rmsmediptr+rmsmedisize/2+1) % rmsmedisize)];
329
330 rmsmediptr = (rmsmediptr + 1) % rmsmedisize;
331 p->rmsmediptr = rmsmediptr;
332 }
333
334 if (peri==0) {
335 *cps = 0.0;
336 } else {
337 *cps = srate / (SPFLOAT)peri;
338 }
339
340 *rms_out = rms;
341 p->index = index;
342 p->peri = peri;
343 p->readp = readp;
344 return SP_OK;
345 }
346