xue@11: /*
xue@11:     Harmonic sinusoidal modelling and tools
xue@11: 
xue@11:     C++ code package for harmonic sinusoidal modelling and relevant signal processing.
xue@11:     Centre for Digital Music, Queen Mary, University of London.
xue@11:     This file copyright 2011 Wen Xue.
xue@11: 
xue@11:     This program is free software; you can redistribute it and/or
xue@11:     modify it under the terms of the GNU General Public License as
xue@11:     published by the Free Software Foundation; either version 2 of the
xue@11:     License, or (at your option) any later version.
xue@11: */
xue@1: //---------------------------------------------------------------------------
xue@1: 
xue@1: #include <math.h>
Chris@2: #include <string.h>
xue@1: #include "hssf.h"
xue@1: #include "arrayalloc.h"
Chris@2: #include "matrix.h"
xue@1: #include "vibrato.h"
xue@1: 
Chris@5: /** \file hssf.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
xue@1: //method TSF::TSF: default constructor.
xue@1: TSF::TSF()
xue@1: {
xue@1: 	memset(this, 0, sizeof(TSF));
xue@1: }//TSF
xue@1: 
xue@1: //method TSF::~TSF: default destructor.
xue@1: TSF::~TSF()
xue@1: {
xue@1: 	if (h) DeAlloc2(h);
xue@1: 	if (b) DeAlloc2(b);
xue@1: 	delete[] lp;
xue@1: 	delete[] F0;
xue@1: 	free(avgh);
xue@1: 	free(avgb);
xue@1: }//~TSF
xue@1: 
Chris@5: /**
xue@1:   method TSF::AllocateL: allocates or reallocates storage space whose size depends on L. This uses an
xue@1:   external L value for allocation and updates L itself.
xue@1: */
xue@1: void TSF::AllocateL(int AnL)
xue@1: {
xue@1:   L=AnL;
xue@1:   delete[] F0; F0=new double[(L+2)*6];
xue@1:   F0C=&F0[L+2], F0D=&F0[(L+2)*2], logA0C=&F0[(L+2)*3], logA0=&F0[(L+2)*4], logA0D=&F0[(L+2)*5];
xue@1: }//AllocateL
xue@1: 
Chris@5: /**
xue@1:   method TSF::AllocateP: allocates or reallocates storage space whose size depends on P, using the
xue@1:   interal P.
xue@1: */
xue@1: void TSF::AllocateP()
xue@1: {
xue@1:   delete[] lp;
xue@1:   lp=new double[P+2];
xue@1: }//AllocateP
xue@1: 
Chris@5: /**
xue@1:   method TSF::AllocateSF: allocates or reallocates storage space for source-filter coefficients.
xue@1: */
xue@1: void TSF::AllocateSF()
xue@1: {
xue@1:   if (h) DeAlloc2(h); int k=1.0/F; Allocate2(double, L, (k+2), h); memset(h[0], 0, sizeof(double)*(L)*(k+2));
xue@1:   if (b) DeAlloc2(b); Allocate2(double, L, M, b);	memset(b[0], 0, sizeof(double)*(L)*M);
xue@1:   avgh=(double*)realloc(avgh, sizeof(double)*(k+2)); avgb=(double*)realloc(avgb, sizeof(double)*M);
xue@1: }//AllocateSF
xue@1: 
Chris@5: /**
xue@1:   method TSF::Duplicate: copies the complete contents from another SF object
xue@1: 
xue@1:   In: SF: source object
xue@1: */
xue@1: void TSF::Duplicate(TSF& SF)
xue@1: {
Chris@13:   this->TSF::~TSF();
xue@1:   memcpy(this, &SF, sizeof(TSF)); lp=F0=avgh=avgb=0, h=b=0;
xue@1:   AllocateL(L); memcpy(F0, SF.F0, sizeof(double)*(L+2)*6);
xue@1:   AllocateP(); memcpy(lp, SF.lp, sizeof(double)*(P+2));
xue@1:   AllocateSF(); int SFL=ceil(lp[P-1])-ceil(lp[0]);
xue@1:   for (int l=0; l<SFL; l++) memcpy(h[l], SF.h[l], sizeof(double)*(K+2)), memcpy(b[l], SF.b[l], sizeof(double)*M);
xue@1:   memcpy(avgh, SF.avgh, sizeof(double)*(K+2)); memcpy(avgb, SF.avgb, sizeof(double)*M);
xue@1: }//Duplicate
xue@1: 
Chris@5: /**
xue@1:   method TSF::LoadFromFileHandle: reads SF object from file
xue@1: */
xue@1: void TSF::LoadFromFileHandle(FILE* f)
xue@1: {
xue@1:   fread(&M, sizeof(int), 1, f);
xue@1:   fread(&L, sizeof(int), 1, f);
xue@1:   fread(&P, sizeof(int), 1, f);
xue@1:   fread(&offst, sizeof(double), 1, f);
xue@1:   AllocateL(L); AllocateP();
xue@1:   fread(F0C, sizeof(double), L, f);
xue@1:   fread(logA0C, sizeof(double), L, f);
xue@1:   fread(logA0D, sizeof(double), L, f);
xue@1:   fread(lp, sizeof(double), P, f);
xue@1:   LoadSFFromFileHandle(f);
xue@1: }//LoadFromFileHandle
xue@1: 
Chris@5: /**
xue@1:   method TSF::LoadSFFromFileHandle: reads SF coefficients from file
xue@1: */
xue@1: void TSF::LoadSFFromFileHandle(FILE* f)
xue@1: {
xue@1:   fread(&K, sizeof(int), 1, f);
xue@1:   fread(&FScaleMode, sizeof(int), 1, f);
xue@1:   fread(&F, sizeof(double), 1, f);
xue@1:   fread(&Fs, sizeof(double), 1, f);
xue@1:   AllocateSF();
xue@1:   for (int l=0; l<L; l++) fread(b[l], sizeof(double), M, f);
xue@1:   for (int l=0; l<L; l++) fread(h[l], sizeof(double), K+2, f);
xue@1:   fread(avgb, sizeof(double), M, f);
xue@1:   fread(avgh, sizeof(double), K+2, f);
xue@1: }//LoadSFFromFileHandle
xue@1: 
Chris@5: /**
xue@1:   method TSF::LogAF: average filter response
xue@1: 
xue@1:   In: f: frequency
xue@1: 
xue@1:   Returns the average response of the filter model at f
xue@1: */
xue@1: double TSF::LogAF(double f)
xue@1: {
xue@1: 	if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
xue@1: 	int k=floor(f/F);
xue@1: 	double f_plus=f/F-k;
xue@1: 	if (k<K+1) return avgh[k]*(1-f_plus)+avgh[k+1]*f_plus;
xue@1: 	else return avgh[K+1];
xue@1: }//LogAF
xue@1: 
Chris@5: /**
xue@1:   method TSF::LogAF: filter response
xue@1: 
xue@1:   In: f: frequency
xue@1:       fr: frame index
xue@1: 
xue@1:   Returns the response of the filter model at f for frame fr
xue@1: */
xue@1: double TSF::LogAF(double f, int fr)
xue@1: {
xue@1: 	int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1: 	int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1;
xue@1: 	if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
xue@1: 	int k=floor(f/F);
xue@1: 	double f_plus=f/F-k;
xue@1: 	if (k<K+1) return h[l][k]*(1-f_plus)+h[l][k+1]*f_plus;
xue@1: 	else return h[l][K+1];
xue@1: }//LogAF
xue@1: 
Chris@5: /**
xue@1:   method TSF::LogAS: source response
xue@1: 
xue@1:   In: m: partial index
xue@1:       fr: frame index
xue@1: 
xue@1:   Returns response of the source model for partial m at frame fr
xue@1: */
xue@1: double TSF::LogAS(int m, int fr)
xue@1: {
xue@1: 	int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1: 	int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1;
xue@1: 	return b[l][m];
xue@1: }//LogAS
xue@1: 
Chris@5: /**
xue@1:   method TSF::ReAllocateL: reallocates storage space whose size depends on L, and transfer the contents.
xue@1:   This uses an external L value for allocation but does not update L itself.
xue@1: */
xue@1: void TSF::ReAllocateL(int newL)
xue@1: {
xue@1:   double* newF0=new double[(newL+2)*4]; F0C=&newF0[newL+2], F0D=&newF0[(newL+2)*2], logA0C=&newF0[(newL+2)*3], logA0=&newF0[(L+2)*4], logA0D=&F0[(L+2)*5];
xue@1:   memcpy(logA0C, &F0[(L+2)*3], sizeof(double)*(L+2));
xue@1:   memcpy(logA0D, &F0[(L+2)*5], sizeof(double)*(L+2));
xue@1:   memcpy(F0D, &F0[(L+2)*2], sizeof(double)*(L+2));
xue@1:   memcpy(F0C, &F0[L+2], sizeof(double)*(L+2));
xue@1:   memcpy(newF0, F0, sizeof(double)*(L+2));
xue@1:   delete[] F0;
xue@1:   F0=newF0;
xue@1: }//ReAllocateL
xue@1: 
Chris@5: /**
xue@1:   method TSF::SaveSFToFileHandle: writes SF coefficients to file
xue@1: */
xue@1: void TSF::SaveSFToFileHandle(FILE* f)
xue@1: {
xue@1:   fwrite(&K, sizeof(int), 1, f);
xue@1:   fwrite(&FScaleMode, sizeof(int), 1, f);
xue@1:   fwrite(&F, sizeof(double), 1, f);
xue@1:   fwrite(&Fs, sizeof(double), 1, f);
xue@1:   for (int l=0; l<L; l++) fwrite(b[l], sizeof(double), M, f);
xue@1:   for (int l=0; l<L; l++) fwrite(h[l], sizeof(double), K+2, f);
xue@1:   fwrite(avgb, sizeof(double), M, f);
xue@1:   fwrite(avgh, sizeof(double), K+2, f);
xue@1: }//SaveSFToFileHandle
xue@1: 
Chris@5: /**
xue@1:   method TSF::SaveToFile: save SF object to file
xue@1: 
xue@1:   In: filename: full path of destination file
xue@1: */
xue@1: void TSF::SaveToFile(char* filename)
xue@1: {
xue@1:   FILE* file;
xue@1:   if ((file=fopen(filename, "wb"))!=NULL)
xue@1:   {
xue@1:     SaveToFileHandle(file); fclose(file);
xue@1:   }
xue@1: }//SaveToFile
xue@1: 
Chris@5: /**
xue@1:   method TSF::SaveToFileHandle: writes SF object to file
xue@1: */
xue@1: void TSF::SaveToFileHandle(FILE* f)
xue@1: {
xue@1:   fwrite(&M, sizeof(int), 1, f);
xue@1:   fwrite(&L, sizeof(int), 1, f);
xue@1:   fwrite(&P, sizeof(int), 1, f);
xue@1:   fwrite(&offst, sizeof(double), 1, f);
xue@1:   fwrite(F0C, sizeof(double), L, f);
xue@1:   fwrite(logA0C, sizeof(double), L, f);
xue@1:   fwrite(logA0D, sizeof(double), L, f);
xue@1:   fwrite(lp, sizeof(double), P, f);
xue@1:   SaveSFToFileHandle(f);
xue@1: }//SaveToFileHandle
xue@1: 
Chris@5: /**
xue@1:   method TSF::ShiftFilterByDB: adds a given number of dBs to the filter model.
xue@1: 
xue@1:   In: dB: amount to add.
xue@1: */
xue@1: void TSF::ShiftFilterByDB(double dB)
xue@1: {
xue@1:   for (int l=0; l<L; l++) for (int k=0; k<K+2; k++) h[l][k]+=dB;
xue@1: 	for (int k=0; k<K+2; k++) avgh[k]+=dB;
xue@1: }//ShiftFilterByDB
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //functions
xue@1: 
xue@1: 
Chris@5: /**
xue@1:   function AnalyzeSF: wrapper function.
xue@1: 
xue@1:   In: HS: a harmonic sinusoid
xue@1:       sps: sampling rate ("samples per second")
xue@1:       offst: hop size, the interval between adjacent harmonic atoms of HS
xue@1:       SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation)
xue@1:       SFF: filter response sampling interval
xue@1:       SFScale: set if filter sampling uses mel scale
xue@1:       SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach
xue@1:   Out: SF: a TSF object estimated from HS.
xue@1:        cyclefrcount: number of cycles
xue@1:        cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void AnalyzeSF(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, double offst, int* cyclefrcount,
xue@1:       int SFMode, double SFF, int SFFScale, double SFtheta)
xue@1: {
xue@1:   AnalyzeSF_1(HS, SF, sps, offst);
xue@1:   AnalyzeSF_2(HS, SF, cyclefrs, cyclefs, sps, cyclefrcount, SFMode, SFF, SFFScale, SFtheta);
xue@1: }//AnalyzeSF
xue@1: 
Chris@5: /**
xue@1:   function AnalyzeSF_1: first stage of source-filter model estimation, in which the duration of a HS is
xue@1:   segmented into what is equivalent to "cycles" in vibrato analysis.
xue@1: 
xue@1:   In: HS: a harmonic sinusoid
xue@1:       sps: sampling rate ("samples per second")
xue@1:       offst: hop size, the interval between adjacent harmonic atoms of HS
xue@1:   Out: SF: a TSF object partially updated, particularly lp[P].
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void AnalyzeSF_1(THS& HS, TSF& SF, double sps, double offst)
xue@1: {
xue@1:   int M=HS.M, Fr=HS.Fr;
xue@1:   if (M<=0 || Fr<=0) return;
xue@1:   atom** Partials=HS.Partials;
xue@1: 
xue@1:   //basic descriptor: offst
xue@1:   SF.offst=offst;
xue@1: 
xue@1:   //demodulating pitch
xue@1:   //Basic descriptor: L
xue@1:   SF.AllocateL(Fr);
xue@1: 
xue@1:   double* A0=SF.logA0;
xue@1:   //find the amplitude and pitch tracks
xue@1:   SF.F0max=0, SF.F0min=1;
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:   {
xue@1:     double suma2=0, suma2p=0;
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double a=Partials[m][fr].a, p=Partials[m][fr].f/(m+1);
xue@1:       if (p<=0) break;
xue@1:       suma2+=a*a, suma2p+=a*a*p;
xue@1:     }
xue@1:     SF.F0[fr]=suma2p/suma2;
xue@1:     if (SF.F0max<SF.F0[fr]) SF.F0max=SF.F0[fr];
xue@1:     if (SF.F0min>SF.F0[fr]) SF.F0min=SF.F0[fr];
xue@1:     A0[fr]=suma2;
xue@1:   }
xue@1: 
xue@1:   //Basic descriptor: M
xue@1:   SF.M=0.45/SF.F0max;
xue@1:   if (SF.M>M) SF.M=M;
xue@1: 
xue@1:   //rough estimation of rate
xue@1:   double* f0d=new double[Fr-1]; for (int i=0; i<Fr-1; i++) f0d[i]=SF.F0[i+1]-SF.F0[i];
xue@1:   RateAndReg(SF.rate, SF.regularity, f0d, 0, Fr-2, 8, sps, offst);
xue@1:   delete[] f0d;
xue@1:   //find peaks of the pitch track
xue@1:   int* peakfr=new int[Fr/2];
xue@1:   double periodinframe=sps/(SF.rate*offst), *dummy=(double*)0xFFFFFFFF;
xue@1:   FindPeaks(peakfr, SF.P, SF.F0, Fr, periodinframe, dummy);
xue@1:   //Basic descriptor: lp[]
xue@1:   SF.AllocateP();
xue@1:   for (int p=0; p<SF.P; p++)
xue@1:   {
xue@1:     double startfr; QIE(&SF.F0[peakfr[p]], startfr); SF.lp[p]=startfr+peakfr[p];
xue@1:   }
xue@1:   delete[] peakfr;
xue@1: }//AnalyzeSF_1
xue@1: 
Chris@5: /**
xue@1:   function AnalyzeSF_2: second stage of source-filter model estimation, in which the HS is demodulated
xue@1:   and its source-filter model is estimated.
xue@1: 
xue@1:   In: HS: a harmonic sinusoid
xue@1:       SF: a TSF object with valid segmentation data (lp[P]) of HS
xue@1:       sps: sampling rate ("samples per second")
xue@1:       SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation)
xue@1:       SFF: filter response sampling interval
xue@1:       SFScale: set if filter sampling uses mel scale
xue@1:       SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach
xue@1:   Out: SF: the TSF object fully estimated.
xue@1:        cyclefrcount: number of cycles
xue@1:        cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void AnalyzeSF_2(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, int* cyclefrcount,
xue@1:       int SFMode, double SFF, int SFFScale, double SFtheta)
xue@1: {
xue@1:   int M=HS.M, Fr=HS.Fr;
xue@1:   if (M<=0 || Fr<=0) return;
xue@1:   atom** Partials=HS.Partials;
xue@1: 
xue@1:   if (SF.P>=1) //de-modulation
xue@1:   {
xue@1:     //Basic descriptor: F0C[]
xue@1:     DeFM(SF.F0C, SF.F0, SF.logA0, SF.P, SF.lp, Fr, SF.F0Overall, *cyclefrcount, cyclefrs, cyclefs);
xue@1:     //Basic descriptor: A0C[]
xue@1:     double *A0C=SF.logA0C, *logA0C=SF.logA0C, *logA0D=SF.logA0D, *logA0=SF.logA0;
xue@1:     DeAM(A0C, SF.logA0, SF.P, SF.lp, Fr);
xue@1:     for (int fr=0; fr<Fr; fr++) logA0C[fr]=log(A0C[fr]);
xue@1:     //Basic descriptor: logA0D[]
xue@1:     int hM=M/2;
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       double loga0d=0;
xue@1:       for (int m=0; m<hM; m++) loga0d+=log(Partials[m][fr].a);
xue@1:       logA0[fr]=loga0d/hM;
xue@1:       logA0D[fr]=logA0[fr]-logA0C[fr];
xue@1:     }
xue@1:   }
xue@1:   //Basic descriptor: F0D[]
xue@1:   SF.F0Cmax=0; SF.F0Dmax=-1; SF.F0Cmin=1; SF.F0Dmin=1;
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:   {
xue@1:     if (SF.F0Cmax<SF.F0C[fr]) SF.F0Cmax=SF.F0C[fr];
xue@1:     if (SF.F0Cmin>SF.F0C[fr]) SF.F0Cmin=SF.F0C[fr];
xue@1:     SF.F0D[fr]=SF.F0[fr]-SF.F0C[fr];
xue@1:   }
xue@1: 
xue@1:   //Basic descriptor: b, h
xue@1:   //Source-filter modeling
xue@1:   {
xue@1:     SF.F=SFF; SF.Fs=sps; SF.AllocateSF();
xue@1:     int M=SF.M; double **h=SF.h, **b=SF.b;// *avgh=SF.avgh, *avgb=SF.avgb;
xue@1: 
xue@1:     double *logA0C=useA0?SF.logA0:SF.logA0C;
xue@1:     if (SFMode==0){
xue@1:       S_F_FB(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, sps);}
xue@1:     else if (SFMode==1){
xue@1:       S_F_SV(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, SFtheta, sps);}
xue@1: 
xue@1:     SF.FScaleMode=SFFScale;
xue@1: //		int K=SF.K, effL=ceil(SF.lp[SF.P-1])-ceil(SF.lp[0]);
xue@1: //		for (int k=0; k<K+2; k++){double sumh=0; for (int l=0; l<effL; l++) sumh+=h[l][k]; avgh[k]=sumh/effL;}
xue@1: //		for (int m=0; m<M; m++){double sumb=0; for (int l=0; l<effL; l++) sumb+=b[l][m]; avgb[m]=sumb/effL;}
xue@1:   }
xue@1: }//AnalyzeSF_2
xue@1: 
Chris@5: /**
xue@1:   function I3: integrates the product of 3 linear functions (0, a*)-(T, b*) (*=1, 2 or 3) over (0, T).
xue@1:   See "further reading", pp.12 eq.(37).
xue@1: 
xue@1:   In: T, a1, a2, a3, b1, b2, b3
xue@1: 
xue@1:   Returns the definite integral.
xue@1: */
xue@1: double I3(double T, double a1, double a2, double a3, double b1, double b2, double b3)
xue@1: {
xue@1:   return T*(a1*(a2*(3*a3+b3)+b2*(a3+b3))+b1*(a2*(a3+b3)+b2*(a3+3*b3)))/12;
xue@1: }//I3
xue@1: 
Chris@5: /**
xue@1:   function P_Ax: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic
xue@1:   coefficient of filter coefficients x[L][K+2] towards the totoal variation. See "further reading",
xue@1:   pp.8, (29a).
xue@1: 
xue@1:   In: x[L][K+2]: filter coefficients
xue@1:       f[L][M]: partial frequencies
xue@1:       F: filter response sampling interval
xue@1:       theta: balancing factor of amplitude and frequency variations
xue@1:   Out: d[L][K+2]: Ax.
xue@1: 
xue@1:   Returns inner product of x[][] and d[][].
xue@1: */
xue@1: double P_Ax(double** d, int L, int M, int K, double** x, double** f, double F, double theta)
xue@1: {
xue@1:   double **dFtr=d;
xue@1:   Alloc2(L, K+2, dSrc);
xue@1:   double DelFtr=P2_DelFtr(dFtr, L, K, x, F);
xue@1:   double DelSrc=P3_DelSrc(dSrc, L, M, K, x, f, F);
xue@1:   Multiply(L, K+2, dFtr, dFtr, (1-theta)/F);
xue@1:   MultiAdd(L, K+2, d, dFtr, dSrc, theta);
xue@1:   DeAlloc2(dSrc);
xue@1:   return DelFtr*(1-theta)/F+DelSrc*theta;
xue@1: }//P_Ax
xue@1: 
Chris@5: /**
xue@1:   function P_Ax_cf: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic
xue@1:   coefficient of time-unvarying filter coefficients x[K+2] towards totoal source variation.
xue@1: 
xue@1:   In: x[K+2]: filter coefficients
xue@1:       f[L][M]: partial frequencies
xue@1:       F: filter response sampling interval
xue@1:   Out: d[K+2]: Ax.
xue@1: 
xue@1:   Returns inner product of x[] and d[].
xue@1: */
xue@1: double P_Ax_cf(double* d, int L, int M, int K, double* x, double** f, double F)
xue@1: {
xue@1:   double xAx=0;
xue@1:   for (int l=0; l<L; l++) memset(d, 0, sizeof(double)*(K+2));
xue@1:   for (int l=0; l<L-1; l++)
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
xue@1:       if (f0f<=0 || f1f<=0) continue;
xue@1:       int k0=floor(f0f), k1=floor(f1f);
xue@1:       double f0=f0f-k0, f1=f1f-k1;
xue@1:       double g0=1-f0, g1=1-f1;
xue@1:       double A=-x[k1]*g1-x[k1+1]*f1+x[k0]*g0+x[k0+1]*f0;
xue@1:       d[k1]-=2*A*g1;
xue@1:       d[k1+1]-=2*A*f1;
xue@1:       d[k0]+=2*A*g0;
xue@1:       d[k0+1]+=2*A*f0;
xue@1:       xAx+=A*A;
xue@1:     }
xue@1:   return xAx;
xue@1: }//P_Ax_cf
xue@1: 
Chris@5: /**
xue@1:   function P1_b: internal procedure P1 of slow-variation SF estimator. See "further reading", pp.8.
xue@1: 
xue@1:   In: a[L][M], f[L][M]: partial amplitudes and frequencies
xue@1:       F: filter response sampling interval
xue@1:       theta: balancing factor of amplitude and frequency variations
xue@1:   Out: b[L][K+2]: linear coefficients of filter coefficients h[L][K+2] towards the total variation.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void P1_b(double** b, int L, int M, int K, double** a, double** f, double F, double theta)
xue@1: {
xue@1:   for (int l=0; l<L; l++) memset(b[l], 0, sizeof(double)*(K+2));
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
xue@1:       if (f0f<=0 || f1f<=0) continue;
xue@1:       int k0=floor(f0f), k1=floor(f1f);
xue@1:       double f0=f0f-k0, f1=f1f-k1;
xue@1:       double g0=1-f0, g1=1-f1;
xue@1:       double delta=a[l+1][m]-a[l][m];
xue@1:       b[l+1][k1]+=g1*delta, b[l+1][k1+1]+=f1*delta;
xue@1:       b[l][k0]-=g0*delta, b[l][k0+1]-=f0*delta;
xue@1:     }
xue@1:   }
xue@1:   Multiply(L, K+2, b, b, theta);
xue@1: }//P1_b
xue@1: 
Chris@5: /**
xue@1:   function P1_b_cf: internal procedure P1 of slow-variation SF estimator, constant-filter version.
xue@1: 
xue@1:   In: a[L][M], f[L][M]: partial amplitudes and frequencies
xue@1:       F: filter response sampling interval
xue@1:   Out: b[K+2]: linear coefficients of filter coefficients h[K+2] towards total source variation.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void P1_b_cf(double* b, int L, int M, int K, double** a, double** f, double F)
xue@1: {
xue@1:   memset(b, 0, sizeof(double)*(K+2));
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
xue@1:       if (f0f<=0 || f1f<=0) continue;
xue@1:       int k0=floor(f0f), k1=floor(f1f);
xue@1:       double f0=f0f-k0, f1=f1f-k1;
xue@1:       double g0=1-f0, g1=1-f1;
xue@1:       double delta=a[l+1][m]-a[l][m];
xue@1:       b[k1]+=g1*delta, b[k1+1]+=f1*delta;
xue@1:       b[k0]-=g0*delta, b[k0+1]-=f0*delta;
xue@1:     }
xue@1:   }
xue@1:   Multiply(K+2, b, b, 2);
xue@1: }//P1_b_cf
xue@1: 
Chris@5: /**
xue@1:   function P2_DelFtr: internal procedure P2 of slow-variation SF estimator. See "further reading", pp.8.
xue@1: 
xue@1:   In: x[L][K+2]: filter coefficients
xue@1:       F: filter response sampling interval
xue@1:   Out: d[L][K+2]: derivative of total filter variation against filter coefficients.
xue@1: 
xue@1:   Returns the inner product of x[][] and d[][].
xue@1: */
xue@1: double P2_DelFtr(double** d, int L, int K, double** x, double F)
xue@1: {
xue@1:   double xAx=0;
xue@1:   for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2));
xue@1:   for (int l=0; l<L-1; l++)
xue@1:     for (int k=0; k<K+1; k++)
xue@1:     {
xue@1:       try{
xue@1:       double A0=x[l+1][k]-x[l][k], A1=x[l+1][k+1]-x[l][k+1];
xue@1:       d[l+1][k]+=(2*A0+A1);
xue@1:       d[l][k]-=(2*A0+A1);
xue@1:       d[l+1][k+1]+=(2*A1+A0);
xue@1:       d[l][k+1]-=(2*A1+A0);
xue@1:       xAx+=A0*A0+A1*A1+A0*A1;
xue@1:       }
xue@1:       catch(...){}
xue@1:     }
xue@1:   Multiply(L, K+2, d, d, F/3);
xue@1:   return xAx*F/3;
xue@1: }//P2_DelFtr
xue@1: 
Chris@5: /**
xue@1:   function P3_DelSec: internal procedure P3 of slow-variation SF estimator. See "further reading", pp.9.
xue@1: 
xue@1:   In: x[L][K+2]: filter coefficients
xue@1:       f[L][M]: partial frequencies
xue@1:       F: filter response sampling interval
xue@1:   Out: d[L][K+2]: derivative of total source variation against filter coefficients.
xue@1: 
xue@1:   Returns the inner product of x[][] and d[][].
xue@1: */
xue@1: double P3_DelSrc(double** d, int L, int M, int K, double** x, double** f, double F)
xue@1: {
xue@1:   double xAx=0;
xue@1:   for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2));
xue@1:   for (int l=0; l<L-1; l++)
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
xue@1:       if (f0f<=0 || f1f<=0) continue;
xue@1:       int k0=floor(f0f), k1=floor(f1f);
xue@1:       double f0=f0f-k0, f1=f1f-k1;
xue@1:       double g0=1-f0, g1=1-f1;
xue@1:       double A=-x[l+1][k1]*g1-x[l+1][k1+1]*f1+x[l][k0]*g0+x[l][k0+1]*f0;
xue@1:       d[l+1][k1]-=2*A*g1;
xue@1:       d[l+1][k1+1]-=2*A*f1;
xue@1:       d[l][k0]+=2*A*g0;
xue@1:       d[l][k0+1]+=2*A*f0;
xue@1:       xAx+=A*A;
xue@1:     }
xue@1:   return xAx;
xue@1: }//P3_DelSec
xue@1: 
Chris@5: /**
xue@1:   function S_F_b: computes source coefficients given amplitudes and filter coefficients
xue@1: 
xue@1:   In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
xue@1:       lp[P]: temporal segmentation points, in frames.
xue@1:       logA0C[Fr]: amplitude carrier
xue@1:       h[L][K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]).
xue@1:       F: filter response sampling interval
xue@1:       FScaleMode: set if mel scale is used as frequency scale
xue@1:       Fs: sampling frequency, effective only when FScaleMode is set.
xue@1:   Out: b[L][M]: source coefficients for L frames starting from ceil(lp[0]).
xue@1:        avgb[L]: average source coefficents for these L frames
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void S_F_b(int M, atom** Partials, double* logA0C, double* lp, int P, int K, double** h, double** b, double* avgb, double F, int FScaleMode, double Fs)
xue@1: {
xue@1:   int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1:   int L=lpp-lp0;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     int fr=lp0+l;
xue@1:     double loga0=logA0C[fr];
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double f=Partials[m][fr].f;
xue@1:       if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
xue@1:       if (f>0)
xue@1:       {
xue@1:         double loga=log(Partials[m][fr].a);
xue@1:         loga-=loga0;
xue@1:         double ff=f/F;
xue@1:         int klm=floor(ff);
xue@1:         double f0=ff-klm;
xue@1:         double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
xue@1:         b[l][m]=loga-hlm;
xue@1:       }
xue@1:       else b[l][m]=-100;
xue@1:     }
xue@1:   }
xue@1:   //	for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
xue@1:   for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}
xue@1: }//S_F_b
xue@1: 
Chris@5: /**
xue@1:   function S_F_b: wrapper function
xue@1: 
xue@1:   In: Partials: HS partials
xue@1:       SF: TSF object containing valid filter coefficients
xue@1:   Out: SF: TSF object with source coefficients updated
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void S_F_b(TSF& SF, atom** Partials)
xue@1: {
xue@1:   S_F_b(SF.M, Partials, useA0?SF.logA0:SF.logA0C, SF.lp, SF.P, SF.K, SF.h, SF.b, SF.avgb, SF.F, SF.FScaleMode, SF.Fs);
xue@1: }//S_F_b
xue@1: 
Chris@5: /**
xue@1:   function S_F_b: filter-bank method for estimating source-filter model. This is a wrapper function of
xue@1:   SF_FB() that transfers and scales values, etc.
xue@1: 
xue@1:   In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
xue@1:       lp[P]: temporal segmentation points, in frames.
xue@1:       logA0C[Fr]: amplitude carrier
xue@1:       F: filter response sampling interval
xue@1:       FScaleMode: set if mel scale is used as frequency scale
xue@1:       Fs: sampling frequency, effective only when FScaleMode is set.
xue@1:   Out: K
xue@1:        h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1:        b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1: 
xue@1:   Returns 0.
xue@1: */
xue@1: double S_F_FB(int M, atom** Partials, double* logA0C, double* lp, int P, int& K, double** h, double* avgh, double** b, double* avgb, double F, int FScaleMode, double Fs)
xue@1: {
xue@1:   int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1:   int L=lpp-lp0;
xue@1:   Alloc2(L, M, a); Alloc2(L, M, f);
xue@1:   double fmax=0;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     int fr=lp0+l;
xue@1:     double ia0c=exp(-logA0C[fr]);
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       f[l][m]=Partials[m][fr].f;
xue@1:       if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700);
xue@1:       if (f[l][m]>0) a[l][m]=Partials[m][fr].a*ia0c; //log(Partials[m][fr].a);
xue@1:       else a[l][m]=0;
xue@1:       if (fmax<f[l][m]) fmax=f[l][m];
xue@1:     }
xue@1:   }
xue@1:   K=floor(fmax/F);
xue@1: 
xue@1: 
xue@1:   SF_FB(h[0], M, K, &a[0], &f[0], F, 1);
xue@1:   for (int l=1; l<L-1; l++) SF_FB(h[l], M, K, &a[l], &f[l], F, 0);
xue@1:   SF_FB(h[L-1], M, K, &a[L-1], &f[L-1], F, -1);
xue@1: 
xue@1:   for (int l=0; l<L; l++)
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double ff=f[l][m]/F;
xue@1:       if (ff<=0) b[l][m]=-100;
xue@1:       else
xue@1:       {
xue@1:         int klm=floor(ff);
xue@1:         double f0=ff-klm;
xue@1:         double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
xue@1:         b[l][m]=log(a[l][m])-hlm;
xue@1:       }
xue@1:     }
xue@1: 
xue@1:   for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
xue@1:   for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}
xue@1: 
xue@1:   DeAlloc2(a); DeAlloc2(f);
xue@1:   return 0;
xue@1: }//S_F_b
xue@1: 
Chris@5: /**
xue@1:   function S_F_SV: slow-variation method for estimating source-filter model. This is a wrapper function
xue@1:   of SF_SV() that transfers and scales values, etc.
xue@1: 
xue@1:   In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
xue@1:       lp[P]: temporal segmentation points, in frames.
xue@1:       logA0C[Fr]: amplitude carrier
xue@1:       theta: balancing factor of amplitude and frequency variations
xue@1:       F: filter response sampling interval
xue@1:       FScaleMode: set if mel scale is used as frequency scale
xue@1:       Fs: sampling frequency, effective only when FScaleMode is set.
xue@1:   Out: K
xue@1:        h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1:        b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1:   Returns 0.
xue@1: */
xue@1: double S_F_SV(int M, atom** Partials, double* logA0C, double* lp, int P, int& K, double** h, double* avgh, double** b, double* avgb, double F, int FScaleMode, double theta, double Fs)
xue@1: {
xue@1:   int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1:   int L=lpp-lp0;
xue@1:   Alloc2(L, M, loga); Alloc2(L, M, f);
xue@1:   double fmax=0;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     int fr=lp0+l;
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       f[l][m]=Partials[m][fr].f;
xue@1:       if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700);
xue@1:       if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a);
xue@1:       else loga[l][m]=-100;
xue@1:       if (fmax<f[l][m]) fmax=f[l][m];
xue@1:     }
xue@1:   }
xue@1:   K=floor(fmax/F);
xue@1: 
xue@1:   for (int l=0; l<L; l++){double loga0=logA0C[lp0+l];	for (int m=0; m<M; m++) loga[l][m]-=loga0;}
xue@1: 
xue@1:   SF_SV(h, L, M, K, loga, f, F, theta, 1e-6, L*(K+2));
xue@1: 
xue@1:   for (int l=0; l<L; l++)
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       double ff=f[l][m]/F;
xue@1:       if (ff<=0) continue;
xue@1:       int klm=floor(ff);
xue@1:       double f0=ff-klm;
xue@1:       double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
xue@1:       b[l][m]=loga[l][m]-hlm;
xue@1:     }
xue@1: 
xue@1:   for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
xue@1:   for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}
xue@1: 
xue@1:   DeAlloc2(loga); DeAlloc2(f);
xue@1:   return 0;
xue@1: }//S_F_SV
xue@1: 
Chris@5: /**
xue@1:   function S_F_SV_cf: slow-variation method for estimating source-(constant)filter model. This is a
xue@1:   wrapper function of SF_SV_cf() that transfers and scales values, as well as converting results to
xue@1:   source filter format used by the vibrato class TVo.
xue@1: 
xue@1:   In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
xue@1:       lp[P]: temporal segmentation points, in frames.
xue@1:       A0C[Fr]: amplitude carrier (linear, not dB)
xue@1:       F: filter response sampling interval
xue@1:       FScaleMode: set if mel scale is used as frequency scale
xue@1:       Fs: sampling frequency, effective only when FScaleMode is set.
xue@1:   Out: K
xue@1:        h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1:        b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
xue@1:        LogAF[afres/2]: TVo-format filter response
xue@1:        LogAS[M]: TVo-format source response
xue@1: 
xue@1:   Returns 0.
xue@1: */
xue@1: double S_F_SV_cf(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, int& K, double** h, double** b, double F, int FScaleMode, double Fs)
xue@1: {
xue@1:   int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
xue@1:   int L=lpp-lp0;
xue@1:   Alloc2(L, M, loga); Alloc2(L, M, f);
xue@1:   double fmax=0;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     int fr=lp0+l;
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       try{
xue@1:       f[l][m]=Partials[m][fr].f;
xue@1:       }
xue@1:       catch(...)
xue@1:       {
xue@1:         m=m;
xue@1:       }
xue@1:       if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700);
xue@1:       if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a);
xue@1:       else loga[l][m]=-100;
xue@1:       if (fmax<f[l][m]) fmax=f[l][m];
xue@1:     }
xue@1:   }
xue@1:   K=floor(fmax/F);
xue@1: 
xue@1:   SF_SV_cf(h[0], b, L, M, K, loga, f, F, 1e-6, K+2);
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     if (l>0) memcpy(h[l], h[0], sizeof(double)*(K+2));
xue@1:     double loga0=log(A0C[l]);
xue@1:     for (int m=0; m<M; m++) b[l][m]-=loga0;
xue@1:   }
xue@1: 
xue@1:   double* lh=new double[K+2]; memset(lh, 0, sizeof(double)*(K+2));
xue@1:   for (int k=0; k<K+2; k++)
xue@1:   {
xue@1:     for (int l=0; l<L; l++) lh[k]+=h[l][k];
xue@1:     lh[k]/=L;
xue@1:   }
xue@1:   for (int i=0; i<afres/2; i++)
xue@1:   {
xue@1:     double freq=i*1.0/afres;
xue@1:     if (FScaleMode) freq=log(1+freq*Fs/700)/log(1+Fs/700);
xue@1:     int k=floor(freq/F);
xue@1:     double f_plus=freq/F-k;
xue@1:     if (k<K+1) LogAF[i]=lh[k]*(1-f_plus)+lh[k+1]*f_plus;
xue@1:     else LogAF[i]=lh[K+1];
xue@1:   }
xue@1:   delete[] lh;
xue@1: 
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     int ccount=0;
xue@1:     for (int fr=lp0; fr<lpp; fr++)
xue@1:     {
xue@1:       double f=Partials[m][fr].f*afres;
xue@1:       if (f<=0) continue;
xue@1:       int intf=floor(f);
xue@1:       LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf));
xue@1:       ccount++;
xue@1:     }
xue@1:     if (ccount>0) LogAS[m]/=ccount;
xue@1:     else LogAS[m]=0;
xue@1:   }
xue@1: 
xue@1:   DeAlloc2(loga); DeAlloc2(f);
xue@1:   return 0;
xue@1: }//S_F_SV_cf
xue@1: 
Chris@5: /**
xue@1:   function SF_FB: computes filter coefficients with filter-bank method: single frame. See "further
xue@1:   reading", pp.12, P5.
xue@1: 
xue@1:   In: al[][M], fl[][M]: partial amplitudes and frequencies, al[0] and fl[0] are of current frame
xue@1:       F: filter response sampling interval
xue@1:       LMode: 1: current frame is the first frame; -1: the last frame; 0: neither first nor last frame
xue@1:   Out: hl[K+2]: filter coefficients integrated at current frame
xue@1: 
xue@1:   Returns K.
xue@1: */
xue@1: int SF_FB(double* hl, int M, int K, double** al, double** fl, double F, int LMode)
xue@1: {
xue@1:   memset(hl, 0, sizeof(double)*(K+2));
xue@1:   double* norm=new double[K+2]; memset(norm, 0, sizeof(double)*(K+2));
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     int dfsgn, k, K1;
xue@1:     if (LMode!=1)
xue@1:     {
xue@1:       if (fl[0][m]-fl[-1][m]==0)
xue@1:       {
xue@1:         double a0, wk, wk_, a1;
xue@1:         int k=floor(fl[0][m]/F);
xue@1:         a0=al[-1][m], a1=al[0][m];
xue@1:         wk=1-(fl[0][m]/F-k), wk_=1-wk;
xue@1:         double dh=(a0+2*a1)/6.0;
xue@1:         hl[k]+=wk*dh;   //I3(1, wk, 0, a0, wk, 1, a1);
xue@1:         hl[k+1]+=wk_*dh;//I3(1, wk_, 0, a0, wk_, 1, a1);
xue@1:         norm[k]+=wk*0.5;//I3(1, wk, 0, 1, wk, 1, 1);
xue@1:         norm[k+1]+=wk_*0.5;//I3(1, wk_, 0, 1, wk_, 1, 1);
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_;
xue@1:         dfsgn=Sign(fl[0][m]-fl[-1][m]);
xue@1:         if (dfsgn>0) K1=ceil(fl[-1][m]/F); else K1=floor(fl[-1][m]/F);
xue@1:         t0=-1; k=K1;
xue@1:         if (fl[-1][m]>0 && fl[0][m]>0) do
xue@1:         {
xue@1:           t1=-1+(k*F-fl[-1][m])/(fl[0][m]-fl[-1][m]);
xue@1:           if (t1>0) t1=0;
xue@1: 
xue@1:           if (t0==-1){a0=al[-1][m],	u0=0,	w0k=1-fabs(fl[-1][m]/F-k); w0k_=1-w0k;}
xue@1:           else{a0=al[0][m]+t0*(al[0][m]-al[-1][m]),	u0=1+t0, w0k=0, w0k_=1;}
xue@1:           if (t1==0){a1=al[0][m], u1=1, w1k=1-fabs(fl[0][m]/F-k); w1k_=1-w1k;}
xue@1:           else{a1=al[0][m]+t1*(al[0][m]-al[-1][m]), u1=1+t1, w1k=1, w1k_=0;}
xue@1: 
xue@1:           hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1);
xue@1:           hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1);
xue@1:           norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1);
xue@1:           norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1);
xue@1: 
xue@1:           t0=t1;
xue@1:           k+=dfsgn;
xue@1:         }
xue@1:         while (t1<0);
xue@1:       }
xue@1:     }
xue@1: 
xue@1:     if (LMode!=-1)
xue@1:     {
xue@1:       double a0, wk, wk_, a1;
xue@1:       if (fl[1][m]-fl[0][m]==0)
xue@1:       {
xue@1:         int k=floor(fl[0][m]/F);
xue@1:         a0=al[0][m], a1=al[1][m];
xue@1:         wk=1-fabs(fl[0][m]/F-k), wk_=1-wk;
xue@1:         double dh=(2*a0+a1)/6.0;
xue@1:         hl[k]+=wk*dh;   //I3(1, wk, 1, a0, wk, 0, a1);
xue@1:         hl[k+1]+=wk_*dh;//I3(1, wk_, 1, a0, wk_, 0, a1);
xue@1:         norm[k]+=wk*0.5;//I3(1, wk, 1, 1, wk, 0, 1);
xue@1:         norm[k+1]+=wk_*0.5;//I3(1, wk_, 1, 1, wk_, 0, 1);
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_;
xue@1:         dfsgn=Sign(fl[1][m]-fl[0][m]);
xue@1:         if (dfsgn>0) K1=ceil(fl[0][m]/F);	else K1=floor(fl[0][m]/F);
xue@1:         t0=0; k=K1;
xue@1:         if (fl[0][m]>0 && fl[1][m]>0)	do
xue@1:         {
xue@1:           try{
xue@1:           t1=(k*F-fl[0][m])/(fl[1][m]-fl[0][m]);}
xue@1:           catch(...){}
xue@1:           if (t1>1) t1=1;
xue@1: 
xue@1:           if (t0==0){a0=al[0][m], u0=1, w0k=1-fabs(fl[0][m]/F-k); w0k_=1-w0k;}
xue@1:           else{a0=al[0][m]+t0*(al[1][m]-al[0][m]), u0=1-t0, w0k=1, w0k_=0;}
xue@1:           if (t1==1){a1=al[1][m], u1=0, w1k=1-fabs(fl[1][m]/F-k); w1k_=1-w1k;}
xue@1:           else{a1=al[0][m]+t1*(al[1][m]-al[0][m]), u1=1-t1, w1k=0, w1k_=1;}
xue@1: 
xue@1:           hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1);
xue@1:           hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1);
xue@1:           norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1);
xue@1:           norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1);
xue@1: 
xue@1:           t0=t1;
xue@1:           k+=dfsgn;
xue@1:         }
xue@1:         while (t1<1);
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   int mink=0; while (mink<=K+1 && norm[mink]==0) mink++;
xue@1:   int maxk=K+1; while (maxk>=0 && norm[maxk]==0) maxk--;
xue@1:   int lastk=mink, nextk=-1;
xue@1:   for (int k=mink; k<=maxk; k++)
xue@1:   {
xue@1:     if (k==nextk) lastk=k;
xue@1:     else if (norm[k]!=0) hl[k]=log(hl[k]/norm[k]), lastk=k;
xue@1:     else
xue@1:     {
xue@1:       if (k>nextk){nextk=k+1; while (norm[nextk]==0) nextk++; hl[nextk]=log(hl[nextk]/norm[nextk]);}
xue@1:       hl[k]=(hl[lastk]*(nextk-k)+hl[nextk]*(k-lastk))/(nextk-lastk);
xue@1:     }
xue@1:   }
xue@1:   for (int k=0; k<mink; k++) hl[k]=hl[mink];
xue@1:   for (int k=maxk+1; k<K+2; k++) hl[k]=hl[maxk];
xue@1: 
xue@1:   delete[] norm;
xue@1:   return K;
xue@1: }//SF_FB
xue@1: 
Chris@5: /**
xue@1:   function SF_SV: CG method for estimation source-filter model using slow-variation criterion. See
xue@1:   "further reading", pp.9, boxed algorithm P4.
xue@1: 
xue@1:   In: a[L][M], f[L][M]: partial amplitudes and frequencies
xue@1:       F: filter response sampling interval
xue@1:       theta: balancing factor of amplitude and frequency variations
xue@1:       ep: convergence test threshold
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: h[L][K+2]: filter coefficients
xue@1: 
xue@1:   Returns K.
xue@1: */
xue@1: int SF_SV(double** h, int L, int M, int K, double** a, double** f, double F, double theta, double ep, int maxiter)
xue@1: {
xue@1:   Alloc2(L*7, K+2, b);
xue@1:   double **Ax=&b[L], **d=&Ax[L], **r=&d[L], **q=&r[L];
xue@1:   for (int l=0; l<L; l++) memset(h[l], 0, sizeof(double)*(K+2));
xue@1:   //calcualte b
xue@1:   P1_b(b, L, M, K, a, f, F, theta);
xue@1:   //calculate Ah
xue@1:   //double hAh=
xue@1:   P_Ax(Ax, L, M, K, h, f, F, theta);
xue@1:   //double **t_Ax=&b[L*5], **t_h=&b[L*6], epdel=1e-10, t_err=0;
xue@1: 
xue@1:   //double bh=Inner(L, K+2, b, h);  //debug
xue@1:   //double Del=0.5*hAh-bh;          //debug
xue@1: 
xue@1:   MultiAdd(L, K+2, r, b, Ax, -1);
xue@1:   Copy(L, K+2, d, r);
xue@1:   double delta=Inner(L, K+2, r, r);
xue@1:   ep=ep*delta;
xue@1: 
xue@1:   int iter=0;
xue@1:   while(iter<maxiter && delta>ep)
xue@1:   {
xue@1:     P_Ax(q, L, M, K, d, f, F, theta);
xue@1:     /*debug point: watch if hAh-bh keeps decreasing
xue@1:     double lastdel=Del;
xue@1:     hAh=P_Ax(t_Ax, L, M, K, h, f, F, theta);
xue@1:     bh=Inner(L, K+2, b, h);
xue@1:     Del=hAh-bh;
xue@1:     if (Del>lastdel)
xue@1:     {lastdel=Del;} //set breakpoint here*/
xue@1:     double alpha=delta/Inner(L, K+2, d, q);
xue@1:     MultiAdd(L, K+2, h, h, d, alpha);
xue@1:     if (iter%50==0 && false){P_Ax(Ax, L, M, K, h, f, F, theta); MultiAdd(L, K+2, r, b, Ax, -1);}
xue@1:     else MultiAdd(L, K+2, r, r, q, -alpha);
xue@1:     double deltanew=Inner(L, K+2, r, r);
xue@1:     MultiAdd(L, K+2, d, r, d, deltanew/delta);
xue@1:     delta=deltanew;
xue@1:     iter++;
xue@1:   }
xue@1: 
xue@1:   DeAlloc2(b);
xue@1:   return K;
xue@1: }//SF_SV
xue@1: 
Chris@5: /**
xue@1:   function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion.
xue@1: 
xue@1:   In: a[L][M], f[L][M]: partial amplitudes and frequencies
xue@1:       F: filter response sampling interval
xue@1:       ep: convergence test threshold
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: h[K+2]: filter coefficients
xue@1: 
xue@1:   Returns K.
xue@1: */
xue@1: int SF_SV_cf(double* h, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter)
xue@1: {
xue@1:   double* b=new double[(K+2)*7];
xue@1:   double *Ax=&b[K+2], *d=&Ax[K+2], *r=&d[K+2], *q=&r[K+2];
xue@1: 
xue@1:   //calcualte b
xue@1:   P1_b_cf(b, L, M, K, a, f, F);
xue@1:   //calculate Ah
xue@1:   //double hAh=
xue@1:   P_Ax_cf(Ax, L, M, K, h, f, F);
xue@1: 
xue@1:   MultiAdd(K+2, r, b, Ax, -1);
xue@1:   Copy(K+2, d, r);
xue@1:   double delta=Inner(K+2, r, r);
xue@1:   ep=ep*delta;
xue@1: 
xue@1:   int iter=0;
xue@1:   while(iter<maxiter && delta>ep)
xue@1:   {
xue@1:     P_Ax_cf(q, L, M, K, d, f, F);
xue@1:     double alpha=delta/Inner(K+2, d, q);
xue@1:     MultiAdd(K+2, h, h, d, alpha);
xue@1:     if (iter%50==0 && false){P_Ax_cf(Ax, L, M, K, h, f, F); MultiAdd(K+2, r, b, Ax, -1);}
xue@1:     else MultiAdd(K+2, r, r, q, -alpha);
xue@1:     double deltanew=Inner(K+2, r, r);
xue@1:     MultiAdd(K+2, d, r, d, deltanew/delta);
xue@1:     delta=deltanew;
xue@1:     iter++;
xue@1:   }
xue@1: 
xue@1:   delete[] b;
xue@1:   return K;
xue@1: }//SF_SV_cf
xue@1: 
Chris@5: /**
xue@1:   function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion.
xue@1: 
xue@1:   In: a[L][M], f[L][M]: partial amplitudes and frequencies
xue@1:       F: filter response sampling interval
xue@1:       ep: convergence test threshold
xue@1:       maxiter: maximal number of iterates
xue@1:   Out: h[K+2]: filter coefficients
xue@1:        b[L][M]: source coefficients
xue@1: 
xue@1:   No reutrn value.
xue@1: */
xue@1: void SF_SV_cf(double* h, double** b, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter)
xue@1: {
xue@1:   memset(h, 0, sizeof(double)*(K+2));
xue@1:   SF_SV_cf(h, L, M, K, a, f, F, ep, maxiter);
xue@1:   for (int l=0; l<L; l++)
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     double ff=f[l][m]/F;
xue@1:     if (ff<=0) continue;
xue@1:     int klm=floor(ff);
xue@1:     double f0=ff-klm;
xue@1:     double hlm=h[klm]*(1-f0)+h[klm+1]*f0;
xue@1:     b[l][m]=a[l][m]-hlm;
xue@1:   }
xue@1: }//SF_SV_cf
xue@1: 
Chris@5: /**
xue@1:   function Sign: sign function
xue@1: 
xue@1:   In: x: a floating-point number
xue@1: 
xue@1:   Returns 0 if x=0, 1 if x>0, -1 if x<0.
xue@1: */
xue@1: int Sign(double x)
xue@1: {
xue@1:   if (x==0) return 0;
xue@1:   else if (x>0) return 1;
xue@1:   else return -1;
xue@1: }//Sign
xue@1: 
Chris@5: /**
xue@1:   function SynthesizeSF: synthesizes a harmonic sinusoid representation from a TSF object.
xue@1: 
xue@1:   In: SF: a TSF object
xue@1:       sps: sampling rate ("samples per second")
xue@1:   Out: HS: teh synthesized HS.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void SynthesizeSF(THS* HS, TSF* SF, double sps)
xue@1: {
xue@1:   int t0=HS->Partials[0][0].t, s0=HS->Partials[0][0].s, L=SF->L, M=SF->M;
xue@1:   double *F0C=SF->F0C, *F0D=SF->F0D, *F0=SF->F0, offst=SF->offst;
xue@1:   double *logA0C=useA0?SF->logA0:SF->logA0C;
xue@1:   HS->Resize(M, L);
xue@1: 
xue@1:   atom** Partials=HS->Partials;
xue@1: 
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     double tl=t0+offst*l;
xue@1:     F0[l]=F0C[l]+F0D[l];
xue@1:     for (int m=0; m<M; m++)
xue@1:     {
xue@1:       Partials[m][l].s=s0;
xue@1:       Partials[m][l].t=tl;
xue@1:       Partials[m][l].f=(m+1)*F0[l];
xue@1: 
xue@1:       if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].a=0;
xue@1:       else Partials[m][l].a=exp(logA0C[l]+SF->LogAS(m,l)+SF->LogAF(Partials[m][l].f,l));
xue@1:     }
xue@1:   }
xue@1: }//SynthesizeSF
xue@1: 
xue@1: 
xue@1: 
xue@1: