Mercurial > hg > x
comparison procedures.cpp @ 1:6422640a802f
first upload
author | Wen X <xue.wen@elec.qmul.ac.uk> |
---|---|
date | Tue, 05 Oct 2010 10:45:57 +0100 |
parents | |
children | fc19d45615d1 |
comparison
equal
deleted
inserted
replaced
0:9b9f21935f24 | 1:6422640a802f |
---|---|
1 //--------------------------------------------------------------------------- | |
2 | |
3 #include <math.h> | |
4 #include <mem.h> | |
5 #include "procedures.h" | |
6 #include "matrix.h" | |
7 #include "opt.h" | |
8 #include "SinEst.h" | |
9 | |
10 //--------------------------------------------------------------------------- | |
11 //TGMM methods | |
12 | |
13 //method TGMM::TGMM: default constructor | |
14 TGMM::TGMM() | |
15 { | |
16 p=0, m=dev=0; | |
17 }//TGMM | |
18 | |
19 //method GMM:~TGMM: default destructor | |
20 TGMM::~TGMM() | |
21 { | |
22 ReleaseGMM(p, m, dev) | |
23 }; | |
24 | |
25 //--------------------------------------------------------------------------- | |
26 //TFSpans methods | |
27 | |
28 //method TTFSpans: default constructor | |
29 TTFSpans::TTFSpans() | |
30 { | |
31 Count=0; | |
32 Capacity=100; | |
33 Items=new TTFSpan[Capacity]; | |
34 }//TTFSpans | |
35 | |
36 //method ~TTFSpans: default destructor | |
37 TTFSpans::~TTFSpans() | |
38 { | |
39 delete[] Items; | |
40 }//~TTFSpans | |
41 | |
42 /* | |
43 method Add: add a new span to the list | |
44 | |
45 In: ATFSpan: the new span to add | |
46 */ | |
47 void TTFSpans::Add(TTFSpan& ATFSpan) | |
48 { | |
49 if (Count==Capacity) | |
50 { | |
51 int OldCapacity=Capacity; | |
52 Capacity+=50; | |
53 TTFSpan* NewItems=new TTFSpan[Capacity]; | |
54 memcpy(NewItems, Items, sizeof(TTFSpan)*OldCapacity); | |
55 delete[] Items; | |
56 Items=NewItems; | |
57 } | |
58 Items[Count]=ATFSpan; | |
59 Count++; | |
60 }//Add | |
61 | |
62 /* | |
63 method Clear: discard the current content without freeing memory. | |
64 */ | |
65 void TTFSpans::Clear() | |
66 { | |
67 Count=0; | |
68 }//Clear | |
69 | |
70 /* | |
71 method Delete: delete a span from current list | |
72 | |
73 In: Index: index to the span to delete | |
74 */ | |
75 int TTFSpans::Delete(int Index) | |
76 { | |
77 if (Index<0 || Index>=Count) | |
78 return 0; | |
79 memmove(&Items[Index], &Items[Index+1], sizeof(TTFSpan)*(Count-1-Index)); | |
80 Count--; | |
81 return 1; | |
82 }//Delete | |
83 | |
84 //--------------------------------------------------------------------------- | |
85 //SpecTrack methods | |
86 | |
87 /* | |
88 method TSpecTrack::Add: adds a SpecPeak to the track. | |
89 | |
90 In: APeak: the SpecPeak to add. | |
91 */ | |
92 int TSpecTrack::Add(TSpecPeak& APeak) | |
93 { | |
94 if (Count>=Capacity) | |
95 { | |
96 Peaks=(TSpecPeak*)realloc(Peaks, sizeof(TSpecPeak)*(Capacity*2)); | |
97 Capacity*=2; | |
98 } | |
99 int ind=LocatePeak(APeak); | |
100 if (ind<0) | |
101 { | |
102 InsertPeak(APeak, -ind-1); | |
103 ind=-ind-1; | |
104 } | |
105 | |
106 int t=APeak.t; | |
107 double f=APeak.f; | |
108 if (Count==1) t1=t2=t, fmin=fmax=f; | |
109 else | |
110 { | |
111 if (t<t1) t1=t; | |
112 else if (t>t2) t2=t; | |
113 if (f<fmin) fmin=f; | |
114 else if (f>fmax) fmax=f; | |
115 } | |
116 return ind; | |
117 }//Add | |
118 | |
119 /* | |
120 method TSpecTrack::TSpecTrack: creates a SpecTrack with an inital capacity. | |
121 | |
122 In: ACapacity: initial capacity, i.e. the number SpecPeak's to allocate storage space for. | |
123 */ | |
124 TSpecTrack::TSpecTrack(int ACapacity) | |
125 { | |
126 Count=0; | |
127 Capacity=ACapacity; | |
128 Peaks=new TSpecPeak[Capacity]; | |
129 }//TSpecTrack | |
130 | |
131 //method TSpecTrack::~TSpecTrack: default destructor. | |
132 TSpecTrack::~TSpecTrack() | |
133 { | |
134 delete[] Peaks; | |
135 }//TSpecTrack | |
136 | |
137 /* | |
138 method InsertPeak: inserts a new SpecPeak into the track at a given index. Internal use only. | |
139 | |
140 In: APeak: the SpecPeak to insert. | |
141 index: the position in the list to place the new SpecPeak. Original SpecPeak's at and after this | |
142 position are shifted by 1 posiiton. | |
143 */ | |
144 void TSpecTrack::InsertPeak(TSpecPeak& APeak, int index) | |
145 { | |
146 memmove(&Peaks[index+1], &Peaks[index], sizeof(TSpecPeak)*(Count-index)); | |
147 Peaks[index]=APeak; | |
148 Count++; | |
149 }//InsertPeak | |
150 | |
151 /* | |
152 method TSpecTrack::LocatePeak: looks for a SpecPeak in the track that has the same time (t) as APeak. | |
153 | |
154 In: APeak: a SpecPeak | |
155 | |
156 Returns the index in this track of the first SpecPeak that has the same time stamp as APeak. However, | |
157 if there is no peak with that time stamp, the method returns -1 if APeaks comes before the first | |
158 SpecPeak, -2 if between 1st and 2nd SpecPeak's, -3 if between 2nd and 3rd SpecPeak's, etc. | |
159 */ | |
160 int TSpecTrack::LocatePeak(TSpecPeak& APeak) | |
161 { | |
162 if (APeak.t<Peaks[0].t) return -1; | |
163 if (APeak.t>Peaks[Count-1].t) return -Count-1; | |
164 | |
165 if (APeak.t==Peaks[0].t) return 0; | |
166 else if (APeak.t==Peaks[Count-1].t) return Count-1; | |
167 | |
168 int a=0, b=Count-1, c=(a+b)/2; | |
169 while (a<c) | |
170 { | |
171 if (APeak.t==Peaks[c].t) return c; | |
172 else if (APeak.t<Peaks[c].t) {b=c; c=(a+b)/2;} | |
173 else {a=c; c=(a+b)/2;} | |
174 } | |
175 return -a-2; | |
176 }//LocatePeak | |
177 | |
178 //--------------------------------------------------------------------------- | |
179 /* | |
180 function: ACPower: AC power | |
181 | |
182 In: data[Count]: a signal | |
183 | |
184 Returns the power of its AC content. | |
185 */ | |
186 double ACPower(double* data, int Count, void*) | |
187 { | |
188 if (Count<=0) return 0; | |
189 double power=0, avg=0, tmp; | |
190 for (int i=0; i<Count; i++) | |
191 { | |
192 tmp=*(data++); | |
193 power+=tmp*tmp; | |
194 avg+=tmp; | |
195 } | |
196 power=(power-avg*avg)/Count; | |
197 return power; | |
198 }//ACPower | |
199 | |
200 //--------------------------------------------------------------------------- | |
201 /* | |
202 function Add: vector addition | |
203 | |
204 In: dest[Count], source[Count]: two vectors | |
205 Out: dest[Count]: their sum | |
206 | |
207 No return value. | |
208 */ | |
209 void Add(double* dest, double* source, int Count) | |
210 { | |
211 for (int i=0; i<Count; i++) *(dest++)+=*(source++); | |
212 }//Add | |
213 | |
214 /* | |
215 function Add: vector addition | |
216 | |
217 In: addend[count], adder[count]: two vectors | |
218 Out: out[count]: their sum | |
219 | |
220 No return value. | |
221 */ | |
222 void Add(double* out, double* addend, double* adder, int count) | |
223 { | |
224 for (int i=0; i<count; i++) *(out++)=*(addend++)+*(adder++); | |
225 }//Add | |
226 | |
227 //--------------------------------------------------------------------------- | |
228 | |
229 /* | |
230 function ApplyWindow: applies window function to signal buffer. | |
231 | |
232 In: Buffer[Size]: signal to be windowed | |
233 Weight[Size]: the window | |
234 Out: OutBuffer[Size]: windowed signal | |
235 | |
236 No return value; | |
237 */ | |
238 void ApplyWindow(double* OutBuffer, double* Buffer, double* Weights, int Size) | |
239 { | |
240 for (int i=0; i<Size; i++) *(OutBuffer++)=*(Buffer++)**(Weights++); | |
241 }//ApplyWindow | |
242 | |
243 //--------------------------------------------------------------------------- | |
244 /* | |
245 function Avg: average | |
246 | |
247 In: data[Count]: a data array | |
248 | |
249 Returns the average of the array. | |
250 */ | |
251 double Avg(double* data, int Count, void*) | |
252 { | |
253 if (Count<=0) return 0; | |
254 double avg=0; | |
255 for (int i=0; i<Count; i++) avg+=*(data++); | |
256 avg/=Count; | |
257 return avg; | |
258 }//Avg | |
259 | |
260 //--------------------------------------------------------------------------- | |
261 /* | |
262 function AvgFilter: get slow-varying wave trace by averaging | |
263 | |
264 In: data[Count]: input signal | |
265 HWid: half the size of the averaging window | |
266 Out: datout[Count]: the slow-varying part of data[]. | |
267 | |
268 No return value. | |
269 */ | |
270 void AvgFilter(double* dataout, double* data, int Count, int HWid) | |
271 { | |
272 double sum=0; | |
273 | |
274 dataout[0]=data[0]; | |
275 | |
276 for (int i=1; i<=HWid; i++) | |
277 { | |
278 sum+=data[2*i-1]+data[2*i]; | |
279 dataout[i]=sum/(2*i+1); | |
280 } | |
281 | |
282 for (int i=HWid+1; i<Count-HWid; i++) | |
283 { | |
284 sum=sum+data[i+HWid]-data[i-HWid-1]; | |
285 dataout[i]=sum/(2*HWid+1); | |
286 } | |
287 | |
288 for (int i=Count-HWid; i<Count; i++) | |
289 { | |
290 sum=sum-data[2*i-Count-1]-data[2*i-Count]; | |
291 dataout[i]=sum/(2*(Count-i)-1); | |
292 } | |
293 }//AvgFilter | |
294 | |
295 //--------------------------------------------------------------------------- | |
296 /* | |
297 function CalculateSpectrogram: computes the spectrogram of a signal | |
298 | |
299 In: data[Count]: the time-domain signal | |
300 start, end: start and end points marking the section for which the spectrogram is to be computed | |
301 Wid, Offst: frame size and hop size | |
302 Window: window function | |
303 amp: a pre-amplifier | |
304 half: specifies if the spectral values at Wid/2 are to be retried | |
305 Out: Spec[][Wid/2] or Spec[][Wid/2+1]: amplitude spectrogram | |
306 ph[][][Wid/2] or Ph[][Wid/2+1]: phase spectrogram | |
307 | |
308 No return value. The caller is repsonse to arrange storage spance of output buffers. | |
309 */ | |
310 void CalculateSpectrogram(double* data, int Count, int start, int end, int Wid, int Offst, double* Window, double** Spec, double** Ph, double amp, bool half) | |
311 { | |
312 AllocateFFTBuffer(Wid, fft, w, x); | |
313 | |
314 int Fr=(end-start-Wid)/Offst+1; | |
315 | |
316 for (int i=0; i<Fr; i++) | |
317 { | |
318 RFFTCW(&data[i*Offst+start], Window, 0, 0, log2(Wid), w, x); | |
319 | |
320 if (Spec) | |
321 { | |
322 for (int j=0; j<Wid/2; j++) | |
323 Spec[i][j]=sqrt(x[j].x*x[j].x+x[j].y*x[j].y)*amp; | |
324 if (half) | |
325 Spec[i][Wid/2]=sqrt(x[Wid/2].x*x[Wid/2].x+x[Wid/2].y*x[Wid/2].y)*amp; | |
326 } | |
327 if (Ph) | |
328 { | |
329 for (int j=0; j<=Wid/2; j++) | |
330 Ph[i][j]=Atan2(x[j].y, x[j].x); | |
331 if (half) | |
332 Ph[i][Wid/2]=Atan2(x[Wid/2].y, x[Wid/2].x); | |
333 } | |
334 } | |
335 FreeFFTBuffer(fft); | |
336 }//CalculateSpectrogram | |
337 | |
338 //--------------------------------------------------------------------------- | |
339 /* | |
340 function Conv: simple convolution | |
341 | |
342 In: in1[N1], in2[N2]: two sequences | |
343 Out: out[N1+N2-1]: their convolution | |
344 | |
345 No return value. | |
346 */ | |
347 void Conv(double* out, int N1, double* in1, int N2, double* in2) | |
348 { | |
349 int N=N1+N1-1; | |
350 memset(out, 0, sizeof(double)*N); | |
351 for (int n1=0; n1<N1; n1++) | |
352 for (int n2=0; n2<N2; n2++) | |
353 out[n1+n2]+=in1[n1]*in2[n2]; | |
354 }//Conv | |
355 | |
356 //--------------------------------------------------------------------------- | |
357 /* | |
358 function Correlation: computes correlation coefficient of 2 vectors a & b, equals cos(aOb). | |
359 | |
360 In: a[Count], b[Count]: two vectors | |
361 | |
362 Returns their correlation coefficient. | |
363 */ | |
364 double Correlation(double* a, double* b, int Count) | |
365 { | |
366 double aa=0, bb=0, ab=0; | |
367 for (int i=0; i<Count; i++) | |
368 { | |
369 aa+=*a**a; | |
370 bb+=*b**b; | |
371 ab+=*(a++)**(b++); | |
372 } | |
373 return ab/sqrt(aa*bb); | |
374 }//Correlation | |
375 | |
376 //--------------------------------------------------------------------------- | |
377 /* | |
378 function DCAmplitude: DC amplitude | |
379 | |
380 In: data[Count]: a signal | |
381 | |
382 Returns its DC amplitude (=AC amplitude without DC removing) | |
383 */ | |
384 double DCAmplitude(double* data, int Count, void*) | |
385 { | |
386 if (Count<=0) return 0; | |
387 double power=0, tmp; | |
388 for (int i=0; i<Count; i++) | |
389 { | |
390 tmp=*(data++); | |
391 power+=tmp*tmp; | |
392 } | |
393 power/=Count; | |
394 return sqrt(2*power); | |
395 }//DCAmplitude | |
396 | |
397 /* | |
398 function DCPower: DC power | |
399 | |
400 In: data[Count]: a signal | |
401 | |
402 Returns its DC power. | |
403 */ | |
404 double DCPower(double* data, int Count, void*) | |
405 { | |
406 if (Count<=0) return 0; | |
407 double power=0, tmp; | |
408 for (int i=0; i<Count; i++) | |
409 { | |
410 tmp=*(data++); | |
411 power+=tmp*tmp; | |
412 } | |
413 power/=Count; | |
414 return power; | |
415 }//DCPower | |
416 | |
417 //--------------------------------------------------------------------------- | |
418 /* | |
419 DCT: discrete cosine transform, direct computation. For fast DCT, see fft.cpp. | |
420 | |
421 In: input[N]: a signal | |
422 Out: output[N]: its DCT | |
423 | |
424 No return value. | |
425 */ | |
426 void DCT( double* output, double* input, int N) | |
427 { | |
428 double Wn; | |
429 | |
430 for (int n=0; n<N; n++) | |
431 { | |
432 output[n]=0; | |
433 Wn=n*M_PI/2/N; | |
434 for (int k=0; k<N; k++) | |
435 output[n]+=input[k]*cos((2*k+1)*Wn); | |
436 if (n==0) output[n]*=1.4142135623730950488016887242097/N; | |
437 else output[n]*=2.0/N; | |
438 } | |
439 }//DCT | |
440 | |
441 /* | |
442 function IDCT: inverse discrete cosine transform, direct computation. For fast IDCT, see fft.cpp. | |
443 | |
444 In: input[N]: a signal | |
445 Out: output[N]: its IDCT | |
446 | |
447 No return value. | |
448 */ | |
449 void IDCT(double* output, double* input, int N) | |
450 { | |
451 for (int k=0; k<N; k++) | |
452 { | |
453 double Wk=(2*k+1)*M_PI/2/N; | |
454 output[k]=input[0]/1.4142135623730950488016887242097; | |
455 for (int n=1; n<N; n++) | |
456 output[k]+=input[n]*cos(n*Wk); | |
457 } | |
458 }//IDCT | |
459 | |
460 //--------------------------------------------------------------------------- | |
461 /* | |
462 function DeDC: removes DC component of a signal | |
463 | |
464 In: data[Count]: the signal | |
465 HWid: half of averaging window size | |
466 Out: data[Count]: de-DC-ed signal | |
467 | |
468 No return value. | |
469 */ | |
470 void DeDC(double* data, int Count, int HWid) | |
471 { | |
472 double* data2=new double[Count]; | |
473 AvgFilter(data2, data, Count, HWid); | |
474 for (int i=0; i<Count; i++) | |
475 *(data++)-=*(data2++); | |
476 delete[] data2; | |
477 }//DeDC | |
478 | |
479 /* | |
480 function DeDC_static: removes DC component statically | |
481 | |
482 In: data[Count]: the signal | |
483 Out: data[Count]: DC-removed signal | |
484 | |
485 No return value. | |
486 */ | |
487 void DeDC_static(double* data, int Count) | |
488 { | |
489 double avg=Avg(data, Count, 0); | |
490 for (int i=0; i<Count; i++) *(data++)-=avg; | |
491 }//DeDC_static | |
492 | |
493 //--------------------------------------------------------------------------- | |
494 /* | |
495 function DoubleToInt: converts double-precision floating point array to integer array | |
496 | |
497 In: in[Count]: the double array | |
498 BytesPerSample: bytes per sample of destination integers | |
499 Out: out[Count]: the integer array | |
500 | |
501 No return value. | |
502 */ | |
503 void DoubleToInt(void* out, int BytesPerSample, double* in, int Count) | |
504 { | |
505 if (BytesPerSample==1){unsigned char* out8=(unsigned char*)out; for (int k=0; k<Count; k++) *(out8++)=*(in++)+128.5;} | |
506 else {__int16* out16=(__int16*)out; for (int k=0; k<Count; k++) *(out16++)=floor(*(in++)+0.5);} | |
507 }//DoubleToInt | |
508 | |
509 /* | |
510 function DoubleToIntAdd: adds double-precision floating point array to integer array | |
511 | |
512 In: in[Count]: the double array | |
513 out[Count]: the integer array | |
514 BytesPerSample: bytes per sample of destination integers | |
515 Out: out[Count]: the sum of the two arrays | |
516 | |
517 No return value. | |
518 */ | |
519 void DoubleToIntAdd(void* out, int BytesPerSample, double* in, int Count) | |
520 { | |
521 if (BytesPerSample==1) | |
522 { | |
523 unsigned char* out8=(unsigned char*)out; | |
524 for (int k=0; k<Count; k++){*out8=*out8+*in+128.5; out8++; in++;} | |
525 } | |
526 else | |
527 { | |
528 __int16* out16=(__int16*)out; | |
529 for (int k=0; k<Count; k++){*out16=*out16+floor(*in+0.5); out16++; in++;} | |
530 } | |
531 }//DoubleToIntAdd | |
532 | |
533 //--------------------------------------------------------------------------- | |
534 /* | |
535 DPower: in-frame power variation | |
536 | |
537 In: data[Count]: a signal | |
538 | |
539 returns the different between AC powers of its first and second halves. | |
540 */ | |
541 double DPower(double* data, int Count, void*) | |
542 { | |
543 double ene1=ACPower(data, Count/2, 0); | |
544 double ene2=ACPower(&data[Count/2], Count/2, 0); | |
545 return ene2-ene1; | |
546 }//DPower | |
547 | |
548 //--------------------------------------------------------------------------- | |
549 /* | |
550 funciton Energy: energy | |
551 | |
552 In: data[Count]: a signal | |
553 | |
554 Returns its total energy | |
555 */ | |
556 double Energy(double* data, int Count) | |
557 { | |
558 double result=0; | |
559 for (int i=0; i<Count; i++) result+=data[i]*data[i]; | |
560 return result; | |
561 }//Energy | |
562 | |
563 //--------------------------------------------------------------------------- | |
564 /* | |
565 function ExpOnsetFilter: onset filter with exponential impulse response h(t)=Aexp(-t/Tr)-Bexp(-t/Ta), | |
566 A=1-exp(-1/Tr), B=1-exp(-1/Ta). | |
567 | |
568 In: data[Count]: signal to filter | |
569 Tr, Ta: time constants of h(t) | |
570 Out: dataout[Count]: filtered signal, normalized by multiplying a factor. | |
571 | |
572 Returns the normalization factor. Identical data and dataout is allowed. | |
573 */ | |
574 double ExpOnsetFilter(double* dataout, double* data, int Count, double Tr, double Ta) | |
575 { | |
576 double FA=0, FB=0; | |
577 double EA=exp(-1.0/Tr), EB=exp(-1.0/Ta); | |
578 double A=1-EA, B=1-EB; | |
579 double NormFactor=1/sqrt((1-EA)*(1-EA)/(1-EA*EA)+(1-EB)*(1-EB)/(1-EB*EB)-2*(1-EA)*(1-EB)/(1-EA*EB)); | |
580 for (int i=0; i<Count; i++) | |
581 { | |
582 FA=FA*EA+*data; | |
583 FB=FB*EB+*(data++); | |
584 *(dataout++)=(A*FA-B*FB)*NormFactor; | |
585 } | |
586 return NormFactor; | |
587 }//ExpOnsetFilter | |
588 | |
589 /* | |
590 function ExpOnsetFilter_balanced: exponential onset filter without starting step response. It | |
591 extends the input signal at the front end by bal*Ta samples by repeating the its value at 0, then | |
592 applies the onset filter on the extended signal instead. | |
593 | |
594 In: data[Count]: signal to filter | |
595 Tr, Ta: time constants to the impulse response of onset filter, see ExpOnsetFilter(). | |
596 bal: balancing factor | |
597 Out: dataout[Count]: filtered signal, normalized by multiplying a factor. | |
598 | |
599 Returns the normalization factor. Identical data and dataout is allowed. | |
600 */ | |
601 double ExpOnsetFilter_balanced(double* dataout, double* data, int Count, double Tr, double Ta, int bal) | |
602 { | |
603 double* tmpdata=new double[int(Count+bal*Ta)]; | |
604 double* ltmpdata=tmpdata; | |
605 for (int i=0; i<bal*Ta; i++) *(ltmpdata++)=data[0]; | |
606 memcpy(ltmpdata, data, sizeof(double)*Count); | |
607 double result=ExpOnsetFilter(tmpdata, tmpdata, bal*Ta+Count, Tr, Ta); | |
608 memcpy(dataout, ltmpdata, sizeof(double)*Count); | |
609 delete[] tmpdata; | |
610 return result; | |
611 }//ExpOnsetFilter_balanced | |
612 | |
613 //--------------------------------------------------------------------------- | |
614 /* | |
615 function ExtractLinearComponent: Legendre linear component | |
616 | |
617 In: data[Count+1]: a signal | |
618 Out: dataout[Count+1]: its Legendre linear component, optional. | |
619 | |
620 Returns the coefficient to the linear component. | |
621 */ | |
622 double ExtractLinearComponent(double* dataout, double* data, int Count) | |
623 { | |
624 double tmp=0; | |
625 int N=Count*2; | |
626 for (int n=0; n<=Count; n++) tmp+=n**(data++); | |
627 tmp=tmp*24/N/(N+1)/(N+2); | |
628 if (dataout) | |
629 for (int n=0; n<=Count; n++) *(dataout++)=tmp*n; | |
630 return tmp; | |
631 }//ExtractLinearComponent | |
632 | |
633 //--------------------------------------------------------------------------- | |
634 /* | |
635 function FFTConv: fast convolution of two series by FFT overlap-add. In an overlap-add scheme it is | |
636 assumed that one of the convolvends is short compared to the other one, which can be potentially | |
637 infinitely long. The long convolvend is devided into short segments, each of which is convolved with | |
638 the short convolvend, the results of which are then assembled into the final result. The minimal delay | |
639 from input to output is the amount of overlap, which is the size of the short convolvend minus 1. | |
640 | |
641 In: source1[size1]: convolvend | |
642 source2[size2]: second convolvend | |
643 zero: position of first point in convoluton result, relative to main output buffer. | |
644 pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result. | |
645 Out: dest[size1]: the middle part of convolution result | |
646 pre_buffer[-zero]: now updated by adding beginning part of the convolution result | |
647 post_buffer[size2+zero]: end part of the convolution result | |
648 | |
649 No return value. Identical dest and source1 allowed. | |
650 | |
651 The convolution result has length size1+size2 (counting one trailing zero). If zero lies in the range | |
652 between -size2 and 0, then the first -zero samples are added to pre_buffer[], next size1 samples are | |
653 saved to dest[], and the last size2+zero sampled are saved to post_buffer[]; if not, the middle size1 | |
654 samples are saved to dest[], while pre_buffer[] and post_buffer[] are not used. | |
655 */ | |
656 void FFTConv(double* dest, double* source1, int size1, double* source2, int size2, int zero, double* pre_buffer, double* post_buffer) | |
657 { | |
658 int order=log2(size2-1)+1+1; | |
659 int Wid=1<<order; | |
660 int HWid=Wid/2; | |
661 int Fr=size1/HWid; | |
662 int res=size1-HWid*Fr; | |
663 bool trunc=false; | |
664 if (zero<-size2+1 || zero>0) zero=-size2/2, trunc=true; | |
665 if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true; | |
666 | |
667 AllocateFFTBuffer(Wid, fft, w, x1); | |
668 int* hbitinv=CreateBitInvTable(order-1); | |
669 cdouble* x2=new cdouble[Wid]; | |
670 double* tmp=new double[HWid]; | |
671 memset(tmp, 0, sizeof(double)*HWid); | |
672 | |
673 memcpy(fft, source2, sizeof(double)*size2); | |
674 memset(&fft[size2], 0, sizeof(double)*(Wid-size2)); | |
675 RFFTC(fft, 0, 0, order, w, x2, hbitinv); | |
676 | |
677 double r1, r2, i1, i2; | |
678 int ind, ind_; | |
679 for (int i=0; i<Fr; i++) | |
680 { | |
681 memcpy(fft, &source1[i*HWid], sizeof(double)*HWid); | |
682 memset(&fft[HWid], 0, sizeof(double)*HWid); | |
683 | |
684 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
685 | |
686 for (int j=0; j<Wid; j++) | |
687 { | |
688 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
689 x1[j].x=r1*r2-i1*i2; | |
690 x1[j].y=r1*i2+r2*i1; | |
691 } | |
692 CIFFTR(x1, order, w, fft, hbitinv); | |
693 for (int j=0; j<HWid; j++) tmp[j]+=fft[j]; | |
694 | |
695 ind=i*HWid+zero; //(i+1)*HWid<=size1 | |
696 ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1 | |
697 if (ind<0) | |
698 { | |
699 if (!trunc) | |
700 memdoubleadd(pre_buffer, tmp, -ind); | |
701 memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind)); | |
702 } | |
703 else | |
704 memcpy(&dest[ind], tmp, sizeof(double)*HWid); | |
705 memcpy(tmp, &fft[HWid], sizeof(double)*HWid); | |
706 } | |
707 | |
708 if (res>0) | |
709 { | |
710 memcpy(fft, &source1[Fr*HWid], sizeof(double)*res); | |
711 memset(&fft[res], 0, sizeof(double)*(Wid-res)); | |
712 | |
713 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
714 | |
715 for (int j=0; j<Wid; j++) | |
716 { | |
717 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
718 x1[j].x=r1*r2-i1*i2; | |
719 x1[j].y=r1*i2+r2*i1; | |
720 } | |
721 CIFFTR(x1, order, w, fft, hbitinv); | |
722 for (int j=0; j<HWid; j++) | |
723 tmp[j]+=fft[j]; | |
724 | |
725 ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1 | |
726 ind_=ind+HWid; //ind_=size1 -res+zero+HWid | |
727 if (ind<0) | |
728 { | |
729 if (!trunc) | |
730 memdoubleadd(pre_buffer, tmp, -ind); | |
731 memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind)); | |
732 } | |
733 else if (ind_>size1) | |
734 { | |
735 memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind)); | |
736 if (!trunc && post_buffer) | |
737 { | |
738 if (ind_>size1+size2+zero) | |
739 memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero)); | |
740 else | |
741 memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1)); | |
742 } | |
743 } | |
744 else | |
745 memcpy(&dest[ind], tmp, sizeof(double)*HWid); | |
746 memcpy(tmp, &fft[HWid], sizeof(double)*HWid); | |
747 Fr++; | |
748 } | |
749 | |
750 ind=Fr*HWid+zero; | |
751 ind_=ind+HWid; | |
752 | |
753 if (ind<size1) | |
754 { | |
755 if (ind_>size1) | |
756 { | |
757 memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind)); | |
758 if (!trunc && post_buffer) | |
759 { | |
760 if (ind_>size1+size2+zero) | |
761 memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero)); | |
762 else | |
763 memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1)); | |
764 } | |
765 } | |
766 else | |
767 memcpy(&dest[ind], tmp, sizeof(double)*HWid); | |
768 } | |
769 else //ind>=size1 => ind_>=size1+size2+zero | |
770 { | |
771 if (!trunc && post_buffer) | |
772 memcpy(&post_buffer[ind-size1], tmp, sizeof(double)*(size1+size2+zero-ind)); | |
773 } | |
774 | |
775 FreeFFTBuffer(fft); | |
776 delete[] x2; | |
777 delete[] tmp; | |
778 delete[] hbitinv; | |
779 }//FFTConv | |
780 | |
781 /* | |
782 function FFTConv: the simplified version using two output buffers instead of three. This is almost | |
783 equivalent to setting zero=-size2 in the three-output-buffer version (so that post_buffer no longer | |
784 exists), except that this version requires size2 (renamed HWid) be a power of 2, and pre_buffer point | |
785 to the END of the storage (so that pre_buffer=dest automatically connects the two buffers in a | |
786 continuous memory block). | |
787 | |
788 In: source1[size1]: convolvend | |
789 source2[HWid]: second convolved, HWid be a power of 2 | |
790 pre_buffer[-HWid:-1]: buffer hosting values to be overlap-added to the start of the result. | |
791 Out: dest[size1]: main output buffer, now hosting end part of the result (after HWid samples). | |
792 pre_buffer[-HWid:-1]: now updated by added the start of the result | |
793 | |
794 No return value. | |
795 */ | |
796 void FFTConv(double* dest, double* source1, int size1, double* source2, int HWid, double* pre_buffer) | |
797 { | |
798 int Wid=HWid*2; | |
799 int order=log2(Wid); | |
800 int Fr=size1/HWid; | |
801 int res=size1-HWid*Fr; | |
802 | |
803 AllocateFFTBuffer(Wid, fft, w, x1); | |
804 cdouble *x2=new cdouble[Wid]; | |
805 double *tmp=new double[HWid]; | |
806 int* hbitinv=CreateBitInvTable(order-1); | |
807 | |
808 memcpy(fft, source2, sizeof(double)*HWid); | |
809 memset(&fft[HWid], 0, sizeof(double)*HWid); | |
810 RFFTC(fft, 0, 0, order, w, x2, hbitinv); | |
811 | |
812 double r1, r2, i1, i2; | |
813 for (int i=0; i<Fr; i++) | |
814 { | |
815 memcpy(fft, &source1[i*HWid], sizeof(double)*HWid); | |
816 memset(&fft[HWid], 0, sizeof(double)*HWid); | |
817 | |
818 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
819 | |
820 for (int j=0; j<Wid; j++) | |
821 { | |
822 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
823 x1[j].x=r1*r2-i1*i2; | |
824 x1[j].y=r1*i2+r2*i1; | |
825 } | |
826 CIFFTR(x1, order, w, fft, hbitinv); | |
827 | |
828 if (i==0) | |
829 { | |
830 if (pre_buffer!=NULL) | |
831 { | |
832 double* destl=&pre_buffer[-HWid+1]; | |
833 for (int j=0; j<HWid-1; j++) destl[j]+=fft[j]; | |
834 } | |
835 } | |
836 else | |
837 { | |
838 for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j]; | |
839 memcpy(&dest[(i-1)*HWid], tmp, sizeof(double)*HWid); | |
840 } | |
841 memcpy(tmp, &fft[HWid-1], sizeof(double)*HWid); | |
842 } | |
843 | |
844 if (res>0) | |
845 { | |
846 if (Fr==0) memset(tmp, 0, sizeof(double)*HWid); | |
847 | |
848 memcpy(fft, &source1[Fr*HWid], sizeof(double)*res); | |
849 memset(&fft[res], 0, sizeof(double)*(Wid-res)); | |
850 | |
851 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
852 for (int j=0; j<Wid; j++) | |
853 { | |
854 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
855 x1[j].x=r1*r2-i1*i2; | |
856 x1[j].y=r1*i2+r2*i1; | |
857 } | |
858 CIFFTR(x1, order, w, fft, hbitinv); | |
859 | |
860 if (Fr==0) | |
861 { | |
862 if (pre_buffer!=NULL) | |
863 { | |
864 double* destl=&pre_buffer[-HWid+1]; | |
865 for (int j=0; j<HWid-1; j++) destl[j]+=fft[j]; | |
866 } | |
867 } | |
868 else | |
869 { | |
870 for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j]; | |
871 memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid); | |
872 } | |
873 | |
874 memcpy(&dest[Fr*HWid], &fft[HWid-1], sizeof(double)*res); | |
875 } | |
876 else | |
877 memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid); | |
878 | |
879 FreeFFTBuffer(fft); | |
880 delete[] x2; delete[] tmp; delete[] hbitinv; | |
881 }//FFTConv | |
882 | |
883 /* | |
884 function FFTConv: fast convolution of two series by FFT overlap-add. Same as the three-output-buffer | |
885 version above but using integer output buffers as well as integer source1. | |
886 | |
887 In: source1[size1]: convolvend | |
888 bps: bytes per sample of integer units in source1[]. | |
889 source2[size2]: second convolvend | |
890 zero: position of first point in convoluton result, relative to main output buffer. | |
891 pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result. | |
892 Out: dest[size1]: the middle part of convolution result | |
893 pre_buffer[-zero]: now updated by adding beginning part of the convolution result | |
894 post_buffer[size2+zero]: end part of the convolution result | |
895 | |
896 No return value. Identical dest and source1 allowed. | |
897 */ | |
898 void FFTConv(unsigned char* dest, unsigned char* source1, int bps, int size1, double* source2, int size2, int zero, unsigned char* pre_buffer, unsigned char* post_buffer) | |
899 { | |
900 int order=log2(size2-1)+1+1; | |
901 int Wid=1<<order; | |
902 int HWid=Wid/2; | |
903 int Fr=size1/HWid; | |
904 int res=size1-HWid*Fr; | |
905 bool trunc=false; | |
906 if (zero<-size2+1 || zero>0) zero=-size2/2, trunc=true; | |
907 if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true; | |
908 | |
909 AllocateFFTBuffer(Wid, fft, w, x1); | |
910 cdouble* x2=new cdouble[Wid]; | |
911 double* tmp=new double[HWid]; | |
912 memset(tmp, 0, sizeof(double)*HWid); | |
913 int* hbitinv=CreateBitInvTable(order-1); | |
914 | |
915 memcpy(fft, source2, sizeof(double)*size2); | |
916 memset(&fft[size2], 0, sizeof(double)*(Wid-size2)); | |
917 RFFTC(fft, 0, 0, order, w, x2, hbitinv); | |
918 | |
919 double r1, r2, i1, i2; | |
920 int ind, ind_; | |
921 for (int i=0; i<Fr; i++) | |
922 { | |
923 IntToDouble(fft, &source1[i*HWid*bps], bps, HWid); | |
924 memset(&fft[HWid], 0, sizeof(double)*HWid); | |
925 | |
926 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
927 | |
928 for (int j=0; j<Wid; j++) | |
929 { | |
930 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
931 x1[j].x=r1*r2-i1*i2; | |
932 x1[j].y=r1*i2+r2*i1; | |
933 } | |
934 CIFFTR(x1, order, w, fft, hbitinv); | |
935 for (int j=0; j<HWid; j++) tmp[j]+=fft[j]; | |
936 | |
937 ind=i*HWid+zero; //(i+1)*HWid<=size1 | |
938 ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1 | |
939 if (ind<0) | |
940 { | |
941 if (!trunc) | |
942 DoubleToIntAdd(pre_buffer, bps, tmp, -ind); | |
943 DoubleToInt(dest, bps, &tmp[-ind], HWid+ind); | |
944 } | |
945 else | |
946 DoubleToInt(&dest[ind*bps], bps, tmp, HWid); | |
947 memcpy(tmp, &fft[HWid], sizeof(double)*HWid); | |
948 } | |
949 | |
950 if (res>0) | |
951 { | |
952 IntToDouble(fft, &source1[Fr*HWid*bps], bps, res); | |
953 memset(&fft[res], 0, sizeof(double)*(Wid-res)); | |
954 | |
955 RFFTC(fft, 0, 0, order, w, x1, hbitinv); | |
956 | |
957 for (int j=0; j<Wid; j++) | |
958 { | |
959 r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; | |
960 x1[j].x=r1*r2-i1*i2; | |
961 x1[j].y=r1*i2+r2*i1; | |
962 } | |
963 CIFFTR(x1, order, w, fft, hbitinv); | |
964 for (int j=0; j<HWid; j++) | |
965 tmp[j]+=fft[j]; | |
966 | |
967 ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1 | |
968 ind_=ind+HWid; //ind_=size1 -res+zero+HWid | |
969 if (ind<0) | |
970 { | |
971 if (!trunc) | |
972 DoubleToIntAdd(pre_buffer, bps, tmp, -ind); | |
973 DoubleToInt(dest, bps, &tmp[-ind], HWid+ind); | |
974 } | |
975 else if (ind_>size1) | |
976 { | |
977 DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind); | |
978 if (!trunc && post_buffer) | |
979 { | |
980 if (ind_>size1+size2+zero) | |
981 DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero); | |
982 else | |
983 DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1); | |
984 } | |
985 } | |
986 else | |
987 DoubleToInt(&dest[ind*bps], bps, tmp, HWid); | |
988 memcpy(tmp, &fft[HWid], sizeof(double)*HWid); | |
989 Fr++; | |
990 } | |
991 | |
992 ind=Fr*HWid+zero; | |
993 ind_=ind+HWid; | |
994 | |
995 if (ind<size1) | |
996 { | |
997 if (ind_>size1) | |
998 { | |
999 DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind); | |
1000 if (!trunc && post_buffer) | |
1001 { | |
1002 if (ind_>size1+size2+zero) | |
1003 DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero); | |
1004 else | |
1005 DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1); | |
1006 } | |
1007 } | |
1008 else | |
1009 DoubleToInt(&dest[ind*bps], bps, tmp, HWid); | |
1010 } | |
1011 else //ind>=size1 => ind_>=size1+size2+zero | |
1012 { | |
1013 if (!trunc && post_buffer) | |
1014 DoubleToInt(&post_buffer[(ind-size1)*bps], bps, tmp, size1+size2+zero-ind); | |
1015 } | |
1016 | |
1017 FreeFFTBuffer(fft); | |
1018 delete[] x2; | |
1019 delete[] tmp; | |
1020 delete[] hbitinv; | |
1021 }//FFTConv | |
1022 | |
1023 //--------------------------------------------------------------------------- | |
1024 /* | |
1025 function FFTFilter: FFT with cosine-window overlap-add: This FFT filter is not an actural LTI system, | |
1026 but an block processing with overlap-add. In this function the blocks are overlapped by 50% and summed | |
1027 up with Hann windowing. | |
1028 | |
1029 In: data[Count]: input data | |
1030 Wid: DFT size | |
1031 On, Off: cut-off frequencies of FFT filter. On<Off: band-pass; On>Off: band-stop. | |
1032 Out: dataout[Count]: filtered data | |
1033 | |
1034 No return value. Identical data and dataout allowed | |
1035 */ | |
1036 void FFTFilter(double* dataout, double* data, int Count, int Wid, int On, int Off) | |
1037 { | |
1038 int Order=log2(Wid); | |
1039 int HWid=Wid/2; | |
1040 int Fr=(Count-Wid)/HWid+1; | |
1041 AllocateFFTBuffer(Wid, ldata, w, x); | |
1042 | |
1043 double* win=new double[Wid]; | |
1044 for (int i=0; i<Wid; i++) win[i]=sqrt((1-cos(2*M_PI*i/Wid))/2); | |
1045 double* tmpdata=new double[HWid]; | |
1046 memset(tmpdata, 0, HWid*sizeof(double)); | |
1047 | |
1048 for (int i=0; i<Fr; i++) | |
1049 { | |
1050 memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); | |
1051 if (i>0) | |
1052 for (int k=0; k<HWid; k++) | |
1053 ldata[k]=ldata[k]*win[k]; | |
1054 for (int k=HWid; k<Wid; k++) | |
1055 ldata[k]=ldata[k]*win[k]; | |
1056 | |
1057 RFFTC(ldata, NULL, NULL, Order, w, x, 0); | |
1058 | |
1059 if (On<Off) //band pass: keep [On, Off) and set other bins to zero | |
1060 { | |
1061 memset(x, 0, On*sizeof(cdouble)); | |
1062 if (On>=1) | |
1063 memset(&x[Wid-On+1], 0, (On-1)*sizeof(cdouble)); | |
1064 if (Off*2<=Wid) | |
1065 memset(&x[Off], 0, (Wid-Off*2+1)*sizeof(cdouble)); | |
1066 } | |
1067 else //band stop: set [Off, On) to zero. | |
1068 { | |
1069 memset(&x[Off], 0, sizeof(cdouble)*(On-Off)); | |
1070 memset(&x[Wid-On+1], 0, sizeof(double)*(On-Off)); | |
1071 } | |
1072 | |
1073 CIFFTR(x, Order, w, ldata); | |
1074 | |
1075 if (i>0) for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*win[k]; | |
1076 for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*win[k]; | |
1077 | |
1078 memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); | |
1079 for (int k=0; k<HWid; k++) dataout[i*HWid+k]+=ldata[k]; | |
1080 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
1081 } | |
1082 | |
1083 memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double)); | |
1084 memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double)); | |
1085 | |
1086 delete[] win; | |
1087 delete[] tmpdata; | |
1088 FreeFFTBuffer(ldata); | |
1089 }//FFTFilter | |
1090 | |
1091 /* | |
1092 funtion FFTFilterOLA: FFTFilter with overlap-add support. This is a true LTI filter whose impulse | |
1093 response is constructed using IFFT. The filtering is implemented by fast convolution. | |
1094 | |
1095 In: data[Count]: input data | |
1096 Wid: FFT size | |
1097 On, Off: cut-off frequencies, in bins, of the filter | |
1098 pre_buffer[Wid]: buffer hosting sampled to be added with the start of output | |
1099 Out: dataout[Count]: main output buffer, hosting the last $Count samples of output. | |
1100 pre_buffer[Wid]: now updated by adding the first Wid samples of output | |
1101 | |
1102 No return value. The complete output contains Count+Wid effective samples (including final 0); firt | |
1103 $Wid are added to pre_buffer[], next Count samples saved to dataout[]. | |
1104 */ | |
1105 void FFTFilterOLA(double* dataout, double* data, int Count, int Wid, int On, int Off, double* pre_buffer) | |
1106 { | |
1107 AllocateFFTBuffer(Wid, spec, w, x); | |
1108 memset(x, 0, sizeof(cdouble)*Wid); | |
1109 for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2); | |
1110 CIFFTR(x, log2(Wid), w, spec); | |
1111 FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL); | |
1112 FreeFFTBuffer(spec); | |
1113 }//FFTFilterOLA | |
1114 //version for integer input and output, where BytesPerSample specifies the integer format. | |
1115 void FFTFilterOLA(unsigned char* dataout, unsigned char* data, int BytesPerSample, int Count, int Wid, int On, int Off, unsigned char* pre_buffer) | |
1116 { | |
1117 AllocateFFTBuffer(Wid, spec, w, x); | |
1118 memset(x, 0, sizeof(cdouble)*Wid); | |
1119 for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2); | |
1120 CIFFTR(x, log2(Wid), w, spec); | |
1121 FFTConv(dataout, data, BytesPerSample, Count, spec, Wid, -Wid, pre_buffer, NULL); | |
1122 FreeFFTBuffer(spec); | |
1123 }//FFTFilterOLA | |
1124 | |
1125 /* | |
1126 function FFTFilterOLA: FFT filter with overlap-add support. | |
1127 | |
1128 In: data[Count]: input data | |
1129 amp[0:HWid]: amplitude response | |
1130 ph[0:HWid]: phase response, where ph[0]=ph[HWid]=0; | |
1131 pre_buffer[Wid]: buffer hosting sampled to be added to the beginning of the output | |
1132 Out: dataout[Count]: main output buffer, hosting the middle $Count samples of output. | |
1133 pre_buffer[Wid]: now updated by adding the first Wid/2 samples of output | |
1134 | |
1135 No return value. | |
1136 */ | |
1137 void FFTFilterOLA(double* dataout, double* data, int Count, double* amp, double* ph, int Wid, double* pre_buffer) | |
1138 { | |
1139 int HWid=Wid/2; | |
1140 AllocateFFTBuffer(Wid, spec, w, x); | |
1141 x[0].x=amp[0], x[0].y=0; | |
1142 for (int i=1; i<HWid; i++) | |
1143 { | |
1144 x[i].x=x[Wid-i].x=amp[i]*cos(ph[i]); | |
1145 x[i].y=amp[i]*sin(ph[i]); | |
1146 x[Wid-i].y=-x[i].y; | |
1147 } | |
1148 x[HWid].x=amp[HWid], x[HWid].y=0; | |
1149 CIFFTR(x, log2(Wid), w, spec); | |
1150 FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL); | |
1151 FreeFFTBuffer(spec); | |
1152 }//FFTFilterOLA | |
1153 | |
1154 /* | |
1155 function FFTMask: masks a band of a signal with noise | |
1156 | |
1157 In: data[Count]: input signal | |
1158 DigiOn, DigiOff: cut-off frequences of the band to mask | |
1159 maskcoef: masking noise amplifier. If set to 1 than the mask level is set to the highest signal | |
1160 level in the masking band. | |
1161 Out: dataout[Count]: output data | |
1162 | |
1163 No return value. | |
1164 */ | |
1165 double FFTMask(double* dataout, double* data, int Count, int Wid, double DigiOn, double DigiOff, double maskcoef) | |
1166 { | |
1167 int Order=log2(Wid); | |
1168 int HWid=Wid/2; | |
1169 int Fr=(Count-Wid)/HWid+1; | |
1170 int On=Wid*DigiOn, Off=Wid*DigiOff; | |
1171 AllocateFFTBuffer(Wid, ldata, w, x); | |
1172 | |
1173 double* winhann=new double[Wid]; | |
1174 double* winhamm=new double[Wid]; | |
1175 for (int i=0; i<Wid; i++) | |
1176 {winhamm[i]=0.54-0.46*cos(2*M_PI*i/Wid); winhann[i]=(1-cos(2*M_PI*i/Wid))/2/winhamm[i];} | |
1177 double* tmpdata=new double[HWid]; | |
1178 memset(tmpdata, 0, HWid*sizeof(double)); | |
1179 double max, randfi; | |
1180 | |
1181 max=0; | |
1182 for (int i=0; i<Fr; i++) | |
1183 { | |
1184 memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); | |
1185 if (i>0) | |
1186 for (int k=0; k<HWid; k++) | |
1187 ldata[k]=ldata[k]*winhamm[k]; | |
1188 for (int k=HWid; k<Wid; k++) | |
1189 ldata[k]=ldata[k]*winhamm[k]; | |
1190 | |
1191 RFFTC(ldata, ldata, NULL, Order, w, x, 0); | |
1192 | |
1193 for (int k=On; k<Off; k++) | |
1194 { | |
1195 x[k].x=x[Wid-k].x=x[k].y=x[Wid-k].y=0; | |
1196 if (max<ldata[k]) max=ldata[k]; | |
1197 } | |
1198 | |
1199 CIFFTR(x, Order, w, ldata); | |
1200 | |
1201 if (i>0) | |
1202 for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*winhann[k]; | |
1203 for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*winhann[k]; | |
1204 | |
1205 for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k]; | |
1206 memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); | |
1207 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
1208 } | |
1209 memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double)); | |
1210 | |
1211 max*=maskcoef; | |
1212 | |
1213 for (int i=0; i<Wid; i++) | |
1214 winhann[i]=winhann[i]*winhamm[i]; | |
1215 | |
1216 for (int i=0; i<Fr; i++) | |
1217 { | |
1218 memset(x, 0, sizeof(cdouble)*Wid); | |
1219 for (int k=On; k<Off; k++) | |
1220 { | |
1221 randfi=rand()*M_PI*2/RAND_MAX; | |
1222 x[k].x=x[Wid-k].x=max*cos(randfi); | |
1223 x[k].y=max*sin(randfi); | |
1224 x[Wid-k].y=-x[k].y; | |
1225 } | |
1226 | |
1227 CIFFTR(x, Order, w, ldata); | |
1228 | |
1229 if (i>0) | |
1230 for (int k=0; k<HWid; k++) | |
1231 ldata[k]=ldata[k]*winhann[k]; | |
1232 for (int k=HWid; k<Wid; k++) | |
1233 ldata[k]=ldata[k]*winhann[k]; | |
1234 | |
1235 for (int k=0; k<Wid; k++) dataout[i*HWid+k]+=ldata[k]; | |
1236 } | |
1237 | |
1238 memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double)); | |
1239 | |
1240 delete[] winhann; | |
1241 delete[] winhamm; | |
1242 delete[] tmpdata; | |
1243 FreeFFTBuffer(ldata); | |
1244 | |
1245 return max; | |
1246 }//FFTMask | |
1247 | |
1248 //--------------------------------------------------------------------------- | |
1249 /* | |
1250 function FindInc: find the element in ordered list data that is closest to value. | |
1251 | |
1252 In: data[Count]: a ordered list | |
1253 value: the value to locate in the list | |
1254 | |
1255 Returns the index of the element in the sorted list which is closest to $value. | |
1256 */ | |
1257 int FindInc(double value, double* data, int Count) | |
1258 { | |
1259 if (value>=data[Count-1]) return Count-1; | |
1260 if (value<data[0]) return 0; | |
1261 int end=InsertInc(value, data, Count, false); | |
1262 if (fabs(value-data[end-1])<fabs(value-data[end])) return end-1; | |
1263 else return end; | |
1264 }//FindInc | |
1265 | |
1266 //--------------------------------------------------------------------------- | |
1267 /* | |
1268 function Gaussian: Gaussian function | |
1269 | |
1270 In: Vector[Dim]: a vector | |
1271 Mean[Dim]: mean of Gaussian function | |
1272 Dev[Fim]: diagonal autocorrelation matrix of Gaussian function | |
1273 | |
1274 Returns the value of Gaussian function at Vector[]. | |
1275 */ | |
1276 double Gaussian(int Dim, double* Vector, double* Mean, double* Dev) | |
1277 { | |
1278 double bmt=0, tmp; | |
1279 for (int dim=0; dim<Dim; dim++) | |
1280 { | |
1281 tmp=Vector[dim]-Mean[dim]; | |
1282 bmt+=tmp*tmp/Dev[dim]; | |
1283 } | |
1284 bmt=-bmt/2; | |
1285 tmp=log(Dev[0]); | |
1286 for (int dim=1; dim<Dim; dim++) tmp+=log(Dev[dim]); | |
1287 bmt-=tmp/2; | |
1288 bmt-=Dim*log(M_PI*2)/2; | |
1289 bmt=exp(bmt); | |
1290 return bmt; | |
1291 }//Gaussian | |
1292 | |
1293 | |
1294 //--------------------------------------------------------------------------- | |
1295 /* | |
1296 function Hamming: calculates the amplitude spectrum of Hamming window at a given frequency | |
1297 | |
1298 In: f: frequency | |
1299 T: size of Hamming window | |
1300 | |
1301 Returns the amplitude spectrum at specified frequency. | |
1302 */ | |
1303 double Hamming(double f, double T) | |
1304 { | |
1305 double omg0=2*M_PI/T; | |
1306 double omg=f*2*M_PI; | |
1307 cdouble c1, c2, c3; | |
1308 cdouble nj(0, -1); | |
1309 cdouble pj(0, 1); | |
1310 double a=0.54, b=0.46; | |
1311 | |
1312 cdouble c=1.0-exp(nj*T*omg); | |
1313 double half=0.5; | |
1314 | |
1315 if (fabs(omg)<1e-100) | |
1316 c1=a*T; | |
1317 else | |
1318 c1=a*c/(pj*omg); | |
1319 | |
1320 if (fabs(omg+omg0)<1e-100) | |
1321 c2=b*0.5*T; | |
1322 else | |
1323 c2=c*b*half/(nj*cdouble(omg+omg0)); | |
1324 | |
1325 if (fabs(omg-omg0)<1e-100) | |
1326 c3=b*0.5*T; | |
1327 else | |
1328 c3=b*c*half/(nj*cdouble(omg-omg0)); | |
1329 | |
1330 c=c1+c2+c3; | |
1331 return abs(c); | |
1332 }//Hamming*/ | |
1333 | |
1334 //--------------------------------------------------------------------------- | |
1335 /* | |
1336 function HannSq: computes the square norm of Hann window spectrum (window-size-normalized) | |
1337 | |
1338 In: x: frequency, in bins | |
1339 N: size of Hann window | |
1340 | |
1341 Return the square norm. | |
1342 */ | |
1343 double HannSq(double x, double N) | |
1344 { | |
1345 double re, im; | |
1346 double pim=M_PI*x; | |
1347 double pimf=pim/N; | |
1348 double pif=M_PI/N; | |
1349 | |
1350 double sinpim=sin(pim); | |
1351 double sinpimf=sin(pimf); | |
1352 double sinpimplus1f=sin(pimf+pif); | |
1353 double sinpimminus1f=sin(pimf-pif); | |
1354 | |
1355 double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf; | |
1356 | |
1357 if (sinpimf==0) | |
1358 spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0; | |
1359 else if (sinpimplus1f==0) | |
1360 spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0; | |
1361 else if (sinpimminus1f==0) | |
1362 spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0; | |
1363 else | |
1364 spmdivbyspmf=sinpim/sinpimf, spmdivbyspmpf=sinpim/sinpimplus1f, spmdivbyspmmf=sinpim/sinpimminus1f; | |
1365 | |
1366 re=0.5*spmdivbyspmf-0.25*cos(pif)*(spmdivbyspmpf+spmdivbyspmmf); | |
1367 im=0.25*sin(pif)*(-spmdivbyspmpf+spmdivbyspmmf); | |
1368 | |
1369 return (re*re+im*im)/(N*N); | |
1370 }//HannSq | |
1371 | |
1372 /* | |
1373 function Hann: computes the Hann window amplitude spectrum (window-size-normalized). | |
1374 | |
1375 In: x: frequency, in bins | |
1376 N: size of Hann window | |
1377 | |
1378 Return the amplitude spectrum evaluated at x. Maximum 0.5 is reached at x=0. Time 2 to normalize | |
1379 maximum to 1. | |
1380 */ | |
1381 double Hann(double x, double N) | |
1382 { | |
1383 double pim=M_PI*x; | |
1384 double pif=M_PI/N; | |
1385 double pimf=pif*x; | |
1386 | |
1387 double sinpim=sin(pim); | |
1388 double tanpimf=tan(pimf); | |
1389 double tanpimplus1f=tan(pimf+pif); | |
1390 double tanpimminus1f=tan(pimf-pif); | |
1391 | |
1392 double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf; | |
1393 | |
1394 if (fabs(tanpimf)<1e-10) | |
1395 spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0; | |
1396 else if (fabs(tanpimplus1f)<1e-10) | |
1397 spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0; | |
1398 else if (fabs(tanpimminus1f)<1e-10) | |
1399 spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0; | |
1400 else | |
1401 spmdivbyspmf=sinpim/tanpimf, spmdivbyspmpf=sinpim/tanpimplus1f, spmdivbyspmmf=sinpim/tanpimminus1f; | |
1402 | |
1403 double result=0.5*spmdivbyspmf-0.25*(spmdivbyspmpf+spmdivbyspmmf); | |
1404 | |
1405 return result/N; | |
1406 }//HannC | |
1407 | |
1408 /* | |
1409 function HxPeak2: fine spectral peak detection. This does detection and high-precision LSE estimation | |
1410 in one go. However, since in practise most peaks are spurious, LSE estimation is not necessary on | |
1411 them. Accordingly, HxPeak2 is deprecated in favour of faster but coarser peak picking methods, such as | |
1412 QIFFT, which leaves fine estimation to a later stage of processing. | |
1413 | |
1414 In: F, dF, ddF: pointers to functions that compute LSE peak energy for, plus its 1st and 2nd | |
1415 derivatives against, a given frequency. | |
1416 params: pointer to a data structure (l_hx) hosting input data fed to F, dF, and ddF | |
1417 (st, en): frequency range, in bins, to search for peaks in | |
1418 epf: convergence detection threshold | |
1419 Out: hps[return value]: peak frequencies | |
1420 vps[return value]; peak amplitudes | |
1421 | |
1422 Returns the number of peaks detected. | |
1423 */ | |
1424 int HxPeak2(double*& hps, double*& vhps, double (*F)(double, void*), double (*dF)(double, void*), double(*ddF)(double, void*), void* params, double st, double en, double epf) | |
1425 { | |
1426 struct l_hx {int N; union {double B; struct {int k1; int k2;};}; cdouble* x; double dhxpeak; double hxpeak;} *p=(l_hx *)params; | |
1427 int dfshift=int(&((l_hx*)0)->dhxpeak); | |
1428 int fshift=int(&((l_hx*)0)->hxpeak); | |
1429 double B=p->B; | |
1430 int count=0; | |
1431 | |
1432 int den=ceil(en), dst=floor(st); | |
1433 if (den-dst<3) den++, dst--; | |
1434 if (den-dst<3) den++, dst--; | |
1435 if (dst<1) dst=1; | |
1436 | |
1437 double step=0.5; | |
1438 int num=(den-dst)/step+1; | |
1439 bool allochps=false, allocvhps=false; | |
1440 if (hps==NULL) allochps=true, hps=new double[num]; | |
1441 if (vhps==NULL) allocvhps=true, vhps=new double[num]; | |
1442 | |
1443 { | |
1444 double* inp=new double[num]; | |
1445 for (int i=0; i<num; i++) | |
1446 { | |
1447 double lf=dst+step*i; | |
1448 p->k1=ceil(lf-B); if (p->k1<0) p->k1=0; | |
1449 p->k2=floor(lf+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; | |
1450 inp[i]=F(lf, params); | |
1451 } | |
1452 | |
1453 for (int i=1; i<num-1; i++) | |
1454 { | |
1455 if (inp[i]>=inp[i-1] && inp[i]>=inp[i+1]) //inp[i]=F(dst+step*i) | |
1456 { | |
1457 if (inp[i]==inp[i-1] && inp[i]==inp[i+1]) continue; | |
1458 double fa=dst+step*(i-1), fb=dst+step*(i+1); | |
1459 double ff=dst+step*i; | |
1460 p->k1=ceil(ff-B); if (p->k1<0) p->k1=0; | |
1461 p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; | |
1462 | |
1463 double tmp=Newton1dmax(ff, fa, fb, ddF, params, dfshift, fshift, dF, dfshift, epf); | |
1464 | |
1465 //although we have selected inp[i] to be a local maximum, different truncation | |
1466 // of local spectrum implies it may not hold as the truncation of inp[i] is | |
1467 // used for recalculating inp[i-1] and inp[i+1] in init_Newton method. In this | |
1468 // case we retry the sub-maximal frequency to see if it becomes a local maximum | |
1469 // when the spectrum is truncated to centre on it. | |
1470 | |
1471 if (tmp==-0.5 || tmp==-0.7) //y(fa)<=y(ff)<y(fb) or y(ff)<y(fa)<y(fb) | |
1472 { | |
1473 tmp=Newton1dmax(fb, ff, 2*fb-ff, ddF, params, dfshift, fshift, dF, dfshift, epf); | |
1474 if (tmp==-0.5 || tmp==-0.7) continue; | |
1475 /* | |
1476 double ff2=(ff+fb)/2; | |
1477 p->k1=ceil(ff2-B); if (p->k1<0) p->k1=0; | |
1478 p->k2=floor(ff2+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; | |
1479 tmp=Newton1dmax(ff2, ff, fb, ddF, params, dfshift, fshift, dF, dfshift, epf); | |
1480 p->k1=ceil(ff-B); if (p->k1<0) p->k1=0; | |
1481 p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; */ | |
1482 } | |
1483 else if (tmp==-0.6 || tmp==-0.8) //y(fb)<=y(ff)<y(fa) | |
1484 { | |
1485 tmp=Newton1dmax(fa, 2*fa-ff, ff, ddF, params, dfshift, fshift, dF, dfshift, epf); | |
1486 if (tmp==-0.6 || tmp==-0.8) continue; | |
1487 } | |
1488 if (tmp<0 /*tmp==-0.5 || tmp==-0.6 || tmp==-1 || tmp==-2 || tmp==-3*/) | |
1489 { | |
1490 Search1Dmax(ff, params, F, dst+step*(i-1), dst+step*(i+1), &vhps[count], epf); | |
1491 } | |
1492 else | |
1493 { | |
1494 vhps[count]=p->hxpeak; | |
1495 } | |
1496 if (ff>=st && ff<=en && ff>dst+step*(i-0.99) && ff<dst+step*(i+0.99)) | |
1497 { | |
1498 // if (count==0 || fabs(tmp-hps[count-1])>0.1) | |
1499 // { | |
1500 hps[count]=ff; | |
1501 count++; | |
1502 // } | |
1503 } | |
1504 } | |
1505 } | |
1506 delete[] inp; | |
1507 } | |
1508 | |
1509 if (allochps) hps=(double*)realloc(hps, sizeof(double)*count); | |
1510 if (allocvhps) vhps=(double*)realloc(vhps, sizeof(double)*count); | |
1511 return count; | |
1512 }//HxPeak2 | |
1513 | |
1514 //--------------------------------------------------------------------------- | |
1515 /* | |
1516 function InsertDec: inserts value into sorted decreasing list | |
1517 | |
1518 In: data[Count]: a sorted decreasing list. | |
1519 value: the value to be added | |
1520 Out: data[Count]: the list with $value inserted if the latter is larger than its last entry, in which | |
1521 case the original last entry is discarded. | |
1522 | |
1523 Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to | |
1524 data[Count-1]. | |
1525 */ | |
1526 int InsertDec(int value, int* data, int Count) | |
1527 { | |
1528 if (Count<=0) return -1; | |
1529 if (value<=data[Count-1]) return -1; | |
1530 if (value>data[0]) | |
1531 { | |
1532 memmove(&data[1], &data[0], sizeof(int)*(Count-1)); | |
1533 data[0]=value; | |
1534 return 0; | |
1535 } | |
1536 | |
1537 //now Count>=2 | |
1538 int head=0, end=Count-1, mid; | |
1539 | |
1540 //D(head)>=value>D(end) | |
1541 while (end-head>1) | |
1542 { | |
1543 mid=(head+end)/2; | |
1544 if (value<=data[mid]) head=mid; | |
1545 else end=mid; | |
1546 } | |
1547 | |
1548 //D(head=end-1)>=value>D(end) | |
1549 memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1)); | |
1550 data[end]=value; | |
1551 return end; | |
1552 }//InsertDec | |
1553 //the double version | |
1554 int InsertDec(double value, double* data, int Count) | |
1555 { | |
1556 if (Count<=0) return -1; | |
1557 if (value<=data[Count-1]) return -1; | |
1558 if (value>data[0]) | |
1559 { | |
1560 memmove(&data[1], &data[0], sizeof(double)*(Count-1)); | |
1561 data[0]=value; | |
1562 return 0; | |
1563 } | |
1564 | |
1565 //now Count>=2 | |
1566 int head=0, end=Count-1, mid; | |
1567 | |
1568 //D(head)>=value>D(end) | |
1569 while (end-head>1) | |
1570 { | |
1571 mid=(head+end)/2; | |
1572 if (value<=data[mid]) head=mid; | |
1573 else end=mid; | |
1574 } | |
1575 | |
1576 //D(head=end-1)>=value>D(end) | |
1577 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); | |
1578 data[end]=value; | |
1579 return end; | |
1580 }//InsertDec | |
1581 | |
1582 /* | |
1583 function InsertDec: inserts value and attached integer into sorted decreasing list | |
1584 | |
1585 In: data[Count]: a sorted decreasing list | |
1586 indices[Count]: a list of integers attached to entries of data[] | |
1587 value, index: the value to be added and its attached integer | |
1588 Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is larger than | |
1589 the last entry of data[], in which case the original last entries are discarded. | |
1590 | |
1591 Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to | |
1592 data[Count-1]. | |
1593 */ | |
1594 int InsertDec(double value, int index, double* data, int* indices, int Count) | |
1595 { | |
1596 if (Count<=0) return -1; | |
1597 if (value<=data[Count-1]) return -1; | |
1598 if (value>data[0]) | |
1599 { | |
1600 memmove(&data[1], data, sizeof(double)*(Count-1)); | |
1601 memmove(&indices[1], indices, sizeof(int)*(Count-1)); | |
1602 data[0]=value, indices[0]=index; | |
1603 return 0; | |
1604 } | |
1605 | |
1606 //now Count>=2 | |
1607 int head=0, end=Count-1, mid; | |
1608 | |
1609 //D(head)>=value>D(end) | |
1610 while (end-head>1) | |
1611 { | |
1612 mid=(head+end)/2; | |
1613 if (value<=data[mid]) head=mid; | |
1614 else end=mid; | |
1615 } | |
1616 | |
1617 //D(head=end-1)>=value>D(end) | |
1618 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); | |
1619 memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1)); | |
1620 data[end]=value, indices[end]=index; | |
1621 return end; | |
1622 }//InsertDec | |
1623 | |
1624 /* | |
1625 InsertInc: inserts value into sorted increasing list. | |
1626 | |
1627 In: data[Count]: a sorted increasing list. | |
1628 Capacity: maximal size of data[] | |
1629 value: the value to be added | |
1630 Compare: pointer to function that compare two values | |
1631 Out: data[Count]: the list with $value inserted. If the original list is full (Count=Capacity) then | |
1632 either $value, or the last entry of data[], whichever is larger, is discarded. | |
1633 | |
1634 Returns the index where $value is located in data[], or -1 if it is not inserted, which happens if | |
1635 Count=Capacity and $value is larger than or equal to the last entry in data[Capacity]. | |
1636 */ | |
1637 int InsertInc(void* value, void** data, int Count, int Capacity, int (*Compare)(void*, void*)) | |
1638 { | |
1639 if (Capacity<=0) return -1; | |
1640 if (Count>Capacity) Count=Capacity; | |
1641 | |
1642 //Compare(A,B)<0 if A<B, =0 if A=B, >0 if A>B | |
1643 int PosToInsert; | |
1644 if (Count==0) PosToInsert=0; | |
1645 else if (Compare(value, data[Count-1])>0) PosToInsert=Count; | |
1646 else if (Compare(value, data[0])<0) PosToInsert=0; | |
1647 else | |
1648 { | |
1649 //now Count>=2 | |
1650 int head=0, end=Count-1, mid; | |
1651 | |
1652 //D(head)<=value<D(end) | |
1653 while (end-head>1) | |
1654 { | |
1655 mid=(head+end)/2; | |
1656 if (Compare(value, data[mid])>=0) head=mid; | |
1657 else end=mid; | |
1658 } | |
1659 //D(head=end-1)<=value<D(end) | |
1660 PosToInsert=end; | |
1661 } | |
1662 | |
1663 if (Count<Capacity) | |
1664 { | |
1665 memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert)); | |
1666 data[PosToInsert]=value; | |
1667 } | |
1668 else //Count==Capacity | |
1669 { | |
1670 if (PosToInsert>=Capacity) return -1; | |
1671 memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert-1)); | |
1672 data[PosToInsert]=value; | |
1673 } | |
1674 return PosToInsert; | |
1675 }//InsertInc | |
1676 | |
1677 /* | |
1678 function InsertInc: inserts value into sorted increasing list | |
1679 | |
1680 In: data[Count]: a sorted increasing list. | |
1681 value: the value to be added | |
1682 doinsert: specifies whether the actually insertion is to be performed | |
1683 Out: data[Count]: the list with $value inserted if the latter is smaller than its last entry, in which | |
1684 case the original last entry of data[] is discarded. | |
1685 | |
1686 Returns the index where $value is located in data[], or -1 if value is larger than or equal to | |
1687 data[Count-1]. | |
1688 */ | |
1689 int InsertInc(double value, double* data, int Count, bool doinsert) | |
1690 { | |
1691 if (Count<=0) return -1; | |
1692 if (value>=data[Count-1]) return -1; | |
1693 if (value<data[0]) | |
1694 { | |
1695 memmove(&data[1], &data[0], sizeof(double)*(Count-1)); | |
1696 if (doinsert) data[0]=value; | |
1697 return 0; | |
1698 } | |
1699 | |
1700 //now Count>=2 | |
1701 int head=0, end=Count-1, mid; | |
1702 | |
1703 //D(head)<=value<D(end) | |
1704 while (end-head>1) | |
1705 { | |
1706 mid=(head+end)/2; | |
1707 if (value>=data[mid]) head=mid; | |
1708 else end=mid; | |
1709 } | |
1710 | |
1711 //D(head=end-1)<=value<D(end) | |
1712 if (doinsert) | |
1713 { | |
1714 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); | |
1715 data[end]=value; | |
1716 } | |
1717 return end; | |
1718 }//InsertInc | |
1719 //version where data[] is int. | |
1720 int InsertInc(double value, int* data, int Count, bool doinsert) | |
1721 { | |
1722 if (Count<=0) return -1; | |
1723 if (value>=data[Count-1]) return -1; | |
1724 if (value<data[0]) | |
1725 { | |
1726 memmove(&data[1], &data[0], sizeof(int)*(Count-1)); | |
1727 if (doinsert) data[0]=value; | |
1728 return 0; | |
1729 } | |
1730 | |
1731 //now Count>=2 | |
1732 int head=0, end=Count-1, mid; | |
1733 | |
1734 //D(head)<=value<D(end) | |
1735 while (end-head>1) | |
1736 { | |
1737 mid=(head+end)/2; | |
1738 if (value>=data[mid]) head=mid; | |
1739 else end=mid; | |
1740 } | |
1741 | |
1742 //D(head=end-1)<=value<D(end) | |
1743 if (doinsert) | |
1744 { | |
1745 memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1)); | |
1746 data[end]=value; | |
1747 } | |
1748 return end; | |
1749 }//InsertInc | |
1750 | |
1751 /* | |
1752 function InsertInc: inserts value and attached integer into sorted increasing list | |
1753 | |
1754 In: data[Count]: a sorted increasing list | |
1755 indices[Count]: a list of integers attached to entries of data[] | |
1756 value, index: the value to be added and its attached integer | |
1757 Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is smaller than | |
1758 the last entry of data[], in which case the original last entries are discarded. | |
1759 | |
1760 Returns the index where $value is located in data[], or -1 if $value is larger than or equal to | |
1761 data[Count-1]. | |
1762 */ | |
1763 int InsertInc(double value, int index, double* data, int* indices, int Count) | |
1764 { | |
1765 if (Count<=0) return -1; | |
1766 if (value>=data[Count-1]) return -1; | |
1767 if (value<data[0]) | |
1768 { | |
1769 memmove(&data[1], data, sizeof(double)*(Count-1)); | |
1770 memmove(&indices[1], indices, sizeof(int)*(Count-1)); | |
1771 data[0]=value, indices[0]=index; | |
1772 return 0; | |
1773 } | |
1774 | |
1775 //now Count>=2 | |
1776 int head=0, end=Count-1, mid; | |
1777 | |
1778 //D(head)>=value>D(end) | |
1779 while (end-head>1) | |
1780 { | |
1781 mid=(head+end)/2; | |
1782 if (value>=data[mid]) head=mid; | |
1783 else end=mid; | |
1784 } | |
1785 | |
1786 //D(head=end-1)>=value>D(end) | |
1787 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); | |
1788 memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1)); | |
1789 data[end]=value, indices[end]=index; | |
1790 return end; | |
1791 }//InsertInc | |
1792 //version where indices[] is double-precision floating point. | |
1793 int InsertInc(double value, double index, double* data, double* indices, int Count) | |
1794 { | |
1795 if (Count<=0) return -1; | |
1796 if (value>=data[Count-1]) return -1; | |
1797 if (value<data[0]) | |
1798 { | |
1799 memmove(&data[1], data, sizeof(double)*(Count-1)); | |
1800 memmove(&indices[1], indices, sizeof(double)*(Count-1)); | |
1801 data[0]=value, indices[0]=index; | |
1802 return 0; | |
1803 } | |
1804 | |
1805 //now Count>=2 | |
1806 int head=0, end=Count-1, mid; | |
1807 | |
1808 //D(head)>=value>D(end) | |
1809 while (end-head>1) | |
1810 { | |
1811 mid=(head+end)/2; | |
1812 if (value>=data[mid]) head=mid; | |
1813 else end=mid; | |
1814 } | |
1815 | |
1816 //D(head=end-1)>=value>D(end) | |
1817 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); | |
1818 memmove(&indices[end+1], &indices[end], sizeof(double)*(Count-end-1)); | |
1819 data[end]=value, indices[end]=index; | |
1820 return end; | |
1821 }//InsertInc | |
1822 | |
1823 /* | |
1824 function InsertIncApp: inserts value into flexible-length sorted increasing list | |
1825 | |
1826 In: data[Count]: a sorted increasing list. | |
1827 value: the value to be added | |
1828 Out: data[Count+1]: the list with $value inserted. | |
1829 | |
1830 Returns the index where $value is located in data[], or -1 if Count<0. data[] must have Count+1 | |
1831 storage units on calling. | |
1832 */ | |
1833 int InsertIncApp(double value, double* data, int Count) | |
1834 { | |
1835 if (Count<0) return -1; | |
1836 if (Count==0){data[0]=value; return 0;} | |
1837 if (value>=data[Count-1]){data[Count]=value; return Count;} | |
1838 if (value<data[0]) | |
1839 { | |
1840 memmove(&data[1], &data[0], sizeof(double)*Count); | |
1841 data[0]=value; | |
1842 return 0; | |
1843 } | |
1844 | |
1845 //now Count>=2 | |
1846 int head=0, end=Count-1, mid; | |
1847 | |
1848 //D(head)<=value<D(end) | |
1849 while (end-head>1) | |
1850 { | |
1851 mid=(head+end)/2; | |
1852 if (value>=data[mid]) head=mid; | |
1853 else end=mid; | |
1854 } | |
1855 | |
1856 //D(head=end-1)<=value<D(end) | |
1857 memmove(&data[end+1], &data[end], sizeof(double)*(Count-end)); | |
1858 data[end]=value; | |
1859 | |
1860 return end; | |
1861 }//InsertIncApp | |
1862 | |
1863 //--------------------------------------------------------------------------- | |
1864 /* | |
1865 function InstantFreq; calculates instantaneous frequency from spectrum, evaluated at bin k | |
1866 | |
1867 In: x[hwid]: spectrum with scale 2hwid | |
1868 k: reference frequency, in bins | |
1869 mode: must be 1. | |
1870 | |
1871 Returns an instantaneous frequency near bin k. | |
1872 */ | |
1873 double InstantFreq(int k, int hwid, cdouble* x, int mode) | |
1874 { | |
1875 double result; | |
1876 switch(mode) | |
1877 { | |
1878 //mode 1: the phase vocoder method, based on J. Brown, where the spectrogram | |
1879 // MUST be calculated using rectangular window | |
1880 case 1: | |
1881 { | |
1882 if (k<1) k=1; | |
1883 if (k>hwid-2) k=hwid-2; | |
1884 double hr=0.5*(x[k].x-0.5*(x[k+1].x+x[k-1].x)), hi=0.5*(x[k].y-0.5*(x[k+1].y+x[k-1].y)); | |
1885 double ph0=Atan2(hi, hr); | |
1886 double c=cos(M_PI/hwid), s=sin(M_PI/hwid); | |
1887 hr=0.5*(x[k].x-0.5*(x[k+1].x*c-x[k+1].y*s+x[k-1].x*c+x[k-1].y*s)); | |
1888 hi=0.5*(x[k].y-0.5*(x[k+1].y*c+x[k+1].x*s+x[k-1].y*c-x[k-1].x*s)); | |
1889 double ph1=Atan2(hi, hr); | |
1890 result=(ph1-ph0)/(2*M_PI); | |
1891 if (result<-0.5) result+=1; | |
1892 if (result>0.5) result-=1; | |
1893 result+=k*0.5/hwid; | |
1894 break; | |
1895 } | |
1896 case 2: | |
1897 break; | |
1898 } | |
1899 return result; | |
1900 }//InstantFreq | |
1901 | |
1902 /* | |
1903 function InstantFreq; calculates "frequency spectrum", a sequence of frequencies evaluated at DFT bins | |
1904 | |
1905 In: x[hwid]: spectrum with scale 2hwid | |
1906 mode: must be 1. | |
1907 Out: freqspec[hwid]: the frequency spectrum | |
1908 | |
1909 No return value. | |
1910 */ | |
1911 void InstantFreq(double* freqspec, int hwid, cdouble* x, int mode) | |
1912 { | |
1913 for (int i=0; i<hwid; i++) | |
1914 freqspec[i]=InstantFreq(i, hwid, x, mode); | |
1915 }//InstantFreq | |
1916 | |
1917 //--------------------------------------------------------------------------- | |
1918 /* | |
1919 function IntToDouble: copy content of integer array to double array | |
1920 | |
1921 In: in: pointer to integer array | |
1922 BytesPerSample: number of bytes each integer takes | |
1923 Count: size of integer array, in integers | |
1924 Out: vector out[Count]. | |
1925 | |
1926 No return value. | |
1927 | |
1928 This version is currently commented out in favour of the version implemented in QuickSpec.cpp which | |
1929 supports 24-bit integers. | |
1930 *//* | |
1931 void IntToDouble(double* out, void* in, int BytesPerSample, int Count) | |
1932 { | |
1933 if (BytesPerSample==1){unsigned char* in8=(unsigned char*)in; for (int k=0; k<Count; k++) *(out++)=*(in8++)-128.0;} | |
1934 else {__int16* in16=(__int16*)in; for (int k=0; k<Count; k++) *(out++)=*(in16++);} | |
1935 }//IntToDouble*/ | |
1936 | |
1937 //--------------------------------------------------------------------------- | |
1938 /* | |
1939 function IPHannC: inner product with Hann window spectrum | |
1940 | |
1941 In: x[N]: spectrum | |
1942 f: reference frequency | |
1943 K1, K2: spectral truncation bounds | |
1944 | |
1945 Returns the absolute value of the inner product of x[K1:K2] with the corresponding band of the | |
1946 spectrum of a sinusoid at frequency f. | |
1947 */ | |
1948 double IPHannC(double f, cdouble* x, int N, int K1, int K2) | |
1949 { | |
1950 int M; double c[4], iH2; | |
1951 windowspec(wtHann, N, &M, c, &iH2); | |
1952 return abs(IPWindowC(f, x, N, M, c, iH2, K1, K2)); | |
1953 }//IPHannC | |
1954 | |
1955 | |
1956 //--------------------------------------------------------------------------- | |
1957 /* | |
1958 function lse: linear regression y=ax+b | |
1959 | |
1960 In: x[Count], y[Count]: input points | |
1961 Out: a, b: LSE estimation of coefficients in y=ax+b | |
1962 | |
1963 No return value. | |
1964 */ | |
1965 void lse(double* x, double* y, int Count, double& a, double& b) | |
1966 { | |
1967 double sx=0, sy=0, sxx=0, sxy=0; | |
1968 for (int i=0; i<Count; i++) | |
1969 { | |
1970 sx+=x[i]; | |
1971 sy+=y[i]; | |
1972 sxx+=x[i]*x[i]; | |
1973 sxy+=x[i]*y[i]; | |
1974 } | |
1975 b=(sxx*sy-sx*sxy)/(Count*sxx-sx*sx); | |
1976 a=(sy-Count*b)/sx; | |
1977 }//lse | |
1978 | |
1979 //-------------------------------------------------------------------------- | |
1980 /* | |
1981 memdoubleadd: vector addition | |
1982 | |
1983 In: dest[count], source[count]: addends | |
1984 Out: dest[count]: sum | |
1985 | |
1986 No return value. | |
1987 */ | |
1988 void memdoubleadd(double* dest, double* source, int count) | |
1989 { | |
1990 for (int i=0; i<count; i++){*dest=*dest+*source; dest++; source++;} | |
1991 }//memdoubleadd | |
1992 | |
1993 //-------------------------------------------------------------------------- | |
1994 /* | |
1995 function Mel: converts frequency in Hz to frequency in mel. | |
1996 | |
1997 In: f: frequency, in Hz | |
1998 | |
1999 Returns the frequency measured on mel scale. | |
2000 */ | |
2001 double Mel(double f) | |
2002 { | |
2003 return 1127.01048*log(1+f/700); | |
2004 }//Mel | |
2005 | |
2006 /* | |
2007 function InvMel: converts frequency in mel to frequency in Hz. | |
2008 | |
2009 In: f: frequency, in mel. | |
2010 | |
2011 Returns the frequency in Hz. | |
2012 */ | |
2013 double InvMel(double mel) | |
2014 { | |
2015 return 700*(exp(mel/1127.01048)-1); | |
2016 }//InvMel | |
2017 | |
2018 /* | |
2019 function MFCC: calculates MFCC. | |
2020 | |
2021 In: Data[FrameWidth]: data | |
2022 NumBands: number of frequency bands on mel scale | |
2023 Bands[3*NumBands]: mel frequency bands, comes as $NumBands triples, each containing the lower, | |
2024 middle and high frequencies, in bins, of one band, from which a weighting window is created to | |
2025 weight individual bins. | |
2026 Ceps_Order: number of MFC coefficients (i.e. DCT coefficients) | |
2027 W, X: FFT buffers | |
2028 Out: C[Ceps_Order]: MFCC | |
2029 Amps[NumBands]: log spectrum on MF bands | |
2030 | |
2031 No return value. Use MFCCPrepareBands() to retrieve Bands[]. | |
2032 */ | |
2033 void MFCC(int FrameWidth, int NumBands, int Ceps_Order, double* Data, double* Bands, double* C, double* Amps, cdouble* W, cdouble* X) | |
2034 { | |
2035 double tmp, b2s, b2c, b2e; | |
2036 | |
2037 RFFTC(Data, 0, 0, log2(FrameWidth), W, X, 0); | |
2038 for (int i=0; i<=FrameWidth/2; i++) Amps[i]=X[i].x*X[i].x+X[i].y*X[i].y; | |
2039 | |
2040 for (int i=0; i<NumBands; i++) | |
2041 { | |
2042 tmp=0; | |
2043 b2s=Bands[3*i], b2c=Bands[3*i+1], b2e=Bands[3*i+2]; | |
2044 | |
2045 for (int j=ceil(b2s); j<ceil(b2c); j++) | |
2046 tmp+=Amps[j]*(j-b2s)/(b2c-b2s); | |
2047 for (int j=ceil(b2c); j<b2e; j++) | |
2048 tmp+=Amps[j]*(b2e-j)/(b2e-b2c); | |
2049 | |
2050 if (tmp<3.7200759760208359629596958038631e-44) | |
2051 Amps[i]=-100; | |
2052 else | |
2053 Amps[i]=log(tmp); | |
2054 } | |
2055 | |
2056 for (int i=0; i<Ceps_Order; i++) | |
2057 { | |
2058 tmp=Amps[0]*cos(M_PI*(i+1)/2/NumBands); | |
2059 for (int j=1; j<NumBands; j++) | |
2060 tmp+=Amps[j]*cos(M_PI*(i+0.5)*(j+0.5)/NumBands); | |
2061 C[i]=tmp; | |
2062 } | |
2063 }//MFCC | |
2064 | |
2065 /* | |
2066 function MFCCPrepareBands: returns a array of OVERLAPPING bands given in triples, whose 1st and 3rd | |
2067 entries are the start and end of a band, in bins, and the 2nd is a middle frequency. | |
2068 | |
2069 In: SamplesPerSec: sampling rate | |
2070 NumberOfBins: FFT size | |
2071 NumberOfBands: number of mel-frequency bands | |
2072 | |
2073 Returns pointer to the array of triples. | |
2074 */ | |
2075 double* MFCCPrepareBands(int NumberOfBands, int SamplesPerSec, int NumberOfBins) | |
2076 { | |
2077 double* Bands=new double[NumberOfBands*3]; | |
2078 double naqfreq=SamplesPerSec/2.0; //naqvist freq | |
2079 double binwid=SamplesPerSec*1.0/NumberOfBins; | |
2080 double naqmel=Mel(naqfreq); | |
2081 double b=naqmel/(NumberOfBands+1); | |
2082 | |
2083 Bands[0]=0; | |
2084 Bands[1]=InvMel(b)/binwid; | |
2085 Bands[2]=InvMel(b*2)/binwid; | |
2086 for (int i=1; i<NumberOfBands; i++) | |
2087 { | |
2088 Bands[3*i]=Bands[3*i-2]; | |
2089 Bands[3*i+1]=Bands[3*i-1]; | |
2090 Bands[3*i+2]=InvMel(b*(i+2))/binwid; | |
2091 } | |
2092 return Bands; | |
2093 }//MFCCPrepareBands | |
2094 | |
2095 //--------------------------------------------------------------------------- | |
2096 /* | |
2097 function Multi: vector-constant multiplication | |
2098 | |
2099 In: data[count]: a vector | |
2100 mul: a constant | |
2101 Out: data[count]: their product | |
2102 | |
2103 No return value. | |
2104 */ | |
2105 void Multi(double* data, int count, double mul) | |
2106 { | |
2107 for (int i=0; i<count; i++){*data=*data*mul; data++;} | |
2108 }//Multi | |
2109 | |
2110 /* | |
2111 function Multi: vector-constant multiplication | |
2112 | |
2113 In: in[count]: a vector | |
2114 mul: a constant | |
2115 Out: out[count]: their product | |
2116 | |
2117 No return value. | |
2118 */ | |
2119 void Multi(double* out, double* in, int count, double mul) | |
2120 { | |
2121 for (int i=0; i<count; i++) *(out++)=*(in++)*mul; | |
2122 }//Multi | |
2123 | |
2124 /* | |
2125 function Multi: vector-constant multiply-addition | |
2126 | |
2127 In: in[count], adder[count]: vectors | |
2128 mul: a constant | |
2129 Out: out[count]: in[]+adder[]*mul | |
2130 | |
2131 No return value. | |
2132 */ | |
2133 void MultiAdd(double* out, double* in, double* adder, int count, double mul) | |
2134 { | |
2135 for (int i=0; i<count; i++) *(out++)=*(in++)+*(adder++)*mul; | |
2136 }//MultiAdd | |
2137 | |
2138 //--------------------------------------------------------------------------- | |
2139 /* | |
2140 function NearestPeak: finds a peak value in an array that is nearest to a given index | |
2141 | |
2142 In: data[count]: an array | |
2143 anindex: an index | |
2144 | |
2145 Returns the index to a peak of data[] that is closest to anindex. In case of two cloest indices, | |
2146 returns the index to the higher peak of the two. | |
2147 */ | |
2148 int NearestPeak(double* data, int count, int anindex) | |
2149 { | |
2150 int upind=anindex, downind=anindex; | |
2151 if (anindex<1) anindex=1; | |
2152 if (anindex>count-2) anindex=count-2; | |
2153 | |
2154 if (data[anindex]>data[anindex-1] && data[anindex]>data[anindex+1]) return anindex; | |
2155 | |
2156 if (data[anindex]<data[anindex-1]) | |
2157 while (downind>0 && data[downind-1]>data[downind]) downind--; | |
2158 if (data[anindex]<data[anindex+1]) | |
2159 while (upind<count-1 && data[upind]<data[upind+1]) upind++; | |
2160 | |
2161 if (upind==anindex) return downind; | |
2162 if (downind==anindex) return upind; | |
2163 | |
2164 if (anindex-downind<upind-anindex) return downind; | |
2165 else if (anindex-downind>upind-anindex) return upind; | |
2166 else if (data[upind]<data[downind]) return downind; | |
2167 else return upind; | |
2168 }//NearestPeak | |
2169 | |
2170 //--------------------------------------------------------------------------- | |
2171 /* | |
2172 function NegativeExp: fits the curve y=1-x^lmd. | |
2173 | |
2174 In: x[Count], y[Count]: sample points to fit, x[0]=0, x[Count-1]=1, y[0]=1, y[Count-1]=0 | |
2175 Out: lmd: coefficient of y=1-x^lmd. | |
2176 | |
2177 Returns rms fitting error. | |
2178 */ | |
2179 double NegativeExp(double* x, double* y, int Count, double& lmd, int sample, double step, double eps, int maxiter) | |
2180 { | |
2181 lmd=0; | |
2182 for (int i=1; i<Count-1; i++) | |
2183 { | |
2184 if (y[i]<1) | |
2185 lmd+=log(1-y[i])/log(x[i]); | |
2186 else | |
2187 lmd+=-50/log(x[i]); | |
2188 } | |
2189 lmd/=(Count-2); | |
2190 | |
2191 //lmd has been initialized | |
2192 //coming up will be the recursive calculation of lmd by lgg | |
2193 | |
2194 int iter=0; | |
2195 double laste, lastdel, e=0, del=0, tmp; | |
2196 do | |
2197 { | |
2198 iter++; | |
2199 laste=e; | |
2200 lastdel=del; | |
2201 e=0, del=0; | |
2202 for (int i=1; i<Count-1; i+=sample) | |
2203 { | |
2204 tmp=pow(x[i], lmd); | |
2205 e=e+(y[i]+tmp-1)*(y[i]+tmp-1); | |
2206 del=del+(y[i]+tmp-1)*tmp*log(x[i]); | |
2207 } | |
2208 if (laste && e>laste) lmd+=step*lastdel, step/=2; | |
2209 else lmd+=-step*sample*del; | |
2210 } | |
2211 while (e && iter<=maxiter && (!laste || fabs(laste-e)/e>eps)); | |
2212 return sqrt(e/Count); | |
2213 }//NegativeExp | |
2214 | |
2215 //--------------------------------------------------------------------------- | |
2216 /* | |
2217 function: NL: noise level, calculated on 5% of total frames with least energy | |
2218 | |
2219 In: data[Count]: | |
2220 Wid: window width for power level estimation | |
2221 | |
2222 Returns noise level, in rms. | |
2223 */ | |
2224 double NL(double* data, int Count, int Wid) | |
2225 { | |
2226 int Fr=Count/Wid; | |
2227 int Num=Fr/20+1; | |
2228 double* ene=new double[Num], tmp; | |
2229 for (int i=0; i<Num; i++) ene[i]=1e30; | |
2230 for (int i=0; i<Fr; i++) | |
2231 { | |
2232 tmp=DCPower(&data[i*Wid], Wid, 0); | |
2233 InsertInc(tmp, ene, Num); | |
2234 } | |
2235 double result=Avg(ene, Num, 0); | |
2236 delete[] ene; | |
2237 result=sqrt(result); | |
2238 return result; | |
2239 }//NL | |
2240 | |
2241 //--------------------------------------------------------------------------- | |
2242 /* | |
2243 function Normalize: normalizes data to [-Maxi, Maxi], without zero shift | |
2244 | |
2245 In: data[Count]: data to be normalized | |
2246 Maxi: destination maximal absolute value | |
2247 Out: data[Count]: normalized data | |
2248 | |
2249 Returns the original maximal absolute value. | |
2250 */ | |
2251 double Normalize(double* data, int Count, double Maxi) | |
2252 { | |
2253 double max=0; | |
2254 double* ldata=data; | |
2255 for (int i=0; i<Count; i++) | |
2256 { | |
2257 if (*ldata>max) max=*ldata; | |
2258 else if (-*ldata>max) max=-*ldata; | |
2259 ldata++; | |
2260 } | |
2261 if (max>0) | |
2262 { | |
2263 Maxi=Maxi/max; | |
2264 for (int i=0; i<Count; i++) *(data++)*=Maxi; | |
2265 } | |
2266 return max; | |
2267 }//Normalize | |
2268 | |
2269 /* | |
2270 function Normalize2: normalizes data to a specified Euclidian norm | |
2271 | |
2272 In: data[Count]: data to normalize | |
2273 Norm: destination Euclidian norm | |
2274 Out: data[Count]: normalized data. | |
2275 | |
2276 Returns the original Euclidian norm. | |
2277 */ | |
2278 double Normalize2(double* data, int Count, double Norm) | |
2279 { | |
2280 double norm=0; | |
2281 for (int i=0; i<Count; i++) norm+=data[i]*data[i]; | |
2282 norm=sqrt(norm); | |
2283 double mul=norm/Norm; | |
2284 if (mul!=0) for (int i=0; i<Count; i++) data[i]/=mul; | |
2285 return norm; | |
2286 }//Normalize2 | |
2287 | |
2288 //--------------------------------------------------------------------------- | |
2289 /* | |
2290 function PhaseSpan: computes the unwrapped phase variation across the Nyquist range | |
2291 | |
2292 In: data[Count]: time-domain data | |
2293 aparams: a fftparams structure | |
2294 | |
2295 Returns the difference between unwrapped phase angles at 0 and Nyquist frequency. | |
2296 */ | |
2297 double PhaseSpan(double* data, int Count, void* aparams) | |
2298 { | |
2299 int Pad=1; | |
2300 fftparams* params=(fftparams*)aparams; | |
2301 double* Arg=new double[Count*Pad]; | |
2302 AllocateFFTBuffer(Count*Pad, Amp, w, x); | |
2303 memset(Amp, 0, sizeof(double)*Count*Pad); | |
2304 memcpy(&Amp[Count*(Pad-1)/2], data, sizeof(double)*Count); | |
2305 ApplyWindow(Amp, Amp, params->Amp, Count); | |
2306 RFFTC(Amp, Amp, Arg, log2(Count*Pad), w, x, 0); | |
2307 | |
2308 SmoothPhase(Arg, Count*Pad/2+1); | |
2309 double result=Arg[Count*Pad/2]-Arg[0]; | |
2310 delete[] Arg; | |
2311 FreeFFTBuffer(Amp); | |
2312 return result; | |
2313 }//PhaseSpan | |
2314 | |
2315 //--------------------------------------------------------------------------- | |
2316 /* | |
2317 function PolyFit: least square polynomial fitting y=sum(i){a[i]*x^i} | |
2318 | |
2319 In: x[N], y[N]: sample points | |
2320 P: order of polynomial, P<N | |
2321 Out: a[P+1]: coefficients of polynomial | |
2322 | |
2323 No return value. | |
2324 */ | |
2325 void PolyFit(int P, double* a, int N, double* x, double* y) | |
2326 { | |
2327 Alloc2(P+1, P+1, aa); | |
2328 double ai0, bi, *bb=new double[P+1], *s=new double[N], *r=new double[N]; | |
2329 aa[0][0]=N; bi=0; for (int i=0; i<N; i++) s[i]=1, r[i]=y[i], bi+=y[i]; bb[0]=bi; | |
2330 | |
2331 for (int i=1; i<=P; i++) | |
2332 { | |
2333 ai0=bi=0; for (int j=0; j<N; j++) {s[j]*=x[j], r[j]*=x[j]; ai0+=s[j], bi+=r[j];} | |
2334 for (int j=0; j<=i; j++) aa[i-j][j]=ai0; bb[i]=bi; | |
2335 } | |
2336 for (int i=P+1; i<=2*P; i++) | |
2337 { | |
2338 ai0=0; for (int j=0; j<N; j++) {s[j]*=x[j]; ai0+=s[j];} | |
2339 for (int j=i-P; j<=P; j++) aa[i-j][j]=ai0; | |
2340 } | |
2341 GESCP(P+1, a, aa, bb); | |
2342 DeAlloc2(aa); delete[] bb; delete[] s; delete[] r; | |
2343 }//PolyFit | |
2344 | |
2345 //--------------------------------------------------------------------------- | |
2346 /* | |
2347 function Pow: vector power function | |
2348 | |
2349 In: data[Count]: a vector | |
2350 ex: expontnet | |
2351 Out: data[Count]: point-wise $ex-th power of data[] | |
2352 | |
2353 No return value. | |
2354 */ | |
2355 void Pow(double* data, int Count, double ex) | |
2356 { | |
2357 for (int i=0; i<Count; i++) | |
2358 data[i]=pow(data[i], ex); | |
2359 }//Power | |
2360 | |
2361 //--------------------------------------------------------------------------- | |
2362 /* | |
2363 Rectify: semi-wave rectification | |
2364 | |
2365 In: data[Count]: data to rectify | |
2366 th: rectification threshold: values below th are rectified to th | |
2367 Out: data[Count]: recitified data | |
2368 | |
2369 Returns number of preserved (i.e. not rectified) samples. | |
2370 */ | |
2371 int Rectify(double* data, int Count, double th) | |
2372 { | |
2373 int Result=0; | |
2374 for (int i=0; i<Count; i++) | |
2375 { | |
2376 if (data[i]<=th) data[i]=th; | |
2377 else Result++; | |
2378 } | |
2379 return Result; | |
2380 }//Rectify | |
2381 | |
2382 //--------------------------------------------------------------------------- | |
2383 /* | |
2384 function Res: minimum absolute residue. | |
2385 | |
2386 In: in: a number | |
2387 mod: modulus | |
2388 | |
2389 Returns the minimal absolute residue of $in devided by $mod. | |
2390 */ | |
2391 double Res(double in, double mod) | |
2392 { | |
2393 int i=in/mod; | |
2394 in=in-i*mod; | |
2395 if (in>mod/2) in-=mod; | |
2396 if (in<-mod/2) in+=mod; | |
2397 return in; | |
2398 }//Res | |
2399 | |
2400 //--------------------------------------------------------------------------- | |
2401 /* | |
2402 function Romberg: Romberg algorithm for numerical integration | |
2403 | |
2404 In: f: function to integrate | |
2405 params: extra argument for calling f | |
2406 a, b: integration boundaries | |
2407 n: depth of sampling | |
2408 | |
2409 Returns the integral of f(*, params) over [a, b]. | |
2410 */ | |
2411 double Romberg(int n, double(*f)(double, void*), double a, double b, void* params) | |
2412 { | |
2413 int np=1; | |
2414 double* r1=new double[n+1];. | |
2415 double* r2=new double[n+1]; | |
2416 double h=b-a, *swp; | |
2417 r1[1]=h*(f(a, params)+f(b, params))/2; | |
2418 for (int i=2; i<=n; i++) | |
2419 { | |
2420 double akh=a+0.5*h; r2[1]=f(akh, params); | |
2421 for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h | |
2422 r2[1]=0.5*(r1[1]+h*r2[1]); | |
2423 double fj=4; | |
2424 for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;} //fj=4^(j-1) | |
2425 h/=2; np*=2; | |
2426 swp=r1; r1=r2; r2=swp; | |
2427 } | |
2428 h=r1[n]; | |
2429 delete[] r1; | |
2430 delete[] r2; | |
2431 return h; | |
2432 }//Romberg | |
2433 | |
2434 /* | |
2435 function Romberg: Romberg algorithm for numerical integration, may return before specified depth on | |
2436 convergence. | |
2437 | |
2438 In: f: function to integrate | |
2439 params: extra argument for calling f | |
2440 a, b: integration boundaries | |
2441 n: depth of sampling | |
2442 ep: convergence test threshold | |
2443 | |
2444 Returns the integral of f(*, params) over [a, b]. | |
2445 */ | |
2446 double Romberg(double(*f)(double, void*), double a, double b, void* params, int n, double ep) | |
2447 { | |
2448 int i, np=1; | |
2449 double* r1=new double[n+1]; | |
2450 double* r2=new double[n+1]; | |
2451 double h=b-a, *swp; | |
2452 r1[1]=h*(f(a, params)+f(b, params))/2; | |
2453 bool mep=false; | |
2454 for (i=2; i<=n; i++) | |
2455 { | |
2456 double akh=a+0.5*h; r2[1]=f(akh, params); | |
2457 for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h | |
2458 r2[1]=0.5*(r1[1]+h*r2[1]); | |
2459 double fj=4; | |
2460 for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;} //fj=4^(j-1) | |
2461 | |
2462 if (fabs(r2[i]-r1[i-1])<ep) | |
2463 { | |
2464 if (mep) break; | |
2465 else mep=true; | |
2466 } | |
2467 else mep=false; | |
2468 | |
2469 h/=2; np*=2; | |
2470 swp=r1; r1=r2; r2=swp; | |
2471 } | |
2472 if (i<=n) h=r2[i]; | |
2473 else h=r1[n]; | |
2474 delete[] r1; | |
2475 delete[] r2; | |
2476 return h; | |
2477 }//Romberg | |
2478 | |
2479 //--------------------------------------------------------------------------- | |
2480 //analog and digital sinc functions | |
2481 | |
2482 //sinca(0)=1, sincd(0)=N, sinca(1)=sincd(1)=0. | |
2483 /* | |
2484 function sinca: analog sinc function. | |
2485 | |
2486 In: x: frequency | |
2487 | |
2488 Returns sinc(x)=sin(pi*x)/(pi*x), sinca(0)=1, sinca(1)=0 | |
2489 */ | |
2490 double sinca(double x) | |
2491 { | |
2492 if (x==0) return 1; | |
2493 return sin(M_PI*x)/(M_PI*x); | |
2494 }//sinca | |
2495 | |
2496 /* | |
2497 function sincd_unn: unnormalized discrete sinc function | |
2498 | |
2499 In: x: frequency | |
2500 N: scale (window size, DFT size) | |
2501 | |
2502 Returns sinc(x)=sin(pi*x)/sin(pi*x/N), sincd(0)=N, sincd(1)=0. | |
2503 */ | |
2504 double sincd_unn(double x, int N) | |
2505 { | |
2506 if (x==0) return N; | |
2507 return sin(M_PI*x)/sin(M_PI*x/N); | |
2508 }//sincd | |
2509 | |
2510 //--------------------------------------------------------------------------- | |
2511 /* | |
2512 SmoothPhase: phase unwrapping on module mpi*PI, 2PI by default | |
2513 | |
2514 In: Arg[Count]: phase angles to unwrap | |
2515 mpi: unwrapping modulus, in pi's | |
2516 Out: Arg[Count]: unwrapped phase | |
2517 | |
2518 Returns the amount of unwrap, in pi's, of the last phase angle | |
2519 */ | |
2520 double SmoothPhase(double* Arg, int Count, int mpi) | |
2521 { | |
2522 double m2pi=mpi*M_PI; | |
2523 for (int i=1; i<Count-1; i++) | |
2524 Arg[i]=Arg[i-1]+Res(Arg[i]-Arg[i-1], m2pi); | |
2525 double tmp=Res(Arg[Count-1]-Arg[Count-2], m2pi); | |
2526 double result=(Arg[Count-1]-Arg[Count-2]-tmp)/m2pi; | |
2527 Arg[Count-1]=Arg[Count-2]+tmp; | |
2528 | |
2529 return result; | |
2530 }//SmoothPhase | |
2531 | |
2532 //--------------------------------------------------------------------------- | |
2533 //the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). | |
2534 | |
2535 /* | |
2536 StiffB: computes stiffness coefficient from fundamental and another partial frequency based on the | |
2537 stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). | |
2538 | |
2539 In: f0: fundamental frequency | |
2540 m: 1-based partial index | |
2541 fm: frequency of partial m | |
2542 | |
2543 Returns stiffness coefficient B. | |
2544 */ | |
2545 double StiffB(double f0, double fm, int m) | |
2546 { | |
2547 double f2=fm/m/f0; | |
2548 return (f2*f2-1)/(m*m-1); | |
2549 }//StiffB | |
2550 | |
2551 //StiffF: partial frequency of a stiff string | |
2552 /* | |
2553 StiffFm: computes a partial frequency from fundamental frequency and partial index based on the stiff | |
2554 string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). | |
2555 | |
2556 In: f0: fundamental frequency | |
2557 m: 1-based partial index | |
2558 B: stiffness coefficient | |
2559 | |
2560 Returns frequency of the m-th partial. | |
2561 */ | |
2562 double StiffFm(double f0, int m, double B) | |
2563 { | |
2564 return m*f0*sqrt(1+B*(m*m-1)); | |
2565 }//StiffFm | |
2566 | |
2567 /* | |
2568 StiffF0: computes fundamental frequency from another partial frequency and stiffness coefficient based | |
2569 on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). | |
2570 | |
2571 In: m: 1-based partial index | |
2572 fm: frequency of partial m | |
2573 B: stiffness coefficient | |
2574 | |
2575 Returns the fundamental frequency. | |
2576 */ | |
2577 double StiffF0(double fm, int m, double B) | |
2578 { | |
2579 return fm/m/sqrt(1+B*(m*m-1)); | |
2580 }//StiffF0 | |
2581 | |
2582 /* | |
2583 StiffM: computes 1-based partial index from partial frequency, fundamental frequency and stiffness | |
2584 coefficient based on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). | |
2585 | |
2586 In: f0: fundamental freqency | |
2587 fm: a partial frequency | |
2588 B: stiffness coefficient | |
2589 | |
2590 Returns the 1-based partial index which will generate the specified partial frequency from the model | |
2591 which, however, does not have to be an integer in this function. | |
2592 */ | |
2593 double StiffM(double f0, double fm, double B) | |
2594 { | |
2595 if (B<1e-14) return fm/f0; | |
2596 double b1=B-1, ff=fm/f0; | |
2597 double delta=b1*b1+4*B*ff*ff; | |
2598 if (delta<0) | |
2599 return sqrt(b1/2/B); | |
2600 else | |
2601 return sqrt((b1+sqrt(delta))/2/B); | |
2602 }//StiffMd | |
2603 | |
2604 //--------------------------------------------------------------------------- | |
2605 /* | |
2606 TFFilter: time-frequency filtering with Hann-windowed overlap-add. | |
2607 | |
2608 In: data[Count]: input data | |
2609 Spans: time-frequency spans | |
2610 wt, windp: type and extra parameter of FFT window | |
2611 Sps: sampling rate | |
2612 TOffst: optional offset applied to all time values in Spans, set to Spans timing of of data[0]. | |
2613 Pass: set to pass T-F content covered by Spans, clear to stop T-F content covered by Spans | |
2614 Out: dataout[Count]: filtered data | |
2615 | |
2616 No return value. Identical data and dataout allowed. | |
2617 */ | |
2618 void TFFilter(double* data, double* dataout, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst) | |
2619 { | |
2620 int HWid=Wid/2; | |
2621 int Fr=Count/HWid-1; | |
2622 int Order=log2(Wid); | |
2623 | |
2624 int** lspan=new int*[Fr]; | |
2625 double* lxspan=new double[Fr]; | |
2626 | |
2627 lspan[0]=new int[Fr*Wid]; | |
2628 for (int i=1; i<Fr; i++) | |
2629 lspan[i]=&lspan[0][i*Wid]; | |
2630 | |
2631 //fill local filter span table | |
2632 if (Pass) | |
2633 memset(lspan[0], 0, sizeof(int)*Fr*Wid); | |
2634 else | |
2635 for (int i=0; i<Fr; i++) | |
2636 for (int j=0; j<Wid; j++) | |
2637 lspan[i][j]=1; | |
2638 | |
2639 for (int i=0; i<Spans->Count; i++) | |
2640 { | |
2641 int lx1, lx2, ly1, ly2; | |
2642 lx1=(Spans->Items[i].T1-TOffst)/HWid-1; | |
2643 lx2=(Spans->Items[i].T2-1-TOffst)/HWid; | |
2644 ly1=Spans->Items[i].F1*2/Sps*HWid+0.001; | |
2645 ly2=Spans->Items[i].F2*2/Sps*HWid+1; | |
2646 if (lx1<0) lx1=0; | |
2647 if (lx2>=Fr) lx2=Fr-1; | |
2648 if (ly1<0) ly1=0; | |
2649 if (ly1>HWid) ly1=HWid; | |
2650 if (Pass) | |
2651 for (int x=lx1; x<=lx2; x++) | |
2652 for (int y=ly1; y<=ly2; y++) | |
2653 lspan[x][y]=1; | |
2654 else | |
2655 for (int x=lx1; x<=lx2; x++) | |
2656 for (int y=ly1; y<=ly2; y++) | |
2657 lspan[x][y]=0; | |
2658 } | |
2659 for (int i=0; i<Fr; i++) | |
2660 { | |
2661 lxspan[i]=0; | |
2662 for (int j=0; j<=HWid; j++) | |
2663 { | |
2664 if (lspan[i][j]) | |
2665 lxspan[i]=lxspan[i]+1; | |
2666 } | |
2667 lxspan[i]/=(HWid+1); | |
2668 } | |
2669 double* winf=NewWindow(wt, Wid, 0, &windp); | |
2670 double* wini=NewWindow(wtHann, Wid, NULL, NULL); | |
2671 for (int i=0; i<Wid; i++) | |
2672 if (winf[i]!=0) wini[i]=wini[i]/winf[i]; | |
2673 AllocateFFTBuffer(Wid, ldata, w, x); | |
2674 double* tmpdata=new double[HWid]; | |
2675 memset(tmpdata, 0, HWid*sizeof(double)); | |
2676 | |
2677 for (int i=0; i<Fr; i++) | |
2678 { | |
2679 if (lxspan[i]==0) | |
2680 { | |
2681 memcpy(&dataout[i*HWid], tmpdata, sizeof(double)*HWid); | |
2682 memset(tmpdata, 0, sizeof(double)*HWid); | |
2683 continue; | |
2684 } | |
2685 if (lxspan[i]==1) | |
2686 { | |
2687 memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); | |
2688 if (i>0) | |
2689 for (int k=0; k<HWid; k++) | |
2690 ldata[k]=ldata[k]*winf[k]*wini[k]; | |
2691 for (int k=HWid; k<Wid; k++) | |
2692 ldata[k]=ldata[k]*winf[k]*wini[k]; | |
2693 | |
2694 memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); | |
2695 for (int k=0; k<HWid; k++) | |
2696 dataout[i*HWid+k]+=ldata[k]; | |
2697 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
2698 continue; | |
2699 } | |
2700 memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); | |
2701 if (i>0) | |
2702 for (int k=0; k<HWid; k++) | |
2703 ldata[k]=ldata[k]*winf[k]; | |
2704 for (int k=HWid; k<Wid; k++) | |
2705 ldata[k]=ldata[k]*winf[k]; | |
2706 | |
2707 RFFTC(ldata, NULL, NULL, Order, w, x, 0); | |
2708 | |
2709 if (!lspan[i][0]) x[0].x=x[0].y=0; | |
2710 for (int j=1; j<=HWid; j++) | |
2711 if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0; | |
2712 | |
2713 CIFFTR(x, Order, w, ldata); | |
2714 | |
2715 if (i>0) | |
2716 for (int k=0; k<HWid; k++) | |
2717 ldata[k]=ldata[k]*wini[k]; | |
2718 for (int k=HWid; k<Wid; k++) | |
2719 ldata[k]=ldata[k]*wini[k]; | |
2720 | |
2721 memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); | |
2722 for (int k=0; k<HWid; k++) | |
2723 dataout[i*HWid+k]+=ldata[k]; | |
2724 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
2725 } | |
2726 memcpy(&dataout[Fr*HWid], tmpdata, sizeof(double)*HWid); | |
2727 memset(&dataout[Fr*HWid+HWid], 0, sizeof(double)*(Count-Fr*HWid-HWid)); | |
2728 | |
2729 FreeFFTBuffer(ldata); | |
2730 delete[] lspan[0]; | |
2731 delete[] lspan; | |
2732 delete[] lxspan; | |
2733 delete[] tmpdata; | |
2734 delete[] winf; | |
2735 delete[] wini; | |
2736 }//TFFilter | |
2737 //version on integer data, where BytesPerSample specified the integer format. | |
2738 void TFFilter(void* data, void* dataout, int BytesPerSample, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst) | |
2739 { | |
2740 int HWid=Wid/2; | |
2741 int Fr=Count/HWid-1; | |
2742 int Order=log2(Wid); | |
2743 | |
2744 int** lspan=new int*[Fr]; | |
2745 double* lxspan=new double[Fr]; | |
2746 | |
2747 lspan[0]=new int[Fr*Wid]; | |
2748 for (int i=1; i<Fr; i++) | |
2749 lspan[i]=&lspan[0][i*Wid]; | |
2750 | |
2751 //fill local filter span table | |
2752 if (Pass) | |
2753 memset(lspan[0], 0, sizeof(int)*Fr*Wid); | |
2754 else | |
2755 for (int i=0; i<Fr; i++) | |
2756 for (int j=0; j<Wid; j++) | |
2757 lspan[i][j]=1; | |
2758 | |
2759 for (int i=0; i<Spans->Count; i++) | |
2760 { | |
2761 int lx1, lx2, ly1, ly2; | |
2762 lx1=(Spans->Items[i].T1-TOffst)/HWid-1; | |
2763 lx2=(Spans->Items[i].T2-1-TOffst)/HWid; | |
2764 ly1=Spans->Items[i].F1*2/Sps*HWid+0.001; | |
2765 ly2=Spans->Items[i].F2*2/Sps*HWid+1; | |
2766 if (lx1<0) lx1=0; | |
2767 if (lx2>=Fr) lx2=Fr-1; | |
2768 if (ly1<0) ly1=0; | |
2769 if (ly1>HWid) ly1=HWid; | |
2770 if (Pass) | |
2771 for (int x=lx1; x<=lx2; x++) | |
2772 for (int y=ly1; y<=ly2; y++) | |
2773 lspan[x][y]=1; | |
2774 else | |
2775 for (int x=lx1; x<=lx2; x++) | |
2776 for (int y=ly1; y<=ly2; y++) | |
2777 lspan[x][y]=0; | |
2778 } | |
2779 for (int i=0; i<Fr; i++) | |
2780 { | |
2781 lxspan[i]=0; | |
2782 for (int j=0; j<=HWid; j++) | |
2783 { | |
2784 if (lspan[i][j]) | |
2785 lxspan[i]=lxspan[i]+1; | |
2786 } | |
2787 lxspan[i]/=(HWid+1); | |
2788 } | |
2789 double* winf=NewWindow(wt, Wid, 0, &windp); | |
2790 double* wini=NewWindow(wtHann, Wid, NULL, NULL); | |
2791 for (int i=0; i<Wid; i++) | |
2792 if (winf[i]!=0) wini[i]=wini[i]/winf[i]; | |
2793 AllocateFFTBuffer(Wid, ldata, w, x); | |
2794 double* tmpdata=new double[HWid]; | |
2795 memset(tmpdata, 0, HWid*sizeof(double)); | |
2796 | |
2797 for (int i=0; i<Fr; i++) | |
2798 { | |
2799 if (lxspan[i]==0) | |
2800 { | |
2801 DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); | |
2802 memset(tmpdata, 0, sizeof(double)*HWid); | |
2803 continue; | |
2804 } | |
2805 if (lxspan[i]==1) | |
2806 { | |
2807 IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid); | |
2808 if (i>0) | |
2809 for (int k=0; k<HWid; k++) | |
2810 ldata[k]=ldata[k]*winf[k]*wini[k]; | |
2811 for (int k=HWid; k<Wid; k++) | |
2812 ldata[k]=ldata[k]*winf[k]*wini[k]; | |
2813 | |
2814 for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k]; | |
2815 DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); | |
2816 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
2817 continue; | |
2818 } | |
2819 IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid); | |
2820 if (i>0) | |
2821 for (int k=0; k<HWid; k++) | |
2822 ldata[k]=ldata[k]*winf[k]; | |
2823 for (int k=HWid; k<Wid; k++) | |
2824 ldata[k]=ldata[k]*winf[k]; | |
2825 | |
2826 RFFTC(ldata, NULL, NULL, Order, w, x, 0); | |
2827 | |
2828 if (!lspan[i][0]) x[0].x=x[0].y=0; | |
2829 for (int j=1; j<=HWid; j++) | |
2830 if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0; | |
2831 | |
2832 CIFFTR(x, Order, w, ldata); | |
2833 | |
2834 if (i>0) | |
2835 for (int k=0; k<HWid; k++) | |
2836 ldata[k]=ldata[k]*wini[k]; | |
2837 for (int k=HWid; k<Wid; k++) | |
2838 ldata[k]=ldata[k]*wini[k]; | |
2839 | |
2840 | |
2841 for (int k=0; k<HWid; k++) | |
2842 tmpdata[k]+=ldata[k]; | |
2843 DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); | |
2844 memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); | |
2845 } | |
2846 DoubleToInt(&((char*)dataout)[Fr*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); | |
2847 memset(&((char*)dataout)[(Fr*HWid+HWid)*BytesPerSample], 0, BytesPerSample*(Count-Fr*HWid-HWid)); | |
2848 | |
2849 FreeFFTBuffer(ldata); | |
2850 | |
2851 delete[] lspan[0]; | |
2852 delete[] lspan; | |
2853 delete[] lxspan; | |
2854 delete[] tmpdata; | |
2855 delete[] winf; | |
2856 delete[] wini; | |
2857 }//TFFilter | |
2858 | |
2859 | |
2860 |