Mercurial > hg > x
diff 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 |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/procedures.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,2860 @@ +//--------------------------------------------------------------------------- + +#include <math.h> +#include <mem.h> +#include "procedures.h" +#include "matrix.h" +#include "opt.h" +#include "SinEst.h" + +//--------------------------------------------------------------------------- +//TGMM methods + +//method TGMM::TGMM: default constructor +TGMM::TGMM() +{ + p=0, m=dev=0; +}//TGMM + +//method GMM:~TGMM: default destructor +TGMM::~TGMM() +{ + ReleaseGMM(p, m, dev) +}; + +//--------------------------------------------------------------------------- +//TFSpans methods + +//method TTFSpans: default constructor +TTFSpans::TTFSpans() +{ + Count=0; + Capacity=100; + Items=new TTFSpan[Capacity]; +}//TTFSpans + +//method ~TTFSpans: default destructor +TTFSpans::~TTFSpans() +{ + delete[] Items; +}//~TTFSpans + +/* + method Add: add a new span to the list + + In: ATFSpan: the new span to add +*/ +void TTFSpans::Add(TTFSpan& ATFSpan) +{ + if (Count==Capacity) + { + int OldCapacity=Capacity; + Capacity+=50; + TTFSpan* NewItems=new TTFSpan[Capacity]; + memcpy(NewItems, Items, sizeof(TTFSpan)*OldCapacity); + delete[] Items; + Items=NewItems; + } + Items[Count]=ATFSpan; + Count++; +}//Add + +/* + method Clear: discard the current content without freeing memory. +*/ +void TTFSpans::Clear() +{ + Count=0; +}//Clear + +/* + method Delete: delete a span from current list + + In: Index: index to the span to delete +*/ +int TTFSpans::Delete(int Index) +{ + if (Index<0 || Index>=Count) + return 0; + memmove(&Items[Index], &Items[Index+1], sizeof(TTFSpan)*(Count-1-Index)); + Count--; + return 1; +}//Delete + +//--------------------------------------------------------------------------- +//SpecTrack methods + +/* + method TSpecTrack::Add: adds a SpecPeak to the track. + + In: APeak: the SpecPeak to add. +*/ +int TSpecTrack::Add(TSpecPeak& APeak) +{ + if (Count>=Capacity) + { + Peaks=(TSpecPeak*)realloc(Peaks, sizeof(TSpecPeak)*(Capacity*2)); + Capacity*=2; + } + int ind=LocatePeak(APeak); + if (ind<0) + { + InsertPeak(APeak, -ind-1); + ind=-ind-1; + } + + int t=APeak.t; + double f=APeak.f; + if (Count==1) t1=t2=t, fmin=fmax=f; + else + { + if (t<t1) t1=t; + else if (t>t2) t2=t; + if (f<fmin) fmin=f; + else if (f>fmax) fmax=f; + } + return ind; +}//Add + +/* + method TSpecTrack::TSpecTrack: creates a SpecTrack with an inital capacity. + + In: ACapacity: initial capacity, i.e. the number SpecPeak's to allocate storage space for. +*/ +TSpecTrack::TSpecTrack(int ACapacity) +{ + Count=0; + Capacity=ACapacity; + Peaks=new TSpecPeak[Capacity]; +}//TSpecTrack + +//method TSpecTrack::~TSpecTrack: default destructor. +TSpecTrack::~TSpecTrack() +{ + delete[] Peaks; +}//TSpecTrack + +/* + method InsertPeak: inserts a new SpecPeak into the track at a given index. Internal use only. + + In: APeak: the SpecPeak to insert. + index: the position in the list to place the new SpecPeak. Original SpecPeak's at and after this + position are shifted by 1 posiiton. +*/ +void TSpecTrack::InsertPeak(TSpecPeak& APeak, int index) +{ + memmove(&Peaks[index+1], &Peaks[index], sizeof(TSpecPeak)*(Count-index)); + Peaks[index]=APeak; + Count++; +}//InsertPeak + +/* + method TSpecTrack::LocatePeak: looks for a SpecPeak in the track that has the same time (t) as APeak. + + In: APeak: a SpecPeak + + Returns the index in this track of the first SpecPeak that has the same time stamp as APeak. However, + if there is no peak with that time stamp, the method returns -1 if APeaks comes before the first + SpecPeak, -2 if between 1st and 2nd SpecPeak's, -3 if between 2nd and 3rd SpecPeak's, etc. +*/ +int TSpecTrack::LocatePeak(TSpecPeak& APeak) +{ + if (APeak.t<Peaks[0].t) return -1; + if (APeak.t>Peaks[Count-1].t) return -Count-1; + + if (APeak.t==Peaks[0].t) return 0; + else if (APeak.t==Peaks[Count-1].t) return Count-1; + + int a=0, b=Count-1, c=(a+b)/2; + while (a<c) + { + if (APeak.t==Peaks[c].t) return c; + else if (APeak.t<Peaks[c].t) {b=c; c=(a+b)/2;} + else {a=c; c=(a+b)/2;} + } + return -a-2; +}//LocatePeak + +//--------------------------------------------------------------------------- +/* + function: ACPower: AC power + + In: data[Count]: a signal + + Returns the power of its AC content. +*/ +double ACPower(double* data, int Count, void*) +{ + if (Count<=0) return 0; + double power=0, avg=0, tmp; + for (int i=0; i<Count; i++) + { + tmp=*(data++); + power+=tmp*tmp; + avg+=tmp; + } + power=(power-avg*avg)/Count; + return power; +}//ACPower + +//--------------------------------------------------------------------------- +/* + function Add: vector addition + + In: dest[Count], source[Count]: two vectors + Out: dest[Count]: their sum + + No return value. +*/ +void Add(double* dest, double* source, int Count) +{ + for (int i=0; i<Count; i++) *(dest++)+=*(source++); +}//Add + +/* + function Add: vector addition + + In: addend[count], adder[count]: two vectors + Out: out[count]: their sum + + No return value. +*/ +void Add(double* out, double* addend, double* adder, int count) +{ + for (int i=0; i<count; i++) *(out++)=*(addend++)+*(adder++); +}//Add + +//--------------------------------------------------------------------------- + +/* + function ApplyWindow: applies window function to signal buffer. + + In: Buffer[Size]: signal to be windowed + Weight[Size]: the window + Out: OutBuffer[Size]: windowed signal + + No return value; +*/ +void ApplyWindow(double* OutBuffer, double* Buffer, double* Weights, int Size) +{ + for (int i=0; i<Size; i++) *(OutBuffer++)=*(Buffer++)**(Weights++); +}//ApplyWindow + +//--------------------------------------------------------------------------- +/* + function Avg: average + + In: data[Count]: a data array + + Returns the average of the array. +*/ +double Avg(double* data, int Count, void*) +{ + if (Count<=0) return 0; + double avg=0; + for (int i=0; i<Count; i++) avg+=*(data++); + avg/=Count; + return avg; +}//Avg + +//--------------------------------------------------------------------------- +/* + function AvgFilter: get slow-varying wave trace by averaging + + In: data[Count]: input signal + HWid: half the size of the averaging window + Out: datout[Count]: the slow-varying part of data[]. + + No return value. +*/ +void AvgFilter(double* dataout, double* data, int Count, int HWid) +{ + double sum=0; + + dataout[0]=data[0]; + + for (int i=1; i<=HWid; i++) + { + sum+=data[2*i-1]+data[2*i]; + dataout[i]=sum/(2*i+1); + } + + for (int i=HWid+1; i<Count-HWid; i++) + { + sum=sum+data[i+HWid]-data[i-HWid-1]; + dataout[i]=sum/(2*HWid+1); + } + + for (int i=Count-HWid; i<Count; i++) + { + sum=sum-data[2*i-Count-1]-data[2*i-Count]; + dataout[i]=sum/(2*(Count-i)-1); + } +}//AvgFilter + +//--------------------------------------------------------------------------- +/* + function CalculateSpectrogram: computes the spectrogram of a signal + + In: data[Count]: the time-domain signal + start, end: start and end points marking the section for which the spectrogram is to be computed + Wid, Offst: frame size and hop size + Window: window function + amp: a pre-amplifier + half: specifies if the spectral values at Wid/2 are to be retried + Out: Spec[][Wid/2] or Spec[][Wid/2+1]: amplitude spectrogram + ph[][][Wid/2] or Ph[][Wid/2+1]: phase spectrogram + + No return value. The caller is repsonse to arrange storage spance of output buffers. +*/ +void CalculateSpectrogram(double* data, int Count, int start, int end, int Wid, int Offst, double* Window, double** Spec, double** Ph, double amp, bool half) +{ + AllocateFFTBuffer(Wid, fft, w, x); + + int Fr=(end-start-Wid)/Offst+1; + + for (int i=0; i<Fr; i++) + { + RFFTCW(&data[i*Offst+start], Window, 0, 0, log2(Wid), w, x); + + if (Spec) + { + for (int j=0; j<Wid/2; j++) + Spec[i][j]=sqrt(x[j].x*x[j].x+x[j].y*x[j].y)*amp; + if (half) + Spec[i][Wid/2]=sqrt(x[Wid/2].x*x[Wid/2].x+x[Wid/2].y*x[Wid/2].y)*amp; + } + if (Ph) + { + for (int j=0; j<=Wid/2; j++) + Ph[i][j]=Atan2(x[j].y, x[j].x); + if (half) + Ph[i][Wid/2]=Atan2(x[Wid/2].y, x[Wid/2].x); + } + } + FreeFFTBuffer(fft); +}//CalculateSpectrogram + +//--------------------------------------------------------------------------- +/* + function Conv: simple convolution + + In: in1[N1], in2[N2]: two sequences + Out: out[N1+N2-1]: their convolution + + No return value. +*/ +void Conv(double* out, int N1, double* in1, int N2, double* in2) +{ + int N=N1+N1-1; + memset(out, 0, sizeof(double)*N); + for (int n1=0; n1<N1; n1++) + for (int n2=0; n2<N2; n2++) + out[n1+n2]+=in1[n1]*in2[n2]; +}//Conv + +//--------------------------------------------------------------------------- +/* + function Correlation: computes correlation coefficient of 2 vectors a & b, equals cos(aOb). + + In: a[Count], b[Count]: two vectors + + Returns their correlation coefficient. +*/ +double Correlation(double* a, double* b, int Count) +{ + double aa=0, bb=0, ab=0; + for (int i=0; i<Count; i++) + { + aa+=*a**a; + bb+=*b**b; + ab+=*(a++)**(b++); + } + return ab/sqrt(aa*bb); +}//Correlation + +//--------------------------------------------------------------------------- +/* + function DCAmplitude: DC amplitude + + In: data[Count]: a signal + + Returns its DC amplitude (=AC amplitude without DC removing) +*/ +double DCAmplitude(double* data, int Count, void*) +{ + if (Count<=0) return 0; + double power=0, tmp; + for (int i=0; i<Count; i++) + { + tmp=*(data++); + power+=tmp*tmp; + } + power/=Count; + return sqrt(2*power); +}//DCAmplitude + +/* + function DCPower: DC power + + In: data[Count]: a signal + + Returns its DC power. +*/ +double DCPower(double* data, int Count, void*) +{ + if (Count<=0) return 0; + double power=0, tmp; + for (int i=0; i<Count; i++) + { + tmp=*(data++); + power+=tmp*tmp; + } + power/=Count; + return power; +}//DCPower + +//--------------------------------------------------------------------------- +/* + DCT: discrete cosine transform, direct computation. For fast DCT, see fft.cpp. + + In: input[N]: a signal + Out: output[N]: its DCT + + No return value. +*/ +void DCT( double* output, double* input, int N) +{ + double Wn; + + for (int n=0; n<N; n++) + { + output[n]=0; + Wn=n*M_PI/2/N; + for (int k=0; k<N; k++) + output[n]+=input[k]*cos((2*k+1)*Wn); + if (n==0) output[n]*=1.4142135623730950488016887242097/N; + else output[n]*=2.0/N; + } +}//DCT + +/* + function IDCT: inverse discrete cosine transform, direct computation. For fast IDCT, see fft.cpp. + + In: input[N]: a signal + Out: output[N]: its IDCT + + No return value. +*/ +void IDCT(double* output, double* input, int N) +{ + for (int k=0; k<N; k++) + { + double Wk=(2*k+1)*M_PI/2/N; + output[k]=input[0]/1.4142135623730950488016887242097; + for (int n=1; n<N; n++) + output[k]+=input[n]*cos(n*Wk); + } +}//IDCT + +//--------------------------------------------------------------------------- +/* + function DeDC: removes DC component of a signal + + In: data[Count]: the signal + HWid: half of averaging window size + Out: data[Count]: de-DC-ed signal + + No return value. +*/ +void DeDC(double* data, int Count, int HWid) +{ + double* data2=new double[Count]; + AvgFilter(data2, data, Count, HWid); + for (int i=0; i<Count; i++) + *(data++)-=*(data2++); + delete[] data2; +}//DeDC + +/* + function DeDC_static: removes DC component statically + + In: data[Count]: the signal + Out: data[Count]: DC-removed signal + + No return value. +*/ +void DeDC_static(double* data, int Count) +{ + double avg=Avg(data, Count, 0); + for (int i=0; i<Count; i++) *(data++)-=avg; +}//DeDC_static + +//--------------------------------------------------------------------------- +/* + function DoubleToInt: converts double-precision floating point array to integer array + + In: in[Count]: the double array + BytesPerSample: bytes per sample of destination integers + Out: out[Count]: the integer array + + No return value. +*/ +void DoubleToInt(void* out, int BytesPerSample, double* in, int Count) +{ + if (BytesPerSample==1){unsigned char* out8=(unsigned char*)out; for (int k=0; k<Count; k++) *(out8++)=*(in++)+128.5;} + else {__int16* out16=(__int16*)out; for (int k=0; k<Count; k++) *(out16++)=floor(*(in++)+0.5);} +}//DoubleToInt + +/* + function DoubleToIntAdd: adds double-precision floating point array to integer array + + In: in[Count]: the double array + out[Count]: the integer array + BytesPerSample: bytes per sample of destination integers + Out: out[Count]: the sum of the two arrays + + No return value. +*/ +void DoubleToIntAdd(void* out, int BytesPerSample, double* in, int Count) +{ + if (BytesPerSample==1) + { + unsigned char* out8=(unsigned char*)out; + for (int k=0; k<Count; k++){*out8=*out8+*in+128.5; out8++; in++;} + } + else + { + __int16* out16=(__int16*)out; + for (int k=0; k<Count; k++){*out16=*out16+floor(*in+0.5); out16++; in++;} + } +}//DoubleToIntAdd + +//--------------------------------------------------------------------------- +/* + DPower: in-frame power variation + + In: data[Count]: a signal + + returns the different between AC powers of its first and second halves. +*/ +double DPower(double* data, int Count, void*) +{ + double ene1=ACPower(data, Count/2, 0); + double ene2=ACPower(&data[Count/2], Count/2, 0); + return ene2-ene1; +}//DPower + +//--------------------------------------------------------------------------- +/* + funciton Energy: energy + + In: data[Count]: a signal + + Returns its total energy +*/ +double Energy(double* data, int Count) +{ + double result=0; + for (int i=0; i<Count; i++) result+=data[i]*data[i]; + return result; +}//Energy + +//--------------------------------------------------------------------------- +/* + function ExpOnsetFilter: onset filter with exponential impulse response h(t)=Aexp(-t/Tr)-Bexp(-t/Ta), + A=1-exp(-1/Tr), B=1-exp(-1/Ta). + + In: data[Count]: signal to filter + Tr, Ta: time constants of h(t) + Out: dataout[Count]: filtered signal, normalized by multiplying a factor. + + Returns the normalization factor. Identical data and dataout is allowed. +*/ +double ExpOnsetFilter(double* dataout, double* data, int Count, double Tr, double Ta) +{ + double FA=0, FB=0; + double EA=exp(-1.0/Tr), EB=exp(-1.0/Ta); + double A=1-EA, B=1-EB; + 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)); + for (int i=0; i<Count; i++) + { + FA=FA*EA+*data; + FB=FB*EB+*(data++); + *(dataout++)=(A*FA-B*FB)*NormFactor; + } + return NormFactor; +}//ExpOnsetFilter + +/* + function ExpOnsetFilter_balanced: exponential onset filter without starting step response. It + extends the input signal at the front end by bal*Ta samples by repeating the its value at 0, then + applies the onset filter on the extended signal instead. + + In: data[Count]: signal to filter + Tr, Ta: time constants to the impulse response of onset filter, see ExpOnsetFilter(). + bal: balancing factor + Out: dataout[Count]: filtered signal, normalized by multiplying a factor. + + Returns the normalization factor. Identical data and dataout is allowed. +*/ +double ExpOnsetFilter_balanced(double* dataout, double* data, int Count, double Tr, double Ta, int bal) +{ + double* tmpdata=new double[int(Count+bal*Ta)]; + double* ltmpdata=tmpdata; + for (int i=0; i<bal*Ta; i++) *(ltmpdata++)=data[0]; + memcpy(ltmpdata, data, sizeof(double)*Count); + double result=ExpOnsetFilter(tmpdata, tmpdata, bal*Ta+Count, Tr, Ta); + memcpy(dataout, ltmpdata, sizeof(double)*Count); + delete[] tmpdata; + return result; +}//ExpOnsetFilter_balanced + +//--------------------------------------------------------------------------- +/* + function ExtractLinearComponent: Legendre linear component + + In: data[Count+1]: a signal + Out: dataout[Count+1]: its Legendre linear component, optional. + + Returns the coefficient to the linear component. +*/ +double ExtractLinearComponent(double* dataout, double* data, int Count) +{ + double tmp=0; + int N=Count*2; + for (int n=0; n<=Count; n++) tmp+=n**(data++); + tmp=tmp*24/N/(N+1)/(N+2); + if (dataout) + for (int n=0; n<=Count; n++) *(dataout++)=tmp*n; + return tmp; +}//ExtractLinearComponent + +//--------------------------------------------------------------------------- +/* + function FFTConv: fast convolution of two series by FFT overlap-add. In an overlap-add scheme it is + assumed that one of the convolvends is short compared to the other one, which can be potentially + infinitely long. The long convolvend is devided into short segments, each of which is convolved with + the short convolvend, the results of which are then assembled into the final result. The minimal delay + from input to output is the amount of overlap, which is the size of the short convolvend minus 1. + + In: source1[size1]: convolvend + source2[size2]: second convolvend + zero: position of first point in convoluton result, relative to main output buffer. + pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result. + Out: dest[size1]: the middle part of convolution result + pre_buffer[-zero]: now updated by adding beginning part of the convolution result + post_buffer[size2+zero]: end part of the convolution result + + No return value. Identical dest and source1 allowed. + + The convolution result has length size1+size2 (counting one trailing zero). If zero lies in the range + between -size2 and 0, then the first -zero samples are added to pre_buffer[], next size1 samples are + saved to dest[], and the last size2+zero sampled are saved to post_buffer[]; if not, the middle size1 + samples are saved to dest[], while pre_buffer[] and post_buffer[] are not used. +*/ +void FFTConv(double* dest, double* source1, int size1, double* source2, int size2, int zero, double* pre_buffer, double* post_buffer) +{ + int order=log2(size2-1)+1+1; + int Wid=1<<order; + int HWid=Wid/2; + int Fr=size1/HWid; + int res=size1-HWid*Fr; + bool trunc=false; + if (zero<-size2+1 || zero>0) zero=-size2/2, trunc=true; + if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true; + + AllocateFFTBuffer(Wid, fft, w, x1); + int* hbitinv=CreateBitInvTable(order-1); + cdouble* x2=new cdouble[Wid]; + double* tmp=new double[HWid]; + memset(tmp, 0, sizeof(double)*HWid); + + memcpy(fft, source2, sizeof(double)*size2); + memset(&fft[size2], 0, sizeof(double)*(Wid-size2)); + RFFTC(fft, 0, 0, order, w, x2, hbitinv); + + double r1, r2, i1, i2; + int ind, ind_; + for (int i=0; i<Fr; i++) + { + memcpy(fft, &source1[i*HWid], sizeof(double)*HWid); + memset(&fft[HWid], 0, sizeof(double)*HWid); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + for (int j=0; j<HWid; j++) tmp[j]+=fft[j]; + + ind=i*HWid+zero; //(i+1)*HWid<=size1 + ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1 + if (ind<0) + { + if (!trunc) + memdoubleadd(pre_buffer, tmp, -ind); + memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind)); + } + else + memcpy(&dest[ind], tmp, sizeof(double)*HWid); + memcpy(tmp, &fft[HWid], sizeof(double)*HWid); + } + + if (res>0) + { + memcpy(fft, &source1[Fr*HWid], sizeof(double)*res); + memset(&fft[res], 0, sizeof(double)*(Wid-res)); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + for (int j=0; j<HWid; j++) + tmp[j]+=fft[j]; + + ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1 + ind_=ind+HWid; //ind_=size1 -res+zero+HWid + if (ind<0) + { + if (!trunc) + memdoubleadd(pre_buffer, tmp, -ind); + memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind)); + } + else if (ind_>size1) + { + memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind)); + if (!trunc && post_buffer) + { + if (ind_>size1+size2+zero) + memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero)); + else + memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1)); + } + } + else + memcpy(&dest[ind], tmp, sizeof(double)*HWid); + memcpy(tmp, &fft[HWid], sizeof(double)*HWid); + Fr++; + } + + ind=Fr*HWid+zero; + ind_=ind+HWid; + + if (ind<size1) + { + if (ind_>size1) + { + memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind)); + if (!trunc && post_buffer) + { + if (ind_>size1+size2+zero) + memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero)); + else + memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1)); + } + } + else + memcpy(&dest[ind], tmp, sizeof(double)*HWid); + } + else //ind>=size1 => ind_>=size1+size2+zero + { + if (!trunc && post_buffer) + memcpy(&post_buffer[ind-size1], tmp, sizeof(double)*(size1+size2+zero-ind)); + } + + FreeFFTBuffer(fft); + delete[] x2; + delete[] tmp; + delete[] hbitinv; +}//FFTConv + +/* + function FFTConv: the simplified version using two output buffers instead of three. This is almost + equivalent to setting zero=-size2 in the three-output-buffer version (so that post_buffer no longer + exists), except that this version requires size2 (renamed HWid) be a power of 2, and pre_buffer point + to the END of the storage (so that pre_buffer=dest automatically connects the two buffers in a + continuous memory block). + + In: source1[size1]: convolvend + source2[HWid]: second convolved, HWid be a power of 2 + pre_buffer[-HWid:-1]: buffer hosting values to be overlap-added to the start of the result. + Out: dest[size1]: main output buffer, now hosting end part of the result (after HWid samples). + pre_buffer[-HWid:-1]: now updated by added the start of the result + + No return value. +*/ +void FFTConv(double* dest, double* source1, int size1, double* source2, int HWid, double* pre_buffer) +{ + int Wid=HWid*2; + int order=log2(Wid); + int Fr=size1/HWid; + int res=size1-HWid*Fr; + + AllocateFFTBuffer(Wid, fft, w, x1); + cdouble *x2=new cdouble[Wid]; + double *tmp=new double[HWid]; + int* hbitinv=CreateBitInvTable(order-1); + + memcpy(fft, source2, sizeof(double)*HWid); + memset(&fft[HWid], 0, sizeof(double)*HWid); + RFFTC(fft, 0, 0, order, w, x2, hbitinv); + + double r1, r2, i1, i2; + for (int i=0; i<Fr; i++) + { + memcpy(fft, &source1[i*HWid], sizeof(double)*HWid); + memset(&fft[HWid], 0, sizeof(double)*HWid); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + + if (i==0) + { + if (pre_buffer!=NULL) + { + double* destl=&pre_buffer[-HWid+1]; + for (int j=0; j<HWid-1; j++) destl[j]+=fft[j]; + } + } + else + { + for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j]; + memcpy(&dest[(i-1)*HWid], tmp, sizeof(double)*HWid); + } + memcpy(tmp, &fft[HWid-1], sizeof(double)*HWid); + } + + if (res>0) + { + if (Fr==0) memset(tmp, 0, sizeof(double)*HWid); + + memcpy(fft, &source1[Fr*HWid], sizeof(double)*res); + memset(&fft[res], 0, sizeof(double)*(Wid-res)); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + + if (Fr==0) + { + if (pre_buffer!=NULL) + { + double* destl=&pre_buffer[-HWid+1]; + for (int j=0; j<HWid-1; j++) destl[j]+=fft[j]; + } + } + else + { + for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j]; + memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid); + } + + memcpy(&dest[Fr*HWid], &fft[HWid-1], sizeof(double)*res); + } + else + memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid); + + FreeFFTBuffer(fft); + delete[] x2; delete[] tmp; delete[] hbitinv; +}//FFTConv + +/* + function FFTConv: fast convolution of two series by FFT overlap-add. Same as the three-output-buffer + version above but using integer output buffers as well as integer source1. + + In: source1[size1]: convolvend + bps: bytes per sample of integer units in source1[]. + source2[size2]: second convolvend + zero: position of first point in convoluton result, relative to main output buffer. + pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result. + Out: dest[size1]: the middle part of convolution result + pre_buffer[-zero]: now updated by adding beginning part of the convolution result + post_buffer[size2+zero]: end part of the convolution result + + No return value. Identical dest and source1 allowed. +*/ +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) +{ + int order=log2(size2-1)+1+1; + int Wid=1<<order; + int HWid=Wid/2; + int Fr=size1/HWid; + int res=size1-HWid*Fr; + bool trunc=false; + if (zero<-size2+1 || zero>0) zero=-size2/2, trunc=true; + if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true; + + AllocateFFTBuffer(Wid, fft, w, x1); + cdouble* x2=new cdouble[Wid]; + double* tmp=new double[HWid]; + memset(tmp, 0, sizeof(double)*HWid); + int* hbitinv=CreateBitInvTable(order-1); + + memcpy(fft, source2, sizeof(double)*size2); + memset(&fft[size2], 0, sizeof(double)*(Wid-size2)); + RFFTC(fft, 0, 0, order, w, x2, hbitinv); + + double r1, r2, i1, i2; + int ind, ind_; + for (int i=0; i<Fr; i++) + { + IntToDouble(fft, &source1[i*HWid*bps], bps, HWid); + memset(&fft[HWid], 0, sizeof(double)*HWid); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + for (int j=0; j<HWid; j++) tmp[j]+=fft[j]; + + ind=i*HWid+zero; //(i+1)*HWid<=size1 + ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1 + if (ind<0) + { + if (!trunc) + DoubleToIntAdd(pre_buffer, bps, tmp, -ind); + DoubleToInt(dest, bps, &tmp[-ind], HWid+ind); + } + else + DoubleToInt(&dest[ind*bps], bps, tmp, HWid); + memcpy(tmp, &fft[HWid], sizeof(double)*HWid); + } + + if (res>0) + { + IntToDouble(fft, &source1[Fr*HWid*bps], bps, res); + memset(&fft[res], 0, sizeof(double)*(Wid-res)); + + RFFTC(fft, 0, 0, order, w, x1, hbitinv); + + for (int j=0; j<Wid; j++) + { + r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y; + x1[j].x=r1*r2-i1*i2; + x1[j].y=r1*i2+r2*i1; + } + CIFFTR(x1, order, w, fft, hbitinv); + for (int j=0; j<HWid; j++) + tmp[j]+=fft[j]; + + ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1 + ind_=ind+HWid; //ind_=size1 -res+zero+HWid + if (ind<0) + { + if (!trunc) + DoubleToIntAdd(pre_buffer, bps, tmp, -ind); + DoubleToInt(dest, bps, &tmp[-ind], HWid+ind); + } + else if (ind_>size1) + { + DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind); + if (!trunc && post_buffer) + { + if (ind_>size1+size2+zero) + DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero); + else + DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1); + } + } + else + DoubleToInt(&dest[ind*bps], bps, tmp, HWid); + memcpy(tmp, &fft[HWid], sizeof(double)*HWid); + Fr++; + } + + ind=Fr*HWid+zero; + ind_=ind+HWid; + + if (ind<size1) + { + if (ind_>size1) + { + DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind); + if (!trunc && post_buffer) + { + if (ind_>size1+size2+zero) + DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero); + else + DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1); + } + } + else + DoubleToInt(&dest[ind*bps], bps, tmp, HWid); + } + else //ind>=size1 => ind_>=size1+size2+zero + { + if (!trunc && post_buffer) + DoubleToInt(&post_buffer[(ind-size1)*bps], bps, tmp, size1+size2+zero-ind); + } + + FreeFFTBuffer(fft); + delete[] x2; + delete[] tmp; + delete[] hbitinv; +}//FFTConv + +//--------------------------------------------------------------------------- +/* + function FFTFilter: FFT with cosine-window overlap-add: This FFT filter is not an actural LTI system, + but an block processing with overlap-add. In this function the blocks are overlapped by 50% and summed + up with Hann windowing. + + In: data[Count]: input data + Wid: DFT size + On, Off: cut-off frequencies of FFT filter. On<Off: band-pass; On>Off: band-stop. + Out: dataout[Count]: filtered data + + No return value. Identical data and dataout allowed +*/ +void FFTFilter(double* dataout, double* data, int Count, int Wid, int On, int Off) +{ + int Order=log2(Wid); + int HWid=Wid/2; + int Fr=(Count-Wid)/HWid+1; + AllocateFFTBuffer(Wid, ldata, w, x); + + double* win=new double[Wid]; + for (int i=0; i<Wid; i++) win[i]=sqrt((1-cos(2*M_PI*i/Wid))/2); + double* tmpdata=new double[HWid]; + memset(tmpdata, 0, HWid*sizeof(double)); + + for (int i=0; i<Fr; i++) + { + memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*win[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*win[k]; + + RFFTC(ldata, NULL, NULL, Order, w, x, 0); + + if (On<Off) //band pass: keep [On, Off) and set other bins to zero + { + memset(x, 0, On*sizeof(cdouble)); + if (On>=1) + memset(&x[Wid-On+1], 0, (On-1)*sizeof(cdouble)); + if (Off*2<=Wid) + memset(&x[Off], 0, (Wid-Off*2+1)*sizeof(cdouble)); + } + else //band stop: set [Off, On) to zero. + { + memset(&x[Off], 0, sizeof(cdouble)*(On-Off)); + memset(&x[Wid-On+1], 0, sizeof(double)*(On-Off)); + } + + CIFFTR(x, Order, w, ldata); + + if (i>0) for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*win[k]; + for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*win[k]; + + memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); + for (int k=0; k<HWid; k++) dataout[i*HWid+k]+=ldata[k]; + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + } + + memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double)); + memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double)); + + delete[] win; + delete[] tmpdata; + FreeFFTBuffer(ldata); +}//FFTFilter + +/* + funtion FFTFilterOLA: FFTFilter with overlap-add support. This is a true LTI filter whose impulse + response is constructed using IFFT. The filtering is implemented by fast convolution. + + In: data[Count]: input data + Wid: FFT size + On, Off: cut-off frequencies, in bins, of the filter + pre_buffer[Wid]: buffer hosting sampled to be added with the start of output + Out: dataout[Count]: main output buffer, hosting the last $Count samples of output. + pre_buffer[Wid]: now updated by adding the first Wid samples of output + + No return value. The complete output contains Count+Wid effective samples (including final 0); firt + $Wid are added to pre_buffer[], next Count samples saved to dataout[]. +*/ +void FFTFilterOLA(double* dataout, double* data, int Count, int Wid, int On, int Off, double* pre_buffer) +{ + AllocateFFTBuffer(Wid, spec, w, x); + memset(x, 0, sizeof(cdouble)*Wid); + for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2); + CIFFTR(x, log2(Wid), w, spec); + FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL); + FreeFFTBuffer(spec); +}//FFTFilterOLA +//version for integer input and output, where BytesPerSample specifies the integer format. +void FFTFilterOLA(unsigned char* dataout, unsigned char* data, int BytesPerSample, int Count, int Wid, int On, int Off, unsigned char* pre_buffer) +{ + AllocateFFTBuffer(Wid, spec, w, x); + memset(x, 0, sizeof(cdouble)*Wid); + for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2); + CIFFTR(x, log2(Wid), w, spec); + FFTConv(dataout, data, BytesPerSample, Count, spec, Wid, -Wid, pre_buffer, NULL); + FreeFFTBuffer(spec); +}//FFTFilterOLA + +/* + function FFTFilterOLA: FFT filter with overlap-add support. + + In: data[Count]: input data + amp[0:HWid]: amplitude response + ph[0:HWid]: phase response, where ph[0]=ph[HWid]=0; + pre_buffer[Wid]: buffer hosting sampled to be added to the beginning of the output + Out: dataout[Count]: main output buffer, hosting the middle $Count samples of output. + pre_buffer[Wid]: now updated by adding the first Wid/2 samples of output + + No return value. +*/ +void FFTFilterOLA(double* dataout, double* data, int Count, double* amp, double* ph, int Wid, double* pre_buffer) +{ + int HWid=Wid/2; + AllocateFFTBuffer(Wid, spec, w, x); + x[0].x=amp[0], x[0].y=0; + for (int i=1; i<HWid; i++) + { + x[i].x=x[Wid-i].x=amp[i]*cos(ph[i]); + x[i].y=amp[i]*sin(ph[i]); + x[Wid-i].y=-x[i].y; + } + x[HWid].x=amp[HWid], x[HWid].y=0; + CIFFTR(x, log2(Wid), w, spec); + FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL); + FreeFFTBuffer(spec); +}//FFTFilterOLA + +/* + function FFTMask: masks a band of a signal with noise + + In: data[Count]: input signal + DigiOn, DigiOff: cut-off frequences of the band to mask + maskcoef: masking noise amplifier. If set to 1 than the mask level is set to the highest signal + level in the masking band. + Out: dataout[Count]: output data + + No return value. +*/ +double FFTMask(double* dataout, double* data, int Count, int Wid, double DigiOn, double DigiOff, double maskcoef) +{ + int Order=log2(Wid); + int HWid=Wid/2; + int Fr=(Count-Wid)/HWid+1; + int On=Wid*DigiOn, Off=Wid*DigiOff; + AllocateFFTBuffer(Wid, ldata, w, x); + + double* winhann=new double[Wid]; + double* winhamm=new double[Wid]; + for (int i=0; i<Wid; i++) + {winhamm[i]=0.54-0.46*cos(2*M_PI*i/Wid); winhann[i]=(1-cos(2*M_PI*i/Wid))/2/winhamm[i];} + double* tmpdata=new double[HWid]; + memset(tmpdata, 0, HWid*sizeof(double)); + double max, randfi; + + max=0; + for (int i=0; i<Fr; i++) + { + memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winhamm[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winhamm[k]; + + RFFTC(ldata, ldata, NULL, Order, w, x, 0); + + for (int k=On; k<Off; k++) + { + x[k].x=x[Wid-k].x=x[k].y=x[Wid-k].y=0; + if (max<ldata[k]) max=ldata[k]; + } + + CIFFTR(x, Order, w, ldata); + + if (i>0) + for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*winhann[k]; + for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*winhann[k]; + + for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k]; + memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + } + memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double)); + + max*=maskcoef; + + for (int i=0; i<Wid; i++) + winhann[i]=winhann[i]*winhamm[i]; + + for (int i=0; i<Fr; i++) + { + memset(x, 0, sizeof(cdouble)*Wid); + for (int k=On; k<Off; k++) + { + randfi=rand()*M_PI*2/RAND_MAX; + x[k].x=x[Wid-k].x=max*cos(randfi); + x[k].y=max*sin(randfi); + x[Wid-k].y=-x[k].y; + } + + CIFFTR(x, Order, w, ldata); + + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winhann[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winhann[k]; + + for (int k=0; k<Wid; k++) dataout[i*HWid+k]+=ldata[k]; + } + + memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double)); + + delete[] winhann; + delete[] winhamm; + delete[] tmpdata; + FreeFFTBuffer(ldata); + + return max; +}//FFTMask + +//--------------------------------------------------------------------------- +/* + function FindInc: find the element in ordered list data that is closest to value. + + In: data[Count]: a ordered list + value: the value to locate in the list + + Returns the index of the element in the sorted list which is closest to $value. +*/ +int FindInc(double value, double* data, int Count) +{ + if (value>=data[Count-1]) return Count-1; + if (value<data[0]) return 0; + int end=InsertInc(value, data, Count, false); + if (fabs(value-data[end-1])<fabs(value-data[end])) return end-1; + else return end; +}//FindInc + +//--------------------------------------------------------------------------- +/* + function Gaussian: Gaussian function + + In: Vector[Dim]: a vector + Mean[Dim]: mean of Gaussian function + Dev[Fim]: diagonal autocorrelation matrix of Gaussian function + + Returns the value of Gaussian function at Vector[]. +*/ +double Gaussian(int Dim, double* Vector, double* Mean, double* Dev) +{ + double bmt=0, tmp; + for (int dim=0; dim<Dim; dim++) + { + tmp=Vector[dim]-Mean[dim]; + bmt+=tmp*tmp/Dev[dim]; + } + bmt=-bmt/2; + tmp=log(Dev[0]); + for (int dim=1; dim<Dim; dim++) tmp+=log(Dev[dim]); + bmt-=tmp/2; + bmt-=Dim*log(M_PI*2)/2; + bmt=exp(bmt); + return bmt; +}//Gaussian + + +//--------------------------------------------------------------------------- +/* + function Hamming: calculates the amplitude spectrum of Hamming window at a given frequency + + In: f: frequency + T: size of Hamming window + + Returns the amplitude spectrum at specified frequency. +*/ +double Hamming(double f, double T) +{ + double omg0=2*M_PI/T; + double omg=f*2*M_PI; + cdouble c1, c2, c3; + cdouble nj(0, -1); + cdouble pj(0, 1); + double a=0.54, b=0.46; + + cdouble c=1.0-exp(nj*T*omg); + double half=0.5; + + if (fabs(omg)<1e-100) + c1=a*T; + else + c1=a*c/(pj*omg); + + if (fabs(omg+omg0)<1e-100) + c2=b*0.5*T; + else + c2=c*b*half/(nj*cdouble(omg+omg0)); + + if (fabs(omg-omg0)<1e-100) + c3=b*0.5*T; + else + c3=b*c*half/(nj*cdouble(omg-omg0)); + + c=c1+c2+c3; + return abs(c); +}//Hamming*/ + +//--------------------------------------------------------------------------- +/* + function HannSq: computes the square norm of Hann window spectrum (window-size-normalized) + + In: x: frequency, in bins + N: size of Hann window + + Return the square norm. +*/ +double HannSq(double x, double N) +{ + double re, im; + double pim=M_PI*x; + double pimf=pim/N; + double pif=M_PI/N; + + double sinpim=sin(pim); + double sinpimf=sin(pimf); + double sinpimplus1f=sin(pimf+pif); + double sinpimminus1f=sin(pimf-pif); + + double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf; + + if (sinpimf==0) + spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0; + else if (sinpimplus1f==0) + spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0; + else if (sinpimminus1f==0) + spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0; + else + spmdivbyspmf=sinpim/sinpimf, spmdivbyspmpf=sinpim/sinpimplus1f, spmdivbyspmmf=sinpim/sinpimminus1f; + + re=0.5*spmdivbyspmf-0.25*cos(pif)*(spmdivbyspmpf+spmdivbyspmmf); + im=0.25*sin(pif)*(-spmdivbyspmpf+spmdivbyspmmf); + + return (re*re+im*im)/(N*N); +}//HannSq + +/* + function Hann: computes the Hann window amplitude spectrum (window-size-normalized). + + In: x: frequency, in bins + N: size of Hann window + + Return the amplitude spectrum evaluated at x. Maximum 0.5 is reached at x=0. Time 2 to normalize + maximum to 1. +*/ +double Hann(double x, double N) +{ + double pim=M_PI*x; + double pif=M_PI/N; + double pimf=pif*x; + + double sinpim=sin(pim); + double tanpimf=tan(pimf); + double tanpimplus1f=tan(pimf+pif); + double tanpimminus1f=tan(pimf-pif); + + double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf; + + if (fabs(tanpimf)<1e-10) + spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0; + else if (fabs(tanpimplus1f)<1e-10) + spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0; + else if (fabs(tanpimminus1f)<1e-10) + spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0; + else + spmdivbyspmf=sinpim/tanpimf, spmdivbyspmpf=sinpim/tanpimplus1f, spmdivbyspmmf=sinpim/tanpimminus1f; + + double result=0.5*spmdivbyspmf-0.25*(spmdivbyspmpf+spmdivbyspmmf); + + return result/N; +}//HannC + +/* + function HxPeak2: fine spectral peak detection. This does detection and high-precision LSE estimation + in one go. However, since in practise most peaks are spurious, LSE estimation is not necessary on + them. Accordingly, HxPeak2 is deprecated in favour of faster but coarser peak picking methods, such as + QIFFT, which leaves fine estimation to a later stage of processing. + + In: F, dF, ddF: pointers to functions that compute LSE peak energy for, plus its 1st and 2nd + derivatives against, a given frequency. + params: pointer to a data structure (l_hx) hosting input data fed to F, dF, and ddF + (st, en): frequency range, in bins, to search for peaks in + epf: convergence detection threshold + Out: hps[return value]: peak frequencies + vps[return value]; peak amplitudes + + Returns the number of peaks detected. +*/ +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) +{ + struct l_hx {int N; union {double B; struct {int k1; int k2;};}; cdouble* x; double dhxpeak; double hxpeak;} *p=(l_hx *)params; + int dfshift=int(&((l_hx*)0)->dhxpeak); + int fshift=int(&((l_hx*)0)->hxpeak); + double B=p->B; + int count=0; + + int den=ceil(en), dst=floor(st); + if (den-dst<3) den++, dst--; + if (den-dst<3) den++, dst--; + if (dst<1) dst=1; + + double step=0.5; + int num=(den-dst)/step+1; + bool allochps=false, allocvhps=false; + if (hps==NULL) allochps=true, hps=new double[num]; + if (vhps==NULL) allocvhps=true, vhps=new double[num]; + + { + double* inp=new double[num]; + for (int i=0; i<num; i++) + { + double lf=dst+step*i; + p->k1=ceil(lf-B); if (p->k1<0) p->k1=0; + p->k2=floor(lf+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; + inp[i]=F(lf, params); + } + + for (int i=1; i<num-1; i++) + { + if (inp[i]>=inp[i-1] && inp[i]>=inp[i+1]) //inp[i]=F(dst+step*i) + { + if (inp[i]==inp[i-1] && inp[i]==inp[i+1]) continue; + double fa=dst+step*(i-1), fb=dst+step*(i+1); + double ff=dst+step*i; + p->k1=ceil(ff-B); if (p->k1<0) p->k1=0; + p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; + + double tmp=Newton1dmax(ff, fa, fb, ddF, params, dfshift, fshift, dF, dfshift, epf); + + //although we have selected inp[i] to be a local maximum, different truncation + // of local spectrum implies it may not hold as the truncation of inp[i] is + // used for recalculating inp[i-1] and inp[i+1] in init_Newton method. In this + // case we retry the sub-maximal frequency to see if it becomes a local maximum + // when the spectrum is truncated to centre on it. + + if (tmp==-0.5 || tmp==-0.7) //y(fa)<=y(ff)<y(fb) or y(ff)<y(fa)<y(fb) + { + tmp=Newton1dmax(fb, ff, 2*fb-ff, ddF, params, dfshift, fshift, dF, dfshift, epf); + if (tmp==-0.5 || tmp==-0.7) continue; + /* + double ff2=(ff+fb)/2; + p->k1=ceil(ff2-B); if (p->k1<0) p->k1=0; + p->k2=floor(ff2+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; + tmp=Newton1dmax(ff2, ff, fb, ddF, params, dfshift, fshift, dF, dfshift, epf); + p->k1=ceil(ff-B); if (p->k1<0) p->k1=0; + p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1; */ + } + else if (tmp==-0.6 || tmp==-0.8) //y(fb)<=y(ff)<y(fa) + { + tmp=Newton1dmax(fa, 2*fa-ff, ff, ddF, params, dfshift, fshift, dF, dfshift, epf); + if (tmp==-0.6 || tmp==-0.8) continue; + } + if (tmp<0 /*tmp==-0.5 || tmp==-0.6 || tmp==-1 || tmp==-2 || tmp==-3*/) + { + Search1Dmax(ff, params, F, dst+step*(i-1), dst+step*(i+1), &vhps[count], epf); + } + else + { + vhps[count]=p->hxpeak; + } + if (ff>=st && ff<=en && ff>dst+step*(i-0.99) && ff<dst+step*(i+0.99)) + { +// if (count==0 || fabs(tmp-hps[count-1])>0.1) +// { + hps[count]=ff; + count++; +// } + } + } + } + delete[] inp; + } + + if (allochps) hps=(double*)realloc(hps, sizeof(double)*count); + if (allocvhps) vhps=(double*)realloc(vhps, sizeof(double)*count); + return count; +}//HxPeak2 + +//--------------------------------------------------------------------------- +/* + function InsertDec: inserts value into sorted decreasing list + + In: data[Count]: a sorted decreasing list. + value: the value to be added + Out: data[Count]: the list with $value inserted if the latter is larger than its last entry, in which + case the original last entry is discarded. + + Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to + data[Count-1]. +*/ +int InsertDec(int value, int* data, int Count) +{ + if (Count<=0) return -1; + if (value<=data[Count-1]) return -1; + if (value>data[0]) + { + memmove(&data[1], &data[0], sizeof(int)*(Count-1)); + data[0]=value; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)>=value>D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value<=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)>=value>D(end) + memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1)); + data[end]=value; + return end; +}//InsertDec +//the double version +int InsertDec(double value, double* data, int Count) +{ + if (Count<=0) return -1; + if (value<=data[Count-1]) return -1; + if (value>data[0]) + { + memmove(&data[1], &data[0], sizeof(double)*(Count-1)); + data[0]=value; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)>=value>D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value<=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)>=value>D(end) + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); + data[end]=value; + return end; +}//InsertDec + +/* + function InsertDec: inserts value and attached integer into sorted decreasing list + + In: data[Count]: a sorted decreasing list + indices[Count]: a list of integers attached to entries of data[] + value, index: the value to be added and its attached integer + Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is larger than + the last entry of data[], in which case the original last entries are discarded. + + Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to + data[Count-1]. +*/ +int InsertDec(double value, int index, double* data, int* indices, int Count) +{ + if (Count<=0) return -1; + if (value<=data[Count-1]) return -1; + if (value>data[0]) + { + memmove(&data[1], data, sizeof(double)*(Count-1)); + memmove(&indices[1], indices, sizeof(int)*(Count-1)); + data[0]=value, indices[0]=index; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)>=value>D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value<=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)>=value>D(end) + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); + memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1)); + data[end]=value, indices[end]=index; + return end; +}//InsertDec + +/* + InsertInc: inserts value into sorted increasing list. + + In: data[Count]: a sorted increasing list. + Capacity: maximal size of data[] + value: the value to be added + Compare: pointer to function that compare two values + Out: data[Count]: the list with $value inserted. If the original list is full (Count=Capacity) then + either $value, or the last entry of data[], whichever is larger, is discarded. + + Returns the index where $value is located in data[], or -1 if it is not inserted, which happens if + Count=Capacity and $value is larger than or equal to the last entry in data[Capacity]. +*/ +int InsertInc(void* value, void** data, int Count, int Capacity, int (*Compare)(void*, void*)) +{ + if (Capacity<=0) return -1; + if (Count>Capacity) Count=Capacity; + +//Compare(A,B)<0 if A<B, =0 if A=B, >0 if A>B + int PosToInsert; + if (Count==0) PosToInsert=0; + else if (Compare(value, data[Count-1])>0) PosToInsert=Count; + else if (Compare(value, data[0])<0) PosToInsert=0; + else + { + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)<=value<D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (Compare(value, data[mid])>=0) head=mid; + else end=mid; + } + //D(head=end-1)<=value<D(end) + PosToInsert=end; + } + + if (Count<Capacity) + { + memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert)); + data[PosToInsert]=value; + } + else //Count==Capacity + { + if (PosToInsert>=Capacity) return -1; + memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert-1)); + data[PosToInsert]=value; + } + return PosToInsert; +}//InsertInc + +/* + function InsertInc: inserts value into sorted increasing list + + In: data[Count]: a sorted increasing list. + value: the value to be added + doinsert: specifies whether the actually insertion is to be performed + Out: data[Count]: the list with $value inserted if the latter is smaller than its last entry, in which + case the original last entry of data[] is discarded. + + Returns the index where $value is located in data[], or -1 if value is larger than or equal to + data[Count-1]. +*/ +int InsertInc(double value, double* data, int Count, bool doinsert) +{ + if (Count<=0) return -1; + if (value>=data[Count-1]) return -1; + if (value<data[0]) + { + memmove(&data[1], &data[0], sizeof(double)*(Count-1)); + if (doinsert) data[0]=value; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)<=value<D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value>=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)<=value<D(end) + if (doinsert) + { + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); + data[end]=value; + } + return end; +}//InsertInc +//version where data[] is int. +int InsertInc(double value, int* data, int Count, bool doinsert) +{ + if (Count<=0) return -1; + if (value>=data[Count-1]) return -1; + if (value<data[0]) + { + memmove(&data[1], &data[0], sizeof(int)*(Count-1)); + if (doinsert) data[0]=value; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)<=value<D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value>=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)<=value<D(end) + if (doinsert) + { + memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1)); + data[end]=value; + } + return end; +}//InsertInc + +/* + function InsertInc: inserts value and attached integer into sorted increasing list + + In: data[Count]: a sorted increasing list + indices[Count]: a list of integers attached to entries of data[] + value, index: the value to be added and its attached integer + Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is smaller than + the last entry of data[], in which case the original last entries are discarded. + + Returns the index where $value is located in data[], or -1 if $value is larger than or equal to + data[Count-1]. +*/ +int InsertInc(double value, int index, double* data, int* indices, int Count) +{ + if (Count<=0) return -1; + if (value>=data[Count-1]) return -1; + if (value<data[0]) + { + memmove(&data[1], data, sizeof(double)*(Count-1)); + memmove(&indices[1], indices, sizeof(int)*(Count-1)); + data[0]=value, indices[0]=index; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)>=value>D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value>=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)>=value>D(end) + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); + memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1)); + data[end]=value, indices[end]=index; + return end; +}//InsertInc +//version where indices[] is double-precision floating point. +int InsertInc(double value, double index, double* data, double* indices, int Count) +{ + if (Count<=0) return -1; + if (value>=data[Count-1]) return -1; + if (value<data[0]) + { + memmove(&data[1], data, sizeof(double)*(Count-1)); + memmove(&indices[1], indices, sizeof(double)*(Count-1)); + data[0]=value, indices[0]=index; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)>=value>D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value>=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)>=value>D(end) + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1)); + memmove(&indices[end+1], &indices[end], sizeof(double)*(Count-end-1)); + data[end]=value, indices[end]=index; + return end; +}//InsertInc + +/* + function InsertIncApp: inserts value into flexible-length sorted increasing list + + In: data[Count]: a sorted increasing list. + value: the value to be added + Out: data[Count+1]: the list with $value inserted. + + Returns the index where $value is located in data[], or -1 if Count<0. data[] must have Count+1 + storage units on calling. +*/ +int InsertIncApp(double value, double* data, int Count) +{ + if (Count<0) return -1; + if (Count==0){data[0]=value; return 0;} + if (value>=data[Count-1]){data[Count]=value; return Count;} + if (value<data[0]) + { + memmove(&data[1], &data[0], sizeof(double)*Count); + data[0]=value; + return 0; + } + + //now Count>=2 + int head=0, end=Count-1, mid; + + //D(head)<=value<D(end) + while (end-head>1) + { + mid=(head+end)/2; + if (value>=data[mid]) head=mid; + else end=mid; + } + + //D(head=end-1)<=value<D(end) + memmove(&data[end+1], &data[end], sizeof(double)*(Count-end)); + data[end]=value; + + return end; +}//InsertIncApp + +//--------------------------------------------------------------------------- +/* + function InstantFreq; calculates instantaneous frequency from spectrum, evaluated at bin k + + In: x[hwid]: spectrum with scale 2hwid + k: reference frequency, in bins + mode: must be 1. + + Returns an instantaneous frequency near bin k. +*/ +double InstantFreq(int k, int hwid, cdouble* x, int mode) +{ + double result; + switch(mode) + { + //mode 1: the phase vocoder method, based on J. Brown, where the spectrogram + // MUST be calculated using rectangular window + case 1: + { + if (k<1) k=1; + if (k>hwid-2) k=hwid-2; + 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)); + double ph0=Atan2(hi, hr); + double c=cos(M_PI/hwid), s=sin(M_PI/hwid); + 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)); + 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)); + double ph1=Atan2(hi, hr); + result=(ph1-ph0)/(2*M_PI); + if (result<-0.5) result+=1; + if (result>0.5) result-=1; + result+=k*0.5/hwid; + break; + } + case 2: + break; + } + return result; +}//InstantFreq + +/* + function InstantFreq; calculates "frequency spectrum", a sequence of frequencies evaluated at DFT bins + + In: x[hwid]: spectrum with scale 2hwid + mode: must be 1. + Out: freqspec[hwid]: the frequency spectrum + + No return value. +*/ +void InstantFreq(double* freqspec, int hwid, cdouble* x, int mode) +{ + for (int i=0; i<hwid; i++) + freqspec[i]=InstantFreq(i, hwid, x, mode); +}//InstantFreq + +//--------------------------------------------------------------------------- +/* + function IntToDouble: copy content of integer array to double array + + In: in: pointer to integer array + BytesPerSample: number of bytes each integer takes + Count: size of integer array, in integers + Out: vector out[Count]. + + No return value. + + This version is currently commented out in favour of the version implemented in QuickSpec.cpp which + supports 24-bit integers. +*//* +void IntToDouble(double* out, void* in, int BytesPerSample, int Count) +{ + if (BytesPerSample==1){unsigned char* in8=(unsigned char*)in; for (int k=0; k<Count; k++) *(out++)=*(in8++)-128.0;} + else {__int16* in16=(__int16*)in; for (int k=0; k<Count; k++) *(out++)=*(in16++);} +}//IntToDouble*/ + +//--------------------------------------------------------------------------- +/* + function IPHannC: inner product with Hann window spectrum + + In: x[N]: spectrum + f: reference frequency + K1, K2: spectral truncation bounds + + Returns the absolute value of the inner product of x[K1:K2] with the corresponding band of the + spectrum of a sinusoid at frequency f. +*/ +double IPHannC(double f, cdouble* x, int N, int K1, int K2) +{ + int M; double c[4], iH2; + windowspec(wtHann, N, &M, c, &iH2); + return abs(IPWindowC(f, x, N, M, c, iH2, K1, K2)); +}//IPHannC + + +//--------------------------------------------------------------------------- +/* + function lse: linear regression y=ax+b + + In: x[Count], y[Count]: input points + Out: a, b: LSE estimation of coefficients in y=ax+b + + No return value. +*/ +void lse(double* x, double* y, int Count, double& a, double& b) +{ + double sx=0, sy=0, sxx=0, sxy=0; + for (int i=0; i<Count; i++) + { + sx+=x[i]; + sy+=y[i]; + sxx+=x[i]*x[i]; + sxy+=x[i]*y[i]; + } + b=(sxx*sy-sx*sxy)/(Count*sxx-sx*sx); + a=(sy-Count*b)/sx; +}//lse + +//-------------------------------------------------------------------------- +/* + memdoubleadd: vector addition + + In: dest[count], source[count]: addends + Out: dest[count]: sum + + No return value. +*/ +void memdoubleadd(double* dest, double* source, int count) +{ + for (int i=0; i<count; i++){*dest=*dest+*source; dest++; source++;} +}//memdoubleadd + +//-------------------------------------------------------------------------- +/* + function Mel: converts frequency in Hz to frequency in mel. + + In: f: frequency, in Hz + + Returns the frequency measured on mel scale. +*/ +double Mel(double f) +{ + return 1127.01048*log(1+f/700); +}//Mel + +/* + function InvMel: converts frequency in mel to frequency in Hz. + + In: f: frequency, in mel. + + Returns the frequency in Hz. +*/ +double InvMel(double mel) +{ + return 700*(exp(mel/1127.01048)-1); +}//InvMel + +/* + function MFCC: calculates MFCC. + + In: Data[FrameWidth]: data + NumBands: number of frequency bands on mel scale + Bands[3*NumBands]: mel frequency bands, comes as $NumBands triples, each containing the lower, + middle and high frequencies, in bins, of one band, from which a weighting window is created to + weight individual bins. + Ceps_Order: number of MFC coefficients (i.e. DCT coefficients) + W, X: FFT buffers + Out: C[Ceps_Order]: MFCC + Amps[NumBands]: log spectrum on MF bands + + No return value. Use MFCCPrepareBands() to retrieve Bands[]. +*/ +void MFCC(int FrameWidth, int NumBands, int Ceps_Order, double* Data, double* Bands, double* C, double* Amps, cdouble* W, cdouble* X) +{ + double tmp, b2s, b2c, b2e; + + RFFTC(Data, 0, 0, log2(FrameWidth), W, X, 0); + for (int i=0; i<=FrameWidth/2; i++) Amps[i]=X[i].x*X[i].x+X[i].y*X[i].y; + + for (int i=0; i<NumBands; i++) + { + tmp=0; + b2s=Bands[3*i], b2c=Bands[3*i+1], b2e=Bands[3*i+2]; + + for (int j=ceil(b2s); j<ceil(b2c); j++) + tmp+=Amps[j]*(j-b2s)/(b2c-b2s); + for (int j=ceil(b2c); j<b2e; j++) + tmp+=Amps[j]*(b2e-j)/(b2e-b2c); + + if (tmp<3.7200759760208359629596958038631e-44) + Amps[i]=-100; + else + Amps[i]=log(tmp); + } + + for (int i=0; i<Ceps_Order; i++) + { + tmp=Amps[0]*cos(M_PI*(i+1)/2/NumBands); + for (int j=1; j<NumBands; j++) + tmp+=Amps[j]*cos(M_PI*(i+0.5)*(j+0.5)/NumBands); + C[i]=tmp; + } +}//MFCC + +/* + function MFCCPrepareBands: returns a array of OVERLAPPING bands given in triples, whose 1st and 3rd + entries are the start and end of a band, in bins, and the 2nd is a middle frequency. + + In: SamplesPerSec: sampling rate + NumberOfBins: FFT size + NumberOfBands: number of mel-frequency bands + + Returns pointer to the array of triples. +*/ +double* MFCCPrepareBands(int NumberOfBands, int SamplesPerSec, int NumberOfBins) +{ + double* Bands=new double[NumberOfBands*3]; + double naqfreq=SamplesPerSec/2.0; //naqvist freq + double binwid=SamplesPerSec*1.0/NumberOfBins; + double naqmel=Mel(naqfreq); + double b=naqmel/(NumberOfBands+1); + + Bands[0]=0; + Bands[1]=InvMel(b)/binwid; + Bands[2]=InvMel(b*2)/binwid; + for (int i=1; i<NumberOfBands; i++) + { + Bands[3*i]=Bands[3*i-2]; + Bands[3*i+1]=Bands[3*i-1]; + Bands[3*i+2]=InvMel(b*(i+2))/binwid; + } + return Bands; +}//MFCCPrepareBands + +//--------------------------------------------------------------------------- +/* + function Multi: vector-constant multiplication + + In: data[count]: a vector + mul: a constant + Out: data[count]: their product + + No return value. +*/ +void Multi(double* data, int count, double mul) +{ + for (int i=0; i<count; i++){*data=*data*mul; data++;} +}//Multi + +/* + function Multi: vector-constant multiplication + + In: in[count]: a vector + mul: a constant + Out: out[count]: their product + + No return value. +*/ +void Multi(double* out, double* in, int count, double mul) +{ + for (int i=0; i<count; i++) *(out++)=*(in++)*mul; +}//Multi + +/* + function Multi: vector-constant multiply-addition + + In: in[count], adder[count]: vectors + mul: a constant + Out: out[count]: in[]+adder[]*mul + + No return value. +*/ +void MultiAdd(double* out, double* in, double* adder, int count, double mul) +{ + for (int i=0; i<count; i++) *(out++)=*(in++)+*(adder++)*mul; +}//MultiAdd + +//--------------------------------------------------------------------------- +/* + function NearestPeak: finds a peak value in an array that is nearest to a given index + + In: data[count]: an array + anindex: an index + + Returns the index to a peak of data[] that is closest to anindex. In case of two cloest indices, + returns the index to the higher peak of the two. +*/ +int NearestPeak(double* data, int count, int anindex) +{ + int upind=anindex, downind=anindex; + if (anindex<1) anindex=1; + if (anindex>count-2) anindex=count-2; + + if (data[anindex]>data[anindex-1] && data[anindex]>data[anindex+1]) return anindex; + + if (data[anindex]<data[anindex-1]) + while (downind>0 && data[downind-1]>data[downind]) downind--; + if (data[anindex]<data[anindex+1]) + while (upind<count-1 && data[upind]<data[upind+1]) upind++; + + if (upind==anindex) return downind; + if (downind==anindex) return upind; + + if (anindex-downind<upind-anindex) return downind; + else if (anindex-downind>upind-anindex) return upind; + else if (data[upind]<data[downind]) return downind; + else return upind; +}//NearestPeak + +//--------------------------------------------------------------------------- +/* + function NegativeExp: fits the curve y=1-x^lmd. + + In: x[Count], y[Count]: sample points to fit, x[0]=0, x[Count-1]=1, y[0]=1, y[Count-1]=0 + Out: lmd: coefficient of y=1-x^lmd. + + Returns rms fitting error. +*/ +double NegativeExp(double* x, double* y, int Count, double& lmd, int sample, double step, double eps, int maxiter) +{ + lmd=0; + for (int i=1; i<Count-1; i++) + { + if (y[i]<1) + lmd+=log(1-y[i])/log(x[i]); + else + lmd+=-50/log(x[i]); + } + lmd/=(Count-2); + +//lmd has been initialized +//coming up will be the recursive calculation of lmd by lgg + + int iter=0; + double laste, lastdel, e=0, del=0, tmp; + do + { + iter++; + laste=e; + lastdel=del; + e=0, del=0; + for (int i=1; i<Count-1; i+=sample) + { + tmp=pow(x[i], lmd); + e=e+(y[i]+tmp-1)*(y[i]+tmp-1); + del=del+(y[i]+tmp-1)*tmp*log(x[i]); + } + if (laste && e>laste) lmd+=step*lastdel, step/=2; + else lmd+=-step*sample*del; + } + while (e && iter<=maxiter && (!laste || fabs(laste-e)/e>eps)); + return sqrt(e/Count); +}//NegativeExp + +//--------------------------------------------------------------------------- +/* + function: NL: noise level, calculated on 5% of total frames with least energy + + In: data[Count]: + Wid: window width for power level estimation + + Returns noise level, in rms. +*/ +double NL(double* data, int Count, int Wid) +{ + int Fr=Count/Wid; + int Num=Fr/20+1; + double* ene=new double[Num], tmp; + for (int i=0; i<Num; i++) ene[i]=1e30; + for (int i=0; i<Fr; i++) + { + tmp=DCPower(&data[i*Wid], Wid, 0); + InsertInc(tmp, ene, Num); + } + double result=Avg(ene, Num, 0); + delete[] ene; + result=sqrt(result); + return result; +}//NL + +//--------------------------------------------------------------------------- +/* + function Normalize: normalizes data to [-Maxi, Maxi], without zero shift + + In: data[Count]: data to be normalized + Maxi: destination maximal absolute value + Out: data[Count]: normalized data + + Returns the original maximal absolute value. +*/ +double Normalize(double* data, int Count, double Maxi) +{ + double max=0; + double* ldata=data; + for (int i=0; i<Count; i++) + { + if (*ldata>max) max=*ldata; + else if (-*ldata>max) max=-*ldata; + ldata++; + } + if (max>0) + { + Maxi=Maxi/max; + for (int i=0; i<Count; i++) *(data++)*=Maxi; + } + return max; +}//Normalize + +/* + function Normalize2: normalizes data to a specified Euclidian norm + + In: data[Count]: data to normalize + Norm: destination Euclidian norm + Out: data[Count]: normalized data. + + Returns the original Euclidian norm. +*/ +double Normalize2(double* data, int Count, double Norm) +{ + double norm=0; + for (int i=0; i<Count; i++) norm+=data[i]*data[i]; + norm=sqrt(norm); + double mul=norm/Norm; + if (mul!=0) for (int i=0; i<Count; i++) data[i]/=mul; + return norm; +}//Normalize2 + +//--------------------------------------------------------------------------- +/* + function PhaseSpan: computes the unwrapped phase variation across the Nyquist range + + In: data[Count]: time-domain data + aparams: a fftparams structure + + Returns the difference between unwrapped phase angles at 0 and Nyquist frequency. +*/ +double PhaseSpan(double* data, int Count, void* aparams) +{ + int Pad=1; + fftparams* params=(fftparams*)aparams; + double* Arg=new double[Count*Pad]; + AllocateFFTBuffer(Count*Pad, Amp, w, x); + memset(Amp, 0, sizeof(double)*Count*Pad); + memcpy(&Amp[Count*(Pad-1)/2], data, sizeof(double)*Count); + ApplyWindow(Amp, Amp, params->Amp, Count); + RFFTC(Amp, Amp, Arg, log2(Count*Pad), w, x, 0); + + SmoothPhase(Arg, Count*Pad/2+1); + double result=Arg[Count*Pad/2]-Arg[0]; + delete[] Arg; + FreeFFTBuffer(Amp); + return result; +}//PhaseSpan + +//--------------------------------------------------------------------------- +/* + function PolyFit: least square polynomial fitting y=sum(i){a[i]*x^i} + + In: x[N], y[N]: sample points + P: order of polynomial, P<N + Out: a[P+1]: coefficients of polynomial + + No return value. +*/ +void PolyFit(int P, double* a, int N, double* x, double* y) +{ + Alloc2(P+1, P+1, aa); + double ai0, bi, *bb=new double[P+1], *s=new double[N], *r=new double[N]; + 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; + + for (int i=1; i<=P; i++) + { + ai0=bi=0; for (int j=0; j<N; j++) {s[j]*=x[j], r[j]*=x[j]; ai0+=s[j], bi+=r[j];} + for (int j=0; j<=i; j++) aa[i-j][j]=ai0; bb[i]=bi; + } + for (int i=P+1; i<=2*P; i++) + { + ai0=0; for (int j=0; j<N; j++) {s[j]*=x[j]; ai0+=s[j];} + for (int j=i-P; j<=P; j++) aa[i-j][j]=ai0; + } + GESCP(P+1, a, aa, bb); + DeAlloc2(aa); delete[] bb; delete[] s; delete[] r; +}//PolyFit + +//--------------------------------------------------------------------------- +/* + function Pow: vector power function + + In: data[Count]: a vector + ex: expontnet + Out: data[Count]: point-wise $ex-th power of data[] + + No return value. +*/ +void Pow(double* data, int Count, double ex) +{ + for (int i=0; i<Count; i++) + data[i]=pow(data[i], ex); +}//Power + +//--------------------------------------------------------------------------- +/* + Rectify: semi-wave rectification + + In: data[Count]: data to rectify + th: rectification threshold: values below th are rectified to th + Out: data[Count]: recitified data + + Returns number of preserved (i.e. not rectified) samples. +*/ +int Rectify(double* data, int Count, double th) +{ + int Result=0; + for (int i=0; i<Count; i++) + { + if (data[i]<=th) data[i]=th; + else Result++; + } + return Result; +}//Rectify + +//--------------------------------------------------------------------------- +/* + function Res: minimum absolute residue. + + In: in: a number + mod: modulus + + Returns the minimal absolute residue of $in devided by $mod. +*/ +double Res(double in, double mod) +{ + int i=in/mod; + in=in-i*mod; + if (in>mod/2) in-=mod; + if (in<-mod/2) in+=mod; + return in; +}//Res + +//--------------------------------------------------------------------------- +/* + function Romberg: Romberg algorithm for numerical integration + + In: f: function to integrate + params: extra argument for calling f + a, b: integration boundaries + n: depth of sampling + + Returns the integral of f(*, params) over [a, b]. +*/ +double Romberg(int n, double(*f)(double, void*), double a, double b, void* params) +{ + int np=1; + double* r1=new double[n+1];. + double* r2=new double[n+1]; + double h=b-a, *swp; + r1[1]=h*(f(a, params)+f(b, params))/2; + for (int i=2; i<=n; i++) + { + double akh=a+0.5*h; r2[1]=f(akh, params); + for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h + r2[1]=0.5*(r1[1]+h*r2[1]); + double fj=4; + for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;} //fj=4^(j-1) + h/=2; np*=2; + swp=r1; r1=r2; r2=swp; + } + h=r1[n]; + delete[] r1; + delete[] r2; + return h; +}//Romberg + +/* + function Romberg: Romberg algorithm for numerical integration, may return before specified depth on + convergence. + + In: f: function to integrate + params: extra argument for calling f + a, b: integration boundaries + n: depth of sampling + ep: convergence test threshold + + Returns the integral of f(*, params) over [a, b]. +*/ +double Romberg(double(*f)(double, void*), double a, double b, void* params, int n, double ep) +{ + int i, np=1; + double* r1=new double[n+1]; + double* r2=new double[n+1]; + double h=b-a, *swp; + r1[1]=h*(f(a, params)+f(b, params))/2; + bool mep=false; + for (i=2; i<=n; i++) + { + double akh=a+0.5*h; r2[1]=f(akh, params); + for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h + r2[1]=0.5*(r1[1]+h*r2[1]); + double fj=4; + for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;} //fj=4^(j-1) + + if (fabs(r2[i]-r1[i-1])<ep) + { + if (mep) break; + else mep=true; + } + else mep=false; + + h/=2; np*=2; + swp=r1; r1=r2; r2=swp; + } + if (i<=n) h=r2[i]; + else h=r1[n]; + delete[] r1; + delete[] r2; + return h; +}//Romberg + +//--------------------------------------------------------------------------- +//analog and digital sinc functions + +//sinca(0)=1, sincd(0)=N, sinca(1)=sincd(1)=0. +/* + function sinca: analog sinc function. + + In: x: frequency + + Returns sinc(x)=sin(pi*x)/(pi*x), sinca(0)=1, sinca(1)=0 +*/ +double sinca(double x) +{ + if (x==0) return 1; + return sin(M_PI*x)/(M_PI*x); +}//sinca + +/* + function sincd_unn: unnormalized discrete sinc function + + In: x: frequency + N: scale (window size, DFT size) + + Returns sinc(x)=sin(pi*x)/sin(pi*x/N), sincd(0)=N, sincd(1)=0. +*/ +double sincd_unn(double x, int N) +{ + if (x==0) return N; + return sin(M_PI*x)/sin(M_PI*x/N); +}//sincd + +//--------------------------------------------------------------------------- +/* + SmoothPhase: phase unwrapping on module mpi*PI, 2PI by default + + In: Arg[Count]: phase angles to unwrap + mpi: unwrapping modulus, in pi's + Out: Arg[Count]: unwrapped phase + + Returns the amount of unwrap, in pi's, of the last phase angle +*/ +double SmoothPhase(double* Arg, int Count, int mpi) +{ + double m2pi=mpi*M_PI; + for (int i=1; i<Count-1; i++) + Arg[i]=Arg[i-1]+Res(Arg[i]-Arg[i-1], m2pi); + double tmp=Res(Arg[Count-1]-Arg[Count-2], m2pi); + double result=(Arg[Count-1]-Arg[Count-2]-tmp)/m2pi; + Arg[Count-1]=Arg[Count-2]+tmp; + + return result; +}//SmoothPhase + +//--------------------------------------------------------------------------- +//the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). + +/* + StiffB: computes stiffness coefficient from fundamental and another partial frequency based on the + stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). + + In: f0: fundamental frequency + m: 1-based partial index + fm: frequency of partial m + + Returns stiffness coefficient B. +*/ +double StiffB(double f0, double fm, int m) +{ + double f2=fm/m/f0; + return (f2*f2-1)/(m*m-1); +}//StiffB + +//StiffF: partial frequency of a stiff string +/* + StiffFm: computes a partial frequency from fundamental frequency and partial index based on the stiff + string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). + + In: f0: fundamental frequency + m: 1-based partial index + B: stiffness coefficient + + Returns frequency of the m-th partial. +*/ +double StiffFm(double f0, int m, double B) +{ + return m*f0*sqrt(1+B*(m*m-1)); +}//StiffFm + +/* + StiffF0: computes fundamental frequency from another partial frequency and stiffness coefficient based + on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). + + In: m: 1-based partial index + fm: frequency of partial m + B: stiffness coefficient + + Returns the fundamental frequency. +*/ +double StiffF0(double fm, int m, double B) +{ + return fm/m/sqrt(1+B*(m*m-1)); +}//StiffF0 + +/* + StiffM: computes 1-based partial index from partial frequency, fundamental frequency and stiffness + coefficient based on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)). + + In: f0: fundamental freqency + fm: a partial frequency + B: stiffness coefficient + + Returns the 1-based partial index which will generate the specified partial frequency from the model + which, however, does not have to be an integer in this function. +*/ +double StiffM(double f0, double fm, double B) +{ + if (B<1e-14) return fm/f0; + double b1=B-1, ff=fm/f0; + double delta=b1*b1+4*B*ff*ff; + if (delta<0) + return sqrt(b1/2/B); + else + return sqrt((b1+sqrt(delta))/2/B); +}//StiffMd + +//--------------------------------------------------------------------------- +/* + TFFilter: time-frequency filtering with Hann-windowed overlap-add. + + In: data[Count]: input data + Spans: time-frequency spans + wt, windp: type and extra parameter of FFT window + Sps: sampling rate + TOffst: optional offset applied to all time values in Spans, set to Spans timing of of data[0]. + Pass: set to pass T-F content covered by Spans, clear to stop T-F content covered by Spans + Out: dataout[Count]: filtered data + + No return value. Identical data and dataout allowed. +*/ +void TFFilter(double* data, double* dataout, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst) +{ + int HWid=Wid/2; + int Fr=Count/HWid-1; + int Order=log2(Wid); + + int** lspan=new int*[Fr]; + double* lxspan=new double[Fr]; + + lspan[0]=new int[Fr*Wid]; + for (int i=1; i<Fr; i++) + lspan[i]=&lspan[0][i*Wid]; + + //fill local filter span table + if (Pass) + memset(lspan[0], 0, sizeof(int)*Fr*Wid); + else + for (int i=0; i<Fr; i++) + for (int j=0; j<Wid; j++) + lspan[i][j]=1; + + for (int i=0; i<Spans->Count; i++) + { + int lx1, lx2, ly1, ly2; + lx1=(Spans->Items[i].T1-TOffst)/HWid-1; + lx2=(Spans->Items[i].T2-1-TOffst)/HWid; + ly1=Spans->Items[i].F1*2/Sps*HWid+0.001; + ly2=Spans->Items[i].F2*2/Sps*HWid+1; + if (lx1<0) lx1=0; + if (lx2>=Fr) lx2=Fr-1; + if (ly1<0) ly1=0; + if (ly1>HWid) ly1=HWid; + if (Pass) + for (int x=lx1; x<=lx2; x++) + for (int y=ly1; y<=ly2; y++) + lspan[x][y]=1; + else + for (int x=lx1; x<=lx2; x++) + for (int y=ly1; y<=ly2; y++) + lspan[x][y]=0; + } + for (int i=0; i<Fr; i++) + { + lxspan[i]=0; + for (int j=0; j<=HWid; j++) + { + if (lspan[i][j]) + lxspan[i]=lxspan[i]+1; + } + lxspan[i]/=(HWid+1); + } + double* winf=NewWindow(wt, Wid, 0, &windp); + double* wini=NewWindow(wtHann, Wid, NULL, NULL); + for (int i=0; i<Wid; i++) + if (winf[i]!=0) wini[i]=wini[i]/winf[i]; + AllocateFFTBuffer(Wid, ldata, w, x); + double* tmpdata=new double[HWid]; + memset(tmpdata, 0, HWid*sizeof(double)); + + for (int i=0; i<Fr; i++) + { + if (lxspan[i]==0) + { + memcpy(&dataout[i*HWid], tmpdata, sizeof(double)*HWid); + memset(tmpdata, 0, sizeof(double)*HWid); + continue; + } + if (lxspan[i]==1) + { + memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winf[k]*wini[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winf[k]*wini[k]; + + memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); + for (int k=0; k<HWid; k++) + dataout[i*HWid+k]+=ldata[k]; + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + continue; + } + memcpy(ldata, &data[i*HWid], Wid*sizeof(double)); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winf[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winf[k]; + + RFFTC(ldata, NULL, NULL, Order, w, x, 0); + + if (!lspan[i][0]) x[0].x=x[0].y=0; + for (int j=1; j<=HWid; j++) + if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0; + + CIFFTR(x, Order, w, ldata); + + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*wini[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*wini[k]; + + memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double)); + for (int k=0; k<HWid; k++) + dataout[i*HWid+k]+=ldata[k]; + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + } + memcpy(&dataout[Fr*HWid], tmpdata, sizeof(double)*HWid); + memset(&dataout[Fr*HWid+HWid], 0, sizeof(double)*(Count-Fr*HWid-HWid)); + + FreeFFTBuffer(ldata); + delete[] lspan[0]; + delete[] lspan; + delete[] lxspan; + delete[] tmpdata; + delete[] winf; + delete[] wini; +}//TFFilter +//version on integer data, where BytesPerSample specified the integer format. +void TFFilter(void* data, void* dataout, int BytesPerSample, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst) +{ + int HWid=Wid/2; + int Fr=Count/HWid-1; + int Order=log2(Wid); + + int** lspan=new int*[Fr]; + double* lxspan=new double[Fr]; + + lspan[0]=new int[Fr*Wid]; + for (int i=1; i<Fr; i++) + lspan[i]=&lspan[0][i*Wid]; + + //fill local filter span table + if (Pass) + memset(lspan[0], 0, sizeof(int)*Fr*Wid); + else + for (int i=0; i<Fr; i++) + for (int j=0; j<Wid; j++) + lspan[i][j]=1; + + for (int i=0; i<Spans->Count; i++) + { + int lx1, lx2, ly1, ly2; + lx1=(Spans->Items[i].T1-TOffst)/HWid-1; + lx2=(Spans->Items[i].T2-1-TOffst)/HWid; + ly1=Spans->Items[i].F1*2/Sps*HWid+0.001; + ly2=Spans->Items[i].F2*2/Sps*HWid+1; + if (lx1<0) lx1=0; + if (lx2>=Fr) lx2=Fr-1; + if (ly1<0) ly1=0; + if (ly1>HWid) ly1=HWid; + if (Pass) + for (int x=lx1; x<=lx2; x++) + for (int y=ly1; y<=ly2; y++) + lspan[x][y]=1; + else + for (int x=lx1; x<=lx2; x++) + for (int y=ly1; y<=ly2; y++) + lspan[x][y]=0; + } + for (int i=0; i<Fr; i++) + { + lxspan[i]=0; + for (int j=0; j<=HWid; j++) + { + if (lspan[i][j]) + lxspan[i]=lxspan[i]+1; + } + lxspan[i]/=(HWid+1); + } + double* winf=NewWindow(wt, Wid, 0, &windp); + double* wini=NewWindow(wtHann, Wid, NULL, NULL); + for (int i=0; i<Wid; i++) + if (winf[i]!=0) wini[i]=wini[i]/winf[i]; + AllocateFFTBuffer(Wid, ldata, w, x); + double* tmpdata=new double[HWid]; + memset(tmpdata, 0, HWid*sizeof(double)); + + for (int i=0; i<Fr; i++) + { + if (lxspan[i]==0) + { + DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); + memset(tmpdata, 0, sizeof(double)*HWid); + continue; + } + if (lxspan[i]==1) + { + IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winf[k]*wini[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winf[k]*wini[k]; + + for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k]; + DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + continue; + } + IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid); + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*winf[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*winf[k]; + + RFFTC(ldata, NULL, NULL, Order, w, x, 0); + + if (!lspan[i][0]) x[0].x=x[0].y=0; + for (int j=1; j<=HWid; j++) + if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0; + + CIFFTR(x, Order, w, ldata); + + if (i>0) + for (int k=0; k<HWid; k++) + ldata[k]=ldata[k]*wini[k]; + for (int k=HWid; k<Wid; k++) + ldata[k]=ldata[k]*wini[k]; + + + for (int k=0; k<HWid; k++) + tmpdata[k]+=ldata[k]; + DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); + memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double)); + } + DoubleToInt(&((char*)dataout)[Fr*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid); + memset(&((char*)dataout)[(Fr*HWid+HWid)*BytesPerSample], 0, BytesPerSample*(Count-Fr*HWid-HWid)); + + FreeFFTBuffer(ldata); + + delete[] lspan[0]; + delete[] lspan; + delete[] lxspan; + delete[] tmpdata; + delete[] winf; + delete[] wini; +}//TFFilter + + +