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