Mercurial > hg > x
changeset 1:6422640a802f
first upload
| author | Wen X <xue.wen@elec.qmul.ac.uk> |
|---|---|
| date | Tue, 05 Oct 2010 10:45:57 +0100 |
| parents | 9b9f21935f24 |
| children | fc19d45615d1 |
| files | Matrix.cpp Matrix.h QuickSpec.cpp QuickSpec.h SinEst.cpp SinEst.h SinSyn.cpp SinSyn.h TStream.h WindowFunctions.cpp WindowFunctions.h align8.cpp align8.h arrayalloc.h fft.cpp fft.h hs.cpp hs.h hsedit.cpp hsedit.h hssf.cpp hssf.h multires.cpp multires.h opt.cpp opt.h procedures.cpp procedures.h splines.cpp splines.h vibrato.cpp vibrato.h wavelet.cpp wavelet.h xcomplex.h |
| diffstat | 35 files changed, 24844 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,2245 @@ +//--------------------------------------------------------------------------- +#include <math.h> +#include <memory.h> +#include "Matrix.h" +//--------------------------------------------------------------------------- +/* + function BalanceSim: applies a similarity transformation to matrix a so that a is "balanced". This is + used by various eigenvalue evaluation routines. + + In: matrix A[n][n] + Out: balanced matrix a + + No return value. +*/ +void BalanceSim(int n, double** A) +{ + if (n<2) return; + const int radix=2; + double sqrdx; + sqrdx=radix*radix; + bool finish=false; + while (!finish) + { + finish=true; + for (int i=0; i<n; i++) + { + double s, sr=0, sc=0, ar, ac; + for (int j=0; j<n; j++) + if (j!=i) + { + sc+=fabs(A[j][i]); + sr+=fabs(A[i][j]); + } + if (sc!=0 && sr!=0) + { + ar=sr/radix; + ac=1.0; + s=sr+sc; + while (sc<ar) + { + ac*=radix; + sc*=sqrdx; + } + ar=sr*radix; + while (sc>ar) + { + ac/=radix; + sc/=sqrdx; + } + } + if ((sc+sr)/ac<0.95*s) + { + finish=false; + ar=1.0/ac; + for (int j=0; j<n; j++) A[i][j]*=ar; + for (int j=0; j<n; j++) A[j][i]*=ac; + } + } + } +}//BalanceSim + +//--------------------------------------------------------------------------- +/* + function Choleski: Choleski factorization A=LL', where L is lower triangular. The symmetric matrix + A[N][N] is positive definite iff A can be factored as LL', where L is lower triangular with nonzero + diagonl entries. + + In: matrix A[N][N] + Out: mstrix L[N][N]. + + Returns 0 if successful. On return content of matrix a is not changed. +*/ +int Choleski(int N, double** L, double** A) +{ + if (A[0][0]==0) return 1; + L[0][0]=sqrt(A[0][0]); + memset(&L[0][1], 0, sizeof(double)*(N-1)); + for (int j=1; j<N; j++) L[j][0]=A[j][0]/L[0][0]; + for (int i=1; i<N-1; i++) + { + L[i][i]=A[i][i]; for (int k=0; k<i; k++) L[i][i]-=L[i][k]*L[i][k]; L[i][i]=sqrt(L[i][i]); + if (L[i][i]==0) return 1; + for (int j=i+1; j<N; j++) + { + L[j][i]=A[j][i]; for (int k=0; k<i; k++) L[j][i]-=L[j][k]*L[i][k]; L[j][i]/=L[i][i]; + } + memset(&L[i][i+1], 0, sizeof(double)*(N-1-i)); + } + L[N-1][N-1]=A[N-1][N-1]; for (int k=0; k<N-1; k++) L[N-1][N-1]-=L[N-1][k]*L[N-1][k]; L[N-1][N-1]=sqrt(L[N-1][N-1]); + return 0; +}//Choleski + +//--------------------------------------------------------------------------- +//matrix duplication routines + +/* + function Copy: duplicate the matrix A as matrix Z. + + In: matrix A[M][N] + Out: matrix Z[M][N] + + Returns pointer to Z. Z is created anew if Z=0 is supplied on start. +*/ +double** Copy(int M, int N, double** Z, double** A, MList* List) +{ + if (!Z) {Allocate2(double, M, N, Z); if (List) List->Add(Z, 2);} + int sizeN=sizeof(double)*N; + for (int m=0; m<M; m++) memcpy(Z[m], A[m], sizeN); + return Z; +}//Copy +//complex version +cdouble** Copy(int M, int N, cdouble** Z, cdouble** A, MList* List) +{ + if (!Z) {Allocate2(cdouble, M, N, Z); if (List) List->Add(Z, 2);} + int sizeN=sizeof(cdouble)*N; + for (int m=0; m<M; m++) memcpy(Z[m], A[m], sizeN); + return Z; +}//Copy +//version without specifying pre-allocated z +double** Copy(int M, int N, double** A, MList* List){return Copy(M, N, 0, A, List);} +cdouble** Copy(int M, int N, cdouble** A, MList* List){return Copy(M, N, 0, A, List);} +//for square matrices +double** Copy(int N, double** Z, double ** A, MList* List){return Copy(N, N, Z, A, List);} +double** Copy(int N, double** A, MList* List){return Copy(N, N, 0, A, List);} +cdouble** Copy(int N, cdouble** Z, cdouble** A, MList* List){return Copy(N, N, Z, A, List);} +cdouble** Copy(int N, cdouble** A, MList* List){return Copy(N, N, 0, A, List);} + +//--------------------------------------------------------------------------- +//vector duplication routines + +/* + function Copy: duplicating vector a as vector z + + In: vector a[N] + Out: vector z[N] + + Returns pointer to z. z is created anew is z=0 is specified on start. +*/ +double* Copy(int N, double* z, double* a, MList* List) +{ + if (!z){z=new double[N]; if (List) List->Add(z, 1);} + memcpy(z, a, sizeof(double)*N); + return z; +}//Copy +cdouble* Copy(int N, cdouble* z, cdouble* a, MList* List) +{ + if (!z){z=new cdouble[N]; if (List) List->Add(z, 1);} + memcpy(z, a, sizeof(cdouble)*N); + return z; +}//Copy +//version without specifying pre-allocated z +double* Copy(int N, double* a, MList* List){return Copy(N, 0, a, List);} +cdouble* Copy(int N, cdouble* a, MList* List){return Copy(N, 0, a, List);} + +//--------------------------------------------------------------------------- +/* + function det: computes determinant by Gaussian elimination method with column pivoting + + In: matrix A[N][N] + + Returns det(A). On return content of matrix A is unchanged if mode=0. +*/ +double det(int N, double** A, int mode) +{ + int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + double m, **b, result=1; + + if (mode==0) + { + int sizeN=sizeof(double)*N; + b=new double*[N]; b[0]=new double[N*N]; for (int i=0; i<N; i++) {b[i]=&b[0][i*N]; memcpy(b[i], A[i], sizeN);} + A=b; + } + + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (fabs(A[rp[ip]][i])>fabs(A[rp[p]][i])) p=ip; ip++;} + if (A[rp[p]][i]==0) {result=0; goto ret;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;} + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=0; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + } + } + if (A[rp[N-1]][N-1]==0) {result=0; goto ret;} + + for (int i=0; i<N; i++) + result*=A[rp[i]][i]; + +ret: + if (mode==0) {delete[] b[0]; delete[] b;} + delete[] rp; + return result; +}//det +//complex version +cdouble det(int N, cdouble** A, int mode) +{ + int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + double mm, mp; + cdouble m, **b, result=1; + + if (mode==0) + { + int sizeN=sizeof(cdouble)*N; + b=new cdouble*[N]; b[0]=new cdouble[N*N]; + for (int i=0; i<N; i++) {b[i]=&b[0][i*N]; memcpy(b[i], A[i], sizeN);} + A=b; + } + + //Gaussian elimination + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; m=A[rp[p]][i]; mp=~m; + while (ip<N){m=A[rp[ip]][i]; mm=~m; if (mm>mp) mp=mm, p=ip; ip++;} + if (mp==0) {result=0; goto ret;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c;} + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=0; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + } + } + if (operator==(A[rp[N-1]][N-1],0)) {result=0; goto ret;} + + for (int i=0; i<N; i++) result=result*A[rp[i]][i]; +ret: + if (mode==0) {delete[] b[0]; delete[] b;} + delete[] rp; + return result; +}//det + +//--------------------------------------------------------------------------- +/* + function EigPower: power method for solving dominant eigenvalue and eigenvector + + In: matrix A[N][N], initial arbitrary vector x[N]. + Out: eigenvalue l, eigenvector x[N]. + + Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero. +*/ +int EigPower(int N, double& l, double* x, double** A, double ep, int maxiter) +{ + int k=0; + int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i; + Multiply(N, x, x, 1/x[p]); + double e, ty,te, *y=new double[N]; + + while (k<maxiter) + { + MultiplyXy(N, N, y, A, x); + l=y[p]; + int p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i; + if (y[p]==0) {l=0; delete[] y; return 0;} + ty=y[0]/y[p]; e=fabs(x[0]-ty); x[0]=ty; + for (int i=1; i<N; i++) + { + ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty; + } + if (e<ep) {delete[] y; return 0;} + k++; + } + delete[] y; return 1; +}//EigPower + +//--------------------------------------------------------------------------- +/* + function EigPowerA: EigPower with Aitken acceleration + + In: matrix A[N][N], initial arbitrary vector x[N]. + Out: eigenvalue l, eigenvector x[N]. + + Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero. +*/ +int EigPowerA(int N, double& l, double* x, double** A, double ep, int maxiter) +{ + int k=0; + int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i; + Multiply(N, x, x, 1/x[p]); + double m, m0=0, m1=0, e, ty,te, *y=new double[N]; + + while (k<maxiter) + { + MultiplyXy(N, N, y, A, x); + m=y[p]; + int p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i; + if (y[p]==0) {l=0; delete[] y; return 0;} + ty=y[0]/y[p]; e=fabs(x[0]-ty); x[0]=ty; + for (int i=1; i<N; i++) + { + ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty; + } + if (e<ep && k>2) {l=m0-(m1-m0)*(m1-m0)/(m-2*m1+m0); delete[] y; return 0;} + k++; m0=m1; m1=m; + } + delete[] y; return 1; +}//EigPowerA + +//--------------------------------------------------------------------------- +/* + function EigPowerI: Inverse power method for solving the eigenvalue given an approximate non-zero + eigenvector. + + In: matrix A[N][N], approximate eigenvector x[N]. + Out: eigenvalue l, eigenvector x[N]. + + Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero. +*/ +int EigPowerI(int N, double& l, double* x, double** A, double ep, int maxiter) +{ + int sizeN=sizeof(double)*N; + double* y=new double[N]; MultiplyXy(N, N, y, A, x); + double q=Inner(N, x, y)/Inner(N, x, x), dt; + double** aa=new double*[N]; aa[0]=new double[N*N]; + for (int i=0; i<N; i++) {aa[i]=&aa[0][i*N]; memcpy(aa[i], A[i], sizeN); aa[i][i]-=q;} + dt=GISCP(N, aa); + if (dt==0) {l=q; delete[] aa[0]; delete[] aa; delete[] y; return 0;} + + int k=0; + int p=0; for (int i=1; i<N; i++) if (fabs(x[p])<fabs(x[i])) p=i; + Multiply(N, x, x, 1/x[p]); + + double m, e, ty, te; + while (k<N) + { + MultiplyXy(N, N, y, aa, x); + m=y[p]; + p=0; for (int i=1; i<N; i++) if (fabs(y[p])<fabs(y[i])) p=i; + ty=y[0]/y[p]; te=x[0]-ty; e=fabs(te); x[0]=ty; + for (int i=1; i<N; i++) + { + ty=y[i]/y[p]; te=fabs(x[i]-ty); if (e<te) e=te; x[i]=ty; + } + if (e<ep) {l=1/m+q; delete[] aa[0]; delete[] aa; delete[] y; return 0;} + } + delete[] aa[0]; delete[] aa; + delete[] y; return 1; +}//EigPowerI + +//--------------------------------------------------------------------------- +/* + function EigPowerS: symmetric power method for solving the dominant eigenvalue with its eigenvector + + In: matrix A[N][N], initial arbitrary vector x[N]. + Out: eigenvalue l, eigenvector x[N]. + + Returns 0 is successful. Content of matrix A is unchangd on return. Initial x[N] must not be zero. +*/ +int EigPowerS(int N, double& l, double* x, double** A, double ep, int maxiter) +{ + int k=0; + Multiply(N, x, x, 1/sqrt(Inner(N, x, x))); + double y2, e, ty, te, *y=new double[N]; + while (k<maxiter) + { + MultiplyXy(N, N, y, A, x); + l=Inner(N, x, y); + y2=sqrt(Inner(N, y, y)); + if (y2==0) {l=0; delete[] y; return 0;} + ty=y[0]/y2; te=x[0]-ty; e=te*te; x[0]=ty; + for (int i=1; i<N; i++) + { + ty=y[i]/y2; te=x[i]-ty; e+=te*te; x[i]=ty; + } + e=sqrt(e); + if (e<ep) {delete[] y; return 0;} + k++; + } + delete[] y; + return 1; +}//EigPowerS + +//--------------------------------------------------------------------------- +/* + function EigPowerWielandt: Wielandt's deflation algorithm for solving a second dominant eigenvalue and + eigenvector (m,u) given the dominant eigenvalue and eigenvector (l,v). + + In: matrix A[N][N], first eigenvalue l with eigenvector v[N] + Out: second eigenvalue m with eigenvector u + + Returns 0 if successful. Content of matrix A is unchangd on return. Initial u[N] must not be zero. +*/ +int EigPowerWielandt(int N, double& m, double* u, double l, double* v, double** A, double ep, int maxiter) +{ + int result; + double** b=new double*[N-1]; b[0]=new double[(N-1)*(N-1)]; for (int i=1; i<N-1; i++) b[i]=&b[0][i*(N-1)]; + double* w=new double[N]; + int i=0; for (int j=1; j<N; j++) if (fabs(v[i])<fabs(v[j])) i=j; + if (i!=0) + for (int k=0; k<i; k++) + for (int j=0; j<i; j++) + b[k][j]=A[k][j]-v[k]*A[i][j]/v[i]; + if (i!=0 && i!=N-1) + for (int k=i; k<N-1; k++) + for (int j=0; j<i; j++) + b[k][j]=A[k+1][j]-v[k+1]*A[i][j]/v[i], b[j][k]=A[j][k+1]-v[j]*A[i][k+1]/v[i]; + if (i!=N-1) + for (int k=i; k<N-1; k++) + for (int j=i; j<N-1; j++) b[k][j]=A[k+1][j+1]-v[k+1]*A[i][j+1]/v[i]; + memcpy(w, u, sizeof(double)*(N-1)); + if ((result=EigPower(N-1, m, w, b, ep, maxiter))==0) + { //* + if (i!=N-1) memmove(&w[i+1], &w[i], sizeof(double)*(N-i-1)); + w[i]=0; + for (int k=0; k<N; k++) u[k]=(m-l)*w[k]+Inner(N, A[i], w)*v[k]/v[i]; //*/ + } + delete[] w; delete[] b[0]; delete[] b; + return result; +}//EigPowerWielandt + +//--------------------------------------------------------------------------- +//NR versions of eigensystem + +/* + function EigenValues: solves for eigenvalues of general system + + In: matrix A[N][N] + Out: eigenvalues ev[N] + + Returns 0 if successful. Content of matrix A is destroyed on return. +*/ +int EigenValues(int N, double** A, cdouble* ev) +{ + BalanceSim(N, A); + Hessenb(N, A); + return QR(N, A, ev); +}//EigenValues + +/* + function EigSym: Solves real symmetric eigensystem A + + In: matrix A[N][N] + Out: eigenvalues d[N], transform matrix Q[N][N], so that diag(d)=Q'AQ, A=Q diag(d) Q', AQ=Q diag(d) + + Returns 0 if successful. Content of matrix A is unchanged on return. +*/ +int EigSym(int N, double** A, double* d, double** Q) +{ + Copy(N, Q, A); + double* t=new double[N]; + HouseHolder(5, Q, d, t); + double result=QL(5, d, t, Q); + delete[] t; + return result; +}//EigSym + +//--------------------------------------------------------------------------- +/* + function GEB: Gaussian elimination with backward substitution for solving linear system Ax=b. + + In: coefficient matrix A[N][N], vector b[N] + Out: vector x[N] + + Returns 0 if successful. Contents of matrix A and vector b are destroyed on return. +*/ +int GEB(int N, double* x, double** A, double* b) +{ + //Gaussian eliminating + int c, p, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + double m; + for (int i=0; i<N-1; i++) + { + p=i; + while (p<N && A[rp[p]][i]==0) p++; + if (p>=N) {delete[] rp; return 1;} + if (p!=i){c=rp[i]; rp[i]=rp[p]; rp[p]=c;} + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=0; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + b[rp[j]]-=m*b[rp[i]]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] rp; return 1;} + else + { + //backward substitution + x[N-1]=b[rp[N-1]]/A[rp[N-1]][N-1]; + for (int i=N-2; i>=0; i--) + { + x[i]=b[rp[i]]; for (int j=i+1; j<N; j++) x[i]-=A[rp[i]][j]*x[j]; x[i]/=A[rp[i]][i]; + } + } + delete[] rp; + return 0; +}//GEB + +//--------------------------------------------------------------------------- +/* + function GESCP: Gaussian elimination with scaled column pivoting for solving linear system Ax=b + + In: matrix A[N][N], vector b[N] + Out: vector x[N] + + Returns 0 is successful. Contents of matrix A and vector b are destroyed on return. +*/ +int GESCP(int N, double* x, double** A, double *b) +{ + int c, p, ip, *rp=new int[N]; + double m, *s=new double[N]; + for (int i=0; i<N; i++) + { + s[i]=fabs(A[i][0]); + for (int j=1; j<N; j++) if (s[i]<fabs(A[i][j])) s[i]=fabs(A[i][j]); + if (s[i]==0) {delete[] s; delete[] rp; return 1;} + rp[i]=i; + } + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (fabs(A[rp[ip]][i])/s[rp[ip]]>fabs(A[rp[p]][i])/s[rp[p]]) p=ip; ip++;} + if (A[rp[p]][i]==0) {delete[] s; delete[] rp; return 1;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c;} + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=0; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + b[rp[j]]-=m*b[rp[i]]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] s; delete[] rp; return 1;} + //backward substitution + x[N-1]=b[rp[N-1]]/A[rp[N-1]][N-1]; + for (int i=N-2; i>=0; i--) + { + x[i]=b[rp[i]]; for (int j=i+1; j<N; j++) x[i]-=A[rp[i]][j]*x[j]; x[i]/=A[rp[i]][i]; + } + delete[] s; delete[] rp; + return 0; +}//GESCP + +//--------------------------------------------------------------------------- +/* + function GExL: solves linear system xL=a, L being lower-triangular. This is used in LU factorization + for solving linear systems. + + In: lower-triangular matrix L[N][N], vector a[N] + Out: vector x[N] + + No return value. Contents of matrix L and vector a are unchanged at return. +*/ +void GExL(int N, double* x, double** L, double* a) +{ + for (int n=N-1; n>=0; n--) + { + double xn=a[n]; + for (int m=n+1; m<N; m++) xn-=x[m]*L[m][n]; + x[n]=xn/L[n][n]; + } +}//GExL + +/* + function GExLAdd: solves linear system *L=a, L being lower-triangular, and add the solution * to x[]. + + In: lower-triangular matrix L[N][N], vector a[N] + Out: updated vector x[N] + + No return value. Contents of matrix L and vector a are unchanged at return. +*/ +void GExLAdd(int N, double* x, double** L, double* a) +{ + double* lx=new double[N]; + GExL(N, lx, L, a); + for (int i=0; i<N; i++) x[i]+=lx[i]; + delete[] lx; +}//GExLAdd + +/* + function GExL1: solves linear system xL=(0, 0, ..., 0, a)', L being lower-triangular. + + In: lower-triangular matrix L[N][N], a + Out: vector x[N] + + No return value. Contents of matrix L and vector a are unchanged at return. +*/ +void GExL1(int N, double* x, double** L, double a) +{ + double xn=a; + for (int n=N-1; n>=0; n--) + { + for (int m=n+1; m<N; m++) xn-=x[m]*L[m][n]; + x[n]=xn/L[n][n]; + xn=0; + } +}//GExL1 + +/* + function GExL1Add: solves linear system *L=(0, 0, ..., 0, a)', L being lower-triangular, and add the + solution * to x[]. + + In: lower-triangular matrix L[N][N], vector a + Out: updated vector x[N] + + No return value. Contents of matrix L and vector a are unchanged at return. +*/ +void GExL1Add(int N, double* x, double** L, double a) +{ + double* lx=new double[N]; + GExL1(N, lx, L, a); + for (int i=0; i<N; i++) x[i]+=lx[i]; + delete[] lx; +}//GExL1Add + +//--------------------------------------------------------------------------- +/* + function GICP: matrix inverse using Gaussian elimination with column pivoting: inv(A)->A. + + In: matrix A[N][N] + Out: matrix A[N][N] + + Returns the determinant of the inverse matrix, 0 on failure. +*/ +double GICP(int N, double** A) +{ + int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + double m, result=1; + + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (fabs(A[rp[ip]][i])>fabs(A[rp[p]][i])) p=ip; ip++;} + if (A[rp[p]][i]==0) {delete[] rp; return 0;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;} + result/=A[rp[i]][i]; + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=-m; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] rp; return 0;} + result/=A[rp[N-1]][N-1]; + //backward substitution + for (int i=0; i<N-1; i++) + { + m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]/=m; A[rp[i]][i]=1/m; + for (int j=i+1; j<N; j++) + { + m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m; + } + } + m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]/=m; A[rp[N-1]][N-1]=1/m; + //recover column and row exchange + double* tm=new double[N]; int sizeN=sizeof(double)*N; + for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); } + for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];} + + delete[] tm; delete[] rp; + return result; +}//GICP +//complex version +cdouble GICP(int N, cdouble** A) +{ + int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + cdouble m, result=1; + + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (~A[rp[ip]][i]>~A[rp[p]][i]) p=ip; ip++;} + if (A[rp[p]][i]==0) {delete[] rp; return 0;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;} + result=result/(A[rp[i]][i]); + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=-m; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] rp; return 0;} + result=result/A[rp[N-1]][N-1]; + //backward substitution + for (int i=0; i<N-1; i++) + { + m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]=A[rp[i]][k]/m; A[rp[i]][i]=cdouble(1)/m; + for (int j=i+1; j<N; j++) + { + m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m; + } + } + m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]=A[rp[N-1]][k]/m; A[rp[N-1]][N-1]=cdouble(1)/m; + //recover column and row exchange + cdouble* tm=new cdouble[N]; int sizeN=sizeof(cdouble)*N; + for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); } + for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];} + + delete[] tm; delete[] rp; + return result; +}//GICP + +/* + function GICP: wrapper function that does not overwrite the input matrix: inv(A)->X. + + In: matrix A[N][N] + Out: matrix X[N][N] + + Returns the determinant of the inverse matrix, 0 on failure. +*/ +double GICP(int N, double** X, double** A) +{ + Copy(N, X, A); + return GICP(N, X); +}//GICP + +//--------------------------------------------------------------------------- +/* + function GILT: inv(lower trangular of A)->lower trangular of A + + In: matrix A[N][N] + Out: matrix A[N][N] + + Returns the determinant of the lower trangular of A +*/ +double GILT(int N, double** A) +{ + double result=1; + A[0][0]=1/A[0][0]; + for (int i=1; i<N; i++) + { + result*=A[i][i]; + double tmp=1/A[i][i]; + for (int k=0; k<i; k++) A[i][k]*=tmp; A[i][i]=tmp; + for (int j=0; j<i; j++) + { + double tmp2=A[i][j]; + for (int k=0; k<j; k++) A[i][k]-=A[j][k]*tmp2; A[i][j]=-A[j][j]*tmp2; + } + } + return result; +}//GILT + +/* + function GIUT: inv(upper trangular of A)->upper trangular of A + + In: matrix A[N][N] + Out: matrix A[N][N] + + Returns the determinant of the upper trangular of A +*/ +double GIUT(int N, double** A) +{ + double result=1; + A[0][0]=1/A[0][0]; + for (int i=1; i<N; i++) + { + result*=A[i][i]; + double tmp=1/A[i][i]; + for (int k=0; k<i; k++) A[k][i]*=tmp; A[i][i]=tmp; + for (int j=0; j<i; j++) + { + double tmp2=A[j][i]; + for (int k=0; k<j; k++) A[k][i]-=A[k][j]*tmp2; A[j][i]=-A[j][j]*tmp2; + } + } + return result; +}//GIUT + +//--------------------------------------------------------------------------- +/* + function GISCP: matrix inverse using Gaussian elimination w. scaled column pivoting: inv(A)->A. + + In: matrix A[N][N] + Out: matrix A[N][N] + + Returns the determinant of the inverse matrix, 0 on failure. +*/ +double GISCP(int N, double** A) +{ + int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + double m, result=1, *s=new double[N]; + + for (int i=0; i<N; i++) + { + s[i]=A[i][0]; + for (int j=1; j<N; j++) if (fabs(s[i])<fabs(A[i][j])) s[i]=A[i][j]; + if (s[i]==0) {delete[] s; delete[] rp; return 0;} + rp[i]=i; + } + + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (fabs(A[rp[ip]][i]/s[rp[ip]])>fabs(A[rp[p]][i]/s[rp[p]])) p=ip; ip++;} + if (A[rp[p]][i]==0) {delete[] s; delete[] rp; return 0;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c; result=-result;} + result/=A[rp[i]][i]; + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=-m; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + for (int k=0; k<i; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] s; delete[] rp; return 0;} + result/=A[rp[N-1]][N-1]; + //backward substitution + for (int i=0; i<N-1; i++) + { + m=A[rp[i]][i]; for (int k=0; k<N; k++) A[rp[i]][k]/=m; A[rp[i]][i]=1/m; + for (int j=i+1; j<N; j++) + { + m=A[rp[i]][j]/A[rp[j]][j]; for (int k=0; k<N; k++) A[rp[i]][k]-=A[rp[j]][k]*m; A[rp[i]][j]=-m; + } + } + m=A[rp[N-1]][N-1]; for (int k=0; k<N-1; k++) A[rp[N-1]][k]/=m; A[rp[N-1]][N-1]=1/m; + //recover column and row exchange + double* tm=new double[N]; int sizeN=sizeof(double)*N; + for (int i=0; i<N; i++) { for (int j=0; j<N; j++) tm[rp[j]]=A[i][j]; memcpy(A[i], tm, sizeN); } + for (int j=0; j<N; j++) { for (int i=0; i<N; i++) tm[i]=A[rp[i]][j]; for (int i=0; i<N; i++) A[i][j]=tm[i];} + + delete[] tm; delete[] s; delete[] rp; + return result; +}//GISCP + +/* + function GISCP: wrapper function that does not overwrite input matrix A: inv(A)->X. + + In: matrix A[N][N] + Out: matrix X[N][N] + + Returns the determinant of the inverse matrix, 0 on failure. +*/ +double GISCP(int N, double** X, double** A) +{ + Copy(N, X, A); + return GISCP(N, X); +}//GISCP + +//--------------------------------------------------------------------------- +/* + function GSI: Gaussian-Seidel iterative algorithm for solving linear system Ax=b. Breaks down if any + Aii=0, like the Jocobi method JI(...). + + Gaussian-Seidel iteration is x(k)=(D-L)^(-1)(Ux(k-1)+b), where D is diagonal, L is lower triangular, + U is upper triangular and A=L+D+U. + + In: matrix A[N][N], vector b[N], initial vector x0[N] + Out: vector x0[N] + + Returns 0 is successful. Contents of matrix A and vector b remain unchanged on return. +*/ +int GSI(int N, double* x0, double** A, double* b, double ep, int maxiter) +{ + double e, *x=new double[N]; + int k=0, sizeN=sizeof(double)*N; + while (k<maxiter) + { + for (int i=0; i<N; i++) + { + x[i]=b[i]; + for (int j=0; j<i; j++) x[i]-=A[i][j]*x[j]; + for (int j=i+1; j<N; j++) x[i]-=A[i][j]*x0[j]; + x[i]/=A[i][i]; + } + e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]); + memcpy(x0, x, sizeN); + if (e<ep) break; + k++; + } + delete[] x; + if (k>=maxiter) return 1; + return 0; +}//GSI + +//--------------------------------------------------------------------------- +/* + function Hessenb: reducing a square matrix A to upper Hessenberg form + + In: matrix A[N][N] + Out: matrix A[N][N], in upper Hessenberg form + + No return value. +*/ +void Hessenb(int N, double** A) +{ + double x, y; + for (int m=1; m<N-1; m++) + { + x=0; + int i=m; + for (int j=m; j<N; j++) + { + if (fabs(A[j][m-1]) > fabs(x)) + { + x=A[j][m-1]; + i=j; + } + } + if (i!=m) + { + for (int j=m-1; j<N; j++) + { + double tmp=A[i][j]; + A[i][j]=A[m][j]; + A[m][j]=tmp; + } + for (int j=0; j<N; j++) + { + double tmp=A[j][i]; + A[j][i]=A[j][m]; + A[j][m]=tmp; + } + } + if (x!=0) + { + for (i=m+1; i<N; i++) + { + if ((y=A[i][m-1])!=0) + { + y/=x; + A[i][m-1]=0; + for (int j=m; j<N; j++) A[i][j]-=y*A[m][j]; + for (int j=0; j<N; j++) A[j][m]+=y*A[j][i]; + } + } + } + } +}//Hessenb + +//--------------------------------------------------------------------------- +/* + function HouseHolder: house holder method converting a symmetric matrix into a tridiagonal symmetric + matrix, or a non-symmetric matrix into an upper-Hessenberg matrix, using similarity transformation. + + In: matrix A[N][N] + Out: matrix A[N][N] after transformation + + No return value. +*/ +void HouseHolder(int N, double** A) +{ + double q, alf, prod, r2, *v=new double[N], *u=new double[N], *z=new double[N]; + for (int k=0; k<N-2; k++) + { + q=Inner(N-1-k, &A[k][k+1], &A[k][k+1]); + + if (A[k][k+1]==0) alf=sqrt(q); + else alf=-sqrt(q)*A[k+1][k]/fabs(A[k+1][k]); + + r2=alf*(alf-A[k+1][k]); + + v[k]=0; v[k+1]=A[k][k+1]-alf; + memcpy(&v[k+2], &A[k][k+2], sizeof(double)*(N-k-2)); + + for (int j=k; j<N; j++) u[j]=Inner(N-1-k, &A[j][k+1], &v[k+1])/r2; + + prod=Inner(N-1-k, &v[k+1], &u[k+1]); + + MultiAdd(N-k, &z[k], &u[k], &v[k], -prod/2/r2); + + for (int l=k+1; l<N-1; l++) + { + for (int j=l+1; j<N; j++) A[l][j]=A[j][l]=A[j][l]-v[l]*z[j]-v[j]*z[l]; + A[l][l]=A[l][l]-2*v[l]*z[l]; + } + + A[N-1][N-1]=A[N-1][N-1]-2*v[N-1]*z[N-1]; + + for (int j=k+2; j<N; j++) A[k][j]=A[j][k]=0; + + A[k][k+1]=A[k+1][k]=A[k+1][k]-v[k+1]*z[k]; + } + delete[] u; delete[] v; delete[] z; +}//HouseHolder + +/* + function HouseHolder: house holder transformation T=Q'AQ or A=QTQ', where T is tridiagonal and Q is + unitary i.e. QQ'=I. + + In: matrix A[N][N] + Out: matrix tridiagonal matrix T[N][N] and unitary matrix Q[N][N] + + No return value. Identical A and T allowed. Content of matrix A is unchanged if A!=T. +*/ +void HouseHolder(int N, double** T, double** Q, double** A) +{ + double g, alf, prod, r2, *v=new double[N], *u=new double[N], *z=new double[N]; + int sizeN=sizeof(double)*N; + if (T!=A) for (int i=0; i<N; i++) memcpy(T[i], A[i], sizeN); + for (int i=0; i<N; i++) {memset(Q[i], 0, sizeN); Q[i][i]=1;} + for (int k=0; k<N-2; k++) + { + g=Inner(N-1-k, &T[k][k+1], &T[k][k+1]); + + if (T[k][k+1]==0) alf=sqrt(g); + else alf=-sqrt(g)*T[k+1][k]/fabs(T[k+1][k]); + + r2=alf*(alf-T[k+1][k]); + + v[k]=0; v[k+1]=T[k][k+1]-alf; + memcpy(&v[k+2], &T[k][k+2], sizeof(double)*(N-k-2)); + + for (int j=k; j<N; j++) u[j]=Inner(N-1-k, &T[j][k+1], &v[k+1])/r2; + + prod=Inner(N-1-k, &v[k+1], &u[k+1]); + + MultiAdd(N-k, &z[k], &u[k], &v[k], -prod/2/r2); + + for (int l=k+1; l<N-1; l++) + { + for (int j=l+1; j<N; j++) T[l][j]=T[j][l]=T[j][l]-v[l]*z[j]-v[j]*z[l]; + T[l][l]=T[l][l]-2*v[l]*z[l]; + } + + T[N-1][N-1]=T[N-1][N-1]-2*v[N-1]*z[N-1]; + + for (int j=k+2; j<N; j++) T[k][j]=T[j][k]=0; + + T[k][k+1]=T[k+1][k]=T[k+1][k]-v[k+1]*z[k]; + + for (int i=0; i<N; i++) + MultiAdd(N-k, &Q[i][k], &Q[i][k], &v[k], -Inner(N-k, &Q[i][k], &v[k])/r2); + } + delete[] u; delete[] v; delete[] z; +}//HouseHolder + +/* + function HouseHolder: nr version of householder method for transforming symmetric matrix A to QTQ', + where T is tridiagonal and Q is orthonormal. + + In: matrix A[N][N] + Out: A[N][N]: now containing Q + d[N]: containing diagonal elements of T + sd[N]: containing subdiagonal elements of T as sd[1:N-1]. + + No return value. +*/ +void HouseHolder(int N, double **A, double* d, double* sd) +{ + for (int i=N-1; i>=1; i--) + { + int l=i-1; + double h=0, scale=0; + if (l>0) + { + for (int k=0; k<=l; k++) scale+=fabs(A[i][k]); + if (scale==0.0) sd[i]=A[i][l]; + else + { + for (int k=0; k<=l; k++) + { + A[i][k]/=scale; + h+=A[i][k]*A[i][k]; + } + double f=A[i][l]; + double g=(f>=0?-sqrt(h): sqrt(h)); + sd[i]=scale*g; + h-=f*g; + A[i][l]=f-g; + f=0; + for (int j=0; j<=l; j++) + { + A[j][i]=A[i][j]/h; + g=0; + for (int k=0; k<=j; k++) g+=A[j][k]*A[i][k]; + for (int k=j+1; k<=l; k++) g+=A[k][j]*A[i][k]; + sd[j]=g/h; + f+=sd[j]*A[i][j]; + } + double hh=f/(h+h); + for (int j=0; j<=l; j++) + { + f=A[i][j]; + sd[j]=g=sd[j]-hh*f; + for (int k=0; k<=j; k++) A[j][k]-=(f*sd[k]+g*A[i][k]); + } + } + } + else + sd[i]=A[i][l]; + d[i]=h; + } + + d[0]=sd[0]=0; + + for (int i=0; i<N; i++) + { + int l=i-1; + if (d[i]) + { + for (int j=0; j<=l; j++) + { + double g=0.0; + for (int k=0; k<=l; k++) g+=A[i][k]*A[k][j]; + for (int k=0; k<=l; k++) A[k][j]-=g*A[k][i]; + } + } + d[i]=A[i][i]; + A[i][i]=1.0; + for (int j=0; j<=l; j++) A[j][i]=A[i][j]=0.0; + } +}//HouseHolder + +//--------------------------------------------------------------------------- +/* + function Inner: inner product z=y'x + + In: vectors x[N], y[N] + + Returns inner product of x and y. +*/ +double Inner(int N, double* x, double* y) +{ + double result=0; + for (int i=0; i<N; i++) result+=x[i]*y[i]; + return result; +}//Inner +//complex versions +cdouble Inner(int N, double* x, cdouble* y) +{ + cdouble result=0; + for (int i=0; i<N; i++) result+=x[i]**y[i]; + return result; +}//Inner +cdouble Inner(int N, cdouble* x, cdouble* y) +{ + cdouble result=0; + for (int i=0; i<N; i++) result+=x[i]^y[i]; + return result; +}//Inner +cdouble Inner(int N, cfloat* x, cdouble* y) +{ + cdouble result=0; + for (int i=0; i<N; i++) result+=x[i]^y[i]; + return result; +}//Inner +cfloat Inner(int N, cfloat* x, cfloat* y) +{ + cfloat result=0; + for (int i=0; i<N; i++) result+=x[i]^y[i]; + return result; +}//Inner + +/* + function Inner: inner product z=tr(Y'X) + + In: matrices X[M][N], Y[M][N] + + Returns inner product of X and Y. +*/ +double Inner(int M, int N, double** X, double** Y) +{ + double result=0; + for (int m=0; m<M; m++) for (int n=0; n<N; n++) result+=X[m][n]*Y[m][n]; + return result; +}//Inner + +//--------------------------------------------------------------------------- +/* + function JI: Jacobi interative algorithm for solving linear system Ax=b Breaks down if A[i][i]=0 for + any i. Reorder A so that this does not happen. + + Jacobi iteration is x(k)=D^(-1)((L+U)x(k-1)+b), D is diagonal, L is lower triangular, U is upper + triangular and A=L+D+U. + + In: matrix A[N][N], vector b[N], initial vector x0[N] + Out: vector x0[N] + + Returns 0 if successful. Contents of matrix A and vector b are unchanged on return. +*/ +int JI(int N, double* x0, double** A, double* b, double ep, int maxiter) +{ + double e, *x=new double[N]; + int k=0, sizeN=sizeof(double)*N; + while (k<maxiter) + { + for (int i=0; i<N; i++) + { + x[i]=b[i]; for (int j=0; j<N; j++) if (j!=i) x[i]-=A[i][j]*x0[j]; x[i]=x[i]/A[i][i]; + } + e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]); //inf-norm used here + memcpy(x0, x, sizeN); + if (e<ep) break; + k++; + } + delete[] x; + if (k>=maxiter) return 1; + else return 0; +}//JI + +//--------------------------------------------------------------------------- +/* + function LDL: LDL' decomposition A=LDL', where L is lower triangular and D is diagonal identical l and + a allowed. + + The symmetric matrix A is positive definite iff A can be factorized as LDL', where L is lower + triangular with ones on its diagonal and D is diagonal with positive diagonal entries. + + If a symmetric matrix A can be reduced by Gaussian elimination without row interchanges, then it can + be factored into LDL', where L is lower triangular with ones on its diagonal and D is diagonal with + non-zero diagonal entries. + + In: matrix A[N][N] + Out: lower triangular matrix L[N][N], vector d[N] containing diagonal elements of D + + Returns 0 if successful. Content of matrix A is unchanged on return. +*/ +int LDL(int N, double** L, double* d, double** A) +{ + double* v=new double[N]; + + if (A[0][0]==0) {delete[] v; return 1;} + d[0]=A[0][0]; for (int j=1; j<N; j++) L[j][0]=A[j][0]/d[0]; + for (int i=1; i<N; i++) + { + for (int j=0; j<i; j++) v[j]=L[i][j]*d[j]; + d[i]=A[i][i]; for (int j=0; j<i; j++) d[i]-=L[i][j]*v[j]; + if (d[i]==0) {delete[] v; return 1;} + for (int j=i+1; j<N; j++) + { + L[j][i]=A[j][i]; for (int k=0; k<i; k++) L[j][i]-=L[j][k]*v[k]; L[j][i]/=d[i]; + } + } + delete[] v; + + for (int i=0; i<N; i++) {L[i][i]=1; memset(&L[i][i+1], 0, sizeof(double)*(N-1-i));} + return 0; +}//LDL + +//--------------------------------------------------------------------------- +/* + function LQ_GS: LQ decomposition using Gram-Schmidt method + + In: matrix A[M][N], M<=N + Out: matrices L[M][M], Q[M][N] + + No return value. +*/ +void LQ_GS(int M, int N, double** A, double** L, double** Q) +{ + double *u=new double[N]; + for (int m=0; m<M; m++) + { + memset(L[m], 0, sizeof(double)*M); + memcpy(u, A[m], sizeof(double)*N); + for (int k=0; k<m; k++) + { + double ip=0; for (int n=0; n<N; n++) ip+=Q[k][n]*u[n]; + for (int n=0; n<N; n++) u[n]-=ip*Q[k][n]; + L[m][k]=ip; + } + double iu=0; for (int n=0; n<N; n++) iu+=u[n]*u[n]; iu=sqrt(iu); + L[m][m]=iu; iu=1.0/iu; + for (int n=0; n<N; n++) Q[m][n]=u[n]*iu; + } + delete[] u; +}//LQ_GS + +//--------------------------------------------------------------------------- +/* + function LSLinear2: 2-dtage LS solution of A[M][N]x[N][1]=y[M][1], M>=N. Use of this function requires + the submatrix A[N][N] be invertible. + + In: matrix A[M][N], vector y[M], M>=N. + Out: vector x[N]. + + No return value. Contents of matrix A and vector y are unchanged on return. +*/ +void LSLinear2(int M, int N, double* x, double** A, double* y) +{ + double** A1=Copy(N, N, 0, A); + LU(N, x, A1, y); + if (M>N) + { + double** B=&A[N]; + double* Del=MultiplyXy(M-N, N, B, x); + MultiAdd(M-N, Del, Del, &y[N], -1); + double** A2=MultiplyXtX(N, N, A); + MultiplyXtX(N, M-N, A1, B); + MultiAdd(N, N, A2, A2, A1, 1); + double* b2=MultiplyXty(N, M-N, B, Del); + double* dx=new double[N]; + GESCP(N, dx, A2, b2); + MultiAdd(N, x, x, dx, -1); + delete[] dx; + delete[] Del; + delete[] b2; + DeAlloc2(A2); + } + DeAlloc2(A1); +}//LSLinear2 + +//--------------------------------------------------------------------------- +/* + function LU: LU decomposition A=LU, where L is lower triangular with diagonal entries 1 and U is upper + triangular. + + LU is possible if A can be reduced by Gaussian elimination without row interchanges. + + In: matrix A[N][N] + Out: matrices L[N][N] and U[N][N], subject to input values of L and U: + if L euqals NULL, L is not returned + if U equals NULL or A, U is returned in A, s.t. A is modified + if L equals A, L is returned in A, s.t. A is modified + if L equals U, L and U are returned in the same matrix + when L and U are returned in the same matrix, diagonal of L (all 1) is not returned + + Returns 0 if successful. +*/ +int LU(int N, double** L, double** U, double** A) +{ + double* diagl=new double[N]; + for (int i=0; i<N; i++) diagl[i]=1; + + int sizeN=sizeof(double)*N; + if (U==0) U=A; + if (U!=A) for (int i=0; i<N; i++) memcpy(U[i], A[i], sizeN); + int result=LU_Direct(0, N, diagl, U); + if (result==0) + { + if (L!=U) + { + if (L!=0) for (int i=0; i<N; i++) {memcpy(L[i], U[i], sizeof(double)*i); L[i][i]=1; memset(&L[i][i+1], 0, sizeof(double)*(N-i-1));} + for (int i=1; i<N; i++) memset(U[i], 0, sizeof(double)*i); + } + } + delete[] diagl; + return result; +}//LU + +/* + function LU: Solving linear system Ax=y by LU factorization + + In: matrix A[N][N], vector y[N] + Out: x[N] + + No return value. On return A contains its LU factorization (with pivoting, diag mode 1), y remains + unchanged. +*/ +void LU(int N, double* x, double** A, double* y, int* ind) +{ + int parity; + bool allocind=!ind; + if (allocind) ind=new int[N]; + LUCP(A, N, ind, parity, 1); + for (int i=0; i<N; i++) x[i]=y[ind[i]]; + for (int i=0; i<N; i++) + { + for (int j=i+1; j<N; j++) x[j]=x[j]-x[i]*A[j][i]; + } + for (int i=N-1; i>=0; i--) + { + x[i]/=A[i][i]; + for (int j=0; j<i; j++) x[j]=x[j]-x[i]*A[j][i]; + } + if (allocind) delete[] ind; +}//LU + +//--------------------------------------------------------------------------- +/* + LU_DiagL shows the original procedure for calculating A=LU in separate buffers substitute l and u by a + gives the stand-still method LU_Direct(). +*//* +void LU_DiagL(int N, double** l, double* diagl, double** u, double** a) +{ + l[0][0]=diagl[0]; u[0][0]=a[0][0]/l[0][0]; //here to signal failure if l[00]u[00]=0 + for (int j=1; j<N; j++) u[0][j]=a[0][j]/l[0][0], l[j][0]=a[j][0]/u[0][0]; + memset(&l[0][1], 0, sizeof(double)*(N-1)); + for (int i=1; i<N-1; i++) + { + l[i][i]=diagl[i]; + u[i][i]=a[i][i]; for (int k=0; k<i; k++) u[i][i]-=l[i][k]*u[k][i]; u[i][i]/=l[i][i]; //here to signal failure if l[ii]u[ii]=0 + for (int j=i+1; j<N; j++) + { + u[i][j]=a[i][j]; for (int k=0; k<i; k++) u[i][j]-=l[i][k]*u[k][j]; u[i][j]/=l[i][i]; + l[j][i]=a[j][i]; for (int k=0; k<i; k++) l[j][i]-=l[j][k]*u[k][i]; l[j][i]/=u[i][i]; + } + memset(&l[i][i+1], 0, sizeof(double)*(N-1-i)), memset(u[i], 0, sizeof(double)*i); + } + l[N-1][N-1]=diagl[N-1]; + u[N-1][N-1]=a[N-1][N-1]; for (int k=0; k<N-1; k++) u[N-1][N-1]-=l[N-1][k]*u[k][N-1]; u[N-1][N-1]/=l[N-1][N-1]; + memset(u[N-1], 0, sizeof(double)*(N-1)); +} //LU_DiagL*/ + +//--------------------------------------------------------------------------- +/* + function LU_Direct: LU factorization A=LU. + + In: matrix A[N][N], vector diag[N] specifying main diagonal of L or U, according to mode (0=LDiag, + 1=UDiag). + Out: matrix A[N][N] now containing L and U. + + Returns 0 if successful. +*/ +int LU_Direct(int mode, int N, double* diag, double** A) +{ + if (mode==0) + { + if (A[0][0]==0) return 1; + A[0][0]=A[0][0]/diag[0]; + for (int j=1; j<N; j++) A[0][j]=A[0][j]/diag[0], A[j][0]=A[j][0]/A[0][0]; + for (int i=1; i<N-1; i++) + { + for (int k=0; k<i; k++) A[i][i]-=A[i][k]*A[k][i]; A[i][i]/=diag[i]; + if (A[i][i]==0) return 2; + for (int j=i+1; j<N; j++) + { + for (int k=0; k<i; k++) A[i][j]-=A[i][k]*A[k][j]; A[i][j]/=diag[i]; + for (int k=0; k<i; k++) A[j][i]-=A[j][k]*A[k][i]; A[j][i]/=A[i][i]; + } + } + for (int k=0; k<N-1; k++) A[N-1][N-1]-=A[N-1][k]*A[k][N-1]; A[N-1][N-1]/=diag[N-1]; + } + else if (mode==1) + { + A[0][0]=A[0][0]/diag[0]; + if (A[0][0]==0) return 1; + for (int j=1; j<N; j++) A[0][j]=A[0][j]/A[0][0], A[j][0]=A[j][0]/diag[0]; + for (int i=1; i<N-1; i++) + { + for (int k=0; k<i; k++) A[i][i]-=A[i][k]*A[k][i]; A[i][i]/=diag[i]; + if (A[i][i]==0) return 2; + for (int j=i+1; j<N; j++) + { + for (int k=0; k<i; k++) A[i][j]-=A[i][k]*A[k][j]; A[i][j]/=A[i][i]; + for (int k=0; k<i; k++) A[j][i]-=A[j][k]*A[k][i]; A[j][i]/=diag[i]; + } + } + for (int k=0; k<N-1; k++) A[N-1][N-1]-=A[N-1][k]*A[k][N-1]; A[N-1][N-1]/=diag[N-1]; + } + return 0; +}//LU_Direct + +//--------------------------------------------------------------------------- +/* + function LU_PD: LU factorization for pentadiagonal A=LU + + In: pentadiagonal matrix A[N][N] stored in a compact format, i.e. A[i][j]->b[i-j, j] + the main diagonal is b[0][0]~b[0][N-1] + the 1st upper subdiagonal is b[-1][1]~b[-1][N-1] + the 2nd upper subdiagonal is b[-2][2]~b[-2][N-1] + the 1st lower subdiagonal is b[1][0]~b[1][N-2] + the 2nd lower subdiagonal is b[2][0]~b[2][N-3] + + Out: L[N][N] and U[N][N], main diagonal of L being all 1 (probably), stored in a compact format in + b[-2:2][N]. + + Returns 0 if successful. +*/ +int LU_PD(int N, double** b) +{ + if (b[0][0]==0) return 1; + b[1][0]/=b[0][0], b[2][0]/=b[0][0]; + + //i=1, not to double b[*][i-2], b[-2][i] + b[0][1]-=b[1][0]*b[-1][1]; + if (b[0][1]==0) return 2; + b[-1][2]-=b[1][0]*b[-2][2]; + b[1][1]-=b[2][0]*b[-1][1]; + b[1][1]/=b[0][1]; + b[2][1]/=b[0][1]; + + for (int i=2; i<N-2; i++) + { + b[0][i]-=b[2][i-2]*b[-2][i]; + b[0][i]-=b[1][i-1]*b[-1][i]; + if (b[0][i]==0) return 2; + b[-1][i+1]-=b[1][i-1]*b[-2][i+1]; + b[1][i]-=b[2][i-1]*b[-1][i]; + b[1][i]/=b[0][i]; + b[2][i]/=b[0][i]; + } + //i=N-2, not to tough b[2][i] + b[0][N-2]-=b[2][N-4]*b[-2][N-2]; + b[0][N-2]-=b[1][N-3]*b[-1][N-2]; + if (b[0][N-2]==0) return 2; + b[-1][N-1]-=b[1][N-3]*b[-2][N-1]; + b[1][N-2]-=b[2][N-3]*b[-1][N-2]; + b[1][N-2]/=b[0][N-2]; + + b[0][N-1]-=b[2][N-3]*b[-2][N-1]; + b[0][N-1]-=b[1][N-2]*b[-1][N-1]; + return 0; +}//LU_PD + +/* + This old version is kept here as a reference. +*//* +int LU_PD(int N, double** b) +{ + if (b[0][0]==0) return 1; + for (int j=1; j<3; j++) b[j][0]=b[j][0]/b[0][0]; + for (int i=1; i<N-1; i++) + { + for (int k=i-2; k<i; k++) b[0][i]-=b[i-k][k]*b[k-i][i]; + if (b[0][i]==0) return 2; + for (int j=i+1; j<i+3; j++) + { + for (int k=j-2; k<i; k++) b[i-j][j]-=b[i-k][k]*b[k-j][j]; + for (int k=j-2; k<i; k++) b[j-i][i]-=b[j-k][k]*b[k-i][i]; + b[j-i][i]/=b[0][i]; + } + } + for (int k=N-3; k<N-1; k++) b[0][N-1]-=b[N-1-k][k]*b[k-N+1][N-1]; + return 0; +}//LU_PD*/ + +/* + function LU_PD: solve pentadiagonal system Ax=c + + In: pentadiagonal matrix A[N][N] stored in a compact format in b[-2:2][N], vector c[N] + Out: vector c now containing x. + + Returns 0 if successful. On return b is in the LU form. +*/ +int LU_PD(int N, double** b, double* c) +{ + int result=LU_PD(N, b); + if (result==0) + { + //L loop + c[1]=c[1]-b[1][0]*c[0]; + for (int i=2; i<N; i++) + c[i]=c[i]-b[1][i-1]*c[i-1]-b[2][i-2]*c[i-2]; + //U loop + c[N-1]/=b[0][N-1]; + c[N-2]=(c[N-2]-b[-1][N-1]*c[N-1])/b[0][N-2]; + for (int i=N-3; i>=0; i--) + c[i]=(c[i]-b[-1][i+1]*c[i+1]-b[-2][i+2]*c[i+2])/b[0][i]; + } + return result; +}//LU_PD + +//--------------------------------------------------------------------------- +/* + function LUCP: LU decomposition A=LU with column pivoting + + In: matrix A[N][N] + Out: matrix A[N][N] now holding L and U by L_U[i][j]=A[ind[i]][j], where L_U + hosts L and U according to mode: + mode=0: L diag=abs(U diag), U diag as return + mode=1: L diag=1, U diag as return + mode=2: U diag=1, L diag as return + + Returns the determinant of A. +*/ +double LUCP(double **A, int N, int *ind, int &parity, int mode) +{ + double det=1; + parity=1; + + for (int i=0; i<N; i++) ind[i]=i; + double vmax, *norm=new double[N]; //norm[n] is the maxima of row n + for (int i=0; i<N; i++) + { + vmax=fabs(A[i][0]); + double tmp; + for (int j=1; j<N; j++) if ((tmp=fabs(A[i][j]))>vmax) vmax=tmp; + if (vmax==0) { parity=0; goto deletenorm; } //det=0 at this point + norm[i]=1/vmax; + } + + int maxind; + for (int j=0; j<N; j++) + { //Column j + for (int i=0; i<j; i++) + { + //row i, i<j + double tmp=A[i][j]; + for (int k=0; k<i; k++) tmp-=A[i][k]*A[k][j]; + A[i][j]=tmp; + } + for (int i=j; i<N; i++) + { + //row i, i>=j + double tmp=A[i][j]; for (int k=0; k<j; k++) tmp-=A[i][k]*A[k][j]; A[i][j]=tmp; + double tmp2=norm[i]*fabs(tmp); + if (i==j || tmp2>=vmax) maxind=i, vmax=tmp2; + } + if (vmax==0) { parity=0; goto deletenorm; } //pivot being zero + if (j!=maxind) + { + //do column pivoting: switching rows + for (int k=0; k<N; k++) { double tmp=A[maxind][k]; A[maxind][k]=A[j][k]; A[j][k]=tmp; } + parity=-parity; + norm[maxind]=norm[j]; + } + int itmp=ind[j]; ind[j]=ind[maxind]; ind[maxind]=itmp; + if (j!=N-1) + { + double den=1/A[j][j]; + for (int i=j+1; i<N; i++) A[i][j]*=den; + } + det*=A[j][j]; + } //Go back for the next column in the reduction. + + if (mode==0) + { + for (int i=0; i<N-1; i++) + { + double den=sqrt(fabs(A[i][i])); + double iden=1/den; + for (int j=i+1; j<N; j++) A[j][i]*=den, A[i][j]*=iden; + A[i][i]*=iden; + } + A[N-1][N-1]/=sqrt(fabs(A[N-1][N-1])); + } + else if (mode==2) + { + for (int i=0; i<N-1; i++) + { + double den=A[i][i]; + double iden=1/den; + for (int j=i+1; j<N; j++) A[j][i]*=den, A[i][j]*=iden; + } + } + +deletenorm: + delete[] norm; + return det*parity; +}//LUCP + +//--------------------------------------------------------------------------- +/* + function maxind: returns the index of the maximal value of data[from:(to-1)]. + + In: vector data containing at least $to entries. + Out: the index to the maximal entry of data[from:(to-1)] + + Returns the index to the maximal value. +*/ +int maxind(double* data, int from, int to) +{ + int result=from; + for (int i=from+1; i<to; i++) if (data[result]<data[i]) result=i; + return result; +}//maxind + +//--------------------------------------------------------------------------- +/* + macro Multiply_vect: matrix-vector multiplications + + Each expansion of this macro implements two functions named $MULTIPLY that do matrix-vector + multiplication. Functions are named after their exact functions. For example, MultiplyXty() does + multiplication of the transpose of matrix X with vector y, where postfix "t" attched to Y stands for + transpose. Likewise, the postfix "c" stands for conjugate, and "h" stnads for Hermitian (conjugate + transpose). + + Two dimension arguments are needed by each function. The first of the two is the number of entries to + the output vector; the second of the two is the "other" dimension of the matrix multiplier. +*/ +#define Multiply_vect(MULTIPLY, DbZ, DbX, DbY, xx, yy) \ + DbZ* MULTIPLY(int M, int N, DbZ* z, DbX* x, DbY* y, MList* List) \ + { \ + if (!z){z=new DbZ[M]; if (List) List->Add(z, 1);} \ + for (int m=0; m<M; m++){z[m]=0; for (int n=0; n<N; n++) z[m]+=xx*yy;} \ + return z; \ + } \ + DbZ* MULTIPLY(int M, int N, DbX* x, DbY* y, MList* List) \ + { \ + DbZ* z=new DbZ[M]; if (List) List->Add(z, 1); \ + for (int m=0; m<M; m++){z[m]=0; for (int n=0; n<N; n++) z[m]+=xx*yy;} \ + return z; \ + } +//function MultiplyXy: z[M]=x[M][N]y[N], identical z and y NOT ALLOWED +Multiply_vect(MultiplyXy, double, double*, double, x[m][n], y[n]) +Multiply_vect(MultiplyXy, cdouble, cdouble*, cdouble, x[m][n], y[n]) +Multiply_vect(MultiplyXy, cdouble, double*, cdouble, x[m][n], y[n]) +//function MultiplyxY: z[M]=x[N]y[N][M], identical z and x NOT ALLOWED +Multiply_vect(MultiplyxY, double, double, double*, x[n], y[n][m]) +Multiply_vect(MultiplyxY, cdouble, cdouble, cdouble*, x[n], y[n][m]) +//function MultiplyXty: z[M]=xt[M][N]y[N] +Multiply_vect(MultiplyXty, double, double*, double, x[n][m], y[n]) +Multiply_vect(MultiplyXty, cdouble, cdouble*, cdouble, x[n][m], y[n]) +//function MultiplyXhy: z[M]=xh[M][N]y[N] +Multiply_vect(MultiplyXhy, cdouble, cdouble*, cdouble, *x[n][m], y[n]) +//function MultiplyxYt: z[M]=x[N]yt[N][M] +Multiply_vect(MultiplyxYt, double, double, double*, x[n], y[m][n]) +//function MultiplyXcy: z[M]=(x*)[M][N]y[N] +Multiply_vect(MultiplyXcy, cdouble, cdouble*, cdouble, *x[m][n], y[n]) +Multiply_vect(MultiplyXcy, cdouble, cdouble*, cfloat, *x[m][n], y[n]) + +//--------------------------------------------------------------------------- +/* + function Norm1: L-1 norm of a square matrix A + + In: matrix A[N][N] + Out: its L-1 norm + + Returns the L-1 norm. +*/ +double Norm1(int N, double** A) +{ + double result=0, norm; + for (int i=0; i<N; i++) + { + norm=0; for (int j=0; j<N; j++) norm+=fabs(A[i][j]); + if (result<norm) result=norm; + } + return result; +}//Norm1 + +//--------------------------------------------------------------------------- +/* + function QL: QL method for solving tridiagonal symmetric matrix eigenvalue problem. + + In: A[N][N]: tridiagonal symmetric matrix stored in d[N] and sd[] arranged so that d[0:n-1] contains + the diagonal elements of A, sd[0]=0, sd[1:n-1] contains the subdiagonal elements of A. + z[N][N]: pre-transform matrix z[N][N] compatible with HouseHolder() routine. + Out: d[N]: the eigenvalues of A + z[N][N] the eigenvectors of A. + + Returns 0 if successful. sd[] should have storage for at least N+1 entries. +*/ +int QL(int N, double* d, double* sd, double** z) +{ + const int maxiter=30; + for (int i=1; i<N; i++) sd[i-1]=sd[i]; + sd[N]=0.0; + for (int l=0; l<N; l++) + { + int iter=0, m; + do + { + for (m=l; m<N-1; m++) + { + double dd=fabs(d[m])+fabs(d[m+1]); + if (fabs(sd[m])+dd==dd) break; + } + if (m!=l) + { + iter++; + if (iter>=maxiter) return 1; + double g=(d[l+1]-d[l])/(2*sd[l]); + double r=sqrt(g*g+1); + g=d[m]-d[l]+sd[l]/(g+(g>=0?r:-r)); + double s=1, c=1, p=0; + int i; + for (i=m-1; i>=l; i--) + { + double f=s*sd[i], b=c*sd[i]; + sd[i+1]=(r=sqrt(f*f+g*g)); + if (r==0) + { + d[i+1]-=p; + sd[m]=0; + break; + } + s=f/r, c=g/r; + g=d[i+1]-p; + r=(d[i]-g)*s+2.0*c*b; + p=s*r; + d[i+1]=g+p; + g=c*r-b; + for (int k=0; k<N; k++) + { + f=z[k][i+1]; + z[k][i+1]=s*z[k][i]+c*f; + z[k][i]=c*z[k][i]-s*f; + } + } + if (r==0 && i>=l) continue; + d[l]-=p; + sd[l]=g; + sd[m]=0.0; + } + } + while (m!=l); + } + return 0; +}//QL + +//--------------------------------------------------------------------------- +/* + function QR: nr version of QR method for solving upper Hessenberg system A. This is compatible with + Hessenb method. + + In: matrix A[N][N] + Out: vector ev[N] of eigenvalues + + Returns 0 on success. Content of matrix A is destroyed on return. +*/ +int QR(int N, double **A, cdouble* ev) +{ + int n=N, m, l, k, j, iter, i, mmin, maxiter=30; + double **a=A, z, y, x, w, v, u, t=0, s, r, q, p, a1=0; + for (i=0; i<n; i++) for (j=i-1>0?i-1:0; j<n; j++) a1+=fabs(a[i][j]); + n--; + while (n>=0) + { + iter=0; + do + { + for (l=n; l>0; l--) + { + s=fabs(a[l-1][l-1])+fabs(a[l][l]); + if (s==0) s=a1; + if (fabs(a[l][l-1])+s==s) {a[l][l-1]=0; break;} + } + x=a[n][n]; + if (l==n) {ev[n].x=x+t; ev[n--].y=0;} + else + { + y=a[n-1][n-1], w=a[n][n-1]*a[n-1][n]; + if (l==(n-1)) + { + p=0.5*(y-x); + q=p*p+w; + z=sqrt(fabs(q)); + x+=t; + if (q>=0) + { + z=p+(p>=0?z:-z); + ev[n-1].x=ev[n].x=x+z; + if (z) ev[n].x=x-w/z; + ev[n-1].y=ev[n].y=0; + } + else + { + ev[n-1].x=ev[n].x=x+p; + ev[n].y=z; ev[n-1].y=-z; + } + n-=2; + } + else + { + if (iter>=maxiter) return 1; + if (iter%10==9) + { + t+=x; + for (i=0; i<=n; i++) a[i][i]-=x; + s=fabs(a[n][n-1])+fabs(a[n-1][n-2]); + y=x=0.75*s; + w=-0.4375*s*s; + } + iter++; + for (m=n-2; m>=l; m--) + { + z=a[m][m]; + r=x-z; s=y-z; + p=(r*s-w)/a[m+1][m]+a[m][m+1]; q=a[m+1][m+1]-z-r-s; r=a[m+2][m+1]; + s=fabs(p)+fabs(q)+fabs(r); + p/=s; q/=s; r/=s; + if (m==l) break; + u=fabs(a[m][m-1])*(fabs(q)+fabs(r)); + v=fabs(p)*(fabs(a[m-1][m-1])+fabs(z)+fabs(a[m+1][m+1])); + if (u+v==v) break; + } + for (i=m+2; i<=n; i++) + { + a[i][i-2]=0; + if (i!=m+2) a[i][i-3]=0; + } + for (k=m; k<=n-1; k++) + { + if (k!=m) + { + p=a[k][k-1]; + q=a[k+1][k-1]; + r=0; + if (k!=n-1) r=a[k+2][k-1]; + x=fabs(p)+fabs(q)+fabs(r); + if (x!=0) p/=x, q/=x, r/=x; + } + if (p>=0) s=sqrt(p*p+q*q+r*r); + else s=-sqrt(p*p+q*q+r*r); + if (s!=0) + { + if (k==m) + { + if (l!=m) a[k][k-1]=-a[k][k-1]; + } + else a[k][k-1]=-s*x; + p+=s; + x=p/s; y=q/s; z=r/s; q/=p; r/=p; + for (j=k; j<=n; j++) + { + p=a[k][j]+q*a[k+1][j]; + if (k!=n-1) + { + p+=r*a[k+2][j]; + a[k+2][j]-=p*z; + } + a[k+1][j]-=p*y; a[k][j]-=p*x; + } + mmin=n<k+3?n:k+3; + for (i=l; i<=mmin; i++) + { + p=x*a[i][k]+y*a[i][k+1]; + if (k!=(n-1)) + { + p+=z*a[i][k+2]; + a[i][k+2]-=p*r; + } + a[i][k+1]-=p*q; a[i][k]-=p; + } + } + } + } + } + } while (n>l+1); + } + return 0; +}//QR + +/* + function QR_GS: QR decomposition A=QR using Gram-Schmidt method + + In: matrix A[M][N], M>=N + Out: Q[M][N], R[N][N] + + No return value. +*/ +void QR_GS(int M, int N, double** A, double** Q, double** R) +{ + double *u=new double[M]; + for (int n=0; n<N; n++) + { + memset(R[n], 0, sizeof(double)*N); + for (int m=0; m<M; m++) u[m]=A[m][n]; + for (int k=0; k<n; k++) + { + double ip=0; for (int m=0; m<M; m++) ip+=u[m]*Q[m][k]; + for (int m=0; m<M; m++) u[m]-=ip*Q[m][k]; + R[k][n]=ip; + } + double iu=0; for (int m=0; m<M; m++) iu+=u[m]*u[m]; iu=sqrt(iu); + R[n][n]=iu; + iu=1.0/iu; for (int m=0; m<M; m++) Q[m][n]=u[m]*iu; + } + delete[] u; +}//QR_GS + +/* + function QR_householder: QR decomposition using householder transform + + In: A[M][N], M>=N + Out: Q[M][M], R[M][N] + + No return value. +*/ +void QR_householder(int M, int N, double** A, double** Q, double** R) +{ + double *u=new double[M*3], *ur=&u[M], *qu=&u[M*2]; + for (int m=0; m<M; m++) + { + memcpy(R[m], A[m], sizeof(double)*N); + memset(Q[m], 0, sizeof(double)*M); Q[m][m]=1; + } + for (int n=0; n<N; n++) + { + double alf=0; for (int m=n; m<M; m++) alf+=R[m][n]*R[m][n]; alf=sqrt(alf); + if (R[n][n]>0) alf=-alf; + for (int m=n; m<M; m++) u[m]=R[m][n]; u[n]=u[n]-alf; + double iu2=0; for (int m=n; m<M; m++) iu2+=u[m]*u[m]; iu2=2.0/iu2; + for (int m=n; m<N; m++) + { + ur[m]=0; for (int k=n; k<M; k++) ur[m]+=u[k]*R[k][m]; + } + for (int m=0; m<M; m++) + { + qu[m]=0; for (int k=n; k<M; k++) qu[m]+=Q[m][k]*u[k]; + } + for (int m=n; m<M; m++) u[m]=u[m]*iu2; + for (int m=n; m<M; m++) for (int k=n; k<N; k++) R[m][k]-=u[m]*ur[k]; + for (int m=0; m<M; m++) for (int k=n; k<M; k++) Q[m][k]-=qu[m]*u[k]; + } + delete[] u; +}//QR_householder + +//--------------------------------------------------------------------------- +/* + function QU: Unitary decomposition A=QU, where Q is unitary and U is upper triangular + + In: matrix A[N][N] + Out: matrices Q[N][N], A[n][n] now containing U + + No return value. +*/ +void QU(int N, double** Q, double** A) +{ + int sizeN=sizeof(double)*N; + for (int i=0; i<N; i++) {memset(Q[i], 0, sizeN); Q[i][i]=1;} + + double m, s, c, *tmpi=new double[N], *tmpj=new double[N]; + for (int i=1; i<N; i++) for (int j=0; j<i; j++) + if (A[i][j]!=0) + { + m=sqrt(A[j][j]*A[j][j]+A[i][j]*A[i][j]); + s=A[i][j]/m; + c=A[j][j]/m; + for (int k=0; k<N; k++) tmpi[k]=-s*A[j][k]+c*A[i][k], tmpj[k]=c*A[j][k]+s*A[i][k]; + memcpy(A[i], tmpi, sizeN), memcpy(A[j], tmpj, sizeN); + for (int k=0; k<N; k++) tmpi[k]=-s*Q[j][k]+c*Q[i][k], tmpj[k]=c*Q[j][k]+s*Q[i][k]; + memcpy(Q[i], tmpi, sizeN), memcpy(Q[j], tmpj, sizeN); + } + delete[] tmpi; delete[] tmpj; + transpose(N, Q); +}//QU + +//--------------------------------------------------------------------------- +/* + function Real: extracts the real part of matrix X + + In: matrix x[M][N]; + Out: matrix z[M][N] + + Returns pointer to z. z is created anew if z=0 is specified on start. +*/ +double** Real(int M, int N, double** z, cdouble** x, MList* List) +{ + if (!z){Allocate2(double, M, N, z); if (List) List->Add(z, 2);} + for (int m=0; m<M; m++) for (int n=0; n<N; n++) z[m][n]=x[m][n].x; + return z; +}//Real +double** Real(int M, int N, cdouble** x, MList* List){return Real(M, N, 0, x, List);} + +//--------------------------------------------------------------------------- +/* + function Roots: finds the roots of a polynomial. x^N+p[N-1]x^(N-1)+p[N-2]x^(N-2)...+p[0] + + In: vector p[N] of polynomial coefficients. + Out: vector r[N] of roots. + + Returns 0 if successful. +*/ +int Roots(int N, double* p, cdouble* r) +{ + double** A=new double*[N]; A[0]=new double[N*N]; for (int i=1; i<N; i++) A[i]=&A[0][i*N]; + for (int i=0; i<N; i++) A[0][i]=-p[N-1-i]; + if (N>1) memset(A[1], 0, sizeof(double)*N*(N-1)); + for (int i=1; i<N; i++) A[i][i-1]=1; + BalanceSim(N, A); + double result=QR(N, A, r); + delete[] A[0]; delete[] A; + return result; +}//Roots +//real implementation +int Roots(int N, double* p, double* rr, double* ri) +{ + cdouble* r=new cdouble[N]; + int result=Roots(N, p, r); + for (int n=0; n<N; n++) rr[n]=r[n].x, ri[n]=r[n].y; + delete[] r; + return result; +}//Roots + +//--------------------------------------------------------------------------- +/* + function SorI: Sor iteration algorithm for solving linear system Ax=b. + + Sor method is an extension of the Gaussian-Siedel method, with the latter equivalent to the former + with w set to 1. The Sor iteration is given by x(k)=(D-wL)^(-1)(((1-w)D+wU)x(k-1)+wb), where 0<w<2, D + is diagonal, L is lower triangular, U is upper triangular and A=L+D+U. Sor method converges if A is + positive definite. + + In: matrix A[N][N], vector b[N], initial vector x0[N] + Out: vector x0[N] + + Returns 0 if successful. Contents of matrix A and vector b are unchanged on return. +*/ +int SorI(int N, double* x0, double** a, double* b, double w, double ep, int maxiter) +{ + double e, v=1-w, *x=new double[N]; + int k=0, sizeN=sizeof(double)*N; + while (k<maxiter) + { + for (int i=0; i<N; i++) + { + x[i]=b[i]; + for (int j=0; j<i; j++) x[i]-=a[i][j]*x[j]; + for (int j=i+1; j<N; j++) x[i]-=a[i][j]*x0[j]; + x[i]=v*x0[i]+w*x[i]/a[i][i]; + } + e=0; for (int j=0; j<N; j++) e+=fabs(x[j]-x0[j]); + memcpy(x0, x, sizeN); + if (e<ep) break; + k++; + } + delete[] x; + if (k>=maxiter) return 1; + return 0; +}//SorI + +//--------------------------------------------------------------------------- +//Submatrix routines + +/* + function SetSubMatrix: copy matrix x[Y][X] into matrix z at (Y1, X1). + + In: matrix x[Y][X], matrix z with dimensions no less than [Y+Y1][X+X1] + Out: matrix z, updated. + + No return value. +*/ +void SetSubMatrix(double** z, double** x, int Y1, int Y, int X1, int X) +{ + for (int y=0; y<Y; y++) memcpy(&z[Y1+y][X1], x[y], sizeof(double)*X); +}//SetSubMatrix +//complex version +void SetSubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X) +{ + for (int y=0; y<Y; y++) memcpy(&z[Y1+y][X1], x[y], sizeof(cdouble)*X); +}//SetSubMatrix + +/* + function SubMatrix: extract a submatrix of x at (Y1, X1) to z[Y][X]. + + In: matrix x of dimensions no less than [Y+Y1][X+X1] + Out: matrix z[Y][X]. + + Returns pointer to z. z is created anew if z=0 is specifid on start. +*/ +cdouble** SubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X, MList* List) +{ + if (!z) {Allocate2(cdouble, Y, X, z); if (List) List->Add(z, 2);} + for (int y=0; y<Y; y++) memcpy(z[y], &x[Y1+y][X1], sizeof(cdouble)*X); + return z; +}//SetSubMatrix +//wrapper function +cdouble** SubMatrix(cdouble** x, int Y1, int Y, int X1, int X, MList* List) +{ + return SubMatrix(0, x, Y1, Y, X1, X, List); +}//SetSubMatrix + +/* + function SubVector: extract a subvector of x at X1 to z[X]. + + In: vector x no shorter than X+X1. + Out: vector z[X]. + + Returns pointer to z. z is created anew if z=0 is specifid on start. +*/ +cdouble* SubVector(cdouble* z, cdouble* x, int X1, int X, MList* List) +{ + if (!z){z=new cdouble[X]; if (List) List->Add(z, 1);} + memcpy(z, &x[X1], sizeof(cdouble)*X); + return z; +}//SubVector +//wrapper function +cdouble* SubVector(cdouble* x, int X1, int X, MList* List) +{ + return SubVector(0, x, X1, X, List); +}//SubVector + +//--------------------------------------------------------------------------- +/* + function transpose: matrix transpose: A'->A + + In: matrix a[N][N] + Out: matrix a[N][N] after transpose + + No return value. +*/ +void transpose(int N, double** a) +{ + double tmp; + for (int i=1; i<N; i++) for (int j=0; j<i; j++) {tmp=a[i][j]; a[i][j]=a[j][i]; a[j][i]=tmp;} +}//transpose +//complex version +void transpose(int N, cdouble** a) +{ + cdouble tmp; + for (int i=1; i<N; i++) for (int j=0; j<i; j++) {tmp=a[i][j]; a[i][j]=a[j][i]; a[j][i]=tmp;} +}//transpose + +/* + function transpose: matrix transpose: A'->Z + + In: matrix a[M][N] + Out: matrix z[N][M] + + Returns pointer to z. z is created anew if z=0 is specifid on start. +*/ +double** transpose(int N, int M, double** ta, double** a, MList* List) +{ + if (!ta) {Allocate2(double, N, M, ta); if (List) List->Add(ta, 2);} + for (int n=0; n<N; n++) for (int m=0; m<M; m++) ta[n][m]=a[m][n]; + return ta; +}//transpose +//wrapper function +double** transpose(int N, int M, double** a, MList* List) +{ + return transpose(N, M, 0, a, List); +}//transpose + +//--------------------------------------------------------------------------- +/* + function Unitary: given x & y s.t. |x|=|y|, find unitary matrix P s.t. Px=y. P is given in closed form + as I-(x-y)(x-y)'/(x-y)'x + + In: vectors x[N] and y[N] + Out: matrix P[N][N] + + Returns pointer to P. P is created anew if P=0 is specified on start. +*/ +double** Unitary(int N, double** P, double* x, double* y, MList* List) +{ + if (!P) {Allocate2(double, N, N, P); if (List) List->Add(P, 2);} + int sizeN=sizeof(double)*N; + for (int i=0; i<N; i++) {memset(P[i], 0, sizeN); P[i][i]=1;} + + double* w=MultiAdd(N, x, y, -1.0); //w=x-y + double m=Inner(N, x, w); //m=(x-y)'x + if (m!=0) + { + m=1.0/m; //m=1/(x-y)'x + double* mw=Multiply(N, w, m); + for (int i=0; i<N; i++) for (int j=0; j<N; j++) P[i][j]=P[i][j]-mw[i]*w[j]; + delete[] mw; + } + delete[] w; + return P; +}//Unitary +//complex version +cdouble** Unitary(int N, cdouble** P, cdouble* x, cdouble* y, MList* List) +{ + if (!P) {Allocate2(cdouble, N, N, P);} + int sizeN=sizeof(cdouble)*N; + for (int i=0; i<N; i++) {memset(P[i], 0, sizeN); P[i][i]=1;} + + cdouble *w=MultiAdd(N, x, y, -1); + cdouble m=Inner(N, x, w); + if (m!=0) + { + m=m.cinv(); + cdouble *mw=Multiply(N, w, m); + for (int i=0; i<N; i++) for (int j=0; j<N; j++) P[i][j]=P[i][j]-(mw[i]^w[j]), + delete[] mw; + } + delete[] w; + if (List) List->Add(P, 2); + return P; +}//Unitary +//wrapper functions +double** Unitary(int N, double* x, double* y, MList* List){return Unitary(N, 0, x, y, List);} +cdouble** Unitary(int N, cdouble* x, cdouble* y, MList* List){return Unitary(N, 0, x, y, List);} + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,407 @@ +#ifndef MatrixH +#define MatrixH + +/* + Matrix.cpp - matrix operations. + + Matrices are accessed by double pointers as MATRIX[Y][X], where Y is the row index. +*/ + +#include "xcomplex.h" +#include "arrayalloc.h" + +//--Similar balance---------------------------------------------------------- +void BalanceSim(int n, double** A); + +//--Choleski factorization--------------------------------------------------- +int Choleski(int N, double** L, double** A); + +//--matrix copy-------------------------------------------------------------- +double** Copy(int M, int N, double** Z, double** A, MList* List=0); + double** Copy(int M, int N, double** A, MList* List=0); + double** Copy(int N, double** Z, double ** A, MList* List=0); + double** Copy(int N, double ** A, MList* List=0); +cdouble** Copy(int M, int N, cdouble** Z, cdouble** A, MList* List=0); + cdouble** Copy(int M, int N, cdouble** A, MList* List=0); + cdouble** Copy(int N, cdouble** Z, cdouble** A, MList* List=0); + cdouble** Copy(int N, cdouble** A, MList* List=0); +double* Copy(int N, double* z, double* a, MList* List=0); + double* Copy(int N, double* a, MList* List=0); +cdouble* Copy(int N, cdouble* z, cdouble* a, MList* List=0); + cdouble* Copy(int N, cdouble* a, MList* List=0); + +//--matrix determinant calculation------------------------------------------- +double det(int N, double** A, int mode=0); +cdouble det(int N, cdouble** A, int mode=0); + +//--power methods for solving eigenproblems---------------------------------- +int EigPower(int N, double& l, double* x, double** A, double ep=1e-6, int maxiter=50); +int EigPowerA(int N, double& l, double* x, double** A, double ep=1e-6, int maxiter=50); +int EigPowerI(int N, double& l, double* x, double** A, double ep=1e-6, int maxiter=50); +int EigPowerS(int N, double& l, double* x, double** A, double ep=1e-6, int maxiter=50); +int EigPowerWielandt(int N, double& m, double* u, double l, double* v, double** A, double ep=1e-06, int maxiter=50); +int EigenValues(int N, double** A, cdouble* ev); +int EigSym(int N, double** A, double* d, double** Q); + +//--Gaussian elimination for solving linear systems-------------------------- +int GEB(int N, double* x, double** A, double* b); +int GESCP(int N, double* x, double** A, double*b); +void GExL(int N, double* x, double** L, double* a); +void GExLAdd(int N, double* x, double** L, double* a); +void GExL1(int N, double* x, double** L, double a); +void GExL1Add(int N, double* x, double** L, double a); + +/* + template GECP: Gaussian elimination with maximal column pivoting for solving linear system Ax=b + + In: matrix A[N][N], vector b[N] + Out: vector x[N] + + Returns 0 if successful. Contents of matrix A and vector b are destroyed on return. +*/ +template<class T, class Ta>int GECP(int N, T* x, Ta** A, T *b, Ta* logdet=0) +{ + if (logdet) *logdet=1E-302; int c, p, ip, *rp=new int[N]; for (int i=0; i<N; i++) rp[i]=i; + Ta m; + //Gaussian eliminating + for (int i=0; i<N-1; i++) + { + p=i, ip=i+1; + while (ip<N){if (fabs(A[rp[ip]][i])>fabs(A[rp[p]][i])) p=ip; ip++;} + if (A[rp[p]][i]==0) {delete[] rp; return 1;} + if (p!=i) {c=rp[i]; rp[i]=rp[p]; rp[p]=c;} + for (int j=i+1; j<N; j++) + { + m=A[rp[j]][i]/A[rp[i]][i]; + A[rp[j]][i]=0; + for (int k=i+1; k<N; k++) A[rp[j]][k]-=m*A[rp[i]][k]; + b[rp[j]]-=m*b[rp[i]]; + } + } + if (A[rp[N-1]][N-1]==0) {delete[] rp; return 1;} + //backward substitution + x[N-1]=b[rp[N-1]]/A[rp[N-1]][N-1]; + for (int i=N-2; i>=0; i--) + { + x[i]=b[rp[i]]; for (int j=i+1; j<N; j++) x[i]-=A[rp[i]][j]*x[j]; x[i]/=A[rp[i]][i]; + } + if (logdet){*logdet=log(fabs(A[rp[0]][0])); for (int n=1; n<N; n++) *logdet+=log(fabs(A[rp[n]][n]));} + delete[] rp; + return 0; +}//GECP + +//--inverse lower and upper triangular matrices------------------------------ +double GILT(int N, double** A); +double GIUT(int N, double** A); + +//--inverse matrix calculation with gaussian elimination--------------------- +double GICP(int N, double** X, double** A); +double GICP(int N, double** A); +cdouble GICP(int N, cdouble** A); +double GISCP(int N, double** X, double** A); +double GISCP(int N, double** A); + +//--Gaussian-Seidel method for solving linear systems------------------------ +int GSI(int N, double* x0, double** A, double* b, double ep=1e-4, int maxiter=50); + +//Reduction to upper Heissenberg matrix by elimination with pivoting +void Hessenb(int n, double** A); + +//--Householder algorithm converting a matrix tridiagonal-------------------- +void HouseHolder(int N, double** A); +void HouseHolder(int N, double** T, double** Q, double** A); +void HouseHolder(int n, double **A, double* d, double* sd); + +//--inner product------------------------------------------------------------ +double Inner(int N, double* x, double* y); +cdouble Inner(int N, double* x, cdouble* y); +cdouble Inner(int N, cdouble* x, cdouble* y); +cdouble Inner(int N, cfloat* x, cdouble* y); +cfloat Inner(int N, cfloat* x, cfloat* y); +double Inner(int M, int N, double** X, double** Y); + +/* + template Inner: Inner product <xw, y> + + In: vectors x[N], w[N] and y[N] + + Returns inner product of xw and y. +*/ +template<class Tx, class Tw>cdouble Inner(int N, Tx* x, Tw* w, cdouble* y) +{ + cdouble result=0; + for (int i=0; i<N; i++) result+=(x[i]*w[i])**y[i]; + return result; +}//Inner +template<class Tx, class Tw>cdouble Inner(int N, Tx* x, Tw* w, double* y) +{ + cdouble result=0; + for (int i=0; i<N; i++) result+=x[i]*w[i]*y[i]; + return result; +}//Inner + +//--Jacobi iterative method for solving linear systems----------------------- +int JI(int N, double* x, double** A, double* b, double ep=1e-4, int maxiter=50); + +//--LDL factorization of a symmetric matrix---------------------------------- +int LDL(int N, double** L, double* d, double** A); + +//--LQ factorization by Gram-Schmidt----------------------------------------- +void LQ_GS(int M, int N, double** A, double** L, double** Q); + +//--1-stage Least-square solution of overdetermined linear system------------ +/* + template LSLinera: direct LS solution of A[M][N]x[N]=y[M], M>=N. + + In: matrix A[M][N], vector y[M] + Out: vector x[N] + + Returns the log determinant of AtA. Contents of matrix A and vector y are unchanged on return. +*/ +template <class T> T LSLinear(int M, int N, T* x, T** A, T* y) +{ + MList* mlist=new MList; + T** AtA=MultiplyXtX(N, M, A, mlist); + T* Aty=MultiplyXty(N, M, A, y, mlist); + T result; GECP(N, x, AtA, Aty, &result); + delete mlist; + return result; +}//LSLinear + +//--2-stage Least-square solution of overdetermined linear system------------ +void LSLinear2(int M, int N, double* x, double** A, double* y); + +//--LU factorization of a non-singular matrix-------------------------------- +int LU_Direct(int mode, int N, double* diag, double** a); +int LU(int N, double** l, double** u, double** a); void LU_DiagL(int N, double** l, double* diagl, double** u, double** a); +int LU_PD(int N, double** b); +int LU_PD(int N, double** b, double* c); +double LUCP(double **a, int N, int *ind, int& parity, int mode=1); + +//--LU factorization method for solving a linear system---------------------- +void LU(int N, double* x, double** A, double* y, int* ind=0); + +//--find maximum of vector--------------------------------------------------- +int maxind(double* data, int from, int to); + +//--matrix linear combination------------------------------------------------ +/* + template MultiAdd: matrix linear combination Z=X+aY + + In: matrices X[M][N], Y[M][N], scalar a + Out: matrix Z[M][N] + + Returns pointer to Z. Z is created anew if Z=0 is specified on start. +*/ +template<class Tz, class Tx, class Ty, class Ta> Tz** MultiAdd(int M, int N, Tz** Z, Tx** X, Ty** Y, Ta a, MList* List=0) +{ + if (!Z){Allocate2(Tz, M, N, Z); if (List) List->Add(Z, 2);} + for (int i=0; i<M; i++) for (int j=0; j<N; j++) Z[i][j]=X[i][j]+a*Y[i][j]; + return Z; +}//MultiAdd + +/* + template MultiAdd: vector linear combination z=x+ay + + In: vectors x[N], y[N], scalar a + Out: vector z[N] + + Returns pointer to z. z is created anew if z=0 is specified on start. +*/ +template<class Tz, class Tx, class Ty, class Ta> Tz* MultiAdd(int N, Tz* z, Tx* x, Ty* y, Ta a, MList* List=0) +{ + if (!z){z=new Tz[N]; if (List) List->Add(z, 1);} + for (int i=0; i<N; i++) z[i]=x[i]+Ty(a)*y[i]; + return z; +}//MultiAdd +template<class Tx, class Ty, class Ta> Tx* MultiAdd(int N, Tx* x, Ty* y, Ta a, MList* List=0) +{ + return MultiAdd(N, (Tx*)0, x, y, a, List); +}//MultiAdd + +//--matrix multiplication by constant---------------------------------------- +/* + template Multiply: matrix-constant multiplication Z=aX + + In: matrix X[M][N], scalar a + Out: matrix Z[M][N] + + Returns pointer to Z. Z is created anew if Z=0 is specified on start. +*/ +template<class Tz, class Tx, class Ta> Tz** Multiply(int M, int N, Tz** Z, Tx** X, Ta a, MList* List=0) +{ + if (!Z){Allocate2(Tz, M, N, Z); if (List) List->Add(Z, 2);} + for (int i=0; i<M; i++) for (int j=0; j<N; j++) Z[i][j]=a*X[i][j]; + return Z; +}//Multiply + +/* + template Multiply: vector-constant multiplication z=ax + + In: matrix x[N], scalar a + Out: matrix z[N] + + Returns pointer to z. z is created anew if z=0 is specified on start. +*/ +template<class Tz, class Tx, class Ta> Tz* Multiply(int N, Tz* z, Tx* x, Ta a, MList* List=0) +{ + if (!z){z=new Tz[N]; if (List) List->Add(z, 1);} + for (int i=0; i<N; i++) z[i]=x[i]*a; + return z; +}//Multiply +template<class Tx, class Ta>Tx* Multiply(int N, Tx* x, Ta a, MList* List=0) +{ + return Multiply(N, (Tx*)0, x, a, List); +}//Multiply + +//--matrix multiplication operations----------------------------------------- +/* + macro Multiply_full: matrix-matrix multiplication z=xy and multiplication-accumulation z=z+xy. + + Each expansion of the macro defines three function templates; two are named $MULTIPLY and do matrix + multiplication only; one is named $MULTIADD and accumulates the multiplicated result on top of a + specified matrix. One of the two $MULTIPLY functions allows using a pre-allocated matrix to accept + the matrix product, while the other directly allocates a new matrix and returns the pointer. + + Functions are named after their exact functions. For example, MultiplyXYc multiplies matrix X with + the complex conjugate of matrix Y, where postfix "c" attched to Y stands for conjugate. Likewise, + the postfix "t" stands for transpose, and "h" stnads for Hermitian (conjugate transpose). + + Three dimension arguments are needed by each function template. The first and last of the three are + the number of rows and columns, respectively, of the product (output) matrix. The middle one is the + "other", common dimension of both multiplier matrices. +*/ +#define Multiply_full(MULTIPLY, MULTIADD, xx, yy) \ + template<class Tx, class Ty>Tx** MULTIPLY(int N, int M, int K, Tx** z, Tx** x, Ty** y, MList* List=0){ \ + if (!z) {Allocate2(Tx, N, K, z); if (List) List->Add(z, 2);} \ + for (int n=0; n<N; n++) for (int k=0; k<K; k++){ \ + Tx zz=0; for (int m=0; m<M; m++) zz+=xx*yy; z[n][k]=zz;} return z;} \ + template<class Tx, class Ty>Tx** MULTIPLY(int N, int M, int K, Tx** x, Ty** y, MList* List=0){ \ + Tx** Allocate2(Tx, N, K, z); if (List) List->Add(z, 2); \ + for (int n=0; n<N; n++) for (int k=0; k<K; k++){ \ + Tx zz=0; for (int m=0; m<M; m++) zz+=xx*yy; z[n][k]=zz;} return z;} \ + template<class Tx, class Ty>void MULTIADD(int N, int M, int K, Tx** z, Tx** x, Ty** y){ \ + for (int n=0; n<N; n++) for (int k=0; k<K; k++){ \ + Tx zz=0; for (int m=0; m<M; m++) zz+=xx*yy; z[n][k]+=zz;}} +Multiply_full(MultiplyXY, MultiAddXY, x[n][m], y[m][k]) //z[N][K]=x[N][M]y[M][K], identical z and x or y not allowed +Multiply_full(MultiplyXYc, MultiAddXYc, x[n][m], *y[m][k]) //z[N][K]=x[N][M](y*)[M][K], identical z and x or y not allowed +Multiply_full(MultiplyXYt, MultiAddXYt, x[n][m], y[k][m]) //z[N][K]=x[N][M]Yt[M][K], identical z and x or y not allowed +Multiply_full(MultiplyXYh, MultiAddXYh, x[n][m], *y[k][m]) //z[N][K]=x[N][M](Yt*)[M][K], identical z and x or y not allowed + +/* + macro Multiply_square: square matrix multiplication z=xy and multiplication-accumulation z=z+xy. + Identical z and x allowed for multiplication but not for multiplication-accumulation. + + Each expansion of the macro defines three function templates; two are named $MULTIPLY and do matrix + multiplication only; one is named $MULTIADD and accumulates the multiplicated result on top of a + specified matrix. One of the two $MULTIPLY functions allows using a pre-allocated matrix to accept + the matrix product, while the other directly allocates a new matrix and returns the pointer. +*/ +#define Multiply_square(MULTIPLY, MULTIADD) \ + template<class T>T** MULTIPLY(int N, T** z, T** x, T** y, MList* List=0){ \ + if (!z){Allocate2(T, N, N, z); if (List) List->Add(z, 2);} \ + if (z!=x) MULTIPLY(N, N, N, z, x, y); else{ \ + T* tmp=new T[N]; int sizeN=sizeof(T)*N; for (int i=0; i<N; i++){ \ + MULTIPLY(1, N, N, &tmp, &x[i], y); memcpy(z[i], tmp, sizeN);} delete[] tmp;} \ + return z;} \ + template<class T>T** MULTIPLY(int N, T** x, T** y, MList* List=0){return MULTIPLY(N, N, N, x, y, List);}\ + template<class T>void MULTIADD(int N, T** z, T** x, T** y){MULTIADD(N, N, N, z, x, y);} +Multiply_square(MultiplyXY, MultiAddXY) + +/* + macro Multiply_xx: matrix self multiplication z=xx and self-multiplication-accumulation z=z+xx. + + Each expansion of the macro defines three function templates; two are named $MULTIPLY and do matrix + multiplication only; one is named $MULTIADD and accumulates the multiplicated result on top of a + specified matrix. One of the two $MULTIPLY functions allows using a pre-allocated matrix to accept + the matrix product, while the other directly allocates a new matrix and returns the pointer. +*/ +#define Multiply_xx(MULTIPLY, MULTIADD, xx1, xx2, zzt) \ + template<class T>T** MULTIPLY(int M, int N, T** z, T** x, MList* List=0){ \ + if (!z){Allocate2(T, M, M, z); if (List) List->Add(z, 2);} \ + for (int m=0; m<M; m++) for (int k=0; k<=m; k++){ \ + T zz=0; for (int n=0; n<N; n++) zz+=xx1*xx2; z[m][k]=zz; if (m!=k) z[k][m]=zzt;} \ + return z;} \ + template<class T>T** MULTIPLY(int M, int N, T ** x, MList* List=0){ \ + T** Allocate2(T, M, M, z); if (List) List->Add(z, 2); \ + for (int m=0; m<M; m++) for (int k=0; k<=m; k++){ \ + T zz=0; for (int n=0; n<N; n++) zz+=xx1*xx2; z[m][k]=zz; if (m!=k) z[k][m]=zzt;} \ + return z;} \ + template<class T>void MULTIADD(int M, int N, T** z, T** x){ \ + for (int m=0; m<M; m++) for (int k=0; k<=m; k++){ \ + T zz=0; for (int n=0; n<N; n++) zz+=xx1*xx2; z[m][k]+=zz; if (m!=k) z[k][m]+=zzt;}} +Multiply_xx(MultiplyXtX, MultiAddXtX, x[n][m], x[n][k], zz) //z[M][M]=xt[M][N]x[N][M], identical z and x NOT ALLOWED. +Multiply_xx(MultiplyXXt, MultiAddXXt, x[m][n], x[k][n], zz) //z[M][M]=X[M][N]xt[N][M], identical z and x NOT ALLOWED. +Multiply_xx(MultiplyXhX, MultiAddXhX, *x[n][m], x[n][k], *zz) //z[M][M]=(xt*)[M][N]x[N][M], identical z and x NOT ALLOWED. +Multiply_xx(MultiplyXXh, MultiAddXXh, x[m][n], *x[k][n], *zz) //z[M][M]=x[M][N](xt*)[N][M], identical z and x NOT ALLOWED. +Multiply_xx(MultiplyXcXt, MultiAddXcXt, *x[m][n], x[k][n], *zz) //z[M][M]=(x*)[M][N]xt[N][M], identical z and x NOT ALLOWED. + +//matrix-vector multiplication routines +double* MultiplyXy(int M, int N, double* z, double** x, double* y, MList* List=0); +double* MultiplyXy(int M, int N, double** x, double* y, MList* List=0); +cdouble* MultiplyXy(int M, int N, cdouble* z, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXy(int M, int N, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXy(int M, int N, cdouble* z, double** x, cdouble* y, MList* List=0); +cdouble* MultiplyXy(int M, int N, double** x, cdouble* y, MList* List=0); +double* MultiplyxY(int M, int N, double* zt, double* xt, double** y, MList* List=0); +double* MultiplyxY(int M, int N, double* xt, double** y, MList* List=0); +cdouble* MultiplyxY(int M, int N, cdouble* zt, cdouble* xt, cdouble** y, MList* List=0); +cdouble* MultiplyxY(int M, int N, cdouble* xt, cdouble** y, MList* List=0); +double* MultiplyXty(int M, int N, double* z, double** x, double* y, MList* List=0); +double* MultiplyXty(int M, int N, double** x, double* y, MList* List=0); +cdouble* MultiplyXty(int M, int N, cdouble* z, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXty(int M, int N, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXhy(int M, int N, cdouble* z, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXhy(int M, int N, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXcy(int M, int N, cdouble* z, cdouble** x, cfloat* y, MList* List=0); +cdouble* MultiplyXcy(int M, int N, cdouble** x, cfloat* y, MList* List=0); +cdouble* MultiplyXcy(int M, int N, cdouble* z, cdouble** x, cdouble* y, MList* List=0); +cdouble* MultiplyXcy(int M, int N, cdouble** x, cdouble* y, MList* List=0); +double* MultiplyxYt(int M, int N, double* zt, double* xt, double** y, MList* MList=0); +double* MultiplyxYt(int M, int N, double* xt, double** y, MList* MList=0); + +//--matrix norms------------------------------------------------------------- +double Norm1(int N, double** A); + +//--QR factorization--------------------------------------------------------- +void QR_GS(int M, int N, double** A, double** Q, double** R); +void QR_householder(int M, int N, double** A, double** Q, double** R); + +//--QR factorization of a tridiagonal matrix--------------------------------- +int QL(int N, double* d, double* sd, double** z); +int QR(int N, double **A, cdouble* ev); + +//--QU factorization of a matrix--------------------------------------------- +void QU(int N, double** Q, double** A); + +//--Extract the real part---------------------------------------------------- +double** Real(int M, int N, double** z, cdouble** x, MList* List); +double** Real(int M, int N, cdouble** x, MList* List); + +//--Finding roots of real polynomials---------------------------------------- +int Roots(int N, double* p, double* rr, double* ri); + +//--Sor iterative method for solving linear systems-------------------------- +int SorI(int N, double* x0, double** a, double* b, double w=1, double ep=1e-4, int maxiter=50); + +//--Submatrix---------------------------------------------------------------- +void SetSubMatrix(double** z, double** x, int Y1, int Y, int X1, int X); +void SetSubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X); +cdouble** SubMatrix(cdouble** z, cdouble** x, int Y1, int Y, int X1, int X, MList* List=0); +cdouble** SubMatrix(cdouble** x, int Y1, int Y, int X1, int X, MList* List=0); +cdouble* SubVector(cdouble* z, cdouble* x, int X1, int X, MList* List=0); +cdouble* SubVector(cdouble* x, int X1, int X, MList* List=0); + +//--matrix transpose operation----------------------------------------------- +void transpose(int N, double** a); +void transpose(int N, cdouble** a); +double** transpose(int N, int M, double** ta, double** a, MList* List=0); //z[NM]=a[MN]' +double** transpose(int N, int M, double** a, MList* List=0); + +//--rotation matrix converting between vectors------------------------------- +double** Unitary(int N, double** P, double* x, double* y, MList* List=0); +double** Unitary(int N, double* x, double* y, MList* List=0); +cdouble** Unitary(int N, cdouble** P, cdouble* x, cdouble* y, MList* List=0); +cdouble** Unitary(int N, cdouble* x, cdouble* y, MList* List=0); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QuickSpec.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,349 @@ +//--------------------------------------------------------------------------- + +#include <math.h> +#include <memory.h> +#include "QuickSpec.h" + + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::TQuickSpectrogram: + + In: AParent: pointer argument for calling G, if G is specified + AnId: integer argument for calling G, if G is specified + G: pointer to a function that supplies buffers used for FFT, 0 by default + Ausex: set if complete complex spectrogram is to be buffered and accessible + Auseph: set if phase spectrogram is to be buffered and accessible +*/ +__fastcall TQuickSpectrogram::TQuickSpectrogram(void* AParent, int AnId, bool Ausex, bool Auseph, GetBuf G) +{ + memset(this, 0, sizeof(TQuickSpectrogram)); + Parent=AParent; + Id=AnId; + usex=Ausex; + useph=Auseph; + GetFFTBuffers=G; + BufferSize=QSpec_BufferSize; + fwt=wtRectangle; + Wid=1024; + Offst=512; +}//TQuickSpectrogram + +//TQuickSpectrogram::~TQuickSpectrogram +__fastcall TQuickSpectrogram::~TQuickSpectrogram() +{ + FreeBuffers(); + free8(fw); + free8(fwin); + free(fhbi); +}//~TQuickSpectrogram + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::A: accesses amplitude spectrogram by frame + + In: fr: frame index, 0-based + + Returns pointer to amplitude spectrum of the fr'th frame, NULL if N/A +*/ +QSPEC_FORMAT* __fastcall TQuickSpectrogram::A(int fr) +{ + if (Capacity==0) SetFrCapacity((DataLength-Wid)/Offst+2); + if (fr<0 || fr>=Capacity) return NULL; + if (Frame[fr]<0 || !Valid[fr]) CalculateSpectrum(fr); + return fA[Frame[fr]]; +}//A + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::AddBuffer: increases internal buffer by BufferSize frames. Allocated buffers + beyond Capacity frames will not be indexed or used by TQuickSpectrogram. +*/ +void TQuickSpectrogram::AddBuffer() +{ + int base=1, Dim=Wid/2+1; + if (usex) base+=2; + if (useph) base+=1; + QSPEC_FORMAT* newbuffer=(QSPEC_FORMAT*)malloc(sizeof(QSPEC_FORMAT)*Dim*BufferSize*base); + int fr0=BufferCount*BufferSize; + for (int i=0; i<BufferSize; i++) + { + int fr=fr0+i; + if (fr<Capacity) + { + fA[fr]=&newbuffer[i*Dim]; + int base=1; + if (usex) fSpec[fr]=(cmplx<QSPEC_FORMAT>*)&newbuffer[(BufferSize+i*2)*Dim], base+=2; + if (useph) fPh[fr]=&newbuffer[(BufferSize*base+i)*Dim]; + } + else break; + } + BufferCount++; +}//AddBuffer + +/* + method TQuickSpectrogram::AddBuffer: increase internal buffer by a multiple of BufferSize so that + it will be enough to host another AddFrCount frames. +*/ +void TQuickSpectrogram::AddBuffer(int AddFrCount) +{ + while (FrCount+AddFrCount>BufferSize*BufferCount) AddBuffer(); +}//AddBuffer + +//--------------------------------------------------------------------------- +/* + 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. +*/ +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 if (BytesPerSample==2) {__int16* in16=(__int16*)in; for (int k=0; k<Count; k++) *(out++)=*(in16++);} + else {__pint24 in24=(__pint24)in; for (int k=0; k<Count; k++) *(out++)=*(in24++);} +}//IntToDouble + +/* + function CalculateSpectrum: calculate spectrum of a signal in integer format + + In: Data[Wid]: integer array hosting waveform data + BytesPerSample: number of bytes each integer in Data[] takes + win[Wid]: window function used for computing spectrum + w[Wid/2], x[Wid], hbi[Wid/2]: FFT buffers + Out: x[Wid]: complex spectrum + Amp[Wid/2+1]: amplitude spectrum + Arg[Wid/2+1]: phase spectrum, optional + + No return value. +*/ +void CalculateSpectrum(void* Data, int BytesPerSample, double* win, QSPEC_FORMAT* Amp, QSPEC_FORMAT* Arg, int Wid, cdouble* w, cdouble* x, int* hbi) +{ + if (BytesPerSample==2) RFFTCW((__int16*)Data, win, 0, 0, log2(Wid), w, x, hbi); + else {IntToDouble((double*)x, Data, BytesPerSample, Wid); RFFTCW((double*)x, win, 0, 0, log2(Wid), w, x, hbi);} + for (int j=0; j<=Wid/2; j++) + { + Amp[j]=sqrt(x[j].x*x[j].x+x[j].y*x[j].y); + if (Arg) Arg[j]=(x[j].y==0 && x[j].x==0)?0:atan2(x[j].y, x[j].x); + } +}//CalculateSpectrum + +/* + function CalculateSpectrum: calculate spectrum of a signal in integer format, allowing the signal + length $eff be shorter than the DFT size Wid. + + In: Data[eff]: integer array hosting waveform data + BytesPerSample: number of bytes each integer in Data[] takes + win[Wid]: window function used for computing spectrum + w[Wid/2], x[Wid], hbi[Wid/2]: FFT buffers + Out: x[Wid]: complex spectrum + Amp[Wid/2+1]: amplitude spectrum + Arg[Wid/2+1]: phase spectrum, optional + + No return value. +*/ +void CalculateSpectrum(void* Data, int BytesPerSample, double* win, QSPEC_FORMAT* Amp, QSPEC_FORMAT* Arg, int Wid, int eff, cdouble* w, cdouble* x, int* hbi) +{ + if (eff<=0) + { + memset(Amp, 0, sizeof(double)*(Wid/2+1)); + if (Arg) memset(Arg, 0, sizeof(double)*(Wid/2+1)); + } + else if (eff<Wid) + { + double* doublex=(double*)x; + IntToDouble(doublex, Data, BytesPerSample, eff); memset(&doublex[eff], 0, sizeof(double)*(Wid-eff)); + RFFTCW(doublex, win, 0, 0, log2(Wid), w, x, hbi); + for (int j=0; j<=Wid/2; j++) + { + Amp[j]=sqrt(x[j].x*x[j].x+x[j].y*x[j].y); + if (Arg) Arg[j]=(x[j].y==0 && x[j].x==0)?0:atan2(x[j].y, x[j].x); + } + } + else + CalculateSpectrum(Data, BytesPerSample, win, Amp, Arg, Wid, w, x, hbi); +}//CalculateSpectrum + +/* + method TQuickSpectrogram::CalculateSpectrum: computes spectrogram at fr'th frame. + + In: fr: index to the frame whose spectrum is to be computed. fr must be between 0 and Capacity-1. +*/ +void __fastcall TQuickSpectrogram::CalculateSpectrum(int fr) +{ + cdouble *w, *x; + double* win; + int* hbi; + + //obtain FFT buffers win (window function), w (twiddle factors), x (data buffer), + //hbi (half-size bit-inversed integer table) + if (GetFFTBuffers) //then use external buffers provided through GetFFTBuffers + GetFFTBuffers(Id, w, x, win, hbi, Parent); + else //then use internal buffers + { + if (Wid!=fWid) + { //then update internal buffers to the new window size + free8(fw); free(fhbi); + fw=(cdouble*)malloc8(sizeof(cdouble)*Wid*1.5); SetTwiddleFactors(Wid, fw); + fx=&fw[Wid/2]; + fhbi=CreateBitInvTable(log2(Wid)-1); + } + if (Wid!=fWid || WinType!=fwt || WinParam!=fwdp) + { //then update internal window function to the new window type + fwin=NewWindow8(WinType, Wid, 0, &WinParam); + fwt=WinType; fwdp=WinParam; + } + fWid=Wid; + + //pick up the internal buffers + w=fw, x=fx, win=fwin, hbi=fhbi; + } + + //obtain the index of this frame in internal storage + if (Frame[fr]<0) {AddBuffer(1); Frame[fr]=FrCount; FrCount++;} + int realfr=Frame[fr]; + + //obtain the pointer to this frame's phase spectrum + QSPEC_FORMAT *lph=useph?fPh[realfr]:NULL; + //ontain the pointer to this frame's complex spectrum + cmplx<QSPEC_FORMAT>* lX=usex?fSpec[realfr]:NULL; + //choose the buffer actually used for FFT - use lX if it is specified as complex double array + //because it saves unnecessary data copying operations + cdouble *lx=(usex && sizeof(QSPEC_FORMAT)==sizeof(double))?(cdouble*)lX:x; + + //Actual FFT + if (fr*Offst+Wid<=DataLength) + ::CalculateSpectrum(&((char*)Data)[fr*Offst*BytesPerSample], BytesPerSample, win, fA[realfr], lph, Wid, w, lx, hbi); + else + ::CalculateSpectrum(&((char*)Data)[fr*Offst*BytesPerSample], BytesPerSample, win, fA[realfr], lph, Wid, DataLength-fr*Offst, w, x, hbi); + + //optional data copy from x to lX + if (usex && lx==x) for (int i=0; i<Wid/2+1; i++) lX[i]=x[i]; + + //tag this frame as computed and valid + Valid[fr]=1; +}//CalculateSpectrum + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::FreeBuffers: discards all computed spectra and free all internal buffers. + This returns the TQuickSpectrogram to its initial state before any frame is accessed. After calling + FreeBuffers() all frames will be recomputed when they are accessed. +*/ +void TQuickSpectrogram::FreeBuffers() +{ + if (fA) + { + for (int i=0; i<BufferCount; i++) free(fA[i*BufferSize]); + FrCount=BufferCount=Capacity=0; + free(Frame); free(Valid); + free(fA); + Frame=Valid=0, fA=0; + if (useph) {free(fPh); fPh=0;} + if (usex) {free(fSpec); fSpec=0;} + } +}//FreeBuffers + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::Invalidate: renders all frames that have overlap with interval [From, To], + measured in samples, as invalid. Invalid frames are recomputed when they are accessed again. + + In: [From, To]: an interval spectrogram over which needs to be updated. + + Returns the number of allocated frames affected, no matter if they were valid. +*/ +int TQuickSpectrogram::Invalidate(int From, int To) +{ + int result=0; + if (Frame) + { + int fr1=ceil((From-Wid+1.0)/Offst), fr2=floor(1.0*To/Offst); + if (fr1<0) fr1=0; + if (fr2>=Capacity) fr2=Capacity-1; + for (int fr=fr1; fr<=fr2; fr++) if (Frame[fr]>=0) Valid[fr]=false, result++; + } + return result; +}//Invalidate + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::Ph: accesses phase spectrogram by frame + + In: fr: frame index, 0-based + + Returns pointer to phase spectrum of the fr'th frame, NULL if N/A +*/ +QSPEC_FORMAT* __fastcall TQuickSpectrogram::Ph(int fr) +{ + if (Capacity==0) SetFrCapacity((DataLength-Wid)/Offst+2); + if (fr<0 || fr>=Capacity) return NULL; + if (Frame[fr]<0 || !Valid[fr]) CalculateSpectrum(fr); + return fPh[Frame[fr]]; +}//Ph + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::SetFrCapacity: sets the capacity, i.e. the maximal number of frames handled + by this TQuickSpectrogram. + + In: AnFrCapacity: the new Capacity, in frames + + This method should not be called to set Capacity to a smaller value. +*/ +void TQuickSpectrogram::SetFrCapacity(int AnFrCapacity) +{ + //adjusting the size of index and validity arrays + Frame=(int*)realloc(Frame, sizeof(int)*AnFrCapacity); + Valid=(int*)realloc(Valid, sizeof(int)*AnFrCapacity); + + // + fA=(QSPEC_FORMAT**)realloc(fA, sizeof(QSPEC_FORMAT*)*AnFrCapacity); + if (usex) fSpec=(cmplx<QSPEC_FORMAT>**)realloc(fSpec, sizeof(cmplx<QSPEC_FORMAT>*)*AnFrCapacity); + if (useph) fPh=(QSPEC_FORMAT**)realloc(fPh, sizeof(QSPEC_FORMAT*)*AnFrCapacity); + if (AnFrCapacity>Capacity) + { + memset(&Frame[Capacity], 0xFF, sizeof(int)*(AnFrCapacity-Capacity)); + memset(&Valid[Capacity], 0x00, sizeof(int)*(AnFrCapacity-Capacity)); + + if (Capacity<BufferCount*BufferSize) + { + for (int fr=Capacity; fr<AnFrCapacity; fr++) + { + int bufferno=fr/BufferSize; + if (bufferno<BufferCount) + { + QSPEC_FORMAT* thisbuffer=fA[BufferSize*bufferno]; + int lfr=fr%BufferSize, base=1, Dim=Wid/2+1; + fA[fr]=&thisbuffer[lfr*Dim]; + if (usex) fSpec[fr]=(cmplx<QSPEC_FORMAT>*)(&thisbuffer[(BufferSize+lfr*2)*Dim]), base+=2; + if (useph) fPh[fr]=&thisbuffer[(BufferSize*base+lfr)*Dim]; + } + else break; + } + } + } + Capacity=AnFrCapacity; +}//SetFrCapacity + +//--------------------------------------------------------------------------- +/* + method TQuickSpectrogram::Ph: accesses complex spectrogram by frame + + In: fr: frame index, 0-based + + Returns pointer to complex spectrum of the fr'th frame, NULL if N/A +*/ +cmplx<QSPEC_FORMAT>* __fastcall TQuickSpectrogram::Spec(int fr) +{ + if (Capacity==0) SetFrCapacity((DataLength-Wid)/Offst+2); + if (fr<0 || fr>=Capacity) return NULL; + if (Frame[fr]<0 || !Valid[fr]) CalculateSpectrum(fr); + return fSpec[Frame[fr]]; +}//Spec + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QuickSpec.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,158 @@ +#ifndef QuickSpecH +#define QuickSpecH + +/* + QuickSpec.cpp - TQuickSpectrogram class + + TQuickSpectrogram implements a compute-on-request spectrogram class. Every time one frame of the + spectrogram is read from this class, it checks if the single-frame spectrogram has been computed + already, computes it if not, and return the pointer to the spectrum. + + Further reading: "A buffering technique for real-time spectrogram displaying.pdf" +*/ + + +#include <cstddef> +#include <stdlib.h> +#include "xcomplex.h" +#include "windowfunctions.h" +#include "fft.h" +//--------------------------------------------------------------------------- + +#define QSPEC_FORMAT float //default spectral data format +#define QSpec_BufferSize 1024 //default initial buffer size, in frames, of TQuickSpectrogram + + +/* + __int24 is a 24bit signed integer type and __pint24 is its pointer type. Although __int24* will also + return a pointer to an __int24 structure, operations based on __int24* may have unspecified results, + depending on structure alignments imposed by compiler. It is therefore necessary to have an explicit + pointer type __pint24 to enforce 24-bit data alighment. + + Using __int24: + When storage format is not a concern, __int24 can be used the same way as __int16 or __int32. However, + a default 32-bit compiler may fail to implement compact 24-bit alignment to entries of an __int24[] + array. If 24-bit alignment is desired, then always create the array dynamically with new[], which + returns a __pint24 type pointer. Assigning a __pint24 type pointer to __int24* type variable should + be avoided. +*/ + +#ifndef INT24 +#define INT24 +struct __int24; +struct __pint24 +{ + char* p; + __fastcall __pint24(){} + __fastcall __pint24(const void* ap){p=(char*)ap;} + __pint24& __fastcall operator=(const void* ap){p=(char*)ap; return *this;} + __int24& __fastcall operator*(){return *(__int24*)p;} + __int24& __fastcall operator[](int index){return *(__int24*)&p[3*index];} + __pint24 __fastcall operator++(int){__pint24 result=*this; p+=3; return result;} + __pint24& __fastcall operator++(){p+=3; return *this;} + __pint24 __fastcall operator--(int){__pint24 result=*this; p-=3; return result;} + __pint24& __fastcall operator--(){p-=3; return *this;} + __pint24& __fastcall operator+=(int a){p+=3*a; return *this;} + __pint24& __fastcall operator-=(int a){p-=3*a; return *this;} + __fastcall operator void*() const{return p;} +}; +struct __int24 +{ + __int16 loword; + __int8 hibyte; + __fastcall __int24(){} + __fastcall __int24(const __int32 a){loword=*(__int16*)&a; hibyte=((__int16*)&a)[1];} + __fastcall __int24(const double f){__int32 a=f; loword=*(__int16*)&a; hibyte=((__int16*)&a)[1];} + __int24& __fastcall operator=(const __int32 a){loword=*(__int16*)&a; hibyte=((__int16*)&a)[1]; return *this;} + __int24& __fastcall operator=(const double f){__int32 a=f; loword=*(__int16*)&a; hibyte=((__int16*)&a)[1]; return *this;} + __fastcall operator __int32() const{__int32 result; *(__int16*)&result=loword; ((__int16*)&result)[1]=hibyte; return result;} + __pint24 operator &(){return (__pint24)this;} + void* operator new[](size_t count){void* result=malloc(3*count); return result;} + void operator delete[](void* p){free(p);} +}; +#endif + +/* + TQuickSpectrogram is a spectrogram class the handles the computation and storage of a spectrogram. + + Using TQuickSpectrogram: + + TQuickSpectrogram provides read-only access to the spectrogram of a given waveform. The spectrogram + contains a sequence of windowed Fourier transforms, computed from uniformly placed frames. The 0th + frame starts at 0 (inclusive) and finishes at Wid (exclusive). Each spectrum has Wid/2+1 entries. + + Follow these steps: + 1. create a QuickSpectrogram, specifying usex and useph, and optionally, external buffer provide G + and its arguments Id and Parent; + 2. set Data, DataLength and BytesPerSample; + 3. set frame size and hop size Wid and Offst; + 4. if G is not specified, set window type (optional extra parameter) for computing spectra; + 5. access the spectrogram via A(fr), Spec(fr) (optional) and Ph(fr) (optional). + Steps 2~4 do not have to follow the given order. + + Call Invalidate() to notify the object of changes of waveform content. + Call FreeBuffers() to return the object to the initial state before step 2. +*/ + +typedef void (*GetBuf)(int Id, cdouble* &w, cdouble* &x, double* &win, int* &hbi, void* Parent); +class TQuickSpectrogram +{ + int BufferCount; //number of buffers in use + int BufferSize; //number of frames each additional buffer holds + int FrCount; //number of allocated frames + + //internal storage of spectrogram + QSPEC_FORMAT** fPh; //phase spectrogram, optional + QSPEC_FORMAT** fA; //amplitude spectrogram, compulsory + cmplx<QSPEC_FORMAT>** fSpec; //complete complex spectrogram, optional + + //internal buffers (optional) for FFT + WindowType fwt; //type of window + int fWid; //size of window + int* fhbi; //half-size bit-inversed integer table + double fwdp; //additional parameter specifying window type + double* fwin; //internal window + cdouble* fw; //FFT twiddle factors + cdouble* fx; //FFT data buffer + + //x and ph create-time switch + bool usex; //set at create time if complex spectrogram is required + bool useph; //set at create time if phase spectrogram is required + + //internal methods + void AddBuffer(); + void AddBuffer(int AddFrCount); + void __fastcall CalculateSpectrum(int fr); + void SetFrCapacity(int AnFrCapacity); + +public: + //if specified, Parent is responsible to supply FFT buffers through GetFFTBuffers (optional) + int Id; //an identifier given at create time, used as argument for calling GetFFTBuffers() + void* Parent; //a pointer used as argument for calling GetFFTBuffers() + GetBuf GetFFTBuffers; //if specified, this supplies FFT buffers + + //index and validity arrays associated with internal storage + int Capacity; //size of $Frame[] and &Valid[], usually set to the total number of frames of the data + int* Frame; //indices to individual frames in internal storage + int* Valid; //validity tags to individual frames in internal storage + + WindowType WinType; //window type for computing spectrogram + double WinParam; //additional parameter specifying certain window types (Gaussian, Kaiser, etc.) + + void* Data; //pointer to waveform audio + int DataLength; //length of waveform audio, in samples + int BytesPerSample; //bytes per sample of waveform audio + + int Offst; //frame offset + int Wid; //frame size, the same as window size + + __fastcall TQuickSpectrogram(void* AParent, int AnId, bool Ausex, bool Auseph, GetBuf G); + __fastcall ~TQuickSpectrogram(); + + QSPEC_FORMAT* __fastcall A(int fr); //accesses amplitude spectrogram at frame fr + void FreeBuffers(); //discards all computed frames and free memory + int Invalidate(int From, int To); //discards computed frames + QSPEC_FORMAT* __fastcall Ph(int fr); //accesses phase spectrogram at frame fr + cmplx<QSPEC_FORMAT>* __fastcall Spec(int fr); //accesses complex spectrogram at frame fr +}; +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SinEst.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,3970 @@ +//--------------------------------------------------------------------------- + +#include <stddef.h> +#include "SinEst.h" +#include "fft.h" +#include "opt.h" +#include "SinSyn.h" +#include "splines.h" +#include "WindowFunctions.h" + +//--------------------------------------------------------------------------- +/* + function dsincd_unn: derivative of unnormalized discrete sinc function + + In: x, scale N + + Returns the derivative of sincd_unn(x, N) +*/ +double dsincd_unn(double x, int N) +{ + double r=0; + double omg=M_PI*x; + double domg=omg/N; + if (fabs(x)>1e-6) + { + r=M_PI*(cos(omg)-sin(omg)*cos(domg)/sin(domg)/N)/sin(domg); + } + else + { + if (domg!=0) + { + double sindomg=sin(domg); + r=-omg*omg*omg*(1-1.0/(1.0*N*N))/3*M_PI/N/sindomg/sindomg; + } + else + r=0; + } + return r; +}//dsincd_unn + +/* + function ddsincd_unn: 2nd-order derivative of unnormalized discrete sinc function + + In: x, scale (equivalently, window size) N + + Returns the 2nd-order derivative of sincd_unn(x, N) +*/ +double ddsincd_unn(double x, int N) +{ + double r=0; + double omg=M_PI*x; + double domg=omg/N; + double PI2=M_PI*M_PI; + double NN=1.0/N/N-1; + if (domg==0) + { + r=PI2*N*NN/3; + } + else + { + if (fabs(x)>1e-5) + { + r=sin(domg)*cos(omg)-sin(omg)*cos(domg)/N; + } + else + { + r=omg*omg*omg/N*NN/3; + } + double ss=sin(omg)*NN; + r=-2.0/N*cos(domg)*r/sin(domg)/sin(domg)+ss; + r=r*PI2/sin(domg); + } + return r; +}//ddsincd_unn + +//--------------------------------------------------------------------------- +/* + function Window: calculates the cosine-family-windowed spectrum of a complex sinusoid on [0:N-1] at + frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2. + + Returns pointer to x. x is created anew if x=0 is specified on start. +*/ +cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + + if (!x) x=new cdouble[K2-K1+1]; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l)*M_PI; + double omg=ang/N; + long double si, co, sinn=sin(ang); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si); + double saco=sa*co; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + } + } + } + return x; +}//Window + +/* + function dWindow: calculates the cosine-family-windowed spectrum and its derivative of a complex + sinusoid on [0:N-1] at frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2, + dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2 + + No return value. +*/ +void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + memset(dx, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l), Omg=ang*M_PI, omg=Omg/N; + long double si, co, sinn=sin(Omg), cosn=cos(Omg); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N); + double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, cosnpi=cosn*M_PI; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + dx[kt].x-=c[m]*(-sinnpi_n+dsaco); + dx[kt].y+=c[m]*cosnpi; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + dx[kt].x+=c[m]*(-sinnpi_n+dsaco); + dx[kt].y-=c[m]*cosnpi; + } + } + } +}//dWindow + +/* + function ddWindow: calculates the cosine-family-windowed spectrum and its 1st and 2nd derivatives of + a complex sinusoid on [0:N-1] at frequency f bins with zero central phase. + + In: f: frequency, in bins + N: window size + M, c[]: cosine-family window decomposition coefficients + Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2, + dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2 + ddx[0...K2-K1] containing the 2nd-order derivative spectrum at bins K1, ..., K2 + + No return value. +*/ +void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2) +{ + if (K1<0) K1=0; + if (K2>N/2-1) K2=N/2-1; + memset(x, 0, sizeof(cdouble)*(K2-K1+1)); + memset(dx, 0, sizeof(cdouble)*(K2-K1+1)); + memset(ddx, 0, sizeof(cdouble)*(K2-K1+1)); + + for (int l=K1-M; l<=K2+M; l++) + { + double ang=(f-l), Omg=ang*M_PI, omg=Omg/N; + long double si, co, sinn=sin(Omg), cosn=cos(Omg); + si=sin(omg), co=cos(omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N); + double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, sinnpipi=sinn*M_PI*M_PI, cosnpi=cosn*M_PI, + cosnpipi_n=cosnpi*M_PI/N, sipi_n=si*M_PI/N; + + int k1=l-M, k2=l+M; + if (k1<K1) k1=K1; + if (k2>K2) k2=K2; + + for (int k=k1; k<=k2; k++) + { + int m=k-l, kt=k-K1; + if (m<0) m=-m; + if (k%2) + { + x[kt].x-=c[m]*saco; + x[kt].y+=c[m]*sinn; + dx[kt].x-=c[m]*(-sinnpi_n+dsaco); + dx[kt].y+=c[m]*cosnpi; + ddx[kt].x-=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n); + ddx[kt].y-=c[m]*sinnpipi; + } + else + { + x[kt].x+=c[m]*saco; + x[kt].y-=c[m]*sinn; + dx[kt].x+=c[m]*(-sinnpi_n+dsaco); + dx[kt].y-=c[m]*cosnpi; + ddx[kt].x+=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n); + ddx[kt].y+=c[m]*sinnpipi; + } + } + } +}//ddWindow + +//--------------------------------------------------------------------------- +/* + function IPWindow: computes the truncated inner product of a windowed spectrum with that of a sinusoid + at reference frequency f. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + returnamplitude: specifies return value, true for amplitude, false for angle + + Returns the amplitude or phase of the inner product, as specified by $returnamplitude. The return + value is interpreted as the actual amplitude/phase of a sinusoid being estimated at f. +*/ +double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude) +{ + cdouble r=IPWindowC(f, x, N, M, c, iH2, K1, K2); + double result; + if (returnamplitude) result=sqrt(r.x*r.x+r.y*r.y); + else result=arg(r); + return result; +}//IPWindow +//wrapper function +double IPWindow(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params; + return IPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, true); +}//IPWindow + +/* + function ddIPWindow: computes the norm of the truncated inner product of a windowed spectrum with + that of a sinusoid at reference frequency f, as well as its 1st and 2nd derivatives. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: ipwindow and dipwindow: the truncated inner product norm and its derivative + + Returns the 2nd derivative of the norm of the truncated inner product. +*/ +double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2], *lx=&x[K1]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + delete[] w; + + double R2=~r, + R=sqrt(R2), + dR2=2*(r.x*dr.x+r.y*dr.y), + dR=dR2/(2*R), + ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr), + ddR=(R*ddR2-dR2*dR)/(2*R2); + ipwindow=R*iH2; + dipwindow=dR*iH2; + return ddR*iH2; +}//ddIPWindow +//wrapper function +double ddIPWindow(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params; + return ddIPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow); +}//ddIPWindow + +//--------------------------------------------------------------------------- +/* + function IPWindowC: computes the truncated inner product of a windowed spectrum with that of a + sinusoid at reference frequency f. + + In: x[0:N-1]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + + Returns the inner product. The return value is interpreted as the actual amplitude-phase factor of a + sinusoid being estimated at f. +*/ +cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + cdouble *lx=&x[K1], result=0; + Window(w, f, N, M, c, K1, K2); + for (int k=0; k<K; k++) result+=lx[k]^w[k]; + delete[] w; + result*=iH2; + return result; +}//IPWindowC + +//--------------------------------------------------------------------------- +/* + function sIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f. This does not consider phase alignment + between the spectra, supposedly measured at a sequence of known instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional) + + Returns the energy of the vector of inner products. +*/ +double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd) +{ + double sip=0; + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + Window(w, f, N, M, c, K1, K2); + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w); + if (lmd) lmd[l]=r*iH2; + sip+=~r; + } + sip*=iH2; + delete[] w; + return sip; +}//sIPWindow +//wrapper function +double sIPWindow(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dipwindow; double ipwindow; cdouble* lmd;} *p=(l_ip *)params; + return sIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->lmd); +}//sIPWindow + +/* + function dsIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f, as well as its derivative. This does not + consider phase synchronization between the spectra, supposedly measured at a sequence of known + instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip, the energy of the vector of inner products. + + Returns the derivative of the energy of the vector of inner products. +*/ +double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*2], *dw=&w[K]; + dWindow(dw, w, f, N, M, c, K1, K2); + double dsip; sip=0; + for (int l=0; l<L; l++) + { + cdouble* lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw); + double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y); + sip+=R2, dsip+=dR2; + } + sip*=iH2, dsip*=iH2; + delete[] w; + return dsip; +}//dsIPWindow +//wrapper function +double dsIPWindow(double f, void* params) +{ + struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double sip;} *p=(l_ip1 *)params; + return dsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip); +}//dsIPWindow + +/* + function dsdIPWindow_unn: computes the energy of unnormalized truncated inner products between a given + windowed spectrum and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd + derivatives. "Unnormalized" indicates that the inner product cannot be taken as the actual amplitude- + phase factor of a sinusoid, but deviate from that by an unspecified factor. + + In: x[N]: input spectrum + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sipwindow and dsipwindow, the energy and its derivative of the unnormalized inner product. + + Returns the 2nd derivative of the inner product. +*/ +double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + + double rr=0, ri=0, drr=0, dri=0, ddrr=0, ddri=0; + cdouble *lx=&x[K1]; + for (int k=0; k<K; k++) + { + rr+=lx[k].x*w[k].x+lx[k].y*w[k].y; + ri+=lx[k].y*w[k].x-lx[k].x*w[k].y; + drr+=lx[k].x*dw[k].x+lx[k].y*dw[k].y; + dri+=lx[k].y*dw[k].x-lx[k].x*dw[k].y; + ddrr+=lx[k].x*ddw[k].x+lx[k].y*ddw[k].y; + ddri+=lx[k].y*ddw[k].x-lx[k].x*ddw[k].y; + } + delete[] w; + + double R2=rr*rr+ri*ri, + dR2=2*(rr*drr+ri*dri), + ddR2=2*(rr*ddrr+ri*ddri+drr*drr+dri*dri); + sipwindow=R2; + dsipwindow=dR2; + if (w_unn) w_unn->x=rr, w_unn->y=ri; + return ddR2; +}//ddsIPWindow_unn + +/* + function ddsIPWindow: computes the total energy of truncated inner products between multiple windowed + spectra and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd derivatives. + This does not consider phase synchronization between the spectra, supposedly measured at a sequence + of known instants. + + In: x[L][N]: input spectra + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip and dsip, the energy of the vector of inner products and its derivative. + + Returns the 2nd derivative of the energy of the vector of inner products. +*/ +double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + double ddsip=0; dsip=sip=0; + for (int l=0; l<L; l++) + { + cdouble* lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y), ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr); + sip+=R2, dsip+=dR2, ddsip+=ddR2; + } + sip*=iH2, dsip*=iH2, ddsip*=iH2; + delete[] w; + return ddsip; +}//ddsIPWindow +//wrapper function +double ddsIPWindow(double f, void* params) +{ + struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dsip; double sip;} *p=(l_ip1 *)params; + return ddsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dsip, p->sip); +}//ddsIPWindow + +//--------------------------------------------------------------------------- +/* + function sIPWindowC: computes the total energy of truncated inner products between multiple frames of + a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional) + + Returns the energy of the vector of inner products. +*/ +double sIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + cdouble *w=new cdouble[K]; + double Cr=0; + cdouble Cc=0; + Window(w, f, N, M, c, K1, K2); + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w); + Cr+=~r; + double ph=-4*M_PI*f*offst_rel*l; + cdouble r2=r*r; + Cc+=r2.rotate(ph); + if (lmd) lmd[l]=r; + } + delete[] w; + double result=0.5*iH2*(Cr+abs(Cc)); + if (lmd) + { + double absCc=abs(Cc), hiH2=0.5*iH2; + cdouble ej2ph=Cc/absCc; + for (int l=0; l<L; l++) + { + double ph=4*M_PI*f*offst_rel*l; + lmd[l]=hiH2*(lmd[l]+(ej2ph**lmd[l]).rotate(ph)); + } + } + return result; +}//sIPWindowC +//wrapper function +double sIPWindowC(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params; + return sIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2); +}//sIPWindowC + +/* + function dsIPWindowC: computes the total energy of truncated inner products between multiple frames of + a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, together + with its derivative. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sip: energy of the vector of the inner products + + Returns the 1st derivative of the energy of the vector of inner products. +*/ +double dsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*2], *dw=&w[K]; + dWindow(dw, w, f, N, M, c, K1, K2); + double Cr=0, dCr=0; + cdouble Cc=0, dCc=0; + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw); + Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); + int two=2; + cdouble r2=r*r, dr2=r*dr*two; + double lag=-4*M_PI*offst_rel*l, ph=lag*f; + Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph); + } + double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y); + double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1); + sip=0.5*iH2*(Cr+Cc1); + double dsip=0.5*iH2*(dCr+dCc1); + delete[] w; + return dsip; +}//dsIPWindowC +//wrapper function +double dsIPWindowC(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sip;} *p=(l_ip *)params; + return dsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip); +}//dsIPWindowC + +/* + function ddsIPWindowC: computes the total energy of truncated inner products between multiple frames + of a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, + together with its 1st and 2nd derivatives. + + In: x[L][N]: the spectrogram + offst_rel: frame offset, relative to frame size + f: reference frequency, in bins + M, c[], iH2: cosine-family window specification parameters + K1, K2: spectrum truncation bounds, in bins, inclusive + Out: sipwindow, dsipwindow: energy of the vector of the inner products and its derivative + + Returns the 2nd derivative of the energy of the vector of inner products. +*/ +double ddsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsipwindow, double& sipwindow) +{ + if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1; + int K=K2-K1+1; + + cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2]; + ddWindow(ddw, dw, w, f, N, M, c, K1, K2); + double Cr=0, dCr=0, ddCr=0; + cdouble Cc=0, dCc=0, ddCc=0; + for (int l=0; l<L; l++) + { + cdouble *lx=&x[l][K1]; + cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw); + Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); ddCr+=2*(r.x*ddr.x+r.y*ddr.y+~dr); + int two=2; + cdouble r2=r*r, dr2=r*dr*two, ddr2=(dr*dr+r*ddr)*two; + double lag=-4*M_PI*offst_rel*l, ph=lag*f; + Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph), ddCc=ddCc+(ddr2+cdouble(0,2*lag)*dr2-r2*lag*lag).rotate(ph); + } + double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y), ddCc2=2*(Cc.x*ddCc.x+Cc.y*ddCc.y+~dCc); + double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1), ddCc1=(Cc1*ddCc2-dCc2*dCc1)/(2*Cc2); + sipwindow=0.5*iH2*(Cr+Cc1); + dsipwindow=0.5*iH2*(dCr+dCc1); + double ddsipwindow=0.5*iH2*(ddCr+ddCc1); + delete[] w; + return ddsipwindow; +}//ddsIPWindowC +//wrapper function +double ddsIPWindowC(double f, void* params) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params; + return ddsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow); +}//ddsIPWindowC + +//-------------------------------------------------------------------------- +/* + Least-square-error sinusoid detection function + + version1: picking the highest peak and take measurement of a single sinusoid + version2: given a rough peak location and take measurement of a single sinusoid + + Complex spectrum x is calculated using N data points windowed by a window function that is specified + by the parameter set (M, c, iH2). c[0:M] is provided according to Table 3 in the transfer report, on + pp.11. iH2 is simply 1/H2, where H2 can be calculated using formula (2.17) on pp.12. + + f & epf are given/returned in bins. + + Further reading: "Least-square-error estimation of sinusoids.pdf" +*/ + +/* + function LSESinusoid: LSE estimation of the predominant stationary sinusoid. + + In: x[N]: windowed spectrum + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: a and pp: amplitude and phase estimates + + Returns the frequency estimate, in bins. +*/ +double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; //(l_hx *)¶ms; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int inp; + double minp=0; + for (int i=0; i<N; i++) + { + double lf=i, tmp; + 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; + tmp=IPWindow(lf, &p); + if (minp<tmp) inp=i, minp=tmp; + } + + double f=inp; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + return f; +}//LSESinusoid + +/*function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency. + + In: x[N]: windowed spectrum + f: initial frequency, in bins + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a and pp: frequency, amplitude and phase estimates + + No return value. +*/ +void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); +}//LSESinusoid + +/* + function LSESinusoid: LSE estimation of stationary sinusoid predominant within [f1, f2]. + + In: x[N]: windowed spectrum + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: a and pp: amplitude and phase estimates + + Returns the frequency estimate, in bins. +*/ +double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int inp; + double minp=0; + for (int i=f1; i<f2; i++) + { + double lf=i, tmp; + 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; + tmp=IPWindow(lf, &p); + if (minp<tmp) inp=i, minp=tmp; + } + + double f=inp; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf); + if (tmp==-1) + { + Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf); + } + else + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + return f; +}//LSESinusoid + +/* + function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency within [f1, + f2]. + + In: x[N]: windowed spectrum + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a and pp: frequency, amplitude and phase estimates + + Returns 1 if managed to find a sinusoid, 0 if not, upon which $a and $pp are estimated at the initial + f. +*/ +int LSESinusoid(double& f, double f1, double f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf) +{ + struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};//(l_hx *)¶ms; + int dfshift=int(&((l_hx*)0)->dhxpeak); + + int result=0; + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double tmp=Newton(f, ddIPWindow, &p, dfshift, epf, 100, 1e-256, f1, f2); + if (tmp!=-1 && f>f1 && f<f2) + { + result=1; + a=p.hxpeak; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + } + else + { + Search1DmaxEx(f, &p, IPWindow, f1, f2, &a, epf); + if (f<=f1 || f>=f2) + { + f=inp; + cdouble r=IPWindowC(f, x, N, M, c, iH2, p.k1, p.k2); + a=abs(r); + pp=arg(r); + } + else + { + result=1; + pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false); + } + } + return result; +}//LSESinusoid + +/* + function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without + considering phase-frequency consistency across frames. + + In: x[Fr][N]: spectrogram + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates + + Returns an error bound of the frequency estimate. +*/ +double LSESinusoidMP(double& f, double f1, double f2, cdouble** x, int Fr, int N, double B, int M, double* c, double iH2, double* a, double* ph, double epf) +{ + struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int L; cdouble** x; double dsip; double sip; cdouble* lmd;} p={N, 0, 0, M, c,iH2, Fr, x, 0, 0, 0}; + int dfshift=int(&((l_ip1*)0)->dsip), fshift=int(&((l_ip1*)0)->sip); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double errf=Newton1dmax(f, f1, f2, ddsIPWindow, &p, dfshift, fshift, dsIPWindow, dfshift, epf); + if (errf<0) errf=Search1Dmax(f, &p, sIPWindow, f1, f2, a, epf); + if (a || ph) + { + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2); + if (a) a[fr]=abs(r); + if (ph) ph[fr]=arg(r); + } + } + return errf; +}//LSESinusoidMP + +/* + function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without + considering phase-frequency consistency across frames. + + In: x[Fr][N]: spectrogram + f: initial frequency, in bins + [f1, f2]: frequency range + B: spectral truncation half width, in bins. + M, c[], iH2: cosine-family window specification parameters + epf: frequency error tolerance, in bins + Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates + + Returns an error bound of the frequency estimate. Although the frequencies are estimated assuming + cross-frame frequency-phase consistency, the final output phase angles are reestimated independently + for each frame using the frequency estimate. +*/ +double LSESinusoidMPC(double& f, double f1, double f2, cdouble** x, int Fr, int N, int Offst, double B, int M, double* c, double iH2, double* a, double* ph, double epf) +{ + struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sdip; double sip;} + p={N, 0, 0, M, c,iH2, Fr, Offst*1.0/N, x, 0, 0}; + int dfshift=int(&((l_ip*)0)->sdip), fshift=int(&((l_ip*)0)->sip); + + double inp=f; + p.k1=ceil(inp-B); if (p.k1<0) p.k1=0; + p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1; + double errf=Newton1dmax(f, f1, f2, ddsIPWindowC, &p, dfshift, fshift, dsIPWindowC, dfshift, epf); + if (errf<0) errf=Search1Dmax(f, &p, sIPWindowC, f1, f2, a, epf); + if (a || ph) + { + cdouble* lmd=new cdouble[Fr]; + sIPWindowC(f, Fr, Offst*1.0/N, x, N, M, c, iH2, p.k1, p.k2, lmd); + for (int fr=0; fr<Fr; fr++) + { + lmd[fr]=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2); + + if (a) a[fr]=abs(lmd[fr]); + if (ph) ph[fr]=arg(lmd[fr]); + } + delete[] lmd; + } + return errf; +}//LSESinusoidMPC + +//--------------------------------------------------------------------------- +/* + function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy + suppression index of eps, i.e. the least square error is minimized with an additional eps*||lmd||^2 + term. + + In: x[Wid]: spectrum + f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: lmd[I]: amplitude-phase factors + + No return value. +*/ +void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + cdouble** whw=MultiplyXcXt(I, K, wt, List); + cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + GECP(I, lmd, whw, whx); + delete List; +}//IPMulti + +/* + function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy + suppression index of eps, and optionally returns residue and sensitivity indicators for each sinusoid. + + In: x[Wid]: spectrum + f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: lmd[I]: amplitude-phase factors + sens[I]: sensitivity indicators + r1[I]: residue indicators, measured by correlating residue with sinusoid spectra, optional + + No return value. Sensibitily is computed BEFORE applying eps. +*/ +void IPMulti(int I, double* f, cdouble* lmd, cfloat* x, int Wid, int K1, int K2, int M, double* c, double eps, double* sens, double* r1) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + cdouble** whw=MultiplyXcXt(I, K, wt, List); + + //*computes sensitivity if required + if (sens) + { + cdouble** iwhw=Copy(I, whw, List); + GICP(I, iwhw); + cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List); + for (int i=0; i<I; i++) + { + sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]); + } + } //*/ + cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + GECP(I, lmd, whw, whx); + //compute residue if required + if (r1) + { + cdouble* wlmd=MultiplyXty(K, I, wt, lmd, List); //reconstruct + for (int k=0; k<K; k++) wlmd[k]=wlmd[k]-x[K1+k]; //-residue + for (int i=0; i<I; i++) //r1[i]=Inner(K, wlmd, wt[i]).abs(); //-residue weighted by window + { + r1[i]=0; + for (int k=0; k<K; k++) r1[i]+=abs(wlmd[k])*abs(wt[i][k]); + } + } + delete List; +}//IPMulti + +/* + function IPMultiSens: computes the sensitivity of the least square estimation of multiple sinusoids given + their frequencies . + + In: f[I]: frequencies + M, c[]: cosine-family window specification parameters + K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored + eps: energy suppression factor + Out: sens[I]: sensitivity indicators + + No return value. Sensibility is computed AFTER applying eps +*/ +void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps) +{ + if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1; + MList* List=new MList; + cdouble** Allocate2L(cdouble, I, K, wt, List); + for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2); + + cdouble** whw=MultiplyXcXt(I, K, wt, List); + for (int i=0; i<I; i++) whw[i][i]+=eps; + + cdouble** iwhw=Copy(I, whw, List); + GICP(I, iwhw); + cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List); + for (int i=0; i<I; i++) + { + sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]); + } + delete List; +}//IPMultiSens + +/* + function IPMulti: least square estimation of multi-sinusoids with GIVEN frequencies. This version + operates in groups at least B bins from each other, rather than LSE all frequencies together. + + In: x[Wid]: spectrum + f[I]: frequencies, must be ordered low to high. + B: number of bins beyond which sinusoids are treated as non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: lmd[I]: amplitude-phase factors + + Returns 0. +*/ +double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B) +{ + int i=0, ist=0; + double Bw=B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; int K1=floor(fb-B+0.5), K2=floor(fb+B+0.5); + lmd[ist]=IPWindowC(fb, x, Wid, M, c, iH2, K1, K2); + } + else + { + MList* List=new MList; + int N=i-ist, K1=floor(f[ist]-B+0.5), K2=floor(f[i-1]+B+0.5), K=K2-K1+1; + cdouble** Allocate2L(cdouble, N, K, wt, List); + for (int n=0; n<N; n++) Window(wt[n], f[ist+n], Wid, M, c, K1, K2); + cdouble* whx=MultiplyXcy(N, K, wt, &x[K1], List); //w*'x=(wt*)x + cdouble** whw=MultiplyXcXt(N, K, wt, List); + /*debug cdouble** C=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** C2=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** Bh=SubMatrix(0, whw, 1, 4, 0, 1, List); cdouble* Y2=SubVector(0, whx, 1, 4); + cdouble x2[4]; cdouble x1=lmd[ist], Bhx1[4], dx2[4]; for (int j=0; j<4; j++) Bhx1[j]=x1^Bh[j][0]; GECP(4, x2, C, Y2); GECP(4, dx2, C2, Bhx1);*/ + GECP(N, &lmd[ist], whw, whx); //solving complex linear system (w*'w)a=w*'x + delete List; + } + ist=i; + } + i++; + } + return 0; +}//IPMulti + +/* + function IPMulti_Direct: LSE estimation of multiple sinusoids given frequencies AND PHASES (direct + method) + + In: x[Wid]: spectrum + f[I], ph[I]: frequencies and phase angles. + B: spectral truncation half width, in bins; sinusoids over 3B bins apart are regarded non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: a[I]: amplitudes + + Returns square norm of the residue. +*/ +double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B) +{ + MList* List=new MList; + int i=0, ist=0, hWid=Wid/2; + cdouble* r=Copy(hWid, x, List); //to store the residue + + double Bw=3.0*B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; + cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]); + double ip=Inner(2*hWid, (double*)x, (double*)w); + a[ist]=ip*iH2; + MultiAdd(hWid, r, r, w, -a[ist]); + delete[] w; + } + else + { + int N=i-ist; + cdouble** Allocate2L(cdouble, N, hWid, wt, List); + for (int n=0; n<N; n++) + { + Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]); + } + double* whxr=MultiplyXy(N, hWid*2, (double**)wt, (double*)x, List); //w*'x=(wt*)x + double** whwr=MultiplyXXt(N, hWid*2, (double**)wt, List); + GECP(N, &a[ist], whwr, whxr); //solving complex linear system (w*'w)a=w*'x + for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]); + } + ist=i; + } + i++; + } + double result=Inner(hWid, r, r).x; + delete List; + return result; +}//IPMulti_Direct + +/* + function IPMulti_GS: LSE estimation of multiple sinusoids given frequencies AND PHASES (Gram-Schmidt method) + + In: x[Wid]: spectrum + f[I], ph[I]: frequencies and phase angles. + B: spectral truncation, in bins; sinusoids over 3B bins apart are regarded non-interfering + M, c[], iH2: cosine-family window specification parameters + Out: a[I]: amplitudes + + Returns square norm of the residue. +*/ +double IPMulti_GS(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B, double** L, double** Q) +{ + MList* List=new MList; + int i=0, ist=0, hWid=Wid/2; + cdouble* r=Copy(hWid, x, List); //to store the residue + double Bw=3.0*B; + while (i<I) + { + if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1) + { + if (i==I-1) i++; + + //process frequencies from ist to i-1 + if (i-1==ist) //one sinusoid + { + double fb=f[ist]; + cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]); + double ip=Inner(2*hWid, (double*)x, (double*)w); + a[ist]=ip*iH2; + MultiAdd(hWid, r, r, w, -a[ist]); + delete[] w; + } + else + { + int N=i-ist; + cdouble** Allocate2L(cdouble, N, hWid, wt, List); + Alloc2L(N, N, L, List); Alloc2L(N, hWid*2, Q, List); + for (int n=0; n<N; n++) + { + Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]); + } + LQ_GS(N, hWid*2, (double**)wt, L, Q); + double* atl=MultiplyxYt(N, hWid*2, (double*)x, Q, List); + GExL(N, &a[ist], L, atl); + for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]); + } + ist=i; + } + i++; + } + double result=Inner(hWid, r, r).x; + delete List; + return result; +}//IPMulti_GS + +/* + function IPMulti: LSE estimation of I sinusoids given frequency and phase and J sinusoids given + frequency only + + In: x[Wid]: spectrum + f[I+J], ph[I]: frequencies and phase angles + M, c[], iH2: cosine-family window specification parameters + Out: a[I+J]: amplitudes + ph[I:I+J-1]: phase angles not given on start + wt[I+2J][hWid], Q[I+2J][hWid], L[I+2J][I+2J]: internal w matrix and its LQ factorization, optional + + Returns the residue vector, newly created and registered to RetList, if specified. On start a[] should + have valid storage no less than I+2J. +*/ +cdouble* IPMulti(int I, int J, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, cdouble** wt, cdouble** Q, double** L, MList* RetList) +{ + MList* List=new MList; + int hWid=Wid/2; + cdouble* r=Copy(hWid, x, RetList); //to store the residue + if (!wt){Allocate2L(cdouble, I+J*2, hWid, wt, List);} + if (!Q){Allocate2L(cdouble, I+J*2, hWid, Q, List);} + if (!L){Allocate2L(double, I+J*2, I+J*2, L, List);} + memset(wt[0], 0, sizeof(cdouble)*(I+J*2)*hWid); + memset(Q[0], 0, sizeof(cdouble)*(I+J*2)*hWid); + memset(L[0], 0, sizeof(double)*(I+J*2)*(I+J*2)); + + //*The direct form + for (int i=0; i<I; i++) + { + Window(wt[i], f[i], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) wt[i][k].rotate(ph[i]); + } + for (int j=0; j<J; j++) + { + cdouble *w1=wt[I+j*2], *w2=wt[I+j*2+1]; + Window(w1, f[I+j], Wid, M, c, 0, hWid-1); + for (int k=0; k<hWid; k++) w2[k].y=w1[k].x, w2[k].x=-w1[k].y; + } + + LQ_GS(I+J*2, hWid*2, (double**)wt, L, (double**)Q); + double *atl=MultiplyxYt(I+J*2, hWid*2, (double*)x, (double**)Q, List); + GExL(I+J*2, a, L, atl); + + for (int i=0; i<I+J*2; i++) MultiAdd(hWid, r, r, wt[i], -a[i]); + for (int j=0; j<J; j++) + { + double xx=a[I+j*2], yy=a[I+j*2+1]; + a[I+j]=sqrt(xx*xx+yy*yy); + ph[I+j]=atan2(yy, xx); + } + delete List; + return r; +}//IPMulti + +//--------------------------------------------------------------------------- +/* + Routines for estimation two sinusoids with 1 fixed and 1 flexible frequency + + Further reading: "LSE estimation for 2 sinusoids with 1 at a fixed frequency.pdf" +*/ + +/* + function WindowDuo: calcualtes the square norm of the inner product between windowed spectra of two + sinusoids at frequencies f1 and f2, df=f1-f2. + + In: df: frequency difference, in bins + N: DFT size + M, d[]: cosine-family window specification parameters (see "further reading"). + Out: w[0], the inner product, optional + + Returns square norm of the inner product. +*/ +double WindowDuo(double df, int N, double* d, int M, cdouble* w) +{ + double wr=0, wi=0; + for (int m=-2*M; m<=2*M; m++) + { + double ang=df+m, Omg=ang*M_PI, omg=Omg/N; + double si=sin(omg), co=cos(omg), sinn=sin(Omg); + double sa=(ang==0)?N:(sinn/si); + double dm; if (m<0) dm=d[-m]; else dm=d[m]; + wr+=dm*sa*co, wi+=-dm*sinn; + } + wr*=N, wi*=N; + if (w) w->x=wr, w->y=wi; + double result=wr*wr+wi*wi; + return result; +}//WindowDuo + +/* + function ddWindowDuo: calcualtes the square norm of the inner product between windowed spectra of two + sinusoids at frequencies f1 and f2, df=f1-f2, with its 1st and 2nd derivatives + + In: df: frequency difference, in bins + N: DFT size + M, d[]: cosine-family window specification parameters (see "further reading" for d[]). + Out: w[0], the inner product, optional + window, dwindow: square norm and its derivative, of the inner product + + Returns 2nd derivative of the square norm of the inner product. +*/ +double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w) +{ + double wr=0, wi=0, dwr=0, dwi=0, ddwr=0, ddwi=0, PI_N=M_PI/N, PIPI_N=PI_N*M_PI, PIPI=M_PI*M_PI; + for (int m=-2*M; m<=2*M; m++) + { + double ang=df+m, Omg=ang*M_PI, omg=Omg/N; + double si=sin(omg), co=cos(omg), sinn=sin(Omg), cosn=cos(Omg); + double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N); + double dm; if (m<0) dm=d[-m]; else dm=d[m]; + wr+=dm*sa*co, wi+=-dm*sinn; + dwr+=dm*(dsa*co-PI_N*sinn), dwi+=-dm*M_PI*cosn; + ddwr+=dm*(ddsa*co-PI_N*dsa*si-PIPI_N*cosn), ddwi+=dm*PIPI*sinn; + } + wr*=N, wi*=N, dwr*=N, dwi*=N, ddwr*=N, ddwi*=N; + window=wr*wr+wi*wi; + dwindow=2*(wr*dwr+wi*dwi); + if (w) w->x=wr, w->y=wi; + double ddwindow=2*(wr*ddwr+dwr*dwr+wi*ddwi+dwi*dwi); + return ddwindow; +}//ddWindowDuo + +/* + function sIPWindowDuo: calculates the square norm of the orthogonal projection of a windowed spectrum + onto the linear span of the windowed spectra of two sinusoids at reference frequencies f1 and f2. + + In: x[N]: spectrum + f1, f2: reference frequencies. + M, c[], d[], iH2: cosine-family window specification parameters. + K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored. + Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors + + Returns the square norm of the orthogonal projection. +*/ +double sIPWindowDuo(double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2) +{ + int K=K2-K1+1; + cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K]; + Window(w1, f1, N, M, c, K1, K2); + double w1w1=0; + for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1; + for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k]; + Window(w1, f2, N, M, c, K1, K2); + cdouble r1w2=0, w12; for (int k=0; k<K; k++) r1w2+=(r1[k]^w1[k]); + double w=WindowDuo(f1-f2, N, d, M, &w12); + double v=1.0/iH2-w*iH2; + double result=~xw1/w1w1+~r1w2/v; + cdouble mu2=r1w2/v; + lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2; + delete[] w1; + return result; +}//sIPWindowDuo +//wrapper function +double sIPWindowDuo(double f2, void* params) +{ + struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params; + cdouble r1, r2; + return sIPWindowDuo(p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2); +}//sIPWindowDuo + +/* + function ddsIPWindowDuo: calculates the square norm, and its 1st and 2nd derivatives against f2,, of + the orthogonal projection of a windowed spectrum onto the linear span of the windowed spectra of two + sinusoids at reference frequencies f1 and f2. + + In: x[N]: spectrum + f1, f2: reference frequencies. + M, c[], d[], iH2: cosine-family window specification parameters. + K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored. + + Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors + ddsip[3]: the 2nd, 1st and 0th derivatives (against f2) of the square norm. + + No return value. +*/ +void ddsIPWindowDuo(double* ddsip2, double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2) +{ + int K=K2-K1+1; + cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K]; + Window(w1, f1, N, M, c, K1, K2); + double w1w1=0; + for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1; + for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k]; + + cdouble r1w2, w12; + double u, du, ddu=ddsIPWindow_unn(f2, &r1[-K1], N, M, c, K1, K2, du, u, &r1w2); + double w, dw, ddw=ddWindowDuo(f1-f2, N, d, M, dw, w, &w12); dw=-dw; + double v=1.0/iH2-w*iH2, dv=-iH2*dw, ddv=-iH2*ddw; + double iv=1.0/v;//, div=-dv*iv*iv, ddiv=(2*dv*dv-v*ddv)*iv*iv*iv; + + ddsip2[2]=~xw1/w1w1+u*iv; + ddsip2[1]=iv*(du-iv*u*dv); + ddsip2[0]=iv*(ddu-iv*(u*ddv+2*du*dv-2*iv*u*dv*dv)); + + cdouble mu2=r1w2*iv; + lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2; + + delete[] w1; +}//ddsIPWindowDuo +//wrapper function +double ddsIPWindowDuo(double f2, void* params) +{ + struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params; + double ddsip2[3]; cdouble r1, r2; + ddsIPWindowDuo(ddsip2, p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2); + p->dipwindow=ddsip2[1], p->ipwindow=ddsip2[2]; + return ddsip2[0]; +}//ddsIPWindowDuo + +/* + function LSEDuo: least-square estimation of two sinusoids of which one has a fixed frequency + + In: x[N]: the windowed spectrum + f1: the fixed frequency + f2: initial value of the flexible frequency + fmin, fmax: search range for f2, the flexible frequency + B: spectral truncation half width + M, c[], d[], iH2: + epf: frequency error tolerance + Out: f2: frequency estimate + lmd1, lmd2: amplitude-phase factor estimates + Returns 1 if managed to find a good f2, 0 if not, upon which the initial f2 is used for estimating + + amplitudes and phase angles. +*/ +int LSEDuo(double& f2, double fmin, double fmax, double f1, cdouble* x, int N, double B, double* c, double* d, int M, double iH2, cdouble& r1, cdouble &r2, double epf) +{ + int result=0; + double inp=f2; + int k1=ceil(inp-B); if (k1<0) k1=0; + int k2=floor(inp+B); if (k2>=N/2) k2=N/2-1; + struct l_hx {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} p={N, k1, k2, c, d, M, iH2, x, f1, 0, 0}; + int dfshift=int(&((l_hx*)0)->dipwindow);// fshift=int(&((l_hx*)0)->ipwindow); + + double tmp=Newton(f2, ddsIPWindowDuo, &p, dfshift, epf, 100, 1e-256, fmin, fmax); + if (tmp!=-1 && f2>fmin && f2<fmax) result=1; + else + { + Search1DmaxEx(f2, &p, sIPWindowDuo, fmin, fmax, NULL, epf); + if (f2<=fmin || f2>=fmax) f2=inp; + else result=1; + } + sIPWindowDuo(f1, f2, x, N, c, d, M, iH2, k1, k2, r1, r2); + return result; +}//LSEDuo + +//--------------------------------------------------------------------------- +/* + Time-frequency reassignment sinusoid estimation routines. + + Further reading: A. R?bel, ¡°Estimating partial frequency and frequency slope using reassignment + operators,¡± in Proc. ICMC¡¯02. G?teborg. 2002. +*/ + +/* + function CDFTW: single-frequency windowed DTFT, centre-aligned + + In: data[Wid]: waveform data x + win[Wid+1]: window function + k: frequency, in bins, where bin=1/Wid + Out: X: DTFT of xw at frequency k bins + + No return value. +*/ +void CDFTW(cdouble& X, double k, int Wid, cdouble* data, double* win) +{ + X=0; + int hWid=Wid/2; + for (int i=0; i<Wid; i++) + { + cdouble tmp=data[i]*win[Wid-i]; + double ph=-2*M_PI*(i-hWid)*k/Wid; + tmp.rotate(ph); + X+=tmp; + } +}//CDFTW + +/* + function CuDFTW: single-frequency windowed DTFT of t*data[t], centre-aligned + + In: data[Wid]: waveform data x + wid[Wid+1]: window function + k: frequency, in bins + Out: X: DTFT of txw at frequency k bins + + No return value. +*/ +void CuDFTW(cdouble& X, int k, int Wid, cdouble* data, double* win) +{ + X=0; + int hWid=Wid/2; + for (int i=0; i<Wid; i++) + { + double tw=((i-hWid)*win[Wid-i]); + cdouble tmp=data[i]*tw; + double ph=-2*M_PI*(i-hWid)*k/Wid; + tmp.rotate(ph); + X+=tmp; + } +}//CuDFTW + +/* + function TFReas: time-frequency reassignment + + In: data[Wid]: waveform data + win[Wid+1], dwin[Wid+1], ddwin[Wid+1]: window function and its derivatives + f, t: initial digital frequency and time + Out: f, t: reassigned digital frequency and time + fslope: estimate of frequency derivative + plogaslope[0]: estimate of the derivative of logarithmic amplitude, optional + + No return value. +*/ +void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope) +{ + int fi=floor(f*Wid+0.5); + + cdouble x, xt, xw; + CDFTW(x, fi, Wid, data, win); + CuDFTW(xw, fi, Wid, data, win); xt.x=xw.y; xw.y=-xw.x; xw.x=xt.x; + CDFTW(xt, fi, Wid, data, dwin); + double px=~x; + t=t-(xw.y*x.x-xw.x*x.y)/px; + f=1.0*fi/Wid+(xt.y*x.x-xt.x*x.y)/px/(2*M_PI); + if (plogaslope) plogaslope[0]=-(xt.x*x.x+xt.y*x.y)/px; + cdouble xtt, xtw; + CuDFTW(xtw, fi, Wid, data, dwin); xtt.x=xtw.y; xtw.y=-xtw.x; xtw.x=xtt.x; + CDFTW(xtt, fi, Wid, data, ddwin); + double dtdt=-(xtw.y*x.x-xtw.x*x.y)/px+((xt.y*x.x-xt.x*x.y)*(xw.x*x.x+xw.y*x.y)+(xt.x*x.x+xt.y*x.y)*(xw.y*x.x-xw.x*x.y))/px/px, + dwdt=(xtt.y*x.x-xtt.x*x.y)/px-2*(xt.x*x.x+xt.y*x.y)*(xt.y*x.x-xt.x*x.y)/px/px; + if (dtdt!=0) fslope=dwdt/dtdt/(2*M_PI); + else fslope=0; +} //TFReas*/ + +/* + function TFReas: sinusoid estimation using reassignment method + + In: data[Wid]: waveform data + w[Wid+1], dw[Wid+1], ddw[Wid+1]: window function and its derivatives + win[Wid]: window function used for estimating amplitude and phase by projection onto a chirp + t: time for which the parameters are estimated + f: initial frequency at t + Out: f, a, ph: digital frequency, amplitude and phase angle estimated at t + fslope: frequency derivative estimate + + No return value. +*/ +void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win) +{ + double localt=t, logaslope; + TFReas(f, localt, fslope, Wid, data, w, dw, ddw, &logaslope); + + if (logaslope*Wid>6) logaslope=6.0/Wid; + else if (logaslope*Wid<-6) logaslope=-6.0/Wid; + + f=f+fslope*(t-localt); //obtain frequency estimate at t + + cdouble x=0; + if (win==0) + { + for (int n=0; n<Wid; n++) + { + double ni=n-t; + cdouble tmp=data[n]; + double p=-2*M_PI*(f+0.5*fslope*ni)*ni; + tmp.rotate(p); + x+=tmp; + } + a=abs(x)/Wid; + } + else + { + double sumwin=0; + for (int n=0; n<Wid; n++) + { + double ni=n-t; + cdouble tmp=data[n]*win[n]; + double p=-2*M_PI*(f+0.5*fslope*ni)*ni; + tmp.rotate(p); + x+=tmp; sumwin+=win[n]; + } + a=abs(x)/sumwin; + } + ph=arg(x); +}//TFReas + +//--------------------------------------------------------------------------- +/* + Routines for additive and multiplicative reestimation of sinusoids. + + Further reading: Wen X. and M. Sandler, "Additive and multiplicative reestimation schemes + for the sinusoid modeling of audio," in Proc. EUSIPCO'09, Glasgow, 2009. +*/ + +/* + function AdditiveUpdate: additive reestimation of time-varying sinusoid + + In: x[Count]: waveform data + Wid, Offst: frame size and hop + fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters + das[Count]: initial estimate of amplitude derivative + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update + + No return value. +*/ +void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + int HWid=Wid/2, Fr=(Count-Wid)/Offst+1; + + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + + cdouble *y=new cdouble[Count]; + double *lf=new double[Count*4], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3]; + + __int16* ref=new __int16[Count]; + for (int i=0; i<Count; i++) y[i]=x[i].x-as[i]*cos(phs[i]), ref[i]=floor(fs[i]*Wid+0.5); + memcpy(lf, fs, sizeof(double)*Count); + BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, ref, reserved, LogA); + + //merge and interpolate + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3], + *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3]; + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + double a=as[i], b=la[i], fai=phs[i], thet=lp[i], f=fs[i], g=lf[i], delt=fai-thet, da=das[i], db=lda[i]; + xs[fr]=i; + if (fabs(f-g)*Wid>1) + { + afr[fr]=a, pfr[fr]=fai, ffr[fr]=f; + } + else + { + double rr=a*cos(fai)+b*cos(thet); + double ii=a*sin(fai)+b*sin(thet); + ffr[fr]=(a*f*(a+b*cos(delt))+b*g*(b+a*cos(delt))+(da*b-a*db)*sin(delt)/(2*M_PI))/(a*a+b*b+2*a*b*cos(delt)); + afr[fr]=sqrt(rr*rr+ii*ii); + pfr[fr]=atan2(ii, rr); + } + if (LogA) afr[fr]=log(afr[fr]); + } + CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1); + CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1); + for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA); + Sinusoid(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], pfr[0], pfr[1], LogA); + Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA); + delete[] fa; //*/ + /* + for (int i=0; i<Count; i++) + { + double rr=as[i]*cos(phs[i])+la[i]*cos(lp[i]); + double ii=as[i]*sin(phs[i])+la[i]*sin(lp[i]); + as[i]=sqrt(rr*rr+ii*ii); + phs[i]=atan2(ii, rr); + } //*/ + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + delete[] y; delete[] lf; delete[] ref; +}//AdditiveUpdate + +/* + function AdditiveAnalyzer: sinusoid analyzer with one additive update + + In: x[Count]: waveform data + Wid, Offst: frame size and hop size + BasicAnalyzer: pointer to a sinusoid analyzer + ref[Count]: reference frequencies, in bins, used by BasicAnalyzer + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates + das[Count]: estimate of amplitude derivative + + No return value. +*/ +void AdditiveAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA); + AdditiveUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved, LogA); +}//AdditiveAnalyzer + +/* + function MultiplicativeUpdate: multiplicative reestimation of time-varying sinusoid + + In: x[Count]: waveform data + Wid, Offst: frame size and hop + fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters + das[Count]: initial estimate of amplitude derivative + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update + + No return value. +*/ +void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + int HWid=Wid/2; + cdouble *y=new cdouble[Count]; + double *lf=new double[Count*8], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3], + *lf2=&lf[Count*4], *la2=&lf2[Count], *lp2=&lf2[Count*2], *lda2=&lf2[Count*3]; + __int16 *lref=new __int16[Count]; + + for (int i=0; i<Count; i++) y[i]=x[i]*(cdouble(1.0).rotate(-phs[i]+i*0.15*2*M_PI)), + lref[i]=0.15*Wid; + BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, lref, reserved, LogA); + for (int i=0; i<Count; i++) y[i]=y[i]*(cdouble(1.0/la[i]).rotate(-lp[i]+i*0.15*2*M_PI)), lref[i]=0.15*Wid; + BasicAnalyzer(lf2, la2, lp2, lda2, y, Count, Wid, Offst, lref, reserved, LogA); + + /* + for (int i=0; i<Count; i++) + { + as[i]=la[i]*la2[i]; + phs[i]=phs[i]+lp[i]+lp2[i]-0.3*2*M_PI*i; + fs[i]=fs[i]+lf[i]+lf2[i]-0.3; + } //*/ + + //merge + int Fr=(Count-Wid)/Offst+1; + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3], + *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3]; + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + xs[fr]=i; + afr[fr]=la[i]*la2[i]; + if (LogA) afr[fr]=log(afr[fr]); + ffr[fr]=fs[i]+lf[i]-0.15+lf2[i]-0.15; + pfr[fr]=phs[i]+lp[i]+lp2[i]-0.3*i*2*M_PI; + } + CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1); + CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1); + for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA); + Sinusoid(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], pfr[0], pfr[1], LogA); + Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA); + delete[] fa; //*/ + + for (int fr=0; fr<Fr; fr++) + { + int i=HWid+Offst*fr; + if (fs[i]<0 || fs[i]>0.5){} + } + + delete[] y; delete[] lf; delete[] lref; +}//MultiplicativeUpdate + +/* + function MultiplicativeAnalyzer: sinusoid analyzer with one multiplicative update + + In: x[Count]: waveform data + Wid, Offst: frame size and hop size + BasicAnalyzer: pointer to a sinusoid analyzer + ref[Count]: reference frequencies, in bins, used by BasicAnalyzer + BasicAnalyzer: pointer to a sinusoid analyzer + LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline + Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates + das[Count]: estimate of amplitude derivative + + No return value. +*/ +void MultiplicativeAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA) +{ + BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA); + MultiplicativeUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved); +}//MultiplicativeAnalyzer + +/* + This is an earlier version of the multiplicative method without using a user-provided BasicAnalyzer. + This updates the sinusoid estimates at the selected consecutive FRAMES of x. Only frequency modulation + is included in the multiplier. The first frame (0) is centred at x[Wid/2]. fs, as, and phs are based + on frames rather than samples. Updates include frame frst, but not frame fren. +*/ +void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst) +{ + int HWid=Wid/2; + + double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3], + *xs=&fa[Fr*8]; + for (int fr=0; fr<Fr; fr++) xs[fr]=HWid+Offst*fr; + CubicSpline(Fr-1, fa, fb, fc, fd, xs, fs, 1, 1); + + int dst=Offst*frst, den=Offst*(fren-1)+Wid, dcount=den-dst; + double *f=new double[dcount*2], *ph=&f[dcount]; + for (int fr=frst; fr<fren-1; fr++) Sinusoid(&f[int(xs[fr])-dst], &ph[int(xs[fr])-dst], 0, Offst, fa[fr], fb[fr], fc[fr], fd[fr], phs[fr], phs[fr+1]); + if (frst==0) Sinusoid(&f[int(xs[0])-dst], &ph[int(xs[0])-dst], -HWid, 0, fa[0], fb[0], fc[0], fd[0], phs[0], phs[1]); + else Sinusoid(&f[int(xs[frst-1])-dst], &ph[int(xs[frst-1])-dst], 0, Offst, fa[frst-1], fb[frst-1], fc[frst-1], fd[frst-1], phs[frst-1], phs[frst]); + if (fren==Fr) Sinusoid(&f[int(xs[fren-2])-dst], &ph[int(xs[fren-2])-dst], Offst, Offst+HWid, fa[fren-2], fb[fren-2], fc[fren-2], fd[fren-2], phs[fren-2], phs[fren-1]); + else Sinusoid(&f[int(xs[fren-1])-dst], &ph[int(xs[fren-1])-dst], 0, Offst, fa[fren-1], fb[fren-1], fc[fren-1], fd[fren-1], phs[fren-1], phs[fren]); + + cdouble* y=new cdouble[Wid]; + AllocateFFTBuffer(Wid, Amp, W, X); + double* win=NewWindow(wtHann, Wid); + int M; double c[10], iH2; windowspec(wtHann, Wid, &M, c, &iH2); + for (int fr=frst; fr<fren; fr++) + { + __int16* lx=&x[Offst*fr]; + double* lph=&ph[Offst*(fr-frst)]; + for (int i=0; i<Wid; i++) y[i]=cdouble(lx[i]).rotate(-lph[i]+i*0.15*2*M_PI); + CFFTCW(y, win, Amp, 0, log2(Wid), W, X); + int pf=0.15*Wid, mpf=pf; + for (int k=pf-4; k<=pf+4; k++) if (Amp[k]>Amp[mpf]) mpf=k; + if (mpf>pf-4 && mpf<pf+4) pf=mpf; + double lfs=pf, lphs; + LSESinusoid(lfs, pf-3, pf+3, X, Wid, 3, M, c, iH2, as[fr], lphs, 1e-3); + fs[fr]=fs[fr]+lfs/Wid-0.15; + phs[fr]+=lphs-0.15*Wid*M_PI; + as[fr]*=2; + } + + delete[] y; + delete[] f; + delete[] win; + delete[] fa; + FreeFFTBuffer(Amp); +}//MultiplicativeUpdateF + +//--------------------------------------------------------------------------- +/* + Earlier reestimation method routines. + + Further reading: Wen X. and M. Sandler, "Evaluating parameters of time-varying + sinusoids by demodulation," in Proc. DAFx'08, Espoo, 2008. +*/ + +/* + function ReEstFreq: sinusoid reestimation by demodulating frequency. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing + win[Wid]: window function for estimating demodulated sinusoid + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, optional + w[Wid/2], ps[Wid], xs[Wid], xc[Wid], fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +void ReEstFreq(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* fa, double* fb, double* fc, double* fd, double* ns, int* Wids) +{ + int hWid=Wid/2; + //reestimate using frequency track + CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1); + for (int fr=0; fr<FrCount; fr++) + { + //find ps + if (fr==0) + { + double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0]; + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4))); + } +// memset(ps, 0, sizeof(double)*hWid); + } + else if (fr==FrCount-1) + { + int lfr=FrCount-2; + double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa))); + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<Wid; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } +// memset(&ps[hWid], 0, sizeof(double)*hWid); + } + else + { + int lfr=fr-1; + double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<hWid+1; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + lfr=fr; + lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + for (int j=1; j<hWid; j++) + { + ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + } + double* ldata=&x[fr*Offst]; + for (int j=0; j<Wid; j++) + { + xs[j].x=ldata[j]*cos(-ps[j]); + xs[j].y=ldata[j]*sin(-ps[j]); + } + + if (Wids) + { + int lWid=Wids[fr], lhWid=Wids[fr]/2, lM; + SetTwiddleFactors(lWid, w); + double *lwin=NewWindow(wtHann, lWid), lc[4], liH2; + windowspec(wtHann, lWid, &lM, lc, &liH2); + CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc); + delete[] lwin; + double lf=fbuf[fr]*lWid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp; + } + else + { + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) + fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp; + } + } +}//ReEstFreq + +/* + function ReEstFreq_2: sinusoid reestimation by demodulating frequency. This is that same as ReEstFreq(...) + except that it calls Sinusoid(...) to synthesize the phase track used for demodulation and that it + does not allow variable window sizes for estimating demodulated sinusoid. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing + win[Wid]: window function for LSE sinusoid estimation + M, c[], iH2: cosine-family window specification parameters, must be consistent with M, c, iH2 + w[Wid/2], xs[Wid], xc[Wid], f3[FrCount-1], f2[FrCount-1], f1[FrCount-1], f0[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +void ReEstFreq_2(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* f3, double* f2, double* f1, double* f0, double* ns) +{ + int hWid=Wid/2; + //reestimate using frequency track + CubicSpline(FrCount-1, f3, f2, f1, f0, ns, fbuf, 1, 1); + double *refcos=(double*)malloc8(sizeof(double)*Wid), *refsin=&refcos[hWid], ph=0, centralph; + + memset(f0, 0, sizeof(double)*FrCount); + + int N=Wid+Offst*(FrCount-1); + double* cosine=new double[N], *sine=new double[N]; + Sinusoid(&cosine[hWid], &sine[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph); + for (int fr=0; fr<FrCount-1; fr++) + { + int ncentre=hWid+Offst*fr; + if (fr==FrCount-2) Sinusoid(&cosine[ncentre], &sine[ncentre], 0, Wid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + else Sinusoid(&cosine[ncentre], &sine[ncentre], 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + } + double err=0; + for (int n=0; n<N; n++) {double tmp=cosine[n]-x[n-hWid]; err+=tmp*tmp; tmp=cosine[n]*cosine[n]+sine[n]*sine[n]-1; err+=tmp*tmp;} + + ph=0; + for (int fr=0; fr<FrCount; fr++) + { + double* ldata=&x[fr*Offst-hWid]; + + //store first half of demodulated frame to xs[0:hWid-1] + if (fr==0) + { + Sinusoid(&refcos[hWid], &refsin[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph); + for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i]; + } + else + { + ph=0; + Sinusoid(refcos, refsin, 0, hWid, f3[fr-1], f2[fr-1], f1[fr-1], f0[fr-1], ph); + for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i]; + } + + //taking care of phase angles + if (fr==FrCount-1) {double tmp=ph; ph=centralph; centralph=tmp;} + else centralph=ph; + + double *lrefcos=&refcos[-hWid], *lrefsin=&refsin[-hWid]; + //store second half of demodulated frame to xs[hWid:Wid-1] + if (fr==FrCount-1) + { + Sinusoid(lrefcos, lrefsin, hWid, Wid, f3[FrCount-2], f2[FrCount-2], f1[FrCount-2], f0[FrCount-2], ph); + for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i]; + } + else + { + Sinusoid(refcos, refsin, 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph); + for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i]; + } + + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) + fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp+centralph; + } +}//ReEstFreq_2 + +/* + function ReEstFreqAmp: sinusoid reestimation by demodulating frequency and amplitude. + + In: x[Wid+Offst*(FrCount-1)]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + fbuf[FrCount], abuf[FrCount], ns[FrCount]: initial frequency and amplitude estiamtes and their + timing + win[Wid]: window function for estimating demodulated sinusoid + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, + optional + w[Wid/2], ps[Wid], xs[Wid], xc[Wid]: buffers + fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers + aa[FrCount-1], ab[FrCount-1], ac[FrCount-1], ad[FrCount-1]: buffers + Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles + + No return value. +*/ +void ReEstFreqAmp(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* as, double* fa, double* fb, double* fc, double* fd, double* aa, double* ab, double* ac, double* ad, double* ns, int* Wids) +{ + int hWid=Wid/2; + //reestimate using amplitude and frequency track + CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1); + CubicSpline(FrCount-1, aa, ab, ac, ad, ns, abuf, 0, 1); + for (int fr=0; fr<FrCount; fr++) + { + if (fr==0) + { + double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0], + lad=ad[0], lac=ac[0], lab=ab[0], laa=aa[0]; + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4))); + } + for (int j=0; j<Wid; j++) + { + double lx=j-hWid; + as[j]=lad+lx*(lac+lx*(lab+lx*laa)); + } + } + else if (fr==FrCount-1) + { + int lfr=FrCount-2; + double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa))); + double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=1; j<Wid; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + } + as[0]=ad[lfr]; + for (int j=0; j<Wid; j++) + { + as[j]=lad+j*(lac+j*(lab+j*laa)); + } + } + else + { + int lfr=fr-1; + double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4))); + for (int j=0; j<hWid+1; j++) + { + ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + as[j]=lad+j*(lac+j*(lab+j*laa)); + } + lfr=fr; + lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr]; + lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr]; + for (int j=1; j<hWid; j++) + { + ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4))); + as[j+hWid]=lad+j*(lac+j*(lab+j*laa)); + } + } + double *ldata=&x[fr*Offst]; + for (int j=0; j<Wid; j++) + { + double tmp; + if ((fr==0 && j<hWid) || (fr==FrCount-1 && j>=hWid)) tmp=1; + else if (as[hWid]>100*as[j]) tmp=100; + else tmp=as[hWid]/as[j]; + tmp=tmp*ldata[j]; + xs[j].x=tmp*cos(-ps[j]); + xs[j].y=tmp*sin(-ps[j]); + } + + if (Wids) + { + int lWid=Wids[fr], lhWid=Wids[fr]/2, lM; + SetTwiddleFactors(lWid, w); + double *lwin=NewWindow(wtHann, lWid), lc[4], liH2; + windowspec(wtHann, lWid, &lM, lc, &liH2); + CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc); + delete[] lwin; + double lf=fbuf[fr]*lWid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp; + } + else + { + CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc); + double lf=fbuf[fr]*Wid, la, lp; + LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3); + if (la*2>abuf[fr]) fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp; + } + } +}//ReEstFreqAmp + +/* + function Reestimate2: iterative demodulation method for sinusoid parameter reestimation. + + In: x[(FrCount-1)*Offst+Wid]: waveform data + FrCount, Wid, Offst: frame count, frame size and hop size + win[Wid]: window function + M, c[], iH2: cosine-family window specification parameters, must be consistent with win[] + Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, + optional + maxiter: maximal number of iterates + ae[FrCount], fe[FrCount], pe[FrCount]: initial amplitude, frequency and phase estimates + Out: aret[FrCount], fret[FrCount], pret[FrCount]: reestimated amplitudes, frequencies and phase angles + + Returns the number of unused iterates left of the total of maxiter. +*/ +int Reestimate2(int FrCount, int Wid, int Offst, double* win, int M, double* c, double iH2, double* x, double* ae, double* fe, double* pe, double* aret, double* fret, double *pret, int maxiter, int* Wids) +{ + AllocateFFTBuffer(Wid, fft, w, xc); + double convep=1e-4, dif=0, lastdif=0; //convep is the hard-coded threshold that stops the iteration + int iter=1, hWid=Wid/2; + + double *ns=new double[FrCount*12], *as=new double[Wid*5]; + double *fbuf=&ns[FrCount], *abuf=&ns[FrCount*2], + *aa=&ns[FrCount*3], *ab=&ns[FrCount*4], *ac=&ns[FrCount*5], *ad=&ns[FrCount*6], + *fa=&ns[FrCount*7], *fb=&ns[FrCount*8], *fc=&ns[FrCount*9], *fd=&ns[FrCount*10], + *pbuf=&ns[FrCount*11]; + double *ps=&as[Wid]; + cdouble *xs=(cdouble*)&as[Wid*3]; + + memcpy(fbuf, fe, sizeof(double)*FrCount); + memcpy(abuf, ae, sizeof(double)*FrCount); + memcpy(pbuf, pe, sizeof(double)*FrCount); + for (int i=0; i<FrCount; i++) + { + ns[i]=hWid+i*Offst; + } + + while (iter<=maxiter) + { + ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids); + ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids); + ReEstFreqAmp(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, as, fa, fb, fc, fd, aa, ab, ac, ad, ns, Wids); + + if (iter>1) lastdif=dif; + dif=0; + if (iter==1) + { + for (int fr=0; fr<FrCount; fr++) + { + if (fabs(abuf[fr])>fabs(ae[fr])) + dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/abuf[fr]); + else + dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/ae[fr]); + } + } + else + { + for (int fr=0; fr<FrCount; fr++) + { + if (fabs(abuf[fr])>fabs(aret[fr])) + dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/abuf[fr]); + else + dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/aret[fr]); + } + } + memcpy(fret, fbuf, sizeof(double)*FrCount); + memcpy(aret, abuf, sizeof(double)*FrCount); + dif/=FrCount; + if (fabs(dif)<convep || (iter>1 && fabs(dif-lastdif)<convep*lastdif)) break; + iter++; + } + + memcpy(pret, pbuf, sizeof(double)*FrCount); + + delete[] ns; + delete[] as; + delete[] fft; + + return maxiter-iter; +}//Reestimate2 + +//--------------------------------------------------------------------------- +/* + Derivative method as proposed in DAFx09 + + Further reading: Wen X. and M. Sandler, "Notes on model-based non-stationary sinusoid estimation methods + using derivatives," in Proc. DAFx'09, Como, 2009. +*/ + +/* + function Derivative: derivative method for estimating amplitude derivative, frequency, and frequency derivative given + signal and its derivatives. + + In: x[Wid], dx[Wid], ddx[Wid]: waveform and its derivatives + win[Wid]: window function + f0: initial digital frequency estimate + Out: f0: new estimate of digital frequency + f1, a1: estimates of frequency and amplitude derivatives + + No return value. +*/ +void Derivative(int Wid, double* win, cdouble* x, cdouble* dx, cdouble* ddx, double& f0, double* f1, double* a0, double* a1, double* ph) +{ + AllocateFFTBuffer(Wid, fft, W, X); + CFFTCW(x, win, fft, NULL, log2(Wid), W, X); + int m=f0*Wid, m0=m-10, m1=m+10, hWid=Wid/2; + if (m0<0) m0=0; if (m1>hWid) m1=hWid; + for (int n=m0; n<=m1; n++) if (fft[n]>fft[m]) m=n; + cdouble Sw=0, S1w=0, S2w=0; + for (int n=0; n<Wid; n++) + { + cdouble tmp=x[n]*win[n]; + Sw+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid); + tmp=dx[n]*win[n]; + S1w+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid); + } + double omg0=(S1w/Sw).y; + Sw=0, S1w=0; + for (int n=0; n<Wid; n++) + { + cdouble tmp=x[n]*win[n]; + Sw+=tmp.rotate(-omg0*(n-hWid)/Wid); + tmp=dx[n]*win[n]; + S1w+=tmp.rotate(-omg0*(n-hWid)/Wid); + tmp=ddx[n]*win[n]; + S2w+=tmp.rotate(-omg0*(n-hWid)/Wid); + } + omg0=(S1w/Sw).y; + double miu0=(S1w/Sw).x; + double psi0=(S2w/Sw).y-2*miu0*omg0; + + f0=omg0/(2*M_PI); + *f1=psi0/(2*M_PI); + *a1=miu0; + + FreeFFTBuffer(fft); +}//Derivative + +/* + function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency omg, + from x using window w and its derivatives. + + In: x[Wid]: waveform data + w[K+1][Wid]: window functions and its derivatives up to order K + omg: angular frequency + Out: X[K+1]: windowed spectrum and its derivatives up to order K + + No return value. This function is for internal use. +*/ +void Xkw(cdouble* X, int K, int Wid, double* x, double** w, double omg) +{ + int hWid=Wid/2; + //calculate the first row + memset(X, 0, sizeof(cdouble)*(K+1)); + for (int i=0; i<Wid; i++) + { + double n=i-hWid; + double ph=omg*n; + for (int k=0; k<=K; k++) + { + cdouble tmp=x[i]*w[k][i]; + X[k]+=tmp.rotate(-ph); + } + } + //calculate the rest rows + for (int k=1; k<=K; k++) + { + cdouble *thisX=&X[k], *lastX=&X[k-1]; + for (int kk=K-k; kk>=0; kk--) thisX[kk]=-lastX[kk+1]+cdouble(0, omg)*lastX[kk]; + } +}//Xkw + +/* + function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency + omg, from x and its derivatives using window w. + + In: x[K+1][Wid]: waveform data and its derivatives up to order K. + w[Wid]: window function + omg: angular frequency + Out: X[K+1]: windowed spectrum and its derivatives up to order K + + No return value. This function is for testing only. +*/ +void Xkw(cdouble* X, int K, int Wid, double** x, double* w, double omg) +{ + int hWid=Wid/2; + memset(X, 0, sizeof(cdouble)*(K+1)); + for (int i=0; i<Wid; i++) + { + double n=i-hWid; + double ph=omg*n; + for (int k=0; k<=K; k++) + { + cdouble tmp=x[k][i]*w[i]; + X[k]+=tmp.rotate(-ph); + } + } +}//Xkw + +/* + function Derivative: derivative method for estimating the model log(s)=h[M]'r[M], by discarding extra + equations + + In: s[Wid]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +void Derivative(int M, double (**h)(double t, void*), double (**dh)(double t, void*), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg) +{ + int hWid=Wid/2, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0); //number of derivatives required + + //ind marks the 2*M1 real elements of an M1-array of complex unknowns with + // numerical indices (0-based) or -1 if it is not a real unknown variable + //uind marks the Kr real unknowns with their positions in ind + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + { + int p=0, up=0; + while (p<2*M1) + { + if (ind[p]>=0) + { + uind[up]=p; + ind[p]=up; + up++; + } + p++; + } + if (up!=Kr) throw(""); + } + + cdouble* Skw=new cdouble[M]; + Xkw(Skw, Kc, Wid, s, win, omg); + + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + //allocate buffer for linear system A(pq)=b + Alloc2(2*Kc+2, Kr, A); double** AA; double *bb, *pqpq; + double *b=A[2*Kc], *pq=A[2*Kc+1]; + for (int k=0; k<Kr; k++) b[k]=((double*)(&Skw[1]))[k]; + // *pq=(double*)(&r[1]); + for (int k=0; k<Kc; k++) //looping through rows of A + { + //columns of A includes rows of Smkw corresponding to real unknowns + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) //the real part being unknown + { + A[2*k][lind]=Smkw[m+1][k].x; + A[2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) //the imag part being unknown + { + A[2*k+1][lind]=Smkw[m+1][k].x; + A[2*k][lind]=-Smkw[m+1][k].y; + } + } + } + + bool dropeq=(2*Kc-1==Kr); + if (dropeq) + { + Allocate2(double, Kr+2, Kr, AA); + bb=AA[Kr], pqpq=AA[Kr+1]; + memcpy(AA[0], A[0], sizeof(double)*Kr*(Kr-1)); + memcpy(AA[Kr-1], A[Kr], sizeof(double)*Kr); + memcpy(bb, b, sizeof(double)*(Kr-1)); + bb[Kr-1]=((double*)(&Skw[1]))[Kr]; + } + + double det; + GECP(Kr, pq, A, b, &det); + if (dropeq) + { + double det2; + GECP(Kr, pqpq, AA, bb, &det2); + if (fabs(det2)>fabs(det)) memcpy(pq, pqpq, sizeof(double)*Kr); + DeAlloc2(AA); + } + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + + //estiamte r0 + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw[0]/e0); + + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); +}//Derivative*/ + +/* + function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M], least-square + implementation + + In: s[Wid]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + K: number of derivatives to take + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +void DerivativeLS(int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0) +{ + int hWid=Wid/2, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0); //number of derivatives required + if (Kc<K) Kc=K; + + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + {int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;} if (up!=Kr) throw("");} + + //allocate buffer for linear system A(pq)=b + cdouble* Skw=new cdouble[Kc+1]; + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + + Alloc2(2*Kc+2, 2*Kc, A); + double *b=A[2*Kc], *pq=A[2*Kc+1]; + + Xkw(Skw, Kc, Wid, s, win, omg); + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + for (int k=0; k<2*Kc; k++) b[k]=((double*)(&Skw[1]))[k]; + for (int k=0; k<Kc; k++) + { + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) + { + A[2*k][lind]=Smkw[m+1][k].x; + A[2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) + { + A[2*k+1][lind]=Smkw[m+1][k].x; + A[2*k][lind]=-Smkw[m+1][k].y; + } + } + } + + if (2*Kc==Kr) GECP(Kr, pq, A, b); + else LSLinear2(2*Kc, Kr, pq, A, b); + + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + //estiamte r0 + if (r0) + { + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw[0]/e0); + } + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); +}//DerivativeLS + +/* + function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M] using Fr + measurement points a quarter of Wid apart from each other, implemented by least-square. + + In: s[Wid+(Fr-1)*Wid/4]: waveform data + win[][Wid]: window function and its derivatives + h[M], dh[M]: pointers to basis functions and their derivatives + harg: pointer argument to be used by calls to functions in h[] amd dh[]. + Fr: number of measurement points + K: number of derivatives to take at each measurement point + p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0. + q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0. + omg: initial angular frequency + r0: specifies if r[0] is to be computed. + Out: r[M]: estimated coefficients to h[M]. + + No return value. +*/ +void DerivativeLS(int Fr, int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0) +{ + int hWid=Wid/2, qWid=Wid/4, M1=M-1; + int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + int Kc=ceil(Kr/2.0/Fr); //number of derivatives required + if (Kc<K) Kc=K; + + int *uind=new int[Kr], *ind=new int[2*M1]; + memset(ind, 0, sizeof(int)*2*M1); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + {int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;}} + + //allocate buffer for linear system A(pq)=b + cdouble* Skw=new cdouble[Kc+1], Skw00; + double* x=new double[Wid]; + cdouble** Allocate2(cdouble, M, Kc, Smkw); + + Alloc2(2*Fr*Kc, 2*Fr*Kc, A); + double *pq=new double[2*Fr*Kc], *b=new double[2*Fr*Kc]; + + for (int fr=0; fr<Fr; fr++) + { + int Offst=qWid*fr; double* ss=&s[Offst]; + + Xkw(Skw, Kc, Wid, ss, win, omg); if (fr==0) Skw00=Skw[0]; + for (int m=1; m<M; m++) + { + for (int i=0; i<Wid; i++) x[i]=dh[m](i+Offst-hWid, harg)*ss[i]; + Xkw(Smkw[m], Kc-1, Wid, x, win, omg); + } + + for (int k=0; k<2*Kc; k++) b[2*fr*Kc+k]=((double*)(&Skw[1]))[k]; + for (int k=0; k<Kc; k++) + { + for (int m=0; m<M1; m++) + { + int lind; + if ((lind=ind[2*m])>=0) + { + A[2*fr*Kc+2*k][lind]=Smkw[m+1][k].x; + A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].y; + } + if ((lind=ind[2*m+1])>=0) + { + A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].x; + A[2*fr*Kc+2*k][lind]=-Smkw[m+1][k].y; + } + } + } + } + if (2*Fr*Kc==Kr) GECP(Kr, pq, A, b); + else LSLinear2(2*Fr*Kc, Kr, pq, A, b); + + memset(&r[1], 0, sizeof(double)*M1*2); + for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k]; + //estiamte r0 + if (r0) + { + cdouble e0=0; + for (int i=0; i<Wid; i++) + { + cdouble expo=0; + double n=i-hWid; + for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;} + cdouble tmp=exp(expo)*win[0][i]; + e0+=tmp.rotate(-omg*n); + } + r[0]=log(Skw00/e0); + } + delete[] x; + delete[] Skw; + delete[] uind; + delete[] ind; + DeAlloc2(Smkw); + DeAlloc2(A); + delete[] pq; delete[] b; +}//DerivativeLS + +//--------------------------------------------------------------------------- +/* + Abe-Smith sinusoid estimator 2005 + + Further reading: M. Abe and J. O. Smith III, ¡°AM/FM rate estimation for time-varying sinusoidal + modeling,¡± in Proc. ICASSP'05, Philadelphia, 2005. +*/ + +/* + function RDFTW: windowed DTFT at frequency k bins + + In: data[Wid]: waveform data + w[Wid]: window function + k: frequency, in bins + Out: Xr, Xi: real and imaginary parts of the DTFT of xw at frequency k bins + + No return value. +*/ +void RDFTW(double& Xr, double& Xi, double k, int Wid, double* data, double* w) +{ + Xr=Xi=0; + int hWid=Wid/2; + double* lw=&w[Wid]; + for (int i=0; i<=Wid; i++) + { + double tmp; + tmp=*data**lw; + data++, lw--; +//* + double ph=-2*M_PI*(i-hWid)*k/Wid; + Xr+=tmp*cos(ph); + Xi+=tmp*sin(ph); //*/ + } +}//RDFTW + +/* + function TFAS05: the Abe-Smith method 2005 + + In: data[Wid]: waveform data + w[Wid]: window function + res: resolution of frequency for QIFFT + Out: f, a, ph: frequency, amplitude and phase angle estimates + aesp, fslope: estimates of log amplitude and frequency derivatives + + No return value. +*/ +void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res) +{ + double fi=floor(f*Wid+0.5); //frequency (int) in bins + double xr0, xi0, xr_1, xi_1, xr1, xi1; + RDFTW(xr0, xi0, fi, Wid, data, w); + RDFTW(xr_1, xi_1, fi-res, Wid, data, w); + RDFTW(xr1, xi1, fi+res, Wid, data, w); + double winnorm=0; for (int i=0; i<=Wid; i++) winnorm+=w[i]; + double y0=log(sqrt(xr0*xr0+xi0*xi0)/winnorm), + y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm), + y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm); + double df=0; +//* + if (y0<y1) + { + double newfi=fi+res; + while (y0<y1) + { + y_1=y0, xr_1=xr0, xi_1=xi0; + y0=y1, xr0=xr1, xi0=xi1; + newfi+=res; + RDFTW(xr1, xi1, newfi, Wid, data, w); + y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm); + fi+=res; + } + } + else if(y0<y_1) + { + double newfi=fi-res; + while (y0<y_1) + { + y1=y0, xr1=xr0, xi1=xi0; + y0=y_1, xr0=xr_1, xi0=xi_1; + newfi-=res; + RDFTW(xr_1, xi_1, newfi, Wid, data, w); + y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm); + fi-=res; + } + } //*/ + + double a2=(y1+y_1)*0.5-y0, a1=(y1-y_1)*0.5, a0=y0; + df=-a1*0.5/a2; + f=fi+df*res; //in bins + double y=a0-0.25*a1*a1/a2; + a=exp(y); + double ph0=(xi0==0 && xr0==0)?0:atan2(xi0, xr0), + ph_1=(xi_1==0 && xr_1==0)?0:atan2(xi_1, xr_1), + ph1=(xi1==0 && xr1==0)?0:atan2(xi1, xr1); + if (fabs(ph_1-ph0)>M_PI) + { + if (ph_1-ph0>0) ph_1-=M_PI*2; + else ph_1+=M_PI*2; + } + if (fabs(ph1-ph0)>M_PI) + { + if (ph1-ph0>0) ph1-=M_PI*2; + else ph1+=M_PI*2; + } + double b2=(ph1+ph_1)*0.5-ph0, b1=(ph1-ph_1)*0.5, b0=ph0; + ph=b0+b1*(df+b2*df); + //now we have the QI estimates + double uff=2*a2, vf=b1+2*b2*df, vff=2*b2; + double dfdp=Wid/(2*M_PI*res); + double upp=uff*dfdp*dfdp, vp=vf*dfdp, vpp=vff*dfdp*dfdp; + double p=-upp*0.5/(upp*upp+vpp*vpp); + double alf=-2*p*vp, beta=p*vpp/upp; + //*direct method + double beta_p=beta/p; + double feses=f-alf*beta/p /(2*M_PI)*Wid, + yeses=y-alf*alf*0.25/p+0.25*log(1+beta_p*beta_p), + pheses=ph+alf*alf*beta*0.25/p-0.5*atan(beta_p); //*/ + /*adapted method + double zt[]={0, 0.995354, 0.169257, 1.393056, 0.442406, -0.717980, -0.251620, 0.177511, 0.158120, -0.503299}; + double delt=res/Wid; double delt0=df*delt; + beta=zt[3]*beta+zt[4]*delt0*alf; + alf=(zt[1]+zt[2]*delt*delt)*alf; + double beta_p=beta/p; + double feses=f+zt[5]*alf*beta/p /(2*M_PI)*Wid, + yeses=y+zt[6]*alf*alf/p+zt[7]*log(1+beta_p*beta_p), + pheses=ph+zt[8]*alf*alf*beta/p+zt[9]*atan(beta_p); //*/ + f=feses/Wid, a=exp(yeses), ph=pheses, fslope=2*beta/2/M_PI, aesp=alf; +}//TFAS05 + +/* + function TFAS05_enh: the Abe-Smith method 2005 enhanced by LSE amplitude and phase estimation + + In: data[Wid]: waveform data + w[Wid]: window function + res: resolution of frequency for QIFFT + Out: f, a, ph: frequency, amplitude and phase angle estimates + aesp, fslope: estimates of log amplitude and frequency derivatives + + No return value. +*/ +void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res) +{ + TFAS05(f, t, a, ph, aesp, fslope, Wid, data, w, res); + double xr=0, xi=0, p, win2=0; + for (int n=0; n<=Wid; n++) + { + double ni=n-Wid/2, tmp=data[n]*w[n]*w[n];//*exp(-aesp*(n-Wid/2)); if (IsInfinite(tmp)) continue; + p=-2*M_PI*(f+0.5*fslope*ni)*ni; + xr+=tmp*cos(p); + xi+=tmp*sin(p); + win2+=w[n]*w[n]; + } + a=sqrt(xr*xr+xi*xi)/win2; + ph=(xr==0 && xi==0)?0:atan2(xi, xr); +}//TFAS05_enh +//version without returning aesp and fslope +void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res) +{ + double aesp, fslope; + TFAS05_enh(f, t, a, ph, aesp, fslope, Wid, data, w, res); +}//TFAS05_enh + +//--------------------------------------------------------------------------- +/* + function DerivativeLSv_AmpPh: estimate the constant-term in the local derivative method. This is used + by the local derivative algorithm, whose implementation is found in the header file as templates. + + In: sv0: inner product <s, v0>, where s is the sinusoid being estimated. + integr_h[M][Wid]: M vectors containing samples of the integral of basis functions h[M]. + v0[M]: a test function + lmd[M]: coefficients to h[M] + + Returns coefficient of integr_h[0]=1. +*/ +cdouble DerivativeLSv_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, cdouble* v0, cdouble sv0) +{ + cdouble e0=0; + for (int n=0; n<Wid; n++) + { + cdouble expo=0; + for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n]; + e0+=exp(expo)**v0[n]; + } + return log(sv0/e0); +}//DerivativeLSv_AmpPh + +//--------------------------------------------------------------------------- +/* + Piecewise derivative algorithm + + Further reading: Wen X. and M. Sandler, "Spline exponential approximation of time-varying + sinusoids," under review. +*/ + +/* + function setv: computes I test functions v[I] by modulation u[I] to frequency f + + In: u[I+1][Wid], du[I+1][Wid]: base-band test functions and their derivatives + f: carrier frequency + Out: v[I][Wid], dv[I][Wid]: test functions and their derivatives + + No return value. +*/ +void setv(int I, int Wid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du) +{ + double fbin=floor(f*Wid+0.5)/Wid; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<Wid; c++) + { + double t=c; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I-1; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I-1; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + //Here it is assumed that elements of u[] are modulated at 0, 1, -1, 2, -2, 3, -3, 4, ...; + //if f is under fbin then the closest ones are in order 0, -1, 1, -2, 3, -3, 3, .... This + //makes a difference to the whole of v[] only if I is even. + if (f>=fbin || I%2==1){v[I-1][c]=u[I-1][c]*rot; dv[I-1][c]=du[I-1][c]*rot+jomg*v[I-1][c];} + else{v[I-1][c]=u[I][c]*rot; dv[I-1][c]=du[I][c]*rot+jomg*v[I-1][c];} + } +}//setv + +/* + function setvhalf: computes I half-size test functions v[I] by modulation u[I] to frequency f. + + In: u[I][hWid*2], du[I][Wid*2]: base-band test functions and their derivatives + f: carrier frequency + Out: v[I][hWid], dv[hWid]: half-size test functions and their derivatives + + No return value. +*/void setvhalf(int I, int hWid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du) +{ + double fbin=floor(f*hWid)/hWid; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<hWid; c++) + { + double t=c; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } +}//setvhalf + +//#define ERROR_CHECK + +/* + function DerivativePiecewise: Piecewise derivative algorithm. In this implementation of the piecewise + method the test functions v are constructed from I "basic" (single-frame) test functions, each + covering the same period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I + test functions are used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + A[L][M][N]: L matrices that map independent coefficients onto component coefficients over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: aita[N]: independent coefficients + + No return value. +*/ +void DerivativePiecewise(int N, cdouble* aita, int L, double* f, int T, cdouble* s, double*** A, int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, N, srv, mlist); + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + +#ifdef ERROR_CHECK + cdouble dsv1[128], dsv2[128]; +#endif + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + double fbin=floor(f[l+1]*T*2)/(T*2.0); + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T*2; c++) + { + double t=c-T; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + //memset(srv[l][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[l], shv1, A[l]); + MultiAddXY(I, M, N, srv[l], shv2, A[l+1]); + +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[l*T]; + for (int i=0; i<I && l*I+1<36; i++) + { + cdouble lsv=Inner(2*T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[l*I+i]=lsv-sv[l][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && l*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[l][i][n]*aita[n]; + dsv2[l*I+i]=lsv-sv[l][i]-dsv1[l*I+i]; + } + } +#endif + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[0]+f[1])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[L_1], shv1, A[0]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[0]; + for (int i=0; i<I && L_1*I+1<36; i++) + { + cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && L_1*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n]; + dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i]; + } + } +#endif + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[L-1]+f[L])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N); + MultiplyXY(I, M, N, srv[L_1], shv1, A[L-1]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> + if (ds) + { + cdouble* lds=&ds[(L-1)*T]; + for (int i=0; i<I && L_1*I+1<36; i++) + { + cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + //cdouble lsv2=Inner(2*T, ls, dv[i]); + dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + } + + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I && L_1*I+i<36; i++) + { + cdouble lsv=0; + for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n]; + dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i]; + } + } +#endif + L_1++; + } + + if (L_1*2*I==2*N) GECP(N, aita, srv[0], sv[0]); + else LSLinear(L_1*I, N, aita, srv[0], sv[0]); + + delete mlist; +}//DerivativePiecewise + +/* + function DerivativePiecewise2: Piecewise derivative algorithm in which the real and imaginary parts of + the exponent are modelled separately. In this implementation of the piecewise method the test + functions v are constructed from I "basic" (single-frame) test functions, each covering the same + period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are + used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + A[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces + B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients + + No return value. +*/ +void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B, + int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist); + cdouble*** srbv; + if (Np==Nq && B==A) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + double fbin=floor(f[l+1]*T*2)/(T*2.0); + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T*2; c++) + { + double t=c-T; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + memset(srav[l][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[l], shv1, A[l]); + MultiAddXY(I, M, Np, srav[l], shv2, A[l+1]); + if (srbv!=srav) //so that either B!=A or Np!=Nq + { + //memset(srbv[l][0], 0, sizeof(cdouble)*I*Nq); + MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]); + MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]); + } + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[0]+f[1])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, A[0]); + if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);} + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + int hT=T/2; + double fbin=floor((f[L-1]+f[L])*hT)/T; + double omg=fbin*2*M_PI; + cdouble jomg=cdouble(0, omg); + for (int c=0; c<T; c++) + { + double t=c-hT; + cdouble rot=polar(1.0, omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot; + for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c]; + } + + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, A[L-1]); + if (srbv!=srav) + { + //memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); + MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]); + } + L_1++; + } + + //real implementation of <sr,v>aita=<s',v> + double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist); + for (int l=0; l<L_1; l++) for (int i=0; i<I; i++) + { + int li=l*I+i, li_H=li+L_1*I; + for (int n=0; n<Np; n++) + { + AM[li][n]=srav[l][i][n].x; + AM[li_H][n]=srav[l][i][n].y; + } + for (int n=0; n<Nq; n++) + { + AM[li][Np+n]=-srbv[l][i][n].y; + AM[li_H][Np+n]=srbv[l][i][n].x; + } + } + //least-square solution of (srv)(aita)=(sv) + double* pq=new double[Np+Nq]; mlist->Add(pq, 1); + double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y; + + if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b); + else LSLinear(2*L_1*I, Np+Nq, pq, AM, b); + + memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq); + + delete mlist; +}//DerivativePiecewise2 + +/* + Error check: test that ds[LT] equals s[LT] times reconstructed R'. Notice that DA is D time A where D + is a pre-emphasis because p[Np] applies to log amplitude rather than its derivative. +*/ +double testds_pqA(int Np, double* p, int Nq, double* q, int L, int T, cdouble* s, cdouble* ds, int M, double** h, double** dh, double*** DA, double*** B, cdouble* errds=0) +{ + double err=0, ene=0, *lamdax=new double[M*2], *lamday=&lamdax[M]; + for (int l=0; l<L; l++) + { + MultiplyXy(M, Np, lamdax, DA[l], p); + MultiplyXy(M, Nq, lamday, B[l], q); + for (int t=0; t<T; t++) + { + double drtx=0; for (int m=0; m<M; m++) drtx+=lamdax[m]*h[m][t]; + double drty=0; for (int m=0; m<M; m++) drty+=lamday[m]*h[m][t]; + cdouble drt=cdouble(drtx, drty); + cdouble eds=ds[l*T+t]-s[l*T+t]*drt; + err+=~eds; ene+=~ds[l*T+t]; + if (errds) errds[l*T+t]=eds; + } + } + delete[] lamdax; + return err/ene; +}//testds_pqA + +/* + Error check: dsv1[I] tests that <s', v[I]> equals -<s, v[I]'>, dsv2[I] tests that <sr, v[I]>*pq= + <s',v[I]> +*/ +void testdsv(cdouble* dsv1, cdouble* dsv2, int Np, double* p, int Nq, double* q, int TT, cdouble* dsl, int I, cdouble** vl, cdouble* svl, cdouble** sravl, cdouble** srbvl) +{ + for (int i=0; i<I; i++) + { + cdouble lsv=Inner(TT, dsl, vl[i]); //compute <s', v[i]> + //cdouble* ls=&s[l*T]; + dsv1[i]=lsv-svl[i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i] + //sv[l][i]=lsv; + } + //error check: srv[l]*pq=<s',v> + for (int i=0; i<I; i++) + { + cdouble lsv=0; + for (int n=0; n<Np; n++) lsv+=sravl[i][n]*p[n]; + for (int n=0; n<Nq; n++) lsv+=srbvl[i][n]*cdouble(0, q[n]); + dsv2[i]=lsv-svl[i]-dsv1[i]; + } +}//testdsv + +/* + Error check: tests A[MN]x[N1]=b[N1], returns square error +*/ +double testlinearsystem(int M, int N, double** A, double* x, double* b) +{ + double err=0; + for (int m=0; m<M; m++) + { + double errli=Inner(N, A[m], x)-b[m]; + err+=errli*errli; + } + return err; +}//testlinearsystem + +/* + Error check: test the total square norm of <s, v> +*/ +double testsv(int L, double* f, int T, cdouble* s, int I, cdouble** u, cdouble** du, int endmode) +{ + cdouble** Allocate2(cdouble, I, T*2, v); + cdouble** Allocate2(cdouble, I, T*2, dv); + double ene=0; + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + setv(I, T*2, v, dv, f[l+1], u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; + for (int i=0; i<I; i++) + { + cdouble d=Inner(2*T, ls, v[i]); + ene+=~d; + } + } + if (endmode==1 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du); + cdouble* ls=&s[0]; + for (int i=0; i<I; i++) + + ene+=~Inner(T, ls, v[i]); + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du); + cdouble* ls=&s[(L-1)*T]; + for (int i=0; i<I; i++) + ene+=~Inner(T, ls, v[i]); + } + DeAlloc2(v); DeAlloc2(dv); + return ene; +}//testsv + +/* + function DerivativePiecewise3: Piecewise derivative algorithm in which the log amplitude and frequeny + are modeled separately as piecewise functions. In this implementation of the piecewise method the test + functions v are constructed from I "basic" (single-frame) test functions, each covering the same + period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are + used. + + In: s[LT+1]: waveform data + ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined. + L, T: number and length of pieces. + N: number of independent coefficients + h[M][T]: piecewise basis functions + dh[M][T]: derivative of h[M][T], used only if ERROR_CHECK is defined. + DA[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces + B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces + u[I][2T}, du[I][2T]: base-band test functions + f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used + endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients + + No return value. +*/ +void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B, + int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds, double** dh) +{ + MList* mlist=new MList; + int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L); + cdouble** Allocate2L(cdouble, L_1, I, sv, mlist); + cdouble** Allocate2L(cdouble, I, T*2, v, mlist); + cdouble** Allocate2L(cdouble, I, T*2, dv, mlist); + //compute <sr, v> + cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist); + cdouble*** srbv; + if (Np==Nq && B==DA) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase + cdouble** Allocate2L(cdouble, I, M, shv1, mlist); + cdouble** Allocate2L(cdouble, I, M, shv2, mlist); + +#ifdef ERROR_CHECK + cdouble dsv1in[128], dsv2in[128]; +#endif + + for (int l=0; l<L-1; l++) + { + //v from u given f[l] + setv(I, T*2, v, dv, f[l+1], u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]); + + //compute <sr, v> over the lth frame + cdouble *ls1=&s[l*T], *ls2=&s[l*T+T]; + for (int i=0; i<I; i++) + for (int m=0; m<M; m++) + shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]); + memset(srav[l][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[l], shv1, DA[l]); + MultiAddXY(I, M, Np, srav[l], shv2, DA[l+1]); + if (srbv!=srav) //so that either B!=A or Np!=Nq + { + MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]); + MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]); + } +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + if (ds) testdsv(&dsv1in[l*I], &dsv2in[l*I], Np, p, Nq, q, T*2, &ds[l*T], I, v, sv[l], srav[l], srbv[l]); +#endif + } + L_1=L-1; + if (endmode==1 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, DA[0]); + if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq); MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);} +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[0], I, v, sv[L_1], srav[L_1], srbv[L_1]); +#endif + L_1++; + } + if (endmode==2 || endmode==3) + { + //v from u given f[l] + setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du); + //compute -<s, v'> over the lth frame + cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]); + //compute <sr, v> over the lth frame + for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]); + memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np); + MultiplyXY(I, M, Np, srav[L_1], shv1, DA[L-1]); + if (srbv!=srav) MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]); +#ifdef ERROR_CHECK + //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v> + if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[(L-1)*T], I, v, sv[L_1], srav[L_1], srbv[L_1]); +#endif + L_1++; + } + + //real implementation of <sr,v>aita=<s',v> + double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist); + for (int l=0; l<L_1; l++) for (int i=0; i<I; i++) + { + int li=l*I+i, li_H=li+L_1*I; + for (int n=0; n<Np; n++) + { + AM[li][n]=srav[l][i][n].x; + AM[li_H][n]=srav[l][i][n].y; + } + for (int n=0; n<Nq; n++) + { + AM[li][Np+n]=-srbv[l][i][n].y; + AM[li_H][Np+n]=srbv[l][i][n].x; + } + } + //least-square solution of (srv)(aita)=(sv) + double* pq=new double[Np+Nq]; mlist->Add(pq, 1); + double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y; mlist->Add(b, 1); +#ifdef ERROR_CHECK + //tests that AM is invariant to a constant shift of p + double errAM=0, errAM2=0, err1, err2; + for (int l=0; l<L_1*I; l++){double errli=0; for (int n=0; n<Np; n++) errli+=AM[l][n]; errAM+=errli*errli; errli=0; for (int n=0; n<Np; n+=2) errli+=AM[l][n]; errAM2+=errli*errli;} + //test square error of the input pq + if (ds) + { + memcpy(pq, p, sizeof(double)*Np); memcpy(&pq[Np], q, sizeof(double)*Nq); + err1=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b); + } + //test error of s'-sR where R is synthesized from the input pq + double errdsin, errdsvin; cdouble* edsin; + if (ds && dh) + { + edsin=new cdouble[L*T]; mlist->Add(edsin, 1); + errdsin=testds_pqA(Np, p, Nq, q, L, T, s, ds, M, h, dh, DA, B, edsin); + errdsvin=testsv(L, f, T, edsin, I, u, du, endmode); + } +#endif + Alloc2L(L_1*I*2, Np+Nq-1, Am, mlist); + for (int l=0; l<L_1*I*2; l++) memcpy(Am[l], &AM[l][1], sizeof(double)*(Np+Nq-1)); + pq[0]=0; + //if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b); + //else LSLinear(2*L_1*I, Np+Nq, pq, AM, b); + if (L_1*2*I==Np+Nq-1) GECP(Np+Nq-1, &pq[1], Am, b); + else LSLinear(2*L_1*I, Np+Nq-1, &pq[1], Am, b); +#ifdef ERROR_CHECK + //test square error of output pq + if (ds) err2=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b); + //test error of s'-sR of the output pq + double errdsout, errdsvout; cdouble* edsout; + if (ds && dh) + { + edsout=new cdouble[L*T]; mlist->Add(edsout, 1); + errdsout=testds_pqA(Np, pq, Nq, &pq[Np], L, T, s, ds, M, h, dh, DA, B, edsout); + errdsvout=testsv(L, f, T, edsout, I, u, du, endmode); + } +#endif + memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq); + + delete mlist; +}//DerivativePiecewise3 + +//initialization routines for the piecewise derivative method + +/* + function seth: set h[M] to a series of power functions. + + In: M, T. + Out: h[M][T], where h[m] is power function of order m. + + No return value. h is allocated anew and must be freed by caller. +*/ +void seth(int M, int T, double**& h, MList* mlist) +{ + if (M<=0){h=0; return;} + Allocate2L(double, M, T, h, mlist); + double* hm=h[0]; for (int t=0; t<T; t++) hm[t]=1; + for (int m=1; m<M; m++) + { + hm=h[m]; for (int t=0; t<T; t++) hm[t]=pow(t*1.0, m); + } +}//seth + +/* + function setdh: set dh[M] to the derivative of a series of power functions. + + In: M, T. + Out: dh[M][T], where dh[m] is derivative of the power function of order m. + + No return value. dh is allocated anew and must be freed by caller. +*/ +void setdh(int M, int T, double**& dh, MList* mlist) +{ + if (M<=0){dh=0; return;} + Allocate2L(double, M, T, dh, mlist); + double* dhm=dh[0]; memset(dhm, 0, sizeof(double)*T); + if (M>1){dhm=dh[1]; for (int t=0; t<T; t++) dhm[t]=1;} + for (int m=2; m<M; m++) + { + dhm=dh[m]; for (int t=0; t<T; t++) dhm[t]=m*pow(t*1.0, m-1); + } +}//setdh + +/* + function setdih: set dih[M] to the difference of the integral of a series of power functions. + + In: M, I + Out: dih[M][I], where the accumulation of dih[m] is the integral of the power function of order m. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void setdih(int M, int T, double**& dih, MList* mlist) +{ + if (M<=0){dih=0; return;} + Allocate2L(double, M, T, dih, mlist); + double* dihm=dih[0]; for (int t=0; t<T; t++) dihm[t]=1; + for (int m=1; m<M; m++) + { + dihm=dih[m]; for (int t=0; t<T; t++) dihm[t]=(pow(t+1.0, m+1)-pow(t*1.0, m+1))/(m+1); + } +}//setdih + +/* + function sshLinear: sets M and h[M] for the linear spline model + + In: T + Out: M=2, h[2][T] filled out for linear spline model. + + No return value. h is allocated anew and must be freed by caller. +*/ +void sshLinear(int T, int& M, double** &h, MList* mlist) +{ + M=2; Allocate2L(double, M, T, h, mlist); + for (int t=0; t<T; t++) h[0][t]=1, h[1][t]=t; +}//sshLinear + +/* + function sdihLinear: sets dih[M] for the linear spline model. For testing only. + + In: T + Out: dih[2][T] filled out for linear spline model. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void sdihLinear(int T, double**& dih, MList* mlist) +{ + Allocate2L(double, 2, T, dih, mlist); + for (int t=0; t<T; t++) dih[0][t]=1, dih[1][t]=t+0.5; +}//sdihLinear + +/* + function sshCubic: sets M and h[M] for cubic spline models. + + In: T + Out: M=4 and h[M] filled out for cubic spline models, including cubic and cubic-Hermite. + + No return value. h is allocated anew and must be freed by caller. +*/ +void sshCubic(int T, int& M, double** &h, MList* mlist) +{ + M=4; Allocate2L(double, M, T, h, mlist); + for (int t=0; t<T; t++) h[3][t]=t*t*t, h[2][t]=t*t, h[1][t]=t, h[0][t]=1; +}//sshCubic + +/* + function sdihCubic: sets dih[M] for cubic spline models. + + In: T + Out: dih[4] filled out for cubic spline models. + + No return value. dih is allocated anew and must be freed by caller. +*/ +void sdihCubic(int T, double** &dih, MList* mlist) +{ + Allocate2L(double, 4, T, dih, mlist); + for (int t=0; t<T; t++) + { + dih[3][t]=t*(t*(t+1.5)+1)+0.25, dih[2][t]=t*(t+1)+1.0/3, dih[1][t]=t+0.5, dih[0][t]=1; + } +}//sdihCubic*/ + +/* + function ssALinearSpline: sets N and A[L] for the linear spline model + + In: L, M, T + Out: N=L+1, A[L][M][N] filled out for the linear spline model + + No return value. A is created anew and bust be freed by caller. +*/ +void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=L+1; + Allocate3L(double, L, M, N, A, mlist); + memset(A[0][0], 0, sizeof(double)*L*M*N); + double iT=1.0/T; for (int l=0; l<L; l++) A[l][0][l]=1, A[l][1][l]=-iT, A[l][1][l+1]=iT; +}//ssALinearSpline + +/* + function ssLinearSpline: sets M, N, h and A for the linear spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the linear spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssALinearSpline(L, T, M, N, A, mlist); +}//ssLinearSpline + +/* + function ssACubicHermite: sets N and A[L] for cubic Hermite spline model + + In: L, M, T + Out: N=2(L+1), A[L][M][N] filled out for the cubic Hermite spline + + No return value. A is created anew and must be freed by caller. +*/ +void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=2*(L+1); + Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N); + double iT=1.0/T, iT2=iT*iT, iT3=iT2*iT; + for (int l=0; l<L; l++) + { + A[l][3][2*l]=2*iT3; A[l][3][2*l+2]=-2*iT3; A[l][3][2*l+1]=A[l][3][2*l+3]=iT2; + A[l][2][2*l]=-3*iT2; A[l][2][2*l+1]=-2*iT; A[l][2][2*l+2]=3*iT2; A[l][2][2*l+3]=-iT; + A[l][1][2*l+1]=1; + A[l][0][2*l]=1; + } +}//ssACubicHermite + +/* + function ssLinearSpline: sets M, N, h and A for the cubic Hermite spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the cubic Hermite spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssACubicHermite(L, T, M, N, A, mlist); +}//ssCubicHermite + +/* + function ssACubicSpline: sets N and A[L] for cubic spline model + + In: L, M, T + mode: boundary mode of cubic spline, 0=natural, 1=quadratic run-out, 2=cubic run-out + Out: N=2(L+1), A[L][M][N] filled out for the cubic spline + + No return value. A is created anew and must be freed by caller. +*/ +void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode) +{ + N=L+1; + Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N); + Alloc2(L+1, L+1, ML); memset(ML[0], 0, sizeof(double)*(L+1)*(L+1)); + Alloc2(L+1, L+1, MR); memset(MR[0], 0, sizeof(double)*(L+1)*(L+1)); + //fill in ML and MR. The only difference between various cubic splines are ML. + double _6iT2=6.0/(T*T); + ML[0][0]=ML[L][L]=1; + for (int l=1; l<L; l++) ML[l][l-1]=ML[l][l+1]=1, ML[l][l]=4, + MR[l][l-1]=MR[l][l+1]=_6iT2, MR[l][l]=-2*_6iT2; + if (mode==0){} //no more coefficients are needed for natural cubic spline + else if (mode==1) ML[0][1]=ML[L][L-1]=-1; //setting for quadratic run-out + else if (mode==2) ML[0][1]=ML[L][L-1]=-2, ML[0][2]=ML[L][L-2]=1; //setting for cubic run-out + GICP(L+1, ML); + double** MM=MultiplyXY(L+1, ML, ML, MR); + double iT=1.0/T; + Alloc2(4, 2, M42); M42[3][0]=-1.0/6/T, M42[3][1]=1.0/6/T, M42[2][0]=0.5, M42[2][1]=M42[0][0]=M42[0][1]=0, M42[1][0]=-T/3.0, M42[1][1]=-T/6.0; + for (int l=0; l<L; l++) + { + MultiplyXY(4, 2, N, A[l], M42, &MM[l]); + A[l][1][l]-=iT; A[l][1][l+1]+=iT; A[l][0][l]+=1; + } + DeAlloc2(ML); DeAlloc2(MR); DeAlloc2(M42); +}//ssACubicSpline + +/* + function ssLinearSpline: sets M, N, h and A for the cubic spline model + + In: L, M, T + Out: N and h[][] and A[][][] filled out for the cubic spline model + + No reutrn value. A and h are created anew and bust be freed by caller. +*/ +void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode) +{ + seth(M, T, h, mlist); + ssACubicSpline(L, T, M, N, A, mlist, mode); +}//ssCubicSpline + +/* + function setu: sets u[I+1] as base-band windowed Fourier atoms, whose frequencies come in the order of + 0, 1, -1, 2, -2, 3, -3, 4, etc, in bins. + + In: I, Wid: number and size of atoms to generate. + WinOrder: order (=vanishing moment) of window function to use (2=Hann, 4=Hann^2, etc.) + Out: u[I+1][Wid], du[I+1]{Wid]: the I+1 atoms and their derivatives. + + No return value. u and du are created anew and must be freed by caller. +*/ +void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder, MList* mlist) +{ + Allocate2L(cdouble, I+1, Wid, u, mlist); + Allocate2L(cdouble, I+1, Wid, du, mlist); + + double** wins=CosineWindows(WinOrder, Wid, (double**)0, 2); + double omg=2*M_PI/Wid; cdouble jomg=cdouble(0, omg); + for (int t=0; t<Wid; t++) + { + u[0][t]=wins[0][t], du[0][t]=wins[1][t]; + int li=1; + for (int i=1; i<=I; i++) + { + cdouble rot=polar(1.0, li*omg*t); + u[i][t]=u[0][t]*rot; du[i][t]=du[0][t]*rot+jomg*li*u[i][t]; + li=-li; if (li>0) li++; + } + } + DeAlloc2(wins); +}//setu + +/* + function DerivativePiecewiseI: wrapper for DerivativePiecewise(), doing the initialization ,etc. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssmode: pointer to a function that fills A[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: aita[N]: independent coefficients, where N is specified by SpecifyA. + + No return vlue. +*/ +void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + cdouble **u, **du; + setu(I, 2*T, u, du, WinOrder, mlist); + + int N; double **h, ***A; + seth(M, T, h, mlist); + SpecifyA(L, T, M, N, A, mlist, ssmode); + + DerivativePiecewise(N, aita, L, f, T, s, A, M, h, I, u, du, endmode, ds); + delete mlist; +}//DerivativePiecewiseI + +/* + function DerivativePiecewiseII: wrapper for DerivativePiecewise2(), doing the initialization ,etc. + This models the derivative of log ampltiude and frequency as separate piecewise polynomials, the first + specified by SpecifyA, the second by SpecifyB. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it + SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB. + + No reutrn value. +*/ +void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, + void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + cdouble **u, **du; + setu(I, 2*T, u, du, WinOrder, mlist); + + int Np, Nq; + double **h, ***A, ***B; + seth(M, T, h, mlist); + SpecifyA(L, T, M, Np, A, mlist, ssAmode); + SpecifyB(L, T, M, Nq, B, mlist, ssBmode); + + DerivativePiecewise2(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds); + + delete mlist; +}//DerivativePiecewiseII + +/* + function DerivativePiecewiseIII: wrapper for DerivativePiecewise3(), doing the initialization ,etc. + Notice that this time the log amplitude, rather than its derivative, is modeled as a piecewise + polynomial specified by SpecifyA. + + In: L, T: number and length of pieces + s[LT]: waveform signal + ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined. + f[L+1]: reference frequencies at knots + M: polynomial degree of piecewise approximation + SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it + SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it + WinOrder: order(=vanishing moment) of window used for constructing test functions + I: number of test functions per frame. + endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT] + Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB. + + No reutrn value. +*/ +void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, + void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, + int WinOrder, int I, int endmode, cdouble* ds) +{ + MList* mlist=new MList; + int Np, Nq; + double **h, ***A, ***B, **dh=0; + cdouble **u, **du; + setu(I, T*2, u, du, WinOrder, mlist); + seth(M, T, h, mlist); + if (ds) setdh(M, T, dh, mlist); + SpecifyA(L, T, M, Np, A, mlist, ssAmode); + SpecifyB(L, T, M, Nq, B, mlist, ssBmode); + Alloc2L(M, M, DM, mlist); + memset(DM[0], 0, sizeof(double)*M*M); for (int m=0; m<M-1; m++) DM[m][m+1]=m+1; + double** DA=0; + + for (int l=0; l<L; l++) + { + DA=MultiplyXY(M, M, Np, DA, DM, A[l], mlist); + Copy(M, Np, A[l], DA); + } + + DerivativePiecewise3(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds, dh); + + delete mlist; +}//DerivativePiecewiseIII + +/* + function AmpPhCorrectionExpA: model-preserving amplitude and phase correction in piecewise derivative + method. + + In: aita[N]: inital independent coefficients + L, T: number and size of pieces + sre[LT]: waveform data + h[M][T], dih[M][T]: piecewise basis functions and their difference-integrals + A[L][M][N]: L coefficient mapping matrices + SpecifyA: pointer to the function used for constructing A + WinOrder: order(=vanishing moment) of window used for constructing test functions + Out: aita[N]: corrected independent coefficients + s2[LT]: reconstruct sinusoid BEFORE correction + + Returns the estimate of phase angle at 0. +*/ +double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A, + void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder) +{ + MList* mlist=new MList; + //*amplitude and phase correction + //amplitude is done by updating p, i.e. Re(aita) + double *s2ph=new double[L+1]; mlist->Add(s2ph, 1); + double *phcor=new double[L+1]; mlist->Add(phcor, 1); + cdouble* lamda=new cdouble[M]; mlist->Add(lamda, 1); + double* lamdax=new double[M]; mlist->Add(lamdax, 1); + double* lamday=new double[M]; mlist->Add(lamday, 1); + { + double tmpph=0; + memset(s2ph, 0, sizeof(double)*(L+1)); + s2ph[0]=tmpph; + for (int l=0; l<L; l++) + { + MultiplyXy(M, N, lamda, A[l], aita); for (int m=0; m<M; m++) lamdax[m]=lamda[m].x, lamday[m]=lamda[m].y; + SinusoidExpA(T, &s2[l*T], M, lamdax, lamday, h, dih, tmpph); s2ph[l+1]=tmpph; + } + double* win=new double[2*T+1]; CosineWindows(WinOrder, 2*T, &win, 1); mlist->Add(win, 1); + for (int l=1; l<L; l++) + { + cdouble inn=Inner(2*T, &sre[l*T-T], win, &s2[l*T-T])/Inner(2*T, &s2[l*T-T], win, &s2[l*T-T]); + cdouble loginn=log(inn); + if (SpecifyA==ssACubicHermite) + { + aita[l*2]+=loginn.x; + s2ph[l]+=loginn.y; + phcor[l]=loginn.y; + if (l==1) aita[0]+=loginn.x, phcor[0]=loginn.y, s2ph[0]+=loginn.y; + if (l==L-1) aita[L*2]+=loginn.x, phcor[L]=loginn.y, s2ph[L]+=loginn.y; + } + else + { + aita[l]+=loginn.x; + s2ph[l]+=loginn.y; + phcor[l]=loginn.y; + if (l==1) + { + inn=Inner(T, sre, &win[T], s2)/Inner(T, s2, &win[T], s2); + loginn=log(inn); + aita[0]+=loginn.x; + s2ph[0]+=loginn.y; + phcor[0]=loginn.y; + } + if (l==L-1) + { + inn=Inner(T, &sre[L*T-T], win, &s2[L*T-T])/Inner(T, &s2[L*T-T], win, &s2[L*T-T]); + loginn=log(inn); + aita[L]+=loginn.x; + s2ph[L]+=loginn.y; + phcor[L]=loginn.y; + } + } + } + + for (int l=1; l<=L; l++) + { + int k=floor((phcor[l]-phcor[l-1])/(2*M_PI)+0.5); + if (k!=0) + phcor[l]+=2*M_PI*k; + } + //* + //now phcor[] contains phase corrector to be interpolated + double *b=new double[L], *zet=new double[L+1], *dzet=new double[L+1]; memset(zet, 0, sizeof(double)*(L+1)); memset(dzet, 0, sizeof(double)*(L+1)); + mlist->Add(b, 1); mlist->Add(zet, 1); mlist->Add(dzet, 1); + double ihT[]={T, T/2.0*T, T/3.0*T*T, T/4.0*T*T*T}; + + Alloc2L(L, N, BB, mlist); + //prepare linear system (BB)(zet)=(b) + for (int l=0; l<L; l++) + { + MultiplyxY(N, 4, BB[l], ihT, A[l]); + b[l]=phcor[l+1]-phcor[l]; + } + Alloc2L(L, L, copyA, mlist); + if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L); + else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2]; + double* copyb=Copy(L, b, mlist); + zet[0]=0; GECP(L, &zet[1], copyA, copyb); + if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L); + else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2]; + Copy(L, copyb, b); for (int l=0; l<L; l++) copyb[l]-=BB[l][0]; + dzet[0]=1; GECP(L, &dzet[1], copyA, copyb); + +#ifdef ERROR_CHECK + //Test that (BB)(zet)=b and (BB)(dzet)=b + double* bbzet=MultiplyXy(L, L+1, BB, zet, mlist); + MultiAdd(L, bbzet, bbzet, b, -1); + double err1=Inner(L, bbzet, bbzet); + double* bbdzet=MultiplyXy(L, L+1, BB, dzet, mlist); + MultiAdd(L, bbdzet, bbdzet, b, -1); + double err2=Inner(L, bbdzet, bbdzet); + MultiAdd(L+1, dzet, dzet, zet, -1); + //Test that (BB)dzet=0 + MultiplyXy(L, L+1, bbdzet, BB, dzet); + double err3=Inner(L, bbzet, bbzet); +#endif + //now that (zet)+(miu)(dzet) is the general solution to (BB)(zet)=b, + // we look for (miu) that maximizes smoothness + + double innuv=0, innvv=0, lmd0[4], lmdd[4], clmdd[4], + T2=T*T, T3=T2*T, T4=T3*T, T5=T4*T; + for (int l=0; l<L; l++) + { + MultiplyXy(4, L+1, lmd0, A[l], zet); + MultiplyXy(4, L+1, lmdd, A[l], dzet); + clmdd[1]=T*lmdd[1]+T2*lmdd[2]+T3*lmdd[3]; + clmdd[2]=T2*lmdd[1]+(4.0/3)*T3*lmdd[2]+1.5*T4*lmdd[3]; + clmdd[3]=T3*lmdd[1]+1.5*T4*lmdd[2]+1.8*T5*lmdd[3]; + innuv+=Inner(3, &lmd0[1], &clmdd[1]); + innvv+=Inner(3, &lmdd[1], &clmdd[1]); + } + MultiAdd(L+1, zet, zet, dzet, -innuv/innvv); + + if (SpecifyA==ssACubicHermite) + for (int l=0; l<=L; l++) aita[2*l].y+=zet[l]; + else + for (int l=0; l<=L; l++) aita[l].y+=zet[l]; + //*/ + } + double result=s2ph[0]; + delete mlist; + return result; +}//AmpPhCorrectionExpA
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SinEst.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,346 @@ +#ifndef SinEstH +#define SinEstH + +/* + SinEst.cpp - sinusoid estimation algorithms +*/ + + +#include <mem.h> +#include "xcomplex.h" +#include "arrayalloc.h" +#include "matrix.h" +#ifdef I +#undef I +#endif + +//--since function derivative------------------------------------------------ +double ddsincd_unn(double x, int N); +double dsincd_unn(double x, int N); + +//--window spectrum and derivatives------------------------------------------ +cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2); +void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2); +void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2); + +//--spectral projection routines--------------------------------------------- +cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2); + +double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude); +double IPWindow(double f, void* params); +double ddIPWindow(double f, void* params); +double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow); + +double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* ej2ph=0); +double sIPWindow(double f, void* params); +double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip); +double dsIPWindow(double f, void* params); +double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip); +double ddsIPWindow(double f, void* params); +double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn=0); + +double sIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* ej2ph=0); +double sIPWindowC(double f, void* params); +double dsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip); +double dsIPWindowC(double f, void* params); +double ddsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip); +double ddsIPWindowC(double f, void* params); + +//--least-square sinusoid estimation routines-------------------------------- +double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf=1e-6); +void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf=1e-6); +double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf); +int LSESinusoid(double& f, double f1, double f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf); +double LSESinusoidMP(double& f, double f1, double f2, cdouble** x, int Fr, int N, double B, int M, double* c, double iH2, double* a, double* ph, double epf); + +//--multi-sinusoid spectral projection routines------------------------------ +void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps=0); +void IPMulti(int I, double* f, cdouble* lmd, cfloat* x, int Wid, int K1, int K2, int M, double* c, double eps=0, double* sens=0, double* r1=0); +void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps=0); +double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B); +double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B); +double IPMulti_GS(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B, double** L=0, double** Q=0); +cdouble* IPMulti(int I, int J, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, cdouble** wt=0, cdouble** Q=0, double** L=0, MList* RetList=0); + +//--dual-sinusoid spectral projection routines------------------------------- +double WindowDuo(double df, int N, double* d, int M, cdouble* w); +double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w); +double sIPWindowDuo(double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2); +double sIPWindowDuo(double f2, void* params); +void ddsIPWindowDuo(double* ddsip2, double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2); +double ddsIPWindowDuo(double f2, void* params); +int LSEDuo(double& f2, double fmin, double fmax, double f1, cdouble* x, int N, double B, double* c, double* d, int M, double iH2, cdouble& r1, cdouble &r2, double epf); + +//--time-frequency reassignment---------------------------------------------- +void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope=0); +void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win=0); + +//--additive and multiplicative reestimation routines------------------------ +typedef double (*TBasicAnalyzer)(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, int reserved, bool LogA); +void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false); +void AdditiveAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false); +void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false); +void MultiplicativeAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA=false); +void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst); + +void ReEstFreq(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* fa, double* fb, double* fc, double* fd, double* ns, int* Wids=0); +void ReEstFreq_2(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* f3, double* f2, double* f1, double* f0, double* ns); +void ReEstFreqAmp(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* as, double* fa, double* fb, double* fc, double* fd, double* aa, double* ab, double* ac, double* ad, double* ns, int* Wids=0); +int Reestimate2(int FrCount, int Wid, int Offst, double* win, int M, double* c, double iH2, double* x, double* ae, double* fe, double* pe, double* aret, double* fret, double *pret, int maxiter, int* Wids=0); + +//--local derivative algorithms - DAFx09------------------------------------- +void Derivative(int M, double (**h)(double t, void*), double (**dh)(double t, void*), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg); +void DerivativeLS(int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0=false); +void DerivativeLS(int Fr, int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0=false); + +//--the Abe-Smith estimator-------------------------------------------------- +void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res=1); +void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res=1); +void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res=1); + +//--piecewise derivative algorithms and tools-------------------------------- +void DerivativePiecewise(int N, cdouble* aita, int L, double* f, int T, cdouble* s, double*** A, int M, double** h, int I, cdouble** u, cdouble** du, int endmode=0, cdouble* ds=0); +void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B, int M, double** h, int I, cdouble** u, cdouble** du, int endmode=0, cdouble* ds=0); +void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B, int M, double** h, int I, cdouble** u, cdouble** du, int endmode=0, cdouble* ds=0, double** dh=0); +void seth(int M, int T, double**& h, MList* mlist); +void setdh(int M, int T, double**& dh, MList* mlist); +void setdih(int M, int T, double**& dih, MList* mlist); +void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder=2, MList* mlist=0); +void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0); +void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0); +void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode=0); +void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode=0); +void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode=0); +void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode=0); +void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M, void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode=0, int WinOrder=2, int I=2, int endmode=0, cdouble* ds=0); +void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, int WinOrder=2, int I=2, int endmode=0, cdouble* ds=0); +void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M, void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode, void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode, int WinOrder=2, int I=2, int endmode=0, cdouble* ds=0); +double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A, void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder); + +//--local derivative algorithms - general------------------------------------ +/* + template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "v" version, + i.e. using tuned test functions. + + In: s[Wid]: waveform data + v[I][Wid], dv[I][Wid]: test functions and their derivatives + h[M+1][Wid]: basis functions + p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero. + Out: lmd[1:M]: coefficients of h[1:M]. + + Returns inner product of s and v[0]. +*/ +template<class Ts>cdouble DerivativeLSv(int Wid, Ts* s, int I, cdouble** v, cdouble** dv, int M, double **h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0) +{ + int Kr=M*2-p0s-q0s; //number of real unknowns apart from p0 and q0 + if (I<ceil(Kr/2.0)) throw("insufficient test functions"); //Kr/2 complex equations are needed to solve the unknowns + + //ind maps the real unknowns to their positions in physical buffer + //uind maps them back + int *uind=new int[Kr], *ind=new int[2*M]; + memset(ind, 0, sizeof(int)*2*M); + for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1; + for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1; + + { + int p=0, up=0; while (p<2*M){if (ind[p]>=0){uind[up]=p; ind[p]=up; up++;} p++;} + if (up!=Kr) throw(""); + } + + cdouble* sv1=new cdouble[I]; + for (int i=0; i<I; i++) sv1[i]=-Inner(Wid, s, dv[i]); + + double** Allocate2(double, 2*I, Kr, A); + for (int m=1; m<=M; m++) + for (int i=0; i<I; i++) + { + int lind; + cdouble shv=Inner(Wid, s, h[m], v[i]); + if ((lind=ind[2*(m-1)])>=0) + { + A[2*i][lind]=shv.x; + A[2*i+1][lind]=shv.y; + } + if ((lind=ind[2*m-1])>=0) + { + A[2*i][lind]=-shv.y; + A[2*i+1][lind]=shv.x; + } + } + + double* pq=new double[Kr]; + if (2*I==Kr) GECP(Kr, pq, A, (double*)sv1); + else LSLinear(2*I, Kr, pq, A, (double*)sv1); + + memset(lmd, 0, sizeof(double)*(M+1)*2); + for (int k=0; k<Kr; k++) ((double*)(&lmd[1]))[uind[k]]=pq[k]; + + cdouble result=Inner(Wid, s, v[0]); + delete[] pq; + delete[] sv1; + delete[] uind; + delete[] ind; + DeAlloc2(A); + return result; +}//DerivativeLSv + +/* + template DerivativeLS: local derivative algorithm for estimating time-varying sinusoids, "u" version, + i.e. using base-band test functions. + + In: s[Wid]: waveform data + u[I][Wid], du[I][Wid]: base-band test functions and their derivatives + omg: angular frequency onto which u[I] and du[I] are modulated to give the test functions + h[M+1][Wid]: basis functions + p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero. + Out: lmd[1:M]: coefficients of h[1:M]. + + Returns inner product of s and v[0]. +*/ +template<class Ts, class Tu>cdouble DerivativeLS(int Wid, Ts* s, int I, double omg, Tu** u, Tu** du, int M, double **h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0) +{ + cdouble** Allocate2(cdouble, I, Wid, v); + cdouble** Allocate2(cdouble, I, Wid, dv); + cdouble jomg=cdouble(0, omg); int hWid=Wid/2; + for (int c=0; c<Wid; c++) + { + double t=c-hWid; + cdouble rot=cdouble(1).rotate(omg*t); + for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot; + for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c]; + } + cdouble result=DerivativeLSv(Wid, s, I, v, dv, M, h, lmd, p0s, p0, q0s, q0); + DeAlloc2(v); DeAlloc2(dv); + return result; +}//DerivativeLS + +/* + template DerivativeLS_AmpPh: amplitude and phase estimation in the local derivative algorithm, "u" + version + + In: sv0: inner product of signal s[Wid] and test function v0 + u0[Wid], omg: base-band test function and carrier frequency used for computing v0[] + integr_h[M+1][Wid]: integrals of basis functions + + Returns coefficient to integr_h[0]=1. +*/ +template<class Tu>cdouble DerivativeLS_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, double omg, Tu* u0, cdouble sv0) +{ + cdouble e0=0; double hWid=Wid/2.0; + for (int n=0; n<Wid; n++) + { + cdouble expo=0; + for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n]; + if (expo.x>300) expo.x=300; + else if (expo.x<-300) expo.x=-300; + e0+=exp(expo)**(cdouble(u0[n]).rotate(omg*(n-hWid))); + } + return log(sv0/e0); +}//DerivativeLS_AmpPh + +/* + template DerivativeLS_AmpPh: amplitude and phase estimation in the local derivative algorithm, "u" + version. + + In: s[Wid]: waveform data + u0[Wid], omg: base-band test function and carrier frequency used for computing v0[] + integr_h[M+1][Wid]: integrals of basis functions + + Returns coefficient to integr_h[0]=1. +*/ +template<class Tu, class Ts>cdouble DerivativeLS_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, double omg, Tu* u0, Ts* s) +{ + cdouble ss0=0, e0=0; double hWid=Wid/2.0; + for (int n=0; n<Wid; n++) + { + cdouble expo=0; + for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n]; + if (expo.x>300) expo.x=300; + else if (expo.x<-300) expo.x=-300; + e0+=~exp(expo)*abs(u0[n]); + ss0+=s[n]**exp(expo)*abs(u0[n]); + } + return log(ss0/e0); +}//DerivativeLS_AmpPh + +cdouble DerivativeLSv_AmpPh(int, int, double**, cdouble*, cdouble*, cdouble); //the "v" version is implemented as a normal function in SinEst.cpp. + +/* + template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "v" version. + + In: s[Wid]: waveform data + v[I][Wid], dv[I][Wid]: test functions and their derivatives + h[M+1][Wid], integr_h[M+1][Wid]: basis functions and their integrals + p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero. + Out: lmd[M+1]: coefficients of h[M+1], including lmd[0]. + + No return value. +*/ +template<class Ts> void DerivativeLSv(int Wid, Ts* s, int I, cdouble** v, cdouble** dv, int M, double **h, double **integr_h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0) +{ + cdouble sv0=DerivativeLSv(Wid, s, I, v, dv, M, h, lmd, p0s, p0, q0s, q0); + lmd[0]=DerivativeLSv_AmpPh(Wid, M, integr_h, lmd, v[0], sv0); +}//DerivativeLSv_AmpPh + +/*template DerivativeLSv: local derivative algorithm for estimating time-varying sinusoids, "u" version. + + In: s[Wid]: waveform data + u[I][Wid], du[I][Wid]: base-band test functions and their derivatives + omg: angular frequency onto which u[I] and du[I] are modulated to give the test functions + h[M+1][Wid], integr_h[M+1][Wid]: basis functions and their integrals + p0[p0s], q0[q0s]: zero-constraints, i.e. Re(lmd[p0[*]]) and Im(lmd[q0[*]]) are constrained zero. + Out: lmd[M+1]: coefficients of h[M+1], including lmd[0]. + + No return value. +*/ +template<class Ts, class Tu>void DerivativeLS(int Wid, Ts* s, int I, double omg, Tu** u, Tu** du, int M, double **h, double **integr_h, cdouble* lmd, int p0s, int* p0, int q0s, int* q0) +{ + cdouble sv0=DerivativeLS(Wid, s, I, omg, u, du, M, h, lmd, p0s, p0, q0s, q0); + lmd[0]=DerivativeLS_AmpPh(Wid, M, integr_h, lmd, omg, u[0], s); //sv0); +}//DerivativeLSv + +/* + template CosineWindows: generates the Hann^(K/2) window and its L-1 derivatives as Result[L][Wid+1] + + In: K, L, Wid + Out: w[L][Wid+1]: Hann^(K/2) window function and its derivatives up to order L-1 + + Returns pointer to w. w is created anew if w=0 is specified on start. +*/ +template<class T>T** CosineWindows(int K, int Wid, T **w, int L=0) +{ + if (L<=0) L=K; + if (!w) {Allocate2(T, L, Wid+1, w);} + memset(w[0], 0, sizeof(T)*L*(Wid+1)); + int hWid=Wid/2, dWid=Wid*2; + double *s=new double[dWid+hWid], *c=&s[hWid]; //s[n]=sin(pi*n/N), n=0, ..., 2N-1 + double *C=new double[K+2], *lK=&C[K/2+1], piC=M_PI/Wid; + //C[i]=C(K, i)(-1)^i*2^(-K+1), the combination number, i=0, ..., K/2 + //ik[i]=(K-2i)^k*(M_PI/Wid)^k, i=0, ..., K/2 + //calculate C(K,i)(-1)^i*2^(-K+1) + C[0]=1.0/(1<<(K-1)); double lC=C[0]; for (int i=1; i+i<=K; i++){lC=lC*(K-i+1)/i; C[i]=(i%2)?(-lC):lC;} + //calculate sin/cos functions + for (int n=0; n<dWid; n++) s[n]=sin(n*piC); memcpy(&s[dWid], s, sizeof(double)*hWid); + for (int k=0; k<L; k++) + { + if (k==0) for (int i=0; i+i<K; i++) lK[i]=C[i]; + else for (int i=0; i+i<K; i++) lK[i]*=(K-2*i)*piC; + + if ((K-k)%2) //K-k is odd + { + for (int i=0; i+i<K; i++) for (int n=0; n<=Wid; n++) w[k][n]+=lK[i]*s[(K-2*i)*n%dWid]; + if ((K-k-1)/2%2) for (int n=0; n<=Wid; n++) w[k][n]=-w[k][n]; + } + else + { + for (int i=0; i+i<K; i++) for (int n=0; n<=Wid; n++) w[k][n]+=lK[i]*c[(K-2*i)*n%dWid]; + if ((K-k)/2%2) for (int n=0; n<=Wid; n++) w[k][n]=-w[k][n]; + } + } + if (K%2==0){double tmp=C[K/2]*0.5; if (K/2%2) tmp=-tmp; for (int n=0; n<=Wid; n++) w[0][n]+=tmp;} + delete[] s; delete[] C; + return w; +}//CosineWindows + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SinSyn.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,776 @@ +//--------------------------------------------------------------------------- + + +#include "align8.h" +#include "SinSyn.h" +#include "splines.h" + +//--------------------------------------------------------------------------- +/* + function Sinuoid: original McAuley-Quatieri synthesizer interpolation between two measurement points. + + In: T: length from measurement point 1 to measurement point 2 + a1, f1, p2: amplitude, frequency and phase angle at measurement point 1 + a2, f2, p2: amplitude, frequency and phase angle at measurement point 2 + ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer + Out: data[T]: output buffer + a[T], f[T], p[T]: resynthesized amplitude, frequency and phase + + No return value. +*/ +void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, double* a, double* f, double* p, bool ad) +{ + int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5); + double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1); + double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1; + double la=a1, da=(a2-a1)/T; + if (ad) + for (int t=0; t<T; t++) + { + double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI; + data[t]+=la*cos(lp); + a[t]=la; + p[t]=lp; + f[t]=lf; + la=la+da; + } + else + for (int t=0; t<T; t++) + { + double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI; + data[t]=la*cos(lp); + a[t]=la; + p[t]=lp; + f[t]=lf; + la=la+da; + } +}//Sinusoid + +/* + function Sinuoid: original McAuley-Quatieri synthesizer interpolation between two measurement points, + without returning interpolated sinusoid parameters. + + In: T: length from measurement point 1 to measurement point 2 + a1, f1, p2: amplitude, frequency and phase angle at measurement point 1 + a2, f2, p2: amplitude, frequency and phase angle at measurement point 2 + ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer + Out: data[T]: output buffer + + No return value. +*/ +void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, bool ad) +{ + int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5); + double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1); + double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1; + double la=a1, da=(a2-a1)/T; + if (ad) + for (int t=0; t<T; t++) + { + data[t]+=la*cos(pd+t*(pc+t*(pa+t*pb))); + la=la+da; + } + else + for (int t=0; t<T; t++) + { + data[t]=la*cos(pd+t*(pc+t*(pa+t*pb))); + la=la+da; + } +}//Sinusoid + +//--------------------------------------------------------------------------- +/* + function Sinusoid_direct: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of + amplitude and frequency, direct implementation. + + In: CountSt, CountEn + aa, ab, ac, ad: trinomial coefficients of amplitude + fa, fb, fc, fd: trinomial coefficients of frequency + p1: initial phase angle at 0 (NOT at CountSt) + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + p1: phase angle at CountEn + + No return value. +*/ +void Sinusoid_direct(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad, + double fa, double fb, double fc, double fd, double &p1, bool add) +{ + int i; double a, ph; + for (i=CountSt; i<CountEn; i++) + { + a=ad+i*(ac+i*(ab+i*aa)); + ph=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4))); + if (add) data[i]+=a*cos(ph); + else data[i]=a*cos(ph); + } + p1=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4))); +}//Sinusoid + +/* + function Sinusoid: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of + amplitude and frequency, recursive implementation. + + In: CountSt, CountEn + a3, a2, a1, a0: trinomial coefficients of amplitude + f3, f2, f1, f0: trinomial coefficients of frequency + ph: initial phase angle at 0 (NOT at CountSt) + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + ph: phase angle at CountEn + + No return value. This function requires 8-byte stack alignment for optimal speed. +*/ +void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, + double f3, double f2, double f1, double f0, double &ph, bool add) +{ + int i; + double a, da, dda, ddda, dph, ddph, dddph, ddddph, + sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph, + p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp; + if (CountSt==0) + { + a=a0; da=a1+a2+a3; dda=2*a2+6*a3; ddda=6*a3; + dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4; + } + else + { + a=a0+CountSt*(a1+CountSt*(a2+CountSt*a3)); + da=a1+a2+a3+CountSt*(2*a2+3*a3+CountSt*3*a3); + dda=2*a2+6*a3+CountSt*6*a3; ddda=6*a3; + ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4))); + dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4)); + ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4); + dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4; + } + sph=sin(ph), cph=cos(ph); + sdph=sin(dph), cdph=cos(dph); + sddph=sin(ddph), cddph=cos(ddph); + sdddph=sin(dddph), cdddph=cos(dddph); + sddddph=sin(ddddph), cddddph=cos(ddddph); + if (add) + { + for (i=CountSt; i<CountEn; i++) + { + data[i]+=a*cph; + a=a+da; da=da+dda; dda=dda+ddda; + tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp; + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + } + else + { + for (i=CountSt; i<CountEn; i++) + { + data[i]=a*cph; + a=a+da; da=da+dda; dda=dda+ddda; + tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp; + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + } + ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); +} + +/* + function SinusoidExp: synthesizes complex sinusoid whose derivative log amplitude and frequency are + trinomials + + In: CountSt, CountEn + a3, a2, a1, a0: trinomial coefficients for the derivative of log amplitude + omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency + ea, ph: initial log amplitude and phase angle at 0 + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + ea, ph: log amplitude and phase angle at CountEn. + + No return value. +*/ +void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, + double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add) +{ + double e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4, + p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4; + if (add) for (int i=CountSt; i<CountEn; i++) + { + double lea=e0+i*(e1+i*(e2+i*(e3+i*e4))); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]+=exp(cdouble(lea, lph)); + } + else for (int i=CountSt; i<CountEn; i++) + { + double lea=e0+i*(e1+i*(e2+i*(e3+i*e4))); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]=exp(cdouble(lea, lph)); + } + ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4))); + ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); +}//SinusoidExp + +/* + function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M]. + This version also synthesizes its derivative. + + In: h[M][T], dih[M][T]: basis functions and their difference-integrals + lamda[M]: coefficients of h[M] + tmpexp: inital logarithm at 0 + Out: s[T], ds[T]: synthesized sinusoid and its derivative + tmpexp: logarithm at T + + No return value. +*/ +void SinusoidExp(int T, cdouble* s, cdouble* ds, int M, cdouble* lamda, double** h, double** dih, cdouble& tmpexp) +{ + for (int t=0; t<T; t++) + { + s[t]=exp(tmpexp); + cdouble dexp=0, dR=0; + for (int m=0; m<M; m++) dexp+=lamda[m]*dih[m][t], dR+=lamda[m]*h[m][t]; + tmpexp+=dexp; + ds[t]=s[t]*dR; + } +}//SinusoidExp + +/* + function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M]. + This version does not synthesize its derivative. + + In: dih[M][T]: difference of integrals of basis functions h[M] + lamda[M]: coefficients of h[M] + tmpexp: inital logarithm at 0 + Out: s[T]: synthesized sinusoid + tmpexp: logarithm at T + + No return value. +*/ +void SinusoidExp(int T, cdouble* s, int M, cdouble* lamda, double** dih, cdouble& tmpexp) +{ + for (int t=0; t<T; t++) + { + s[t]=exp(tmpexp); + cdouble dexp=0; + for (int m=0; m<M; m++) dexp+=lamda[m]*dih[m][t]; + tmpexp+=dexp; + } +}//SinusoidExp + +/* + function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials + + In: CountSt, CountEn + a3, a2, a1, a0: trinomial coefficients for log amplitude + omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency + ph: initial phase angle at 0 + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + ph: phase angle at CountEn. + + No return value. +*/ +void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, + double omg3, double omg2, double omg1, double omg0, double &ph, bool add) +{ + double p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4; + if (add) for (int i=CountSt; i<CountEn; i++) + { + double lea=a0+i*(a1+i*(a2+i*a3)); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]+=exp(cdouble(lea, lph)); + } + else for (int i=CountSt; i<CountEn; i++) + { + double lea=a0+i*(a1+i*(a2+i*a3)); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]=exp(cdouble(lea, lph)); + } + ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); +}//SinusoidExpA + +/* + function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials + with phase angle specified at both ends. + + In: CountSt, CountEn + a3, a2, a1, a0: trinomial coefficients for log amplitude + omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency + ph0, ph2: phase angles at 0 and CountEn. + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + + No return value. +*/ +void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, + double omg3, double omg2, double omg1, double omg0, double ph0, double ph2, bool add) +{ + double p0=ph0, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4; + double pend=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); + + int k=floor((pend-ph2)/2/M_PI+0.5); + double d=ph2-pend+2*M_PI*k; + double _p=-2*d/CountEn/CountEn/CountEn; + double _q=3*d/CountEn/CountEn; + + if (add) for (int i=CountSt; i<CountEn; i++) + { + double lea=a0+i*(a1+i*(a2+i*a3)); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]+=exp(cdouble(lea, lph+(i*i*(_q+i*_p)))); + } + else for (int i=CountSt; i<CountEn; i++) + { + double lea=a0+i*(a1+i*(a2+i*a3)); + double lph=p0+i*(p1+i*(p2+i*(p3+i*p4))); + data[i]=exp(cdouble(lea, lph+(i*i*(_q+i*_p)))); + } +}//SinusoidExpA + +/* + function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and + frequency is h[M]'q[M]. This version also synthesizes its derivative. + + In: h[M][T], dh[M][T], dih[M][T]: basis functions and their derivatives and difference-integrals + p[M], q[M]: real and imaginary parts of coefficients of h[M] + tmpph: inital phase angle at 0 + Out: s[T], ds[T]: synthesized sinusoid and its derivative + tmpph: phase angle at T + + No return value. +*/ +void SinusoidExpA(int T, cdouble* s, cdouble* ds, int M, double* p, double* q, double** h, double** dh, double** dih, double& tmpph) +{ + for (int t=0; t<T; t++) + { + double e=0, dph=0, drr=0, dri=0; + for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t], drr+=p[m]*dh[m][t], dri+=q[m]*h[m][t]; + s[t]=exp(cdouble(e, tmpph)); + ds[t]=s[t]*cdouble(drr, dri); + tmpph+=dph; + } +}//SinusoidExpA + +/* + function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and + frequency is h[M]'q[M]. This version does not synthesize its derivative. + + In: h[M][T], dih[M][T]: basis functions and their difference-integrals + p[M], q[M]: real and imaginary parts of coefficients of h[M] + tmpph: inital phase angle at 0 + Out: s[T]: synthesized sinusoid + tmpph: phase angle at T + + No return value. +*/ +void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double& tmpph) +{ + for (int t=0; t<T; t++) + { + double e=0, dph=0; + for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t]; + s[t]=exp(cdouble(e, tmpph)); + tmpph+=dph; + } +}//SinusoidExpA + +/* + function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and + frequency is h[M]'q[M] with phase angle specified at both ends. This version does not synthesize its + derivative. + + In: h[M][T], dih[M][T]: basis functions and their difference-integrals + p[M], q[M]: real and imaginary parts of coefficients of h[M] + ph1, ph2: phase angles at 0 and T. + Out: s[T]: synthesized sinusoid + + No return value. +*/ +void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double ph1, double ph2) +{ + double pend=ph1; + for (int t=0; t<T; t++) + { + double dph=0; + for (int m=0; m<M; m++) dph+=q[m]*dih[m][t]; + pend+=dph; + } + + int k=floor((pend-ph2)/2/M_PI+0.5); + double d=ph2-pend+2*M_PI*k; + double _p=-2*d/T/T/T; + double _q=3*d/T/T; + + double ph=ph1; + for (int t=0; t<T; t++) + { + double e=0, dph=0; + for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t]; + if (e>300) e=300; + if (e<-300) e=-300; + s[t]=exp(cdouble(e, ph+(t*t*(_q+t*_p)))); + ph+=dph; + } +}//SinusoidExpA + +/* +//This is not used any longer as the recursion does not seem to help saving computation with all its overheads. +void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, + double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add) +{ + int i; + double dea, ddea, dddea, ddddea, + dph, ddph, dddph, ddddph, + sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph, + e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4, + p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4, tmp; + if (CountSt==0) + { + dea=e1+e2+e3+e4; ddea=2*e2+6*e3+14*e4; dddea=6*e3+36*e4; ddddea=24*e3; + dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4; + } + else + { + ea=e0+CountSt*(e1+CountSt*(e2+CountSt*(e3+CountSt*e4))); + dea=e1+e2+e3+e4+CountSt*(2*e2+3*e3+4*e4+CountSt*(3*e3+6*e4+CountSt*4*e4)); + ddea=2*e2+6*e3+14*e4+CountSt*(6*e3+24*e4+CountSt*12*e4); + dddea=6*e3+36*e4+CountSt*24*e4; ddddea=24*e4; + ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4))); + dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4)); + ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4); + dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4; + } + sph=sin(ph), cph=cos(ph); + sdph=sin(dph), cdph=cos(dph); + sddph=sin(ddph), cddph=cos(ddph); + sdddph=sin(dddph), cdddph=cos(dddph); + sddddph=sin(ddddph), cddddph=cos(ddddph); + if (add) + { + for (i=CountSt; i<CountEn; i++) + { + data[i]+=exp(ea)*cdouble(cph, sph); + ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea; + tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp; + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + } + else + { + for (i=CountSt; i<CountEn; i++) + { + data[i]=exp(ea)*cdouble(cph, sph); + ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea; + tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp; + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + } + ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4))); + ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); +} //*/ + +/* + function Sinusoid: recursive cos-sin generator with trinomial frequency + + In: CountSt, CountEn + f3, f2, f1, f0: trinomial coefficients of frequency + ph: initial phase angle at 0 (NOT at CountSt) + Out: datar[CountSt:CountEn-1], datai[CountSt:CountEn-1]: synthesized pair of cosine and sine functions + ph: phase angle at CountEn + + No return value. +*/ +void Sinusoid(double* datar, double* datai, int CountSt, int CountEn, double f3, double f2, double f1, double f0, double &ph) +{ + int i; + double dph, ddph, dddph, ddddph, + sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph, + p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp; + if (CountSt==0) + { + dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4; + } + else + { + ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4))); + dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4)); + ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4); + dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4; + } + sph=sin(ph), cph=cos(ph); + sdph=sin(dph), cdph=cos(dph); + sddph=sin(ddph), cddph=cos(ddph); + sdddph=sin(dddph), cdddph=cos(dddph); + sddddph=sin(ddddph), cddddph=cos(ddddph); + + for (i=CountSt; i<CountEn; i++) + { + datar[i]=cph; datai[i]=sph; + tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp; + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4))); +}//Sinusoid*/ + +/* + function Sinusoids: recursive harmonic multi-sinusoid generator + + In: st, en + M: number of partials + a3[M], a2[M], a1[M], a0[M]: trinomial coefficients for partial amplitudes + f3, f2, f1, f0: trinomial coefficients for fundamental frequency + ph[M]: partial phases at 0. + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[st:en-1]: output buffer. + ph[M]: partial phases at en. + + No return value. +*/ +void Sinusoids(int M, double* data, int st, int en, double* a3, double* a2, double* a1, double* a0, double f3, double f2, double f1, double f0, double* ph, bool add) +{ + double dph, ddph, dddph, ddddph; + double sdph, cdph, cdph2, sddph, cddph, sdddph, cdddph, sddddph, cddddph, sdmph, cdmph, sdm_1ph, cdm_1ph; + double p0, p1, p2, p3, p4, tmp, tmp2; + double *a=(double*)malloc8(sizeof(double)*M*6), *da=&a[M], *dda=&a[M*2], *ddda=&a[M*3], + *sph=&a[M*4], *cph=&a[M*5]; + + for (int m=0; m<M; m++) + { + p0=ph[m], p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4; + if (st==0) + { + a[m]=a0[m]; da[m]=a1[m]+a2[m]+a3[m]; dda[m]=2*a2[m]+6*a3[m]; ddda[m]=6*a3[m]; + } + else + { + a[m]=a0[m]+st*(a1[m]+st*(a2[m]+st*a3[m])); + da[m]=a1[m]+a2[m]+a3[m]+st*(2*a2[m]+3*a3[m]+st*3*a3[m]); + dda[m]=2*a2[m]+6*a3[m]+st*6*a3[m]; ddda[m]=6*a3[m]; + ph[m]=p0+st*(p1+st*(p2+st*(p3+st*p4))); + } + sph[m]=sin(ph[m]), cph[m]=cos(ph[m]); + ph[m]=p0+(m+1)*en*(p1+en*(p2+en*(p3+en*p4))); + } + + if (st==0) + { + dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4; + } + else + { + dph=p1+p2+p3+p4+st*(2*p2+3*p3+4*p4+st*(3*p3+6*p4+st*4*p4)); + ddph=2*p2+6*p3+14*p4+st*(6*p3+24*p4+st*12*p4); + dddph=6*p3+36*p4+st*24*p4; ddddph=24*p4; + } + sdph=sin(dph), cdph=cos(dph); + sddph=sin(ddph), cddph=cos(ddph); + sdddph=sin(dddph), cdddph=cos(dddph); + sddddph=sin(ddddph), cddddph=cos(ddddph); + + if (add) + { + for (int i=st; i<en; i++) + { + data[i]+=a[0]*cph[0]; a[0]+=da[0]; da[0]+=dda[0]; dda[0]+=ddda[0]; + tmp=cph[0]*cdph-sph[0]*sdph; sph[0]=sph[0]*cdph+cph[0]*sdph; cph[0]=tmp; + cdm_1ph=1, sdm_1ph=0, cdmph=cdph, sdmph=sdph, cdph2=2*cdph; + + for (int m=1; m<M; m++) + { + data[i]+=a[m]*cph[m]; a[m]+=da[m]; da[m]+=dda[m]; dda[m]+=ddda[m]; +// asm{mov ecx,m} asm{mov eax,a} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,cph} asm{fld qword ptr [edx+ecx*8]} asm{fmul st(0),st(1)} asm{mov edx,data} asm{mov ebx,i} asm{fadd qword ptr [edx+ebx*8]} asm{fstp qword ptr [edx+ebx*8]} asm{mov edx,da} asm{fld qword ptr [edx+ecx*8]} asm{fadd st(1),st(0)} asm{mov ebx,dda} asm{fld qword ptr [ebx+ecx*8]} asm{fadd st(1),st(0)} asm{mov edi,ddda} asm{fadd qword ptr [edi+ecx*8]} asm{fstp qword ptr [ebx+ecx*8]} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]} + tmp=cdmph, tmp2=sdmph; + cdmph=cdmph*cdph2-cdm_1ph; sdmph=sdmph*cdph2-sdm_1ph; + cdm_1ph=tmp, sdm_1ph=tmp2; + + tmp=cph[m]*cdmph-sph[m]*sdmph; sph[m]=sph[m]*cdmph+cph[m]*sdmph; cph[m]=tmp; +// asm{mov ecx,m} asm{mov eax,cph} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,sph} asm{fld qword ptr [edx+ecx*8]} asm{fld st(1)} asm{fmul sdmph} asm{fld st(1)} asm{fmul sdmph} asm{fld cdmph} asm{fmul st(4),st(0)} asm{fmulp st(3),st(0)} asm{fsubp st(3),st(0)} asm{faddp} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]} + } + + tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp; + tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp; + tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp; + } + } + else + { + } + free8(a); +}//Sinusoids*/ + +/* + function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients. + + In: CountSt, CountEn + aa, ab, ac, ad: trinomial coefficients of amplitude. + fa, fb, fc, fd: trinomial coefficients of frequency. + ph0, ph2: phase angles at 0 and CountEn. + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: data[CountSt:CountEn-1]: output buffer. + + No return value. +*/ +void Sinusoid(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad, + double fa, double fb, double fc, double fd, double ph0, double ph2, bool add) +{ + double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4))); + int k=floor((pend-ph2)/2/M_PI+0.5); + double d=ph2-pend+2*M_PI*k; + double p=-2*d/CountEn/CountEn/CountEn; + double q=3*d/CountEn/CountEn, a, ph; + for (int i=CountSt; i<CountEn; i++) + { + a=ad+i*(ac+i*(ab+i*aa)); if (a<0) a=0; + ph=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p); + if (add) data[i]+=a*cos(ph); + else data[i]=a*cos(ph); + } +}//Sinusoid + +/* + function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients, + returning sinusoid coefficients instead of waveform. + + In: CountSt, CountEn + aa, ab, ac, ad: trinomial coefficients of amplitude (or log amplitude if LogA=true) + fa, fb, fc, fd: trinomial coefficients of frequency. + ph0, ph2: phase angles at 0 and CountEn. + LogA: specifies whether log amplitude or amplitude is a trinomial + Out: f[CountSt:CountEn-1], a[CountSt:CountEn-1], ph[CountSt:CountEn-1]: synthesized sinusoid parameters + da[CountSt:CountEn-1]: derivative of synthesized amplitude, optional + + No return value. +*/ +void Sinusoid(double* f, double* a, double* ph, double* da, int CountSt, int CountEn, double aa, double ab, + double ac, double ad, double fa, double fb, double fc, double fd, double ph0, double ph2, bool LogA) +{ + double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4))); + int k=floor((pend-ph2)/2/M_PI+0.5); + double d=ph2-pend+2*M_PI*k; + double p=-2*d/CountEn/CountEn/CountEn; + double q=3*d/CountEn/CountEn; + if (LogA) for (int i=CountSt; i<CountEn; i++) + { + a[i]=exp(ad+i*(ac+i*(ab+i*aa))); + if (da) da[i]=a[i]*(ac+i*(2*ab+i*3*aa)); + f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI); + ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p); + } + else for (int i=CountSt; i<CountEn; i++) + { + a[i]=ad+i*(ac+i*(ab+i*aa)); + if (da) da[i]=ac+i*(2*ab+i*3*aa); + f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI); + ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p); + } +}//Sinusoid + +/* + function Sinusoid: generates trinomial frequency and phase with phase correction. + + In: CountSt, CountEn + fa, fb, fc, fd: trinomial coefficients of frequency. + ph0, ph2: phase angles at 0 and CountEn. + Out: f[CountSt:CountEn-1], ph[CountSt:CountEn-1]: output buffers holding frequency and phase. + + No return value. +*/ +void Sinusoid(double* f, double* ph, int CountSt, int CountEn, double fa, double fb, + double fc, double fd, double ph0, double ph2) +{ + double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4))); + int k=floor((pend-ph2)/2/M_PI+0.5); + double d=ph2-pend+2*M_PI*k; + double p=-2*d/CountEn/CountEn/CountEn; + double q=3*d/CountEn/CountEn; + for (int i=CountSt; i<CountEn; i++) + { + f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI); + ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p); + } +}//Sinusoid + +/* + function SynthesizeSinusoid: synthesizes a time-varying sinusoid from a sequence of frequencies and amplitudes + + In: xs[Fr]: measurement points, should be integers although *xs has double type. + fs[Fr], as[Fr]: sequence of frequencies and amplitudes at xs[Fr] + phs[0]: initial phase angle at (int)xs[0]. + dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1] + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer + Out: xrec[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den. + phs[Fr]: phase angles at xs[Fr] + + Returns pointer to xrec. +*/ +double* SynthesizeSinusoid(double* xrec, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add, bool* terminatetag) +{ + double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3], + *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3]; + CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1); + CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1); + double ph=phs[0]; + for (int fr=0; fr<Fr-1; fr++) + { + phs[fr]=ph; + ALIGN8(Sinusoid(&xrec[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, add);) + if (terminatetag && *terminatetag) {delete[] f3; return 0;} + } + phs[Fr-1]=ph; + ALIGN8(Sinusoid(&xrec[(int)xs[Fr-2]-dst], xs[Fr-1]-xs[Fr-2], den-xs[Fr-2], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2], ph, add); + Sinusoid(&xrec[(int)xs[0]-dst], dst-xs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], phs[0], add);) + delete[] f3; + return xrec; +}//SynthesizeSinusoid + +/* + function ShiftTrinomial: shifts the origin of a trinomial from 0 to T + + In: a3, a2, a1, a0. + Out: b3, b2, b1, b0, so that a3*x^3+a2*x^2+a1*x+a0=b3(x-T)^3+b2(x-T)^2+b1(x-T)+b0 + + No return value. +*/ +void ShiftTrinomial(double T, double& b3, double& b2, double& b1, double& b0, double a3, double a2, double a1, double a0) +{ + b3=a3; + b2=a2+T*3*b3; + b1=a1+T*(2*b2-T*3*b3); + b0=a0+T*(b1-T*(b2-T*b3)); +}//ShiftTrinomial + +/* + function SynthesizeSinusoidP: synthesizes a time-varying sinusoid from a sequence of frequencies, + amplitudes and phase angles + + In: xs[Fr]: measurement points, should be integers although *xs has double type. + fs[Fr], as[Fr], phs[Fr]: sequence of frequencies, amplitudes and phase angles at xs[Fr] + dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1] + add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output + buffer + Out: xrecm[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den. + + Returns pointer to xrecm. +*/ +double* SynthesizeSinusoidP(double* xrecm, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add) +{ + double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3], + *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3]; + CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1); + CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1); + for (int fr=0; fr<Fr-1; fr++) Sinusoid(&xrecm[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], phs[fr], phs[fr+1], add); + double tmpph=phs[0]; Sinusoid(&xrecm[(int)xs[0]-dst], dst-xs[0], 0, 0, 0, 0, a0[0], f3[0], f2[0], f1[0], f0[0], tmpph, add); + //extend the trinomials on [xs[Fr-2], xs[Fr-1]) based at xs[Fr-2] to beyond xs[Fr-1] based at xs[Fr-1]. + tmpph=phs[Fr-1]; + ShiftTrinomial(xs[Fr-1]-xs[Fr-2], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2]); + ShiftTrinomial(xs[Fr-1]-xs[Fr-2], a3[Fr-1], a2[Fr-1], a1[Fr-1], a0[Fr-1], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2]); + Sinusoid(&xrecm[(int)xs[Fr-1]-dst], 0, den-xs[Fr-1], 0, 0, 0, a0[Fr-1], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], tmpph, add); + delete[] f3; + return xrecm; +}//SynthesizeSinusoidP
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SinSyn.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,33 @@ +#ifndef SinSynH +#define SinSynH + +/* + SinSyn.h - sinusoid synthesis routines for sinusoid modeling cycle. Functions named Sinusoid construct + sinusoid segments whose amplitude and frequency are modeled as polynomials or linear combinations of + basis functions; functions named SinusoidExp construct sinusoid segments whose logarithmic amplitude + derivative and frequency are modeled thus; functions named SinusoidExpA construct sinusoid segments + whose log amplitude and frequency are modeled thus. +*/ + +#include "xcomplex.h" +//--segmental synthesis routines--------------------------------------------- +void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, double* a, double* f, double* p, bool ad=true); +void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, bool ad=true); +void Sinusoid(double* f, double* a, double* ph, double* da, int CountSt, int CountEn, double aa, double ab, double ac, double ad, double fa, double fb, double fc, double fd, double ph0, double ph2, bool LogA=false); +void Sinusoid(double* f, double* ph, int CountSt, int CountEn, double fa, double fb, double fc, double fd, double ph0, double ph2); +void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, double f3, double f2, double f1, double f0, double ph0, double ph2, bool add); +void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, double f3, double f2, double f1, double f0, double &ph, bool add); +void Sinusoid(double* datar, double* datai, int CountSt, int CountEn, double f3, double f2, double f1, double f0, double &ph); +void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add); +void SinusoidExp(int T, cdouble* s, cdouble* ds, int M, cdouble* lamda, double** h, double** dih, cdouble& tmpexp); +void SinusoidExp(int T, cdouble* s, int M, cdouble* lamda, double** dih, cdouble& tmpexp); +void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0, double omg3, double omg2, double omg1, double omg0, double &ph, bool add); +void SinusoidExpA(int T, cdouble* s, cdouble* ds, int M, double* p, double* q, double** h, double** dh, double** dih, double& tmpph); +void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double& tmpph); +void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double ph1, double ph2); + +//--multi-segmental synthesis routines--------------------------------------- +double* SynthesizeSinusoid(double* xrec, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add, bool* terminatetag); +double* SynthesizeSinusoidP(double* xrecm, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TStream.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,23 @@ +#ifndef TSTREAM_H +#define TSTREAM_H + +/* + TStream.h - a stream I/O interface without implementation. + + This file is included to allow compiling relevent functions that uses Borland VCL's TStream class for + abstract I/O purposes. +*/ + +enum TSeekOrigin {soFromBeginning, soFromCurrent, soFromEnd}; +class TStream +{ +public: + TStream(); + ~TStream(); + int Read(void*, int){return 0;} + int Write(void*, int){return 0;} + int Seek(int, TSeekOrigin){return Position;} + int Position; +}; + +#endif // TSTREAM_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WindowFunctions.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,377 @@ +//--------------------------------------------------------------------------- +#include <math.h> +#include <mem.h> +#include "WindowFunctions.h" +#include "align8.h" +//--------------------------------------------------------------------------- + +/* + function I0: Bessel function of order zero + + Returns the Bessel function of x. +*/ +double I0(double x) +{ + const double eps=1e-9; + + int n=1; + double S=1, D=1, T; + + while (D>eps*S) + { + T=x/(2*n++); + D*=T*T; + S+=D; + } + return S; +}//I0 + +/* + function FillWindow: fills a buffer $Result with a window function. + + In: wt:window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newwindow[Count+1]: the window function. + + No return value. This function is designed assuming Count being even, and Count/2 is the symmetry + centre of newwindow. newwindow has physical size Count+1 for compatibility purpose. For all vanishing + windows (e.g. Hann) newwindow[0]=newwindow[Count]=0. +*/ +void FillWindow(double* Result, WindowType wt, int Count, int* ips, double* dps) +{ + switch(wt) + { + case wtRectangle: + for (int i=0; i<=Count; i++) + Result[i]=1; + break; + case wtTriangular: + for (int i=0; i<=Count; i++) + Result[i]=1-2*fabs(i-Count/2.0)/Count; + break; + case wtHamming: + for (int i=0; i<=Count; i++) + Result[i]=(0.54-0.46*cos(2*M_PI*i/Count)); + break; + case wtBlackman: + for (int i=0; i<=Count; i++) + Result[i]=0.42-0.5*cos(2*M_PI*i/Count)+0.08*cos(4*M_PI*i/Count); + break; + case wtGaussian: + { + double num=*dps*M_PI*M_PI/2/Count/Count; + for (int i=0; i<=Count; i++) + Result[i]=exp(-(i-Count/2.0)*(i-Count/2.0)*num); + break; + } + case wtKaiser: + { + double ldps=*dps*M_PI*M_PI/2/Count; + double den=I0(0.5*Count*ldps); + for (int i=0; i<=Count; i++) + Result[i]=I0(ldps*sqrt(0.25*Count*Count-(i-Count*0.5)*(i-Count*0.5)))/den; + break; + } + case wtHalfCosine: + { + for (int i=0; i<=Count; i++) + Result[i]=sin(M_PI*i/Count); + break; + } + case wtHann: + { + for (int i=0; i<=Count; i++) + Result[i]=(0.5-cos(M_PI*2*i/Count)/2); + break; + } + case wtHannSqr: + { + for (int i=0; i<=Count; i++) + Result[i]=(3-4*cos(M_PI*2*i/Count)+cos(M_PI*4*i/Count))/8; + break; + } + case wtHann3sqrt: + { + for (int i=0; i<=Count; i++) + { + double tmp=sin(M_PI*i/Count); + Result[i]=tmp*tmp*tmp; + } + break; + } + } +}//FillWindow + +/* + function NewWindow: creates a window function. + + In: wt: window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newwindow[Count+1]: the window function. + + Returns pointer to newwindow. newwindow is created anew if newwindow=0 is specified on start. +*/ +double* NewWindow(WindowType wt, int Count, int* ips, double* dps, double* newwindow) +{ + double* Result=newwindow; if (!Result) Result=new double[Count+1]; + FillWindow(Result, wt, Count, ips, dps); + return Result; +}//NewWindow + +/* + function NewWindow8: 8-byte aligned version of NewWindow. + + In: wt: window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newwindow[Count+1]: the window function. + + Returns pointer to newwindow. newwindow is created anew and 8-byte aligned if newwindow=0 is + specified on start. However, if newwindow is not 8-byte aligned on start, the unaligned buffer is + returned. +*/ +double* NewWindow8(WindowType wt, int Count, int* ips, double* dps, double* newwindow) +{ + double* Result=newwindow; if (!Result) Result=(double*)malloc8(sizeof(double)*(Count+1)); + FillWindow(Result, wt, Count, ips, dps); + return Result; +}//NewWindow8 + +/* + function NewdWindow: computes the derivative of a window function. + + In: wt: window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newdwindow[Count+1]: the derivative window function. + + Returns pointer to newdwindow. newdwindow is created anew if newdwindow=0 is specified on start. +*/ +double* NewdWindow(WindowType wt, int Count, int* ips, double* dps, double* newdwindow) +{ + double* Result=newdwindow; if (!Result) Result=new double[Count+1]; + double piiCount=M_PI/Count; + switch(wt) + { + case wtRectangle: + memset(Result, 0, sizeof(double)*(Count+1)); + break; + case wtTriangular: + throw("Trying to differentiate triangular window."); + break; + case wtHamming: + for (int i=0; i<=Count; i++) + Result[i]=0.92*piiCount*sin(2*piiCount*i); + break; + case wtBlackman: + for (int i=0; i<=Count; i++) + Result[i]=piiCount*sin(2*piiCount*i)-0.32*piiCount*sin(4*piiCount*i); + break; + case wtGaussian: + throw("Trying to differentiate Gaussian window."); + break; + case wtKaiser: + throw("Trying to differentiate Kaiser window."); + break; + case wtHalfCosine: + { + for (int i=0; i<=Count; i++) + Result[i]=piiCount*cos(piiCount*i); + break; + } + case wtHann: + { + for (int i=0; i<=Count; i++) + Result[i]=piiCount*sin(2*piiCount*i); + break; + } + case wtHannSqr: + { + for (int i=0; i<=Count; i++) + Result[i]=piiCount*sin(2*piiCount*i)-0.5*piiCount*sin(4*piiCount*i); + break; + } + case wtHann3sqrt: + { + for (int i=0; i<=Count; i++) + { + double s=sin(M_PI*i/Count), c=cos(M_PI*i/Count); + Result[i]=3*piiCount*s*s*c; + } + } + } + return Result; +}//NewdWindow + +/* + function NewddWindow: computes the 2nd-order derivative of a window function. + + In: wt: window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newddwindow[Count+1]: the 2nd-order derivative window function. + + Returns pointer to newddwindow. newddwindow is created anew if newddwindow=0 is specified on start. +*/ +double* NewddWindow(WindowType wt, int Count, int* ips, double* dps, double* newddwindow) +{ + double* Result=newddwindow; if (!Result) Result=new double[Count+1]; + double piiCount=M_PI/Count; + double piC2=piiCount*piiCount; + switch(wt) + { + case wtRectangle: + memset(Result, 0, sizeof(double)*(Count+1)); + break; + case wtTriangular: + throw("Trying to double-differentiate triangular window."); + break; + case wtHamming: + for (int i=0; i<=Count; i++) + Result[i]=1.84*piC2*cos(2*piiCount*i); + break; + case wtBlackman: + for (int i=0; i<=Count; i++) + Result[i]=2*piC2*cos(2*piiCount*i)-1.28*piC2*cos(4*piiCount*i); + break; + case wtGaussian: + throw("Trying to double-differentiate Gaussian window."); + break; + case wtKaiser: + throw("Trying to double-differentiate Kaiser window."); + break; + case wtHalfCosine: + { + for (int i=0; i<=Count; i++) + Result[i]=-piC2*sin(piiCount*i); + break; + } + case wtHann: + { + for (int i=0; i<=Count; i++) + Result[i]=2*piC2*cos(2*piiCount*i); + break; + } + case wtHannSqr: + { + for (int i=0; i<=Count; i++) + Result[i]=2*piC2*cos(2*piiCount*i)-2*piC2*cos(4*piiCount*i); + break; + } + case wtHann3sqrt: + { + for (int i=0; i<=Count; i++) + { + double s=sin(M_PI*i/Count), c=cos(M_PI*i/Count); + Result[i]=3*piC2*(2*c*c-s*s)*s; + } + break; + } + } + return Result; +}//NewddWindow + +/* + function NewdddWindow: computes the 3rd-order derivative of a window function. + In: wt: window type + Count: window size + ips & ups: extra window specificatin parameters + Out: newdddwindow[Count+1]: the 3rd-order derivative window function. + + Returns pointer to newdddwindow. newdddwindow is created anew if newdddwindow=0 is specified on start. +*/ +double* NewdddWindow(WindowType wt, int Count, int* ips, double* dps, double* newdddwindow) +{ + double* Result=newdddwindow; if (!Result) Result=new double[Count+1]; + double piiCount=M_PI/Count; + double piC2=piiCount*piiCount; + double piC3=piiCount*piC2; + switch(wt) + { + case wtRectangle: + memset(Result, 0, sizeof(double)*(Count+1)); + break; + case wtTriangular: + throw("Trying to triple-differentiate triangular window."); + break; + case wtHamming: + for (int i=0; i<=Count; i++) + Result[i]=-3.68*piC3*sin(2*piiCount*i); + break; + case wtBlackman: + for (int i=0; i<=Count; i++) + Result[i]=-4*piC3*sin(2*piiCount*i)+5.12*piC3*sin(4*piiCount*i); + break; + case wtGaussian: + throw("Trying to triple-differentiate Gaussian window."); + break; + case wtKaiser: + throw("Trying to triple-differentiate Kaiser window."); + break; + case wtHalfCosine: + { + for (int i=0; i<=Count; i++) + Result[i]=-piC3*cos(piiCount*i); + break; + } + case wtHann: + { + for (int i=0; i<=Count; i++) + Result[i]=-4*piC3*sin(2*piiCount*i); + break; + } + case wtHannSqr: + { + for (int i=0; i<=Count; i++) + Result[i]=-4*piC3*sin(2*piiCount*i)+8*piC3*sin(4*piiCount*i); + break; + } + case wtHann3sqrt: + throw("Trying to triple-differentiate Hann^1.5 window."); + break; + } + return Result; +}//NewdddWindow + +//--------------------------------------------------------------------------- +/* + function windowspec: computes a few descriptors of a cosine family window. A window function in the + cosine window family is the linear combination of a few cosine functions, therefore has a cosine + decomposition. + + In: wt: window type + N: window size + Out: c[M+1], coefficients of cosine decomposition + iH2: reciprocal of square of L2 norm, + d[2M+1]: self-convolution of c[], optional + + No return value. +*/ +void windowspec(WindowType wt, int N, int* M, double* c, double* iH2, double* d) +{ + switch(wt) + { + case wtRectangle: + M[0]=0, c[0]=1, iH2[0]=1.0/N/N; + if (d) d[0]=1; + break; + case wtHamming: + M[0]=1, c[0]=0.54, c[1]=0.23, iH2[0]=1.0/(0.3974*N*N); + if (d) d[0]=0.3974, d[1]=0.2484, d[2]=0.0529; + break; + case wtBlackman: + M[0]=2, c[0]=0.42, c[1]=0.25, c[2]=0.04, iH2[0]=1.0/(0.3046*N*N); + if (d) d[0]=0.3046, d[1]=0.23, d[2]=0.0961, d[4]=0.02, d[5]=0.0016; + break; + case wtHann: + M[0]=1, c[0]=0.5, c[1]=0.25, iH2[0]=1.0/(0.375*N*N); + if (d) d[0]=0.375, d[1]=0.25, d[2]=0.0625; + break; + default: + M[0]=1, c[0]=0.5, c[1]=0.25, iH2[0]=1.0/(0.375*N*N); + if (d) d[0]=0.375, d[1]=0.25, d[2]=0.0625; + break; + } +}//windowspec
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WindowFunctions.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,33 @@ +#ifndef WindowFunctionsH +#define WindowFunctionsH + +/* + WindowFunctions.cpp - implements a few common window functions. +*/ + +enum WindowType +{ + wtRectangle, + wtTriangular, + wtHamming, + wtBlackman, + wtGaussian, + wtKaiser, + wtHalfCosine, + wtHann, + wtHannSqr, + wtHann3sqrt +}; + +//--window function computation routines------------------------------------- +void FillWindow(double* newwindow, WindowType wt, int Count, int* ips=0, double* dps=0); +double* NewWindow(WindowType wt, int Count, int* ips=0, double* dps=0, double* newwindow=0); +double* NewWindow8(WindowType wt, int Count, int* ips=0, double* dps=0, double* newwindow=0); +double* NewdWindow(WindowType wt, int Count, int* ips=0, double* dps=0, double* newdwindow=0); +double* NewddWindow(WindowType wt, int Count, int* ips=0, double* dps=0, double* newddwindow=0); +double* NewdddWindow(WindowType wt, int Count, int* ips=0, double* dps=0, double* newdddwindow=0); + +//--other functions---------------------------------------------------------- +void windowspec(WindowType wt, int N, int* M, double* c, double* iH2, double* d=0); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/align8.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,33 @@ +//--------------------------------------------------------------------------- + + +#include <memory.h> +#include <stdlib.h> +#include "align8.h" + +//--------------------------------------------------------------------------- +/* + function malloc8: 8-byte (64-bit) aligned memory allocation. + + Returns pointer to a memory block of $size starting at an address divisible by 8. +*/ +void* malloc8(unsigned size) +{ + char *buffer, *result; + buffer=(char*)malloc(size+8+sizeof(void*)); + if(!buffer) return(NULL); + char* tmp=&buffer[sizeof(void*)]; + result=&((char*)((unsigned)tmp&0xFFFFFFF8))[8]; + ((void**)result)[-1]=buffer; + return(result); +}//malloc8 + +/* + function free8: deallocation for malloc8() + + No return value. +*/ +void free8(void* buffer8) +{ + if (buffer8) free(((void**)buffer8)[-1]); +}//free8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/align8.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,39 @@ +//--------------------------------------------------------------------------- + +#ifndef align8H +#define align8H + +/* + align8.cpp - 8-byte (64-bit) alignment routines. + + This file provides tools for aligning boundaries of arrays, particularly of 64-byte units, e.g. double- + precision floating points, to physical addresses divisible by 8, including aligning dynamically + allocated memory block and aligning call stack. Currently stack alignment is disabled due to lack + of compiler support for Intel assembly. + + Further reading: "Double precision floating point alignment issue.pdf" +*/ + +//--stack alignment---------------------------------------------------------- +/* + These two macros are used to envelop a function call so that the function is called with a stack + pointer that is a multiple of 8. Aligning the stack does not automatically ensure that local double + -precision floating-point variables (or other variables one wants to align to 8-byte boundaries) are + aligned to 8-byte boundaries; one has to adjust the variable declaration order, using dummy variables + if necessary, to make the alignment happen. +*/ +//macro ALIGN_STACK_8 moves the current stack pointer to a multiple of 8 +#define ALIGN_STACK_8 {int alignstack; asm {mov alignstack, esp} asm {and esp, 0xFFFFFFF8} +//macro RESTORE_STACK moves the stack pointer back to where it was before ALIGN_STACK_8 +#define RESTORE_STACK asm {mov esp, alignstack}} + +//ALIGN8 ensures F is executed with a start stack pointer divisible by 8. Currently disabled. +#define ALIGN8(F) F +//uncomment the following line to enable stack alignment +//#define ALIGN8(F) ALIGN_STACK_8 F RESTORE_STACK + +//--64-bit aligned memory allocation routines-------------------------------- +void* malloc8(unsigned size); +void free8(void* buffer8); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/arrayalloc.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,88 @@ +#ifndef ARRAYALLOC +#define ARRAYALLOC + +/* + arrayalloc.h - 2D, 3D and 4D array memory allocation routines. + + An x-dimensional array (x=2, 3, 4, ...) is managed as a single memory block hosting all records, plus + an index block which is a (x-1)D array itself. Therefore a 2D array is allocated as two 1D arrays, a + 3D array is allocated as three 1D arrays, etc. + + Examples: + Alloc2(4, N, x) declares an array x[4][N] of double-precision floating points. + Allocate3(int, K, L, M, x) allocates an array x[K][L][M] of integers and returns x as int***. + + This file also includes a garbage collector class MList that works with arrays allcoated in this way. +*/ + + +#include <stdlib.h> + +//2D array allocation macros for declaring an array of double +#define Alloc2(M, N, x) \ + double **x=new double*[M]; x[0]=new double[(M)*(N)]; for (int _z=1; _z<M; _z++) x[_z]=&x[0][_z*(N)]; +#define Alloc2L(M, N, x, LIST) Alloc2(M, N, x); if (LIST) LIST->Add(x, 2); +//2D array allocation macros for all data types +#define Allocate2(INT, M, N, x) \ + x=new INT*[M]; x[0]=new INT[(M)*(N)]; for (int _z=1; _z<M; _z++) x[_z]=&x[0][_z*(N)]; + #define Allocate2L(INT, M, N, x, LIST) Allocate2(INT, M, N, x); if (LIST) LIST->Add(x, 2); +//2D array deallocation macro +#define DeAlloc2(x) {if (x) {delete[] (char*)(x[0]); delete[] x; x=0;}} + +//3D array allocation macro for declaring an array of double +#define Alloc3(L, M, N, x) \ + double*** x=new double**[L]; x[0]=new double*[(L)*(M)]; x[0][0]=new double[(L)*(M)*(N)]; \ + for (int _z=0; _z<L; _z++) {x[_z]=&x[0][_z*(M)]; x[_z][0]=&x[0][0][_z*(M)*(N)]; \ + for (int __z=1; __z<M; __z++) x[_z][__z]=&x[_z][0][__z*(N)]; } +//3D array allocation macros for all data types +#define Allocate3(INT, L, M, N, x) \ + x=new INT**[L]; x[0]=new INT*[(L)*(M)]; x[0][0]=new INT[(L)*(M)*(N)]; \ + for (int _z=0; _z<L; _z++) {x[_z]=&x[0][_z*(M)]; x[_z][0]=&x[0][0][_z*(M)*(N)]; \ + for (int __z=1; __z<M; __z++) x[_z][__z]=&x[_z][0][__z*(N)]; } + #define Allocate3L(INT, M, N, O, x, LIST) Allocate3(INT, M, N, O, x); if (LIST) LIST->Add(x, 3); +//3D array deallocation macro +#define DeAlloc3(x) {if (x) {delete[] (char*)(x[0][0]); delete[] x[0]; delete[] x; x=0;}} + +//4D array allocation macro for declaring an array of double +#define Alloc4(L, M, N, O, x) \ + double**** x=new double***[L]; x[0]=new double**[(L)*(M)]; \ + x[0][0]=new double*[(L)*(M)*(N)]; x[0][0][0]=new double[(L)*(M)*(N)*(O)]; \ + for (int _z=0; _z<L; _z++){ \ + x[_z]=&x[0][_z*(M)]; x[_z][0]=&x[0][0][_z*(M)*(N)]; x[_z][0][0]=&x[0][0][0][_z*(M)*(N)*(O)]; \ + for (int __z=0; __z<M; __z++){ \ + x[_z][__z]=&x[_z][0][__z*(N)]; x[_z][__z][0]=&x[_z][0][0][__z*(N)*(O)]; \ + for (int ___z=1; ___z<N; ___z++) x[_z][__z][___z]=&d[_z][__z][0][___z*(O)]; }} +//4D array deallocation macro +#define DeAlloc4(x) {if (x) {delete[] (char*)(x[0][0][0]); delete[] x[0][0]; delete[] x[0]; delete[] x; x=0;}} + + +/* + MList is a garbage collector for arrays created using Alloc* or Allocate* (*=2, 3, 4). After being + added to the list the arrays will be automatically freed when MList is deleted. + + Using MList: + Create an MList object and add all buffers (1D, 2D, 3D or 4D) to be recycled to the MList using + MList::Add(...). Deleting the MList will recycle all the added buffers. + + Example: + Alloc2L(4, N, x, mlist) declares 2D array x and registers it with garbage collector mlist,so that x is + freed when mlist is deleted. +*/ +class MList +{ +public: + int cap[4]; + int count[4]; + void** List[4]; + MList(){for (int i=0; i<4; i++) cap[i]=64, count[i]=0, List[i]=(void**)malloc(sizeof(void*)*cap[i]);} + ~MList(){ + for (int i=0; i<count[0]; i++){delete[] (char*)(List[0][i]); List[0][i]=0;} free(List[0]); + for (int i=0; i<count[1]; i++){void** tmp=(void**)List[1][i]; DeAlloc2(tmp); List[1][i]=0;} free(List[1]); + for (int i=0; i<count[2]; i++){void*** tmp=(void***)List[2][i]; DeAlloc3(tmp); List[2][i]=0;} free(List[2]); + for (int i=0; i<count[3]; i++){void**** tmp=(void****)List[3][i]; DeAlloc4(tmp); List[3][i]=0;} free(List[3]);} + void __fastcall Add(void* item, int Dim){ + int Gr=Dim-1; if (count[Gr]==cap[Gr]) IncCap(Gr); List[Gr][count[Gr]++]=item;} + void IncCap(int Gr){cap[Gr]+=64; List[Gr]=(void**)realloc(List[Gr], sizeof(void*)*cap[Gr]);} +}; + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fft.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,1159 @@ +//--------------------------------------------------------------------------- + +#include <mem.h> +#include <stdlib.h> +#include "fft.h" + +//--------------------------------------------------------------------------- +/* + function Atan2: (0, 0)-safe atan2 + + Returns 0 is x=y=0, atan2(x,y) otherwise. +*/ +double Atan2(double y, double x) +{ + if (x==0 && y==0) return 0; + else return atan2(y, x); +}//Atan2 + +/* + function BitInv: inverse bit order of Value within an $Order-bit expression. + + In: integer Value smaller than 2^Order + + Returns an integer whose lowest Order bits are the lowest Order bits of Value in reverse order. +*/ +int BitInv(int Value, int Order) +{ + int Result; + Result=0; + for (int i=0;i<Order;i++) + { + Result=(Result<<1)+(Value&0x00000001); + Value=Value>>1; + } + return Result; +}//BitInv + +/* + function SetTwiddleFactors: fill w[N/2] with twiddle factors used in N-point complex FFT. + + In: N + Out: array w[N/2] containing twiddle factors + + No return value. +*/ +void SetTwiddleFactors(int N, cdouble* w) +{ + double ep=-M_PI*2/N; + for (int i=0; i<N/2; i++) + { + double tmp=ep*i; + w[i].x=cos(tmp), w[i].y=sin(tmp); + } +}//SetTwiddleFactors + +//--------------------------------------------------------------------------- +/* + function CFFTCbii: basic complex DIF-FFT module, applied after bit-inversed ordering of inputs + + In: Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex waveform + Out: X[Wid]: complex spectrum + + No return value. +*/ +void CFFTCbii(int Order, cdouble* W, cdouble* X) +{ + int i, j, k, ElemsPerGroup, Groups, X0, X1, X2; + cdouble Temp; + for (i=0; i<Order; i++) + { + ElemsPerGroup=1<<i; + Groups=1<<(Order-i-1); + X0=0; + for (j=0; j<Groups; j++) + { + for (k=0; k<ElemsPerGroup; k++) + { + int kGroups=k*Groups; + X1=X0+k; + X2=X1+ElemsPerGroup; + //X(X2)<-X(X2)*W + Temp.x=X[X2].x*W[kGroups].x-X[X2].y*W[kGroups].y, + X[X2].y=X[X2].x*W[kGroups].y+X[X2].y*W[kGroups].x; + X[X2].x=Temp.x; + Temp.x=X[X1].x+X[X2].x, Temp.y=X[X1].y+X[X2].y; + X[X2].x=X[X1].x-X[X2].x, X[X2].y=X[X1].y-X[X2].y; + X[X1]=Temp; + } + X0+=ElemsPerGroup*2; + } + } +}//CFFTCbii + +/* + function CFFTC: in-place complex FFT + + In: Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex waveform + bitinv[Wid]: bit-inversion table + Out: X[Wid]: complex spectrum + + No return value. +*/ +void CFFTC(int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int N=1<<Order, i, jj; + cdouble Temp; + int* bitinv1=bitinv; + if (!bitinv) bitinv=CreateBitInvTable(Order); + for (i=1; i<N-1; i++) + { + jj=bitinv[i]; + if (i<jj) + { + Temp=X[i]; + X[i]=X[jj]; + X[jj]=Temp; + } + } + if (!bitinv1) free(bitinv); + CFFTCbii(Order, W, X); +}//CFFTC + +/* + function CFFTC: complex FFT + + In: Input[Wid]: complex waveform + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + bitinv[Wid]: bit-inversion table + Out:X[Wid]: complex spectrum + Amp[Wid]: amplitude spectrum + Arg[Wid]: phase spectrum + + No return value. +*/ +void CFFTC(cdouble* Input, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int i, N=1<<Order; + + if (X!=Input) memcpy(X, Input, sizeof(cdouble)*N); + CFFTC(Order, W, X, bitinv); + + if (Amp) for (i=0; i<N; i++) Amp[i]=sqrt(X[i].x*X[i].x+X[i].y*X[i].y); + if (Arg) for (i=0; i<N; i++) Arg[i]=Atan2(X[i].y, X[i].x); +}//CFFTC + +//--------------------------------------------------------------------------- +/* + function CIFFTCbii: basic complex IFFT module, applied after bit-inversed ordering of inputs + + In: Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex spectrum + Out: X[Wid]: complex waveform + + No return value. +*/ +void CIFFTCbii(int Order, cdouble* W, cdouble* X) +{ + int N=1<<Order, i, j, k, Groups, ElemsPerGroup, X0, X1, X2; + cdouble Temp; + + for (i=0; i<Order; i++) + { + ElemsPerGroup=1<<i; + Groups=1<<(Order-i-1); + X0=0; + for (j=0; j<Groups; j++) + { + for (k=0; k<ElemsPerGroup; k++) + { + int kGroups=k*Groups; + X1=X0+k; + X2=X1+ElemsPerGroup; + Temp.x=X[X2].x*W[kGroups].x+X[X2].y*W[kGroups].y, + X[X2].y=-X[X2].x*W[kGroups].y+X[X2].y*W[kGroups].x; + X[X2].x=Temp.x; + Temp.x=X[X1].x+X[X2].x, Temp.y=X[X1].y+X[X2].y; + X[X2].x=X[X1].x-X[X2].x, X[X2].y=X[X1].y-X[X2].y; + X[X1]=Temp; + } + X0=X0+ElemsPerGroup*2; + } + } + for (i=0; i<N; i++) + { + X[i].x/=N; + X[i].y/=N; + } +}//CIFFTCbii + +/* + function CIFFTC: in-place complex IFFT + + In: Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex spectrum + bitinv[Wid]: bit-inversion table + Out: X[Wid]: complex waveform + + No return value. +*/ +void CIFFTC(int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int N=1<<Order, i, jj, *bitinv1=bitinv; + cdouble Temp; + if (!bitinv) bitinv=CreateBitInvTable(Order); + for (i=1; i<N-1; i++) + { + jj=bitinv[i]; + if (i<jj) + { + Temp=X[i]; + X[i]=X[jj]; + X[jj]=Temp; + } + } + if (!bitinv1) free(bitinv); + CIFFTCbii(Order, W, X); +}//CIFFTC + +/* + function CIFFTC: complex IFFT + + In: Input[Wid]: complex spectrum + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + bitinv[Wid]: bit-inversion table + Out:X[Wid]: complex waveform + + No return value. +*/ +void CIFFTC(cdouble* Input, int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int N=1<<Order; + if (X!=Input) memcpy(X, Input, sizeof(cdouble)*N); + if (bitinv) CIFFTC(Order, W, X, bitinv); + else CIFFTC(Order, W, X); +}//CIFFTC + +//--------------------------------------------------------------------------- +/* + function CIFFTR: complex-to-real IFFT + + In: Input[Wid/2+1]: complex spectrum, imaginary parts of Input[0] and Input[Wid/2] are ignored + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + hbitinv[Wid/2]: half bit-inversion table + Out:X[Wid]: real waveform + + No return value. +*/ +void CIFFTR(cdouble* Input, int Order, cdouble* W, double* X, int* hbitinv) +{ + int N=1<<Order, i, j, k, Groups, ElemsPerGroup, X0, X1, X2, *hbitinv1=hbitinv; + cdouble Temp; + + Order--; N/=2; + if (!hbitinv) hbitinv=CreateBitInvTable(Order); + + cdouble* Xc=(cdouble*)X; + + Xc[0].y=0.5*(Input[0].x-Input[N].x); + Xc[0].x=0.5*(Input[0].x+Input[N].x); + for (int i=1; i<N/2; i++) + { + double frp=Input[i].x+Input[N-i].x, frn=Input[i].x-Input[N-i].x, + fip=Input[i].y+Input[N-i].y, fin=Input[i].y-Input[N-i].y; + Xc[i].x=0.5*(frp+frn*W[i].y-fip*W[i].x); + Xc[i].y=0.5*(fin+frn*W[i].x+fip*W[i].y); + Xc[N-i].x=0.5*(frp-frn*W[i].y+fip*W[i].x); + Xc[N-i].y=0.5*(-fin+frn*W[i].x+fip*W[i].y); + } + Xc[N/2].x=Input[N/2].x; + Xc[N/2].y=-Input[N/2].y; + + ElemsPerGroup=1<<Order; + Groups=1; + + for (i=0; i<Order; i++) + { + ElemsPerGroup/=2; + X0=0; + for (j=0; j<Groups; j++) + { + int kGroups=hbitinv[j]; + for (k=0; k<ElemsPerGroup; k++) + { + X1=X0+k; + X2=X1+ElemsPerGroup; + Temp.x=Xc[X2].x*W[kGroups].x+Xc[X2].y*W[kGroups].y, + Xc[X2].y=-Xc[X2].x*W[kGroups].y+Xc[X2].y*W[kGroups].x; + Xc[X2].x=Temp.x; + Temp.x=Xc[X1].x+Xc[X2].x, Temp.y=Xc[X1].y+Xc[X2].y; + Xc[X2].x=Xc[X1].x-Xc[X2].x, Xc[X2].y=Xc[X1].y-Xc[X2].y; + Xc[X1].x=Temp.x, Xc[X1].y=Temp.y; + } + X0=X0+(ElemsPerGroup<<1); + } + Groups*=2; + } + + for (i=0; i<N; i++) + { + int jj=hbitinv[i]; + if (i<jj) + { + Temp=Xc[i]; + Xc[i]=Xc[jj]; + Xc[jj]=Temp; + } + } + double norm=1.0/N;; + N*=2; + for (int i=0; i<N; i++) X[i]*=norm; + if (!hbitinv1) free(hbitinv); +}//CIFFTR + +//--------------------------------------------------------------------------- +/* + function CreateBitInvTable: creates a table of bit-inversed integers + + In: Order: interger, equals log2(size of table) + + Returns a pointer to a newly allocated array containing the table. The returned pointer must be freed + using free(), NOT delete[]. +*/ +int* CreateBitInvTable(int Order) +{ + int N=1<<Order; + int* result=(int*)malloc(sizeof(int)*N); + for (int i=0; i<N; i++) result[i]=BitInv(i, Order); + return result; +}//CreateBitInvTable + + +//--------------------------------------------------------------------------- +/* + function RFFTC_ual: unaligned real-to-complex FFT + + In: Input[Wid]: real waveform + Order; integer, equals log2(Wid) + W[Wid/2]: twiddle factors + hbitinv[Wid/2]: half bit-inversion table + Out:X[Wid}: complex spectrum + Amp[Wid]: amplitude spectrum + Arg[Wid]: phase spetrum + + No return value. +*/ +void RFFTC_ual(double* Input, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* hbitinv) +{ + int N=1<<Order, i, j, k, *hbitinv1=hbitinv, Groups, ElemsPerGroup, X0, X1, X2; + cdouble Temp, zp, zn; + + N/=2; Order--; + + //Input being NULL implies external initialization of X. This is used by RFFTCW but is not + //recommended for external use. + if (Input) + { + if (!hbitinv) hbitinv=CreateBitInvTable(Order); + + if (Input==(double*)X) + { + //Input being identical to X is not recommended for external use. + for (int i=0; i<N; i++) + { + int bi=hbitinv[i]; + if (i<bi) {cdouble tmp=X[i]; X[i]=X[bi]; X[bi]=tmp;} + } + } + else + { + for (i=0; i<N; i++) X[i]=((cdouble*)Input)[hbitinv[i]]; + } + if (!hbitinv1) free(hbitinv); + } + + for (i=0; i<Order; i++) + { + ElemsPerGroup=1<<i; + Groups=1<<(Order-i-1); + X0=0; + for (j=0; j<Groups; j++) + { + for (k=0; k<ElemsPerGroup; k++) + { + X1=X0+k; + X2=X1+ElemsPerGroup; + int kGroups=k*2*Groups; + Temp.x=X[X2].x*W[kGroups].x-X[X2].y*W[kGroups].y, + X[X2].y=X[X2].x*W[kGroups].y+X[X2].y*W[kGroups].x; + X[X2].x=Temp.x; + Temp.x=X[X1].x+X[X2].x, Temp.y=X[X1].y+X[X2].y; + X[X2].x=X[X1].x-X[X2].x, X[X2].y=X[X1].y-X[X2].y; + X[X1]=Temp; + } + X0=X0+(ElemsPerGroup<<1); + } + } + zp.x=X[0].x+X[0].y, zn.x=X[0].x-X[0].y; + X[0].x=zp.x; + X[0].y=0; + X[N].x=zn.x; + X[N].y=0; + for (int k=1; k<N/2; k++) + { + zp.x=X[k].x+X[N-k].x, zn.x=X[k].x-X[N-k].x, + zp.y=X[k].y+X[N-k].y, zn.y=X[k].y-X[N-k].y; + X[k].x=0.5*(zp.x+W[k].y*zn.x+W[k].x*zp.y); + X[k].y=0.5*(zn.y-W[k].x*zn.x+W[k].y*zp.y); + X[N-k].x=0.5*(zp.x-W[k].y*zn.x-W[k].x*zp.y); + X[N-k].y=0.5*(-zn.y-W[k].x*zn.x+W[k].y*zp.y); + } + //X[N/2].x=X[N/2].x; + X[N/2].y=-X[N/2].y; + N*=2; + + for (int k=N/2+1; k<N; k++) X[k].x=X[N-k].x, X[k].y=-X[N-k].y; + if (Amp) for (i=0; i<N; i++) Amp[i]=sqrt(X[i].x*X[i].x+X[i].y*X[i].y); + if (Arg) for (i=0; i<N; i++) Arg[i]=Atan2(X[i].x, X[i].y); +}//RFFTC_ual + +//--------------------------------------------------------------------------- +/* + function RFFTCW: real-to-complex FFT with window + + In: Input[Wid]: real waveform + Window[Wid]: window function + Order; integer, equals log2(Wid) + W[Wid/2]: twiddle factors + hbitinv[Wid/2]: half bit-inversion table + Out:X[Wid}: complex spectrum + Amp[Wid]: amplitude spectrum + Arg[Wid]: phase spetrum + + No return value. +*/ +void RFFTCW(double* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* hbitinv) +{ + int N=1<<Order, *hbitinv1=hbitinv; + if (!hbitinv) hbitinv=CreateBitInvTable(Order-1); + N/=2; + + if (Input==(double*)X) + { //so that X[n].x IS Input[2n], X[n].y IS Input[2n+1] + for (int n=0; n<N; n++) + { + int bi=hbitinv[n], n2=n+n, bi2=bi+bi; + if (n<bi) + { + double tmp=X[n].x*Window[n2]; X[n].x=X[bi].x*Window[bi2]; X[bi].x=tmp; + tmp=X[n].y*Window[n2+1]; X[n].y=X[bi].y*Window[bi2+1]; X[bi].y=tmp; + } + else if (n==bi) + { //so that X[n].x IS Input[bi] + X[n].x*=Window[bi2], X[n].y*=Window[bi2+1]; + } + } + } + else + { + for (int n=0; n<N; n++) + { + int bi=hbitinv[n], bi2=bi+bi; X[n].x=Input[bi2]*Window[bi2], X[n].y=Input[bi2+1]*Window[bi2+1]; + } + } + + RFFTC_ual(0, Amp, Arg, Order, W, X, hbitinv); + if (!hbitinv1) free(hbitinv); +}//RFFTCW + +/* + function RFFTCW: real-to-complex FFT with window + + In: Input[Wid]: real waveform as _int16 array + Window[Wid]: window function + Order; integer, equals log2(Wid) + W[Wid/2]: twiddle factors + hbitinv[Wid/2]: half bit-inversion table + Out:X[Wid}: complex spectrum + Amp[Wid]: amplitude spectrum + Arg[Wid]: phase spetrum + + No return value. +*/ +void RFFTCW(__int16* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* hbitinv) +{ + int N=1<<Order, *hbitinv1=hbitinv; + + N/=2; + if (!hbitinv) hbitinv=CreateBitInvTable(Order-1); + for (int n=0; n<N; n++) + { + int bi=hbitinv[n], bi2=bi+bi; X[n].x=Input[bi2]*Window[bi2], X[n].y=Input[bi2+1]*Window[bi2+1]; + } + + RFFTC_ual(0, Amp, Arg, Order, W, X, hbitinv); + if (!hbitinv1) free(hbitinv); +}//RFFTCW + +//--------------------------------------------------------------------------- +/* + function CFFTCW: complex FFT with window + + In: Window[Wid]: window function + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex waveform + bitinv[Wid]: bit-inversion table + Out:X[Wid], complex spectrum + + No return value. +*/ +void CFFTCW(double* Window, int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int N=1<<Order; + for (int i=0; i<N; i++) X[i].x*=Window[i], X[i].y*=Window[i]; + CFFTC(Order, W, X, bitinv); +}//CFFTCW + +/* + function CFFTCW: complex FFT with window + + In: Input[Wid]: complex waveform + Window[Wid]: window function + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + X[Wid]: complex waveform + bitinv[Wid]: bit-inversion table + Out:X[Wid], complex spectrum + Amp[Wid], amplitude spectrum + Arg[Wid], phase spectrum + + No return value. +*/ +void CFFTCW(cdouble* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* bitinv) +{ + int N=1<<Order; + for (int i=0; i<N; i++) X[i].x=Input[i].x*Window[i], X[i].y=Input[i].y*Window[i]; + CFFTC(X, Amp, Arg, Order, W, X, bitinv); +}//CFFTCW + +//--------------------------------------------------------------------------- +/* + function RDCT1: fast DCT1 implemented using FFT. DCT-I has the time scale 0.5-sample shifted from the DFT. + + In: Input[Wid]: real waveform + Order: integer, equals log2(Wid) + qW[Wid/8]: quarter table of twiddle factors + qX[Wid/4]: quarter FFT data buffer + qbitinv[Wid/4]: quarter bit-inversion table + Out:Output[Wid]: DCT-I of Input. + + No return value. Content of qW is destroyed on return. On calling the fft buffers should be allocated + to size 0.25*Wid. +*/ +void RDCT1(double* Input, double* Output, int Order, cdouble* qW, cdouble* qX, int* qbitinv) +{ + const double lmd0=sqrt(0.5); + if (Order==0) + { + Output[0]=Input[0]*lmd0; + return; + } + if (Order==1) + { + double tmp1=(Input[0]+Input[1])*lmd0; + Output[1]=(Input[0]-Input[1])*lmd0; + Output[0]=tmp1; + return; + } + int order=Order-1, N=1<<(Order-1), C=1; + bool createbitinv=!qbitinv; + if (createbitinv) qbitinv=CreateBitInvTable(Order-2); + double *even=(double*)malloc8(sizeof(double)*N*2); + double *odd=&even[N]; + //data pass from Input to Output, combined with the first sequence split + for (int i=0, N2=N*2; i<N; i++) + { + even[i]=Input[i]+Input[N2-1-i]; + odd[i]=Input[i]-Input[N2-1-i]; + } + for (int i=0; i<N; i++) Output[i*2]=even[i], Output[i*2+1]=odd[i]; + while (order>1) + { + //N=2^order, 4|N, 2|hN + //keep subsequence 0, 2C, 4C, ... 2(N-1)C + //process sequence C, 3C, ..., (2N-1)C only + //RDCT4 routine + int hN=N/2, N2=N*2; + for (int i=0; i<hN; i++) + { + double b=Output[(2*(2*i)+1)*C], c=Output[(2*(N-1-2*i)+1)*C], theta=-(i+0.25)*M_PI/N; + double co=cos(theta), si=sin(theta); + qX[i].x=b*co-c*si, qX[i].y=c*co+b*si; + } + CFFTC(order-1, qW, qX, qbitinv); + for (int i=0; i<hN; i++) + { + double gr=qX[i].x, gi=qX[i].y, theta=-i*M_PI/N; + double co=cos(theta), si=sin(theta); + Output[(4*i+1)*C]=gr*co-gi*si; + Output[(N2-4*i-1)*C]=-gr*si-gi*co; + } + N=(N>>1); + C=(C<<1); + for (int i=1; i<N/4; i++) + qW[i]=qW[i*2]; + for (int i=1; i<N/2; i++) + qbitinv[i]=qbitinv[i*2]; + + //splitting subsequence 0, 2C, 4C, ..., 2(N-1)C + for (int i=0, N2=N*2; i<N; i++) + { + even[i]=Output[i*C]+Output[(N2-1-i)*C]; + odd[i]=Output[i*C]-Output[(N2-1-i)*C]; + } + for (int i=0; i<N; i++) + { + Output[2*i*C]=even[i]; + Output[(2*i+1)*C]=odd[i]; + } + order--; + } + //order==1 + //use C and 3C in DCT4 + //use 0 and 2C in DCT1 + double c1=cos(M_PI/8), c2=cos(3*M_PI/8); + double tmp1=c1*Output[C]+c2*Output[3*C]; + Output[3*C]=c2*Output[C]-c1*Output[3*C]; + Output[C]=tmp1; + tmp1=Output[0]+Output[2*C]; + Output[2*C]=(Output[0]-Output[2*C])*lmd0; + Output[0]=tmp1*lmd0; + + if (createbitinv) free(qbitinv); + free8(even); +}//RDCT1 + +//--------------------------------------------------------------------------- +/* + function RDCT4: fast DCT4 implemented using FFT. DCT-IV has both the time and frequency scales + 0.5-sample or 0.5-bin shifted from DFT. + + In: Input[Wid]: real waveform + Order: integer, equals log2(Wid) + hW[Wid/4]: half table of twiddle factors + hX[Wid/2]: hal FFT data buffer + hbitinv[Wid/2]: half bit-inversion table + Out:Output[Wid]: DCT-IV of Input. + + No return value. Content of hW not affected. On calling the fft buffers should be allocated to size + 0.5*Wid. +*/ +void RDCT4(double* Input, double* Output, int Order, cdouble* hW, cdouble* hX, int* hbitinv) +{ + if (Order==0) + { + Output[0]=Input[0]/sqrt(2); + return; + } + if (Order==1) + { + double c1=cos(M_PI/8), c2=cos(3*M_PI/8); + double tmp1=c1*Input[0]+c2*Input[1]; + Output[1]=c2*Input[0]-c1*Input[1]; + Output[0]=tmp1; + return; + } + int N=1<<Order, hN=N/2; + for (int i=0; i<hN; i++) + { + double b=Input[2*i], c=Input[N-1-i*2], theta=-(i+0.25)*M_PI/N; + double co=cos(theta), si=sin(theta); + hX[i].x=b*co-c*si, hX[i].y=c*co+b*si; + } + CFFTC(Order-1, hW, hX, hbitinv); + for (int i=0; i<hN; i++) + { + double gr=hX[i].x, gi=hX[i].y, theta=-i*M_PI/N; + double co=cos(theta), si=sin(theta); + Output[2*i]=gr*co-gi*si; + Output[N-1-2*i]=-gr*si-gi*co; + } +}//RDCT4 + +//--------------------------------------------------------------------------- +/* + function RIDCT1: fast IDCT1 implemented using FFT. + + In: Input[Wid]: DCT-I of some real waveform. + Order: integer, equals log2(Wid) + qW[Wid/8]: quarter table of twiddle factors + qX[Wid/4]: quarter FFT data buffer + qbitinv[Wid/4]: quarter bit-inversion table + Out:Output[Wid]: IDCT-I of Input. + + No return value. Content of qW is destroyed on return. On calling the fft buffers should be allocated + to size 0.25*Wid. +*/ +void RIDCT1(double* Input, double* Output, int Order, cdouble* qW, cdouble* qX, int* qbitinv) +{ + const double lmd0=sqrt(0.5); + if (Order==0) + { + Output[0]=Input[0]/lmd0; + return; + } + if (Order==1) + { + double tmp1=(Input[0]+Input[1])*lmd0; + Output[1]=(Input[0]-Input[1])*lmd0; + Output[0]=tmp1; + return; + } + int order=Order-1, N=1<<(Order-1), C=1; + bool createbitinv=!qbitinv; + if (createbitinv) qbitinv=CreateBitInvTable(Order-2); + double *even=(double*)malloc8(sizeof(double)*N*2); + double *odd=&even[N]; + + while (order>0) + { + //N=2^order, 4|N, 2|hN + //keep subsequence 0, 2C, 4C, ... 2(N-1)C + //process sequence C, 3C, ..., (2N-1)C only + //data pass from Input + for (int i=0; i<N; i++) + { + odd[i]=Input[(i*2+1)*C]; + } + + //IDCT4 routine + //RIDCT4(odd, odd, order, qW, qX, qbitinv); + + if (order==1) + { + double c1=cos(M_PI/8), c2=cos(3*M_PI/8); + double tmp1=c1*odd[0]+c2*odd[1]; + odd[1]=c2*odd[0]-c1*odd[1]; + odd[0]=tmp1; + } + else + { + int hN=N/2; + for (int i=0; i<hN; i++) + { + double b=odd[2*i], c=odd[N-1-i*2], theta=-(i+0.25)*M_PI/N; + double co=cos(theta), si=sin(theta); + qX[i].x=b*co-c*si, qX[i].y=c*co+b*si; + } + CFFTC(order-1, qW, qX, qbitinv); + double i2N=2.0/N; + for (int i=0; i<hN; i++) + { + double gr=qX[i].x, gi=qX[i].y, theta=-i*M_PI/N; + double co=cos(theta), si=sin(theta); + odd[2*i]=(gr*co-gi*si)*i2N; + odd[N-1-2*i]=(-gr*si-gi*co)*i2N; + } + } + + order--; + N=(N>>1); + C=(C<<1); + for (int i=1; i<N/4; i++) + qW[i]=qW[i*2]; + for (int i=1; i<N/2; i++) + qbitinv[i]=qbitinv[i*2]; + + odd=&even[N]; + } + //order==0 + even[0]=Input[0]; + even[1]=Input[C]; + double tmp1=(even[0]+even[1])*lmd0; + Output[1]=(even[0]-even[1])*lmd0; + Output[0]=tmp1; + order++; + + while (order<Order) + { + N=(N<<1); + odd=&even[N]; + for (int i=0; i<N; i++) + { + Output[N*2-1-i]=(Output[i]-odd[i])/2; + Output[i]=(Output[i]+odd[i])/2; + } + order++; + } + + if (createbitinv) free(qbitinv); + free8(even); +}//RIDCT1 + +//--------------------------------------------------------------------------- +/* + function RIDCT4: fast IDCT4 implemented using FFT. + + In: Input[Wid]: DCT-IV of some real waveform + Order: integer, equals log2(Wid) + hW[Wid/4]: half table of twiddle factors + hX[Wid/2]: hal FFT data buffer + hbitinv[Wid/2]: half bit-inversion table + Out:Output[Wid]: IDCT-IV of Input. + + No return value. Content of hW not affected. On calling the fft buffers should be allocated to size + 0.5*Wid. +*/ +void RIDCT4(double* Input, double* Output, int Order, cdouble* hW, cdouble* hX, int* hbitinv) +{ + if (Order==0) + { + Output[0]=Input[0]*sqrt(2); + return; + } + if (Order==1) + { + double c1=cos(M_PI/8), c2=cos(3*M_PI/8); + double tmp1=c1*Input[0]+c2*Input[1]; + Output[1]=c2*Input[0]-c1*Input[1]; + Output[0]=tmp1; + return; + } + int N=1<<Order, hN=N/2; + for (int i=0; i<hN; i++) + { + double b=Input[2*i], c=Input[N-1-i*2], theta=-(i+0.25)*M_PI/N; + double co=cos(theta), si=sin(theta); + hX[i].x=b*co-c*si, hX[i].y=c*co+b*si; + } + CFFTC(Order-1, hW, hX, hbitinv); + double i2N=2.0/N; + for (int i=0; i<hN; i++) + { + double gr=hX[i].x, gi=hX[i].y, theta=-i*M_PI/N; + double co=cos(theta), si=sin(theta); + Output[2*i]=(gr*co-gi*si)*i2N; + Output[N-1-2*i]=(-gr*si-gi*co)*i2N; + } +}//RIDCT4 + +//--------------------------------------------------------------------------- +/* + function RLCT: real local cosine transform of equal frame widths Wid=2^Order + + In: data[Count]: real waveform + Order: integer, equals log2(Wid), Wid being the cosine atom size + g[wid]: lap window, designed so that g[k] increases from 0 to 1 and g[k]^2+g[wid-1-k]^2=1 + example: wid=4, g[k]=sin(pi*(k+0.5)/8). + Out:spec[Fr][Wid]: the local cosine transform + + No return value. +*/ +void RLCT(double** spec, double* data, int Count, int Order, int wid, double* g) +{ + int Wid=1<<Order, Fr=Count/Wid, hwid=wid/2; + int* hbitinv=CreateBitInvTable(Order-1); + cdouble *hx=(cdouble*)malloc8(sizeof(cdouble)*Wid*3/4), *hw=(cdouble*)&hx[Wid/2]; + double norm=sqrt(2.0/Wid); + SetTwiddleFactors(Wid/2, hw); + + for (int fr=0; fr<Fr; fr++) + { + if (fr==0) + { + memcpy(spec[fr], data, sizeof(double)*(Wid-hwid)); + for (int i=0, k=Wid+hwid-1, l=Wid-hwid; i<hwid; i++, k--, l++) + spec[fr][l]=data[l]*g[wid-1-i]-data[k]*g[i]; + } + else if (fr==Fr-1) + { + for (int i=hwid, j=fr*Wid, k=fr*Wid-1, l=0; i<wid; i++, j++, k--, l++) + spec[fr][l]=data[k]*g[wid-1-i]+data[j]*g[i]; + memcpy(&spec[fr][hwid], &data[fr*Wid+hwid], sizeof(double)*(Wid-hwid)); + } + else + { + for (int i=hwid, j=fr*Wid, k=fr*Wid-1, l=0; i<wid; i++, j++, k--, l++) + spec[fr][l]=data[k]*g[wid-1-i]+data[j]*g[i]; + if (Wid>wid) memcpy(&spec[fr][hwid], &data[fr*Wid+hwid], sizeof(double)*(Wid-wid)); + for (int i=0, j=(fr+1)*Wid-hwid, k=(fr+1)*Wid+hwid-1, l=Wid-hwid; i<hwid; i++, j++, k--, l++) + spec[fr][l]=data[j]*g[wid-1-i]-data[k]*g[i]; + } + } + for (int fr=0; fr<Fr; fr++) + { + if (fr==Fr-1) + { + for (int i=1; i<Wid/4; i++) hw[i]=hw[2*i], hbitinv[i]=hbitinv[2*i]; + RDCT1(spec[fr], spec[fr], Order, hw, hx, hbitinv); + } + else + RDCT4(spec[fr], spec[fr], Order, hw, hx, hbitinv); + +////The following line can be removed if the corresponding line in RILCT(...) is removed + for (int i=0; i<Wid; i++) spec[fr][i]*=norm; + } + free(hbitinv); + free8(hx); +}//RLCT + +//--------------------------------------------------------------------------- +/* + function RILCT: inverse local cosine transform of equal frame widths Wid=2^Order + + In: spec[Fr][Wid]: the local cosine transform + Order: integer, equals log2(Wid), Wid being the cosine atom size + g[wid]: lap window, designed so that g[k] increases from 0 to 1 and g[k]^2+g[wid-1-k]^2=1. + example: wid=4, g[k]=sin(pi*(k+0.5)/8). + Out:data[Count]: real waveform + + No return value. +*/ +void RILCT(double* data, double** spec, int Fr, int Order, int wid, double* g) +{ + int Wid=1<<Order, Count=Fr*Wid, hwid=wid/2, *hbitinv=CreateBitInvTable(Order-1); + cdouble *hx=(cdouble*)malloc8(sizeof(cdouble)*Wid*3/4), *hw=&hx[Wid/2]; + double norm=sqrt(Wid/2.0); + SetTwiddleFactors(Wid/2, hw); + + for (int fr=0; fr<Fr; fr++) + { + if (fr==Fr-1) + { + for (int i=1; i<Wid/4; i++) hw[i]=hw[2*i], hbitinv[i]=hbitinv[i*2]; + RIDCT1(spec[fr], &data[fr*Wid], Order, hw, hx, hbitinv); + } + else + RIDCT4(spec[fr], &data[fr*Wid], Order, hw, hx, hbitinv); + } + //unfolding + for (int fr=1; fr<Fr; fr++) + { + double* h=&data[fr*Wid]; + for (int i=0; i<hwid; i++) + { + double a=h[i], b=h[-1-i], c=g[i+hwid], d=g[hwid-1-i]; + h[i]=a*c-b*d, h[-i-1]=b*c+a*d; + } + } + + free8(hx); +////The following line can be removed if the corresponding line in RLCT(...) is removed + for (int i=0; i<Count; i++) data[i]*=norm; +}//RILCT + +//--------------------------------------------------------------------------- +/* + function CMFTC: radix-2 complex multiresolution Fourier transform + + In: x[Wid]: complex waveform + Order: integer, equals log2(Wid) + W[Wid/2]: twiddle factors + Out:X[Order+1][Wid]: multiresolution FT of x. X[0] is the same as x itself. + + No return value. + + Further reading: Wen X. and M. Sandler, "Calculation of radix-2 discrete multiresolution Fourier + transform," Signal Processing, vol.87 no.10, 2007, pp.2455-2460. +*/ +void CMFTC(cdouble* x, int Order, cdouble** X, cdouble* W) +{ + X[0]=x; + for (int n=1, L=1<<(Order-1), M=2; n<=Order; n++, L>>=1, M<<=1) + { + cdouble *Xn=X[n], *Xp=X[n-1]; + for (int l=0; l<L; l++) + { + cdouble* lX=&Xn[l*M]; + for (int m=0; m<M/2; m++) + { + lX[m].x=Xp[l*M+m].x+Xp[l*M+M/2+m].x; + lX[m].y=Xp[l*M+m].y+Xp[l*M+M/2+m].y; + double tmpr=x[l*M+m].x-x[l*M+M/2+m].x, tmpi=x[l*M+m].y-x[l*M+M/2+m].y; + int iw=m*L; + double wr=W[iw].x, wi=W[iw].y; + lX[M/2+m].x=tmpr*wr-tmpi*wi; + lX[M/2+m].y=tmpr*wi+tmpi*wr; + } + if (n==1) {} + else if (n==2) //two-point DFT + { + cdouble *aX=&X[n][l*M+M/2]; + double tmp; + tmp=aX[0].x+aX[1].x; aX[1].x=aX[0].x-aX[1].x; aX[0].x=tmp; + tmp=aX[0].y+aX[1].y; aX[1].y=aX[0].y-aX[1].y; aX[0].y=tmp; + } + else if (n==3) //4-point DFT + { + cdouble *aX=&X[n][l*M+M/2]; + double tmp, tmp2; + tmp=aX[0].x+aX[2].x; aX[2].x=aX[0].x-aX[2].x; aX[0].x=tmp; + tmp=aX[0].y+aX[2].y; aX[2].y=aX[0].y-aX[2].y; aX[0].y=tmp; + tmp=aX[1].y+aX[3].y; tmp2=aX[1].y-aX[3].y; aX[1].y=tmp; + tmp=aX[3].x-aX[1].x; aX[1].x+=aX[3].x; aX[3].x=tmp2; aX[3].y=tmp; + tmp=aX[0].x+aX[1].x; aX[1].x=aX[0].x-aX[1].x; aX[0].x=tmp; + tmp=aX[0].y+aX[1].y; aX[1].y=aX[0].y-aX[1].y; aX[0].y=tmp; + tmp=aX[2].x+aX[3].x; aX[3].x=aX[2].x-aX[3].x; aX[2].x=tmp; + tmp=aX[2].y+aX[3].y; aX[3].y=aX[2].y-aX[3].y; aX[2].y=tmp; + } + else //n>3 + { + cdouble *aX=&X[n][l*M+M/2]; + for (int an=1, aL=1, aM=M/2; an<n; aL*=2, aM/=2, an++) + { + for (int al=0; al<aL; al++) + for (int am=0; am<aM/2; am++) + { + int iw=am*2*aL*L; + cdouble *lX=&aX[al*aM]; + double x1r=lX[am].x, x1i=lX[am].y, + x2r=lX[aM/2+am].x, x2i=lX[aM/2+am].y; + lX[am].x=x1r+x2r, lX[am].y=x1i+x2i; + x1r=x1r-x2r, x1i=x1i-x2i; + lX[aM/2+am].x=x1r*W[iw].x-x1i*W[iw].y, + lX[aM/2+am].y=x1r*W[iw].y+x1i*W[iw].x; + } + } + } + } + } +}//CMFTC + + +//--------------------------------------------------------------------------- +/* + Old versions no longer in use. For reference only. +*/ +void RFFTC_ual_old(double* Input, double *Amp, double *Arg, int Order, cdouble* W, double* XR, double* XI, int* bitinv) +{ + int N=1<<Order, i, j, jj, k, *bitinv1=bitinv, Groups, ElemsPerGroup, X0, X1, X2; + cdouble Temp, zp, zn; + + if (!bitinv) bitinv=CreateBitInvTable(Order); + if (XR!=Input) for (i=0; i<N; i++) XR[i]=Input[bitinv[i]]; + else for (i=0; i<N; i++) {jj=bitinv[i]; if (i<jj) {Temp.x=XR[i]; XR[i]=XR[jj]; XR[jj]=Temp.x;}} + N/=2; + double* XII=&XR[N]; + Order--; + if (!bitinv1) free(bitinv); + for (i=0; i<Order; i++) + { + ElemsPerGroup=1<<i; + Groups=1<<(Order-i-1); + X0=0; + for (j=0; j<Groups; j++) + { + for (k=0; k<ElemsPerGroup; k++) + { + X1=X0+k; + X2=X1+ElemsPerGroup; + int kGroups=k*2*Groups; + Temp.x=XR[X2]*W[kGroups].x-XII[X2]*W[kGroups].y, + XII[X2]=XR[X2]*W[kGroups].y+XII[X2]*W[kGroups].x; + XR[X2]=Temp.x; + Temp.x=XR[X1]+XR[X2], Temp.y=XII[X1]+XII[X2]; + XR[X2]=XR[X1]-XR[X2], XII[X2]=XII[X1]-XII[X2]; + XR[X1]=Temp.x, XII[X1]=Temp.y; + } + X0=X0+(ElemsPerGroup<<1); + } + } + zp.x=XR[0]+XII[0], zn.x=XR[0]-XII[0]; + XR[0]=zp.x; + XI[0]=0; + XR[N]=zn.x; + XI[N]=0; + for (int k=1; k<N/2; k++) + { + zp.x=XR[k]+XR[N-k], zn.x=XR[k]-XR[N-k], + zp.y=XII[k]+XII[N-k], zn.y=XII[k]-XII[N-k]; + XR[k]=0.5*(zp.x+W[k].y*zn.x+W[k].x*zp.y); + XI[k]=0.5*(zn.y-W[k].x*zn.x+W[k].y*zp.y); + XR[N-k]=0.5*(zp.x-W[k].y*zn.x-W[k].x*zp.y); + XI[N-k]=0.5*(-zn.y-W[k].x*zn.x+W[k].y*zp.y); + } + XR[N/2]=XR[N/2]; + XI[N/2]=-XII[N/2]; + N*=2; + + for (int k=N/2+1; k<N; k++) XR[k]=XR[N-k], XI[k]=-XI[N-k]; + if (Amp) for (i=0; i<N; i++) Amp[i]=sqrt(XR[i]*XR[i]+XI[i]*XI[i]); + if (Arg) for (i=0; i<N; i++) Arg[i]=Atan2(XI[i], XR[i]); +}//RFFTC_ual_old + +void CIFFTR_old(cdouble* Input, int Order, cdouble* W, double* X, int* bitinv) +{ + int N=1<<Order, i, j, k, Groups, ElemsPerGroup, X0, X1, X2, *bitinv1=bitinv; + cdouble Temp; + if (!bitinv) bitinv=CreateBitInvTable(Order); + + Order--; + N/=2; + double* XII=&X[N]; + + X[0]=0.5*(Input[0].x+Input[N].x); + XII[0]=0.5*(Input[0].x-Input[N].x); + for (int i=1; i<N/2; i++) + { + double frp=Input[i].x+Input[N-i].x, frn=Input[i].x-Input[N-i].x, + fip=Input[i].y+Input[N-i].y, fin=Input[i].y-Input[N-i].y; + X[i]=0.5*(frp+frn*W[i].y-fip*W[i].x); + XII[i]=0.5*(fin+frn*W[i].x+fip*W[i].y); + X[N-i]=0.5*(frp-frn*W[i].y+fip*W[i].x); + XII[N-i]=0.5*(-fin+frn*W[i].x+fip*W[i].y); + } + X[N/2]=Input[N/2].x; + XII[N/2]=-Input[N/2].y; + + ElemsPerGroup=1<<Order; + Groups=1; + + for (i=0; i<Order; i++) + { + ElemsPerGroup/=2; + X0=0; + for (j=0; j<Groups; j++) + { + int kGroups=bitinv[j]/2; + for (k=0; k<ElemsPerGroup; k++) + { + X1=X0+k; + X2=X1+ElemsPerGroup; + Temp.x=X[X2]*W[kGroups].x+XII[X2]*W[kGroups].y, + XII[X2]=-X[X2]*W[kGroups].y+XII[X2]*W[kGroups].x; + X[X2]=Temp.x; + Temp.x=X[X1]+X[X2], Temp.y=XII[X1]+XII[X2]; + X[X2]=X[X1]-X[X2], XII[X2]=XII[X1]-XII[X2]; + X[X1]=Temp.x, XII[X1]=Temp.y; + } + X0=X0+(ElemsPerGroup<<1); + } + Groups*=2; + } + + N*=2; + Order++; + for (i=0; i<N; i++) + { + int jj=bitinv[i]; + if (i<jj) + { + Temp.x=X[i]; + X[i]=X[jj]; + X[jj]=Temp.x; + } + } + for (int i=0; i<N; i++) X[i]/=(N/2); + if (!bitinv1) free(bitinv); +}//RFFTC_ual_old +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fft.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,82 @@ +#ifndef fftH +#define fftH + +/* + fft.cpp - fast Fourier and cosine transforms + + Arguments of the function in this unit roughly follow the following rules: + Wid : size of transform + Order : log2(Wid) + W : FFT buffer: twiddle factors, standard size Wid/2 + X : FFT buffer: data buffer, standard size Wid + bitinv: FFT buffer: table of bit-inversed integers, standard size Wid + Window: window function, standard size Wid + spec : complex spectrogram + Amp : amplitude spectrogram + Arg : phase spectrogram + prefix h- : half-size + prefix q- : quarter-size +*/ + + +#include <math.h> +#include "xcomplex.h" +#include "align8.h" + +//--------------------------------------------------------------------------- +/* + macro AllocateFFTBuffer: allocates FFT buffers X and W of given length with an extra LDATA buffer the + right size for hosting such arrays as the standard-size window function or output amplitude spectrum. + + This macro allocates buffer with 64-bit alignment, which must be freed with free8() or FreeFFTBuffer + macro. +*/ +#define AllocateFFTBuffer(_WID, _LDATA, _W, _X) \ + double *_LDATA=(double*)malloc8(sizeof(double)*_WID*4); \ + cdouble *_X=(cdouble*)&_LDATA[_WID]; \ + cdouble *_W=(cdouble*)&_X[_WID]; \ + SetTwiddleFactors(_WID, _W); +#define FreeFFTBuffer(_LDATA) free8(_LDATA); + +//--compelx FFT and IFFT----------------------------------------------------- +void CFFTCbii(int Order, cdouble* W, cdouble* X); //complex FFT subroutine after bit-inversed ordering +void CFFTC(int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex FFT, compact in-place +void CFFTC(cdouble* Input, double* Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex FFT +void CFFTCW(double* Window, int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex FFT with window, compact in-place +void CFFTCW(cdouble* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex FFT with window +void CIFFTC(int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex IFFT, compact in-place +void CIFFTC(cdouble* Input, int Order, cdouble* W, cdouble* X, int* bitinv=0); //complex IFFT + +//--real FFT and IFFT-------------------------------------------------------- +void RFFTC_ual(double* Input, double* Amp, double* Arg, int Order, cdouble* W, cdouble* X, int* hbitinv=0); //real-to-complex FFT +void RFFTC_ual_old(double* Input, double *Amp, double *Arg, int Order, cdouble* W, double* XR, double* XI, int* bitinv=0); //depreciated +void RFFTCW(double* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* hbitinv=0); //real-to-complex FFT with window, double-type input +void RFFTCW(__int16* Input, double* Window, double *Amp, double *Arg, int Order, cdouble* W, cdouble* X, int* hbitinv=0); //real-to-complex FFT with window, __int16 input +void CIFFTR(cdouble* Input, int Order, cdouble* W, double* X, int* hbitinv=0); //complex-to-real IFFT + +/* + macro RFFTC: was defined as RFFTC_ual (literally "unaligned RFFTC") enveloped by ALIGN8(). However, as + ALIGN8() is currently disabled for lack of compiler support, RFFTC is equivalent to RFFTC_ual. +*/ +#define RFFTC(Input, Amp, Arg, Order, W, X, hbitinv) RFFTC_ual(Input, Amp, Arg, Order, W, X, hbitinv); + +//--MFT---------------------------------------------------------------------- +void CMFTC(double* xR, double* xI, int Order, cdouble** X, cdouble* W); //complex fast multiresolution FT + +//--DCT and IDCT------------------------------------------------------------- +void RDCT1(double* Input, double* Output, int Order, cdouble* qW, cdouble* qX, int* qbitinv=0); //DCT1 +void RDCT4(double* Input, double* Output, int Order, cdouble* hW, cdouble* hX, int* hbitinv=0); //DCT4 +void RIDCT1(double* Input, double* Output, int Order, cdouble* qW, cdouble* qX, int* qbitinv=0); //IDCT1 +void RIDCT4(double* Input, double* Output, int Order, cdouble* hW, cdouble* hX, int* hbitinv=0); //IDCT4 + +//--LCT---------------------------------------------------------------------- +void RLCT(double** spec, double* data, int Count, int Order, int wid, double* g); //local cosine transform +void RILCT(double* data, double** spec, int Fr, int Order, int wid, double* g); //inverse local cosine transform + +//--tools-------------------------------------------------------------------- +double Atan2(double, double); +int* CreateBitInvTable(int Order); //creates table of bit-inversed integers +void SetTwiddleFactors(int N, cdouble* w); //set twiddle factors + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hs.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,4798 @@ +//--------------------------------------------------------------------------- +#include <math.h> +#include "hs.h" +#include "align8.h" +#include "arrayalloc.h" +#include "procedures.h" +#include "SinEst.h" +#include "SinSyn.h" +#include "splines.h" +#include "xcomplex.h" +#ifdef I +#undef I +#endif + +//--------------------------------------------------------------------------- +//stiffcandid methods + +/* + method stiffcandid::stiffcandid: creates a harmonic atom with minimal info, i.e. fundamantal frequency + between 0 and Nyqvist frequency, stiffness coefficient between 0 and STIFF_B_MAX. + + In: Wid: DFT size (frame size, frequency resolution) +*/ +stiffcandid::stiffcandid(int Wid) +{ + F=new double[6]; + G=&F[3]; + double Fm=Wid*Wid/4; //NyqvistF^2 + F[0]=0, G[0]=0; + F[1]=Fm, G[1]=STIFF_B_MAX*Fm; + F[2]=Fm, G[2]=0; + N=3; + P=0; + s=0; + p=NULL; + f=NULL; +}//stiffcandid + +/* + method stiffcandid::stiffcandid: creates a harmonic atom with a frequency range for the $ap-th partial, + without actually taking this partial as "known". + + In; Wid: DFT size + ap: partial index + af, errf: centre and half-width of the frequency range of the ap-th partial, in bins +*/ +stiffcandid::stiffcandid(int Wid, int ap, double af, double errf) +{ + F=new double[10]; + G=&F[5]; + double Fm=Wid*Wid/4; + F[0]=0, G[0]=0; + F[1]=Fm, G[1]=STIFF_B_MAX*Fm; + F[2]=Fm, G[2]=0; + N=3; + P=0; + s=0; + p=NULL; + f=NULL; + double k=ap*ap-1; + double g1=(af-errf)/ap, g2=(af+errf)/ap; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + //apply constraints F+kG-g2<0 and -F-kG+g1<0 + cutcvpoly(N, F, G, 1, k, -g2); + cutcvpoly(N, F, G, -1, -k, g1); +}//stiffcandid + +/* + method stiffcandid::stiffcandid: creates a harmonic atom from one atom. + + In; Wid: DFT size + ap: partial index of the atom. + af, errf: centre and half-width of the frequency range of the atom, in bins. + ds: contribution to candidate score from this atom. +*/ +stiffcandid::stiffcandid(int Wid, int ap, double af, double errf, double ds) +{ + //the starting polygon implements the trivial constraints 0<f0<NyqvistF, + // 0<B<Bmax. in terms of F and G, 0<F<NF^2, 0<G<FBmax. this is a triangle + // with vertices (0, 0), (NF^2, Bmax*NF^2), (NF^2, 0) + F=new double[12]; + G=&F[5], f=&F[10], p=(int*)&F[11]; + double Fm=Wid*Wid/4; //NyqvistF^2 + F[0]=0, G[0]=0; + F[1]=Fm, G[1]=STIFF_B_MAX*Fm; + F[2]=Fm, G[2]=0; + + N=3; + P=1; + p[0]=ap; + f[0]=af; + s=ds; + + double k=ap*ap-1; + double g1=(af-errf)/ap, g2=(af+errf)/ap; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + //apply constraints F+kG-g2<0 and -F-kG+g1<0 + cutcvpoly(N, F, G, 1, k, -g2); + cutcvpoly(N, F, G, -1, -k, g1); +}//stiffcandid + +/* + method stiffcandid::stiffcandid: creates a harmonic atom by adding a new partial to an existing + harmonic atom. + + In; prev: initial harmonic atom + ap: partial index of new atom + af, errf: centre and half-width of the frequency range of the new atom, in bins. + ds: contribution to candidate score from this atom. +*/ +stiffcandid::stiffcandid(stiffcandid* prev, int ap, double af, double errf, double ds) +{ + N=prev->N, P=prev->P, s=prev->s; + int Np2=N+2, Pp1=P+1; + F=new double[(Np2+Pp1)*2]; + G=&F[Np2]; f=&G[Np2]; p=(int*)&f[Pp1]; + memcpy(F, prev->F, sizeof(double)*N); + memcpy(G, prev->G, sizeof(double)*N); + if (P>0) + { + memcpy(f, prev->f, sizeof(double)*P); + memcpy(p, prev->p, sizeof(int)*P); + } + //see if partial ap is already involved + int i=0; while (i<P && p[i]!=ap) i++; + //if it is not: + if (i>=P) + { + p[P]=ap; + f[P]=af; + s+=ds; + P++; + + double k=ap*ap-1; + double g1=(af-errf)/ap, g2=(af+errf)/ap; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + //apply constraints F+kG-g2<0 and -F-kG+g1<0 + cutcvpoly(N, F, G, 1, k, -g2); + cutcvpoly(N, F, G, -1, -k, g1); + } + //otherwise, the new candid is simply a copy of prev +}//stiffcandid + +//method stiffcandid::~stiffcandid: destructor +stiffcandid::~stiffcandid() +{ + delete[] F; +}//~stiffcandid + + +//--------------------------------------------------------------------------- +//THS methods + +//method THS::THS: default constructor +THS::THS() +{ + memset(this, 0, sizeof(THS)); + memset(BufM, 0, sizeof(void*)*128); +}//THS + +/* + method THS::THS: creates an HS with given number of partials and frames. + + In: aM, aFr: number of partials and frames +*/ +THS::THS(int aM, int aFr) +{ + memset(this, 0, sizeof(THS)); + M=aM; Fr=aFr; Allocate2(atom, M, Fr, Partials); + memset(Partials[0], 0, sizeof(atom)*M*Fr); + memset(BufM, 0, sizeof(void*)*128); +}//THS + +/* + method THS::THS: creates a duplicate HS. This does not duplicate contents of BufM. + + In: HS: the HS to duplicate +*/ +THS::THS(THS* HS) +{ + memcpy(this, HS, sizeof(THS)); + Allocate2(atom, M, Fr, Partials); + for (int m=0; m<M; m++) memcpy(Partials[m], HS->Partials[m], sizeof(atom)*Fr); + Allocate2(double, M, st_count, startamp); + if (st_count) for (int m=0; m<M; m++) memcpy(startamp[m], HS->startamp[m], sizeof(double)*st_count); + memset(BufM, 0, sizeof(void*)*128); +}//THS + +/* + method THS::THS: create a new HS which is a trunction of an existing HS. + + In: HS: the exinting HS from which a sub-HS is to be created + start, end: truncation points. Atoms of *HS beyond these points are discarded. +*/ +THS::THS(THS* HS, double start, double end) +{ + memset(this, 0, sizeof(THS)); + M=HS->M; Fr=HS->Fr; isconstf=HS->isconstf; + int frst=0, fren=Fr; + while (HS->Partials[0][frst].t<start && frst<fren) frst++; + while (HS->Partials[0][fren-1].t>end && fren>frst) fren--; + Fr=fren-frst; + Allocate2(atom, M, Fr, Partials); + for (int m=0; m<M; m++) + { + memcpy(Partials[m], &HS->Partials[m][frst], sizeof(atom)*Fr); + for (int fr=0; fr<Fr; fr++) Partials[m][fr].t-=start; + } + if (frst>0){DeAlloc2(startamp); startamp=0;} +}//THS + +//method THS::~THS: default descructor. +THS::~THS() +{ + DeAlloc2(Partials); + DeAlloc2(startamp); + ClearBufM(); +}//~THS + +/* + method THS::ClearBufM: free buffers pointed to by members of BufM. +*/ +void THS::ClearBufM() +{ + for (int m=0; m<128; m++) delete[] BufM[m]; + memset(BufM, 0, sizeof(void*)*128); +}//ClearBufM + +/* + method THS::EndPos: the end position of an HS. + + Returns the time of the last frame of the first partial. +*/ +int THS::EndPos() +{ + return Partials[0][Fr-1].t; +}//EndPos + +/* + method THS::EndPosEx: the extended end position of an HS. + + Returns the time later than the last frame of the first partial by the interval between the first and + second frames of that partial. +*/ +int THS::EndPosEx() +{ + return Partials[0][Fr-1].t+Partials[0][1].t-Partials[0][0].t; +}//EndPosEx + +/* + method THS::ReadAtomFromStream: reads an atom from a stream. + + Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value. +*/ +int THS::ReadAtomFromStream(TStream* Stream, atom* atm) +{ + int StartPosition=Stream->Position, atomlen; char s[4]; + if (Stream->Read(s, 4)!=4) goto ret0; + if (strcmp(s, "ATM")) goto ret0; + if (Stream->Read(&atomlen, sizeof(int))!=sizeof(int)) goto ret0; + if (Stream->Read(atm, atomlen)!=atomlen) goto ret0; + return atomlen+4+sizeof(int); +ret0: + Stream->Position=StartPosition; + return 0; +}//ReadAtomFromStream + +/* + method THS::ReadFromStream: reads an HS from a stream. + + Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value. +*/ +int THS::ReadFromStream(TStream* Stream) +{ + int StartPosition=Stream->Position, lenevt; char s[4]; + if (Stream->Read(s, 4)!=4) goto ret0; + if (strcmp(s, "EVT")) goto ret0; + if (Stream->Read(&lenevt, sizeof(int))!=sizeof(int)) goto ret0; + if (!ReadHdrFromStream(Stream)) goto ret0; + Resize(M, Fr, false, true); + for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) if (!ReadAtomFromStream(Stream, &Partials[m][fr])) goto ret0; + lenevt=lenevt+4+sizeof(int); + if (lenevt!=Stream->Position-StartPosition) goto ret0; + return lenevt; +ret0: + Stream->Position=StartPosition; + return 0; +}//ReadFromStream + +/* + method THS::ReadHdrFromStream: reads header from a stream into this HS. + + Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value. +*/ +int THS::ReadHdrFromStream(TStream* Stream) +{ + int StartPosition=Stream->Position, hdrlen, tags; char s[4]; + if (Stream->Read(s, 4)!=4) goto ret0; + if (strcmp(s, "HDR")) goto ret0; + if (Stream->Read(&hdrlen, sizeof(int))!=sizeof(int)) goto ret0; + if (Stream->Read(&Channel, sizeof(int))!=sizeof(int)) goto ret0; + if (Stream->Read(&M, sizeof(int))!=sizeof(int)) goto ret0; + if (Stream->Read(&Fr, sizeof(int))!=sizeof(int)) goto ret0; + if (Stream->Read(&tags, sizeof(int))!=sizeof(int)) goto ret0; + isconstf=tags&HS_CONSTF; + hdrlen=hdrlen+4+sizeof(int); //expected read count + if (hdrlen!=Stream->Position-StartPosition) goto ret0; + return hdrlen; +ret0: + Stream->Position=StartPosition; + return 0; +}//ReadHdrFromStream + +/* + method THS::Resize: change the number of partials or frames of an HS structure. Unless forcealloc is + set to true, this allocates new buffers to host HS data only when the new dimensions are larger than + current ones. + + In: aM, aFr: new number of partials and frames + copydata: specifies if HS data is to be copied to new buffers. + forcealloc: specifies if new buffers are to be allocated in all cases. +*/ +void THS::Resize(int aM, int aFr, bool copydata, bool forcealloc) +{ + if (forcealloc || M<aM || Fr<aFr) + { + atom** Allocate2(atom, aM, aFr, NewPartials); + if (copydata) + { + int minM=(M<aM)?M:aM, minFr=(Fr<aFr)?Fr:aFr; + for (int m=0; m<minM; m++) memcpy(NewPartials[m], Partials[m], sizeof(atom)*minFr); + } + DeAlloc2(Partials); + Partials=NewPartials; + } + if (forcealloc || M<aM) + { + double** Allocate2(double, aM, st_count, newstartamp); + if (copydata) + { + for (int m=0; m<M; m++) memcpy(newstartamp[m], startamp[m], sizeof(double)*st_count); + } + DeAlloc2(startamp); + startamp=newstartamp; + } + M=aM, Fr=aFr; +}//Resize + +/* + method THS::StartPos: the start position of an HS. + + Returns the time of the first frame of the first partial. +*/ +int THS::StartPos() +{ + return Partials[0][0].t; +}//StartPos + +/* + method THS::StdOffst: standard hop size of an HS. + Returns the interval between the timing of the first and second frames of the first partial, which is + the "standard" hop size if all measurement points are uniformly positioned. +*/ +int THS::StdOffst() +{ + return Partials[0][1].t-Partials[0][0].t; +}//StdOffst + +/* + method THS::WriteAtomToStream: writes an atom to a stream. An atom is stored in a stream as an RIFF + chunk identified by "ATM\0". + + Returns the number of bytes written. The stream pointer is shifted by this value. +*/ +int THS::WriteAtomToStream(TStream* Stream, atom* atm) +{ + Stream->Write((void*)"ATM", 4); + int atomlen=sizeof(atom); + Stream->Write(&atomlen, sizeof(int)); + atomlen=Stream->Write(atm, atomlen); + return atomlen+4+sizeof(int); +}//WriteAtomToStream + +/* + method THS::WriteHdrToStream: writes header to a stream. HS header is stored in a stream as an RIFF + chunk, identified by "HDR\0". + + Returns the number of bytes written. The stream pointer is shifted by this value. +*/ +int THS::WriteHdrToStream(TStream* Stream) +{ + Stream->Write((void*)"HDR", 4); + int hdrlen=0, tags=0; + if (isconstf) tags=tags|HS_CONSTF; + Stream->Write(&hdrlen, sizeof(int)); + Stream->Write(&Channel, sizeof(int)); hdrlen+=sizeof(int); + Stream->Write(&M, sizeof(int)); hdrlen+=sizeof(int); + Stream->Write(&Fr, sizeof(int)); hdrlen+=sizeof(int); + Stream->Write(&tags, sizeof(int)); hdrlen+=sizeof(int); + Stream->Seek(-hdrlen-sizeof(int), soFromCurrent); Stream->Write(&hdrlen, sizeof(int)); + Stream->Seek(hdrlen, soFromCurrent); + return hdrlen+4+sizeof(int); +}//WriteHdrToStream + +/* + method THS::WriteToStream: write an HS to a stream. An HS is stored in a stream as an RIFF chunk + identified by "EVT\0". Its chunk data contains an HS header chunk followed by M*Fr atom chunks, in + rising order of m then fr. + + Returns the number of bytes written. The stream pointer is shifted by this value. +*/ +int THS::WriteToStream(TStream* Stream) +{ + Stream->Write((void*)"EVT", 4); + int lenevt=0; + Stream->Write(&lenevt, sizeof(int)); + lenevt+=WriteHdrToStream(Stream); + + for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) lenevt+=WriteAtomToStream(Stream, &Partials[m][fr]); + Stream->Seek(-lenevt-sizeof(int), soFromCurrent); + Stream->Write(&lenevt, sizeof(int)); + Stream->Seek(lenevt, soFromCurrent); + return lenevt+4+sizeof(int); +}//WriteToStream + + +//--------------------------------------------------------------------------- +//TPolygon methods + +/* + method TPolygon::TPolygon: create an empty polygon with a storage capacity + + In: cap: number of vertices to allocate memory for. +*/ +TPolygon::TPolygon(int cap) +{ + X=(double*)malloc8(sizeof(double)*cap*2); + Y=&X[cap]; +}//TPolygon + +/* + method TPolygon::TPolygon: create a duplicate polygon. + + In: cap: number of vertices to allocate memory for. + R: the polygon to duplicate. + + If cap is smaller than the number of vertices of R, the latter is used for memory allocation. +*/ +TPolygon::TPolygon(int cap, TPolygon* R) +{ + if (cap<R->N) cap=R->N; X=(double*)malloc8(sizeof(double)*cap*2); Y=&X[cap]; N=R->N; + memcpy(X, R->X, sizeof(double)*R->N); memcpy(Y, R->Y, sizeof(double)*R->N); +}//TPolygon + +//method TPolygon::~TPolygon: default destructor. +TPolygon::~TPolygon() +{ + free8(X); +}//~TPolygon + +//--------------------------------------------------------------------------- +//TTempAtom methods + +/* + method TTempAtom::TTempAtom: initializes an empty atom with a fundamental frequency range. + + In: af, ef: centre and half width of the fundamental frequency + maxB: upper bound of stiffness coefficient +*/ +TTempAtom::TTempAtom(double af, double ef, double maxB) +{ + Prev=NULL; pind=f=a=s=acce=0; + R=new TPolygon(4); + InitializeR(R, af, ef, maxB); +}//TTempAtom + +/* + method TTempAtom::TTempAtom: initialize an empty atom with a frequency range for a certain partial. + + In: apin: partial index + af, ef: centre and half width of the fundamental frequency + maxB: upper bound of stiffness coefficient +*/ +TTempAtom::TTempAtom(int apin, double af, double ef, double maxB) +{ + Prev=NULL; pind=f=a=s=acce=0; + R=new TPolygon(4); + InitializeR(R, apin, af, ef, maxB); +}//TTempAtom + +/* + method TTempAtom::TTempAtom: initialize an empty atom whose F-G polygon is the extension of AR + +In: AR: the F-G polygon to extend + delf1, delf2: amount of extension, in semitones, of fundamental frequency + minf: minimal fundamental frequency: extension will not go below this value +*/ +TTempAtom::TTempAtom(TPolygon* AR, double delf1, double delf2, double minf) +{ + Prev=NULL; pind=f=a=s=acce=0; + R=new TPolygon(AR->N+2); + R->N=AR->N; + memcpy(R->X, AR->X, sizeof(double)*AR->N); memcpy(R->Y, AR->Y, sizeof(double)*AR->N); + ExtendR(R, delf1, delf2, minf); +}//TTempAtom + +/* + method TTempAtom::TTempAtom: initialize a atom as the only partial. + + In: apind: partial index + af, ef: partial frequency and its error range + aa, as: partial amplitude and measurement scale + maxB: stiffness coefficient upper bound +*/ +TTempAtom::TTempAtom(int apind, double af, double ef, double aa, double as, double maxB) +{ + Prev=NULL; pind=apind; f=af; a=aa; s=as; acce=aa*aa; + R=new TPolygon(4); + InitializeR(R, apind, af, ef, maxB); +}//TTempAtom + +/* + method TTempAtom::TTempAtom: creates a duplicate atom. R can be duplicated or snatched according to + DupR. + + In: APrev: the atom to duplicate + DupR: specifies if the newly created atom is to have its F-G polygon duplicated from that of APrev + or to snatch the F-G polygon from APrev (in the latter case APrev losts its polygon). +*/ +TTempAtom::TTempAtom(TTempAtom* APrev, bool DupR) +{ + Prev=APrev; pind=APrev->pind; f=APrev->f; a=APrev->a; s=APrev->s; acce=APrev->acce; + TPolygon* PrevR=APrev->R; + if (DupR) + {//duplicate R + R=new TPolygon(PrevR->N); + R->N=PrevR->N; memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N); memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N); + } + else + {//snatch R from APrev + R=APrev->R; + APrev->R=0; + } +}//TTempAtom + +/* + method TTempAtom::TTempAtom:: initializes an atom by adding a new partial to an existing group. + + In: APrev: the existing atom. + apind: partial index + af, ef: partial frequency and its error range + aa, as: partial amplitude and measurement scale + updateR: specifies if the F-G polygon is updated using (af, ef). +*/ +TTempAtom::TTempAtom(TTempAtom* APrev, int apind, double af, double ef, double aa, double as, bool updateR) +{ + Prev=APrev; pind=apind; f=af; a=aa; + s=as; acce=APrev->acce+aa*aa; + TPolygon* PrevR=APrev->R; + R=new TPolygon(PrevR->N+2); // create R + R->N=PrevR->N; // + memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N); // copy R + memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N); // + if (updateR) CutR(R, apind, af, ef); +}//TTempAtom + +//method TTempAtom::~TTempAtom: default destructor +TTempAtom::~TTempAtom() +{ + delete R; +}//~TTempAtom + + +//--------------------------------------------------------------------------- +//functions + +/* + function areaandcentroid: calculates the area and centroid of a convex polygon. + + In: x[N], y[N]: x- and y-coordinates of vertices of a polygon + Out: A: area + (cx, cy): coordinate of the centroid + + No return value. +*/ +void areaandcentroid(double& A, double& cx, double& cy, int N, double* x, double* y) +{ + if (N==0) {A=0;} + else if (N==1) {A=0; cx=x[0], cy=y[0];} + else if (N==2) {A=0; cx=(x[0]+x[1])/2, cy=(y[0]+y[1])/2;} + A=cx=cy=0; + for (int i=0; i<N; i++) + { + double xi1, yi1; //x[i+1], y[i+1], index modular N + if (i+1==N) xi1=x[0], yi1=y[0]; + else xi1=x[i+1], yi1=y[i+1]; + double tmp=x[i]*yi1-xi1*y[i]; + A+=tmp; + cx+=(x[i]+xi1)*tmp; + cy+=(y[i]+yi1)*tmp; + } + A/=2; + cx=cx/6/A; + cy=cy/6/A; +}//areaandcentroid + +/* + function AtomsToPartials: sort a list of atoms as a number of partials. It is assumed that the atoms + are simply those from a harmonic sinusoid in arbitrary order with uniform timing and partial index + clearly marked, so that it is easy to sort them into a list of partials. However, the sorting process + does not check for harmonicity, missing or duplicate atoms, uniformity of timing, etc. + + In: part[k]: a list of atoms + offst: interval between adjacent measurement points (hop size) + Out: M, Fr, partials[M][Fr]: a list of partials containing the atoms. + + No return value. partials[][] is allocated anew and must be freed by caller. +*/ +void AtomsToPartials(int k, atom* part, int& M, int& Fr, atom**& partials, int offst) +{ + if (k>0) + { + int t1, t2, i=0; + while (i<k && part[i].f<=0) i++; + if (i<k) + { + t1=part[i].t, t2=part[i].t, M=part[i].pin; i++; + while (i<k) + { + if (part[i].f>0) + { + if (M<part[i].pin) M=part[i].pin; + if (t1>part[i].t) t1=part[i].t; + if (t2<part[i].t) t2=part[i].t; + } + i++; + } + Fr=(t2-t1)/offst+1; + Allocate2(atom, M, Fr, partials); memset(partials[0], 0, sizeof(atom)*M*Fr); + for (int i=0; i<k; i++) + if (part[i].f>0) + { + int fr=(part[i].t-t1)/offst; + int pp=part[i].pin; + partials[pp-1][fr]=part[i]; + } + } + else M=0, Fr=0; + } + else M=0, Fr=0; +}//AtomsToPartials + +/* + function conta: a continuity measure between two set of amplitudes + + In: a1[N], a2[N]: the amplitudes + + Return the continuity measure, between 0 and 1 +*/ +double conta(int N, double* a1, double* a2) +{ + double e11=0, e22=0, e12=0; + for (int n=0; n<N; n++) + { + double la1=(a1[n]>0)?a1[n]:0, la2=(a2[n]>0)?a2[n]:0; + if (la1>1e10) la1=la2; + e11+=la1*la1; + e12+=la1*la2; + e22+=la2*la2; + } + return 2*e12/(e11+e22); +}//conta + +/* + function cutcvpoly: severs a polygon by a straight line + + In: x[N], y[N]: x- and y-coordinates of vertices of a polygon, starting from the leftmost (x[0]= + min x[n]) point clockwise; in case of two leftmost points, x[0] is the upper one and x[N-1] is + the lower one. + A, B, C: coefficients of a straight line Ax+By+C=0. + protect: specifies what to do if the whole polygon satisfy Ax+By+C>0, true=do noting, + false=eliminate. + Out: N, x[N], y[N]: the polygon severed by the line, retaining that half for which Ax+By+C<=0. + + No return value. +*/ +void cutcvpoly(int& N, double* x, double* y, double A, double B, double C, bool protect) +{ + int n=0, ns; + while (n<N && A*x[n]+B*y[n]+C<=0) n++; + if (n==N) //all points satisfies the constraint, do nothing + {} + else if (n>0) //points 0, 1, ..., n-1 satisfies the constraint, point n does not + { + ns=n; + n++; + while (n<N && A*x[n]+B*y[n]+C>0) n++; + //points 0, ..., ns-1 and n, ..., N-1 satisfy the constraint, + //points ns, ..., n-1 do not satisfy the constraint, + // ns>0, n>ns, however, it's possible that n-1=ns + int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe + double xs, ys, xe, ye; + + //find intersection of x:y[ns-1:ns] and A:B:C + double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns]; + double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0 + if (z1==0) pts=0; //as point ns-1 IS point s + else + { + pts=1, xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1); + } + + //find intersection of x:y[n-1:n] and A:B:C + x1=x[n-1], y1=y[n-1]; + if (n==N) x2=x[0], y2=y[0]; + else x2=x[n], y2=y[n]; + z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0 + if (z2==0) pte=0; //as point n is point e + else + { + pte=1, xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1); + } + + //the new polygon is formed by points 0, 1, ..., ns-1, (s), (e), n, ..., N-1 + memmove(&x[ns+pts+pte], &x[n], sizeof(double)*(N-n)); + memmove(&y[ns+pts+pte], &y[n], sizeof(double)*(N-n)); + if (pts) x[ns]=xs, y[ns]=ys; + if (pte) x[ns+pts]=xe, y[ns+pts]=ye; + N=ns+pts+pte+N-n; + } + else //n=0, point 0 does not satisfy the constraint + { + n++; + while (n<N && A*x[n]+B*y[n]+C>0) n++; + if (n==N) //no point satisfies the constraint + { + if (!protect) N=0; + } + else + { + //points 0, 1, ..., n-1 do not satisfy the constraint, point n does + ns=n; + n++; + while (n<N && A*x[n]+B*y[n]+C<=0) n++; + //points 0, ..., ns-1 and n, ..., N-1 do not satisfy constraint, + //points ns, ..., n-1 satisfy the constraint + // ns>0, n>ns, however ns can be equal to n-1 + int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe + double xs, ys, xe, ye; + + //find intersection of x:y[ns-1:ns] and A:B:C + double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns]; + double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0 + if (z2==0) pts=0; //as point ns IS point s + else pts=1; + xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1); + + //find intersection of x:y[n-1:n] and A:B:C + x1=x[n-1], y1=y[n-1]; + if (n==N) x2=x[0], y2=y[0]; + else x2=x[n], y2=y[n]; + z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0 + if (z1==0) pte=0; //as point n-1 is point e + else pte=1; + xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1); + + //the new polygon is formed of points (s), ns, ..., n-1, (e) + if (xs<=xe) + { + //the point sequence comes as (s), ns, ..., n-1, (e) + memmove(&x[pts], &x[ns], sizeof(double)*(n-ns)); + memmove(&y[pts], &y[ns], sizeof(double)*(n-ns)); + if (pts) x[0]=xs, y[0]=ys; + N=n-ns+pts+pte; + if (pte) x[N-1]=xe, y[N-1]=ye; + } + else //xe<xs + { + //the sequence comes as e, (s), ns, ..., n-2 if pte=0 (notice point e<->n-1) + //or e, (s), ns, ..., n-1 if pte=1 + if (pte==0) + { + memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns-1)); + memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns-1)); + if (pts) x[1]=xs, y[1]=ys; + } + else + { + memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns)); + memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns)); + if (pts) x[1]=xs, y[1]=ys; + } + x[0]=xe, y[0]=ye; + N=n-ns+pts+pte; + } + } + } +}//cutcvpoly + +/* + funciton CutR: update an F-G polygon with an new partial + + In: R: F-G polygon + apind: partial index + af, ef: partial frequency and error bound + protect: specifies what to do if the new partial has no intersection with R, true=do noting, + false=eliminate R + Out: R: the updated F-G polygon + + No return value. +*/ +void CutR(TPolygon* R, int apind, double af, double ef, bool protect) +{ + double k=apind*apind-1; + double g1=(af-ef)/apind, g2=(af+ef)/apind; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + //apply constraints F+kG-g2<0 and -F-kG+g1<0 + cutcvpoly(R->N, R->X, R->Y, 1, k, -g2, protect); // update R + cutcvpoly(R->N, R->X, R->Y, -1, -k, g1, protect); // +}//CutR + +/* + function DeleteByHalf: reduces a list of stiffcandid objects by half, retaining those with higher s + and delete the others, freeing their memory. + + In: cands[pcs]: the list of stiffcandid objects + Out: cands[return value]: the list of retained ones + + Returns the size of the new list. +*/ +int DeleteByHalf(stiffcandid** cands, int pcs) +{ + int newpcs=pcs/2; + double* tmp=new double[newpcs]; + memset(tmp, 0, sizeof(double)*newpcs); + for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs); + int k=0; + for (int j=0; j<pcs; j++) + { + if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j]; + else delete cands[j]; + } + return k; +}//DeleteByHalf + +/* + function DeleteByHalf: reduces a list of TTempAtom objects by half, retaining those with higher s and + delete the others, freeing their memory. + + In: cands[pcs]: the list of TTempAtom objects + Out: cands[return value]: the list of retained ones + + Returns the size of the new list. +*/ +int DeleteByHalf(TTempAtom** cands, int pcs) +{ + int newpcs=pcs/2; + double* tmp=new double[newpcs]; + memset(tmp, 0, sizeof(double)*newpcs); + for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs); + int k=0; + for (int j=0; j<pcs; j++) + { + if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j]; + else delete cands[j]; + } + delete[] tmp; + return k; +}//DeleteByHalf + +/* + function ds0: atom score 0 - energy + + In: a: atom amplitude + + Returns a^2, or s[0]+a^2 is s is not NULL, as atom score. +*/ +double ds0(double a, void* s) +{ + if (s) return *(double*)s+a*a; + else return a*a; +}//ds0 + +/* + function ds2: atom score 1 - cross-correlation coefficient with another HA, e.g. from previous frame + + In: a: atom amplitude + params: pointer to dsprams1 structure supplying other inputs. + Out: cross-correlation coefficient as atom score. + + Returns the cross-correlation coefficient. +*/ +double ds1(double a, void* params) +{ + dsparams1* lparams=(dsparams1*)params; + double hs=lparams->lastene+lparams->currentacce, hsaa=hs+a*a; + if (lparams->p<=lparams->lastP) return (lparams->s*hs+a*lparams->lastvfp[lparams->p-1])/hsaa; + else return (lparams->s*hs+a*a)/hsaa; +}//ds1 + +/* + function ExBStiff: finds the minimal and maximal values of stiffness coefficient B given a F-G polygon + + In: F[N], G[N]: vertices of a F-G polygon + Out: Bmin, Bmax: minimal and maximal values of the stiffness coefficient + + No reutrn value. +*/ +void ExBStiff(double& Bmin, double& Bmax, int N, double* F, double* G) +{ + Bmax=G[0]/F[0]; + Bmin=Bmax; + for (int i=1; i<N; i++) + { + double vi=G[i]/F[i]; + if (Bmin>vi) Bmin=vi; + else if (Bmax<vi) Bmax=vi; + } +}//ExBStiff + +/* + function ExFmStiff: finds the minimal and maximal frequecies of partial m given a F-G polygon + + In: F[N], G[N]: vertices of a F-G polygon + m: partial index, fundamental=1 + Out: Fmin, Fmax: minimal and maximal frequencies of partial m + + No return value. +*/ +void ExFmStiff(double& Fmin, double& Fmax, int m, int N, double* F, double* G) +{ + int k=m*m-1; + double vmax=F[0]+k*G[0]; + double vmin=vmax; + for (int i=1; i<N; i++) + { + double vi=F[i]+k*G[i]; + if (vmin>vi) vmin=vi; + else if (vmax<vi) vmax=vi; + } + testnn(vmin); Fmin=m*sqrt(vmin); + testnn(vmax); Fmax=m*sqrt(vmax); +}//ExFmStiff + +/* + function ExtendR: extends a F-G polygon on the f axis (to allow a larger pitch range) + + In: R: the F-G polygon + delp1: amount of extension to the low end, in semitones + delpe: amount of extension to the high end, in semitones + minf: minimal fundamental frequency + Out: R: the extended F-G polygon + + No return value. +*/ +void ExtendR(TPolygon* R, double delp1, double delp2, double minf) +{ + double mdelf1=pow(2, -delp1/12), mdelf2=pow(2, delp2/12); + int N=R->N; + double *G=R->Y, *F=R->X, slope, prevslope, f, ftmp; + if (N==0) {} + else if (N==1) + { + R->N=2; + slope=G[0]/F[0]; + testnn(F[0]); f=sqrt(F[0]); + ftmp=f*mdelf2; + F[1]=ftmp*ftmp; + ftmp=f*mdelf1; + if (ftmp<minf) ftmp=minf; + F[0]=ftmp*ftmp; + G[0]=F[0]*slope; + G[1]=F[1]*slope; + } + else if (N==2) + { + prevslope=G[0]/F[0]; + slope=G[1]/F[1]; + if (fabs((slope-prevslope)/prevslope)<1e-10) + { + testnn(F[0]); ftmp=sqrt(F[0])*mdelf1; if (ftmp<0) ftmp=0; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope; + testnn(F[1]); ftmp=sqrt(F[1])*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*slope; + } + else + { + if (prevslope>slope) + { + R->N=4; + testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[3]=ftmp*ftmp; G[3]=F[3]*slope; + ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope; + testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope; + ftmp=f*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*prevslope; + if (F[0]==0 && F[3]==0) R->N=3; + } + else + { + R->N=4; + testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[1]=ftmp*ftmp; G[1]=F[1]*slope; + ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope; + testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope; + ftmp=f*mdelf2; F[4]=ftmp*ftmp; G[4]=F[4]*prevslope; + if (F[0]==0 && F[1]==0) {R->N=3; F[1]=F[2]; F[2]=F[3]; G[1]=G[2]; G[3]=G[3];} + } + } + } + else + { + //find the minimal and maximal angles of R + //maximal slope is taken at Ms if Mt=1, or Ms-1 and Ms if Mt=0 + //minimal slope is taken at ms if mt=1, or ms-1 and ms if mt=0 + int Ms, ms, Mt=-1, mt=-1; + double prevslope=G[0]/F[0], dslope; + for (int n=1; n<N; n++) + { + double slope=G[n]/F[n]; + dslope=atan(slope)-atan(prevslope); + if (Mt==-1) + { + if (dslope<-1e-10) Ms=n-1, Mt=1; + else if (dslope<1e-10) Ms=n, Mt=0; + } + else if (mt==-1) + { + if (dslope>1e-10) ms=n-1, mt=1; + else if (dslope>-1e-10) ms=n, mt=0; + } + prevslope=slope; + } + if (mt==-1) + { + slope=G[0]/F[0]; + dslope=atan(slope)-atan(prevslope); + if (dslope>1e-10) ms=N-1, mt=1; + else if (dslope>-1e-10) ms=0, mt=0; + } + R->N=N+mt+Mt; + int newn=R->N-1; + if (ms==0 && mt==1) + { + slope=G[0]/F[0]; + testnn(F[0]); ftmp=sqrt(F[0])*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + mt=-1; + } + else if (ms==0) + mt=-1; + for (int n=N-1; n>=0; n--) + { + if (mt!=-1) + { + slope=G[n]/F[n]; + testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + if (ms==n && mt==1) + { + ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + mt=-1; + } + else if (ms==n) //mt=0 + mt=-1; + } + else if (Mt!=-1) + { + slope=G[n]/F[n]; + testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + if (Ms==n && Mt==1) + { + ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + Mt=-1; + } + else if (Ms==n) + Mt=-1; + } + else + { + slope=G[n]/F[n]; + testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--; + } + } + //merge multiple vertices at the origin + N=R->N; + while (N>0 && F[N-1]==0) N--; + R->N=N; + int n=1; + while (n<N && F[n]==0) n++; + if (n!=1) + { + R->N=N-(n-1); + memcpy(&F[1], &F[n], sizeof(double)*(N-n)); + memcpy(&G[1], &G[n], sizeof(double)*(N-n)); + } + } +}//ExtendR + +/* + function FGFrom2: solves the following equation system given m[2], f[2], norm[2]. This is interpreted + as searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied, where + k[*]=m[*]^2-1. + + m[0](F+k[0]G)^0.5-f[0] m[1](F+k[1]G)^0.5-f[1] + ------------------------ = ------------------------ >0 + norm[0] norm[1] + + F=F0+lmd*dF + G=G0+lmd*dG + + In: (F0, G0): search origin + (dF, dG): search direction + m[2], f[2], norm[2]: coefficients in the first equation + Out: F[return value], G[return value]: solutions. + + Returns the number of solutions for (F, G). +*/ +int FGFrom2(double* F, double* G, double F0, double dF, double G0, double dG, int* m, double* f, double* norm) +{ + double m1=m[0]/norm[0], m2=m[1]/norm[1], + f1=f[0]/norm[0], f2=f[1]/norm[1], + k1=m[0]*m[0]-1, k2=m[1]*m[1]-1; + double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG), + B=2*m1*(f2-f1), + C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2; + if (A==0) + { + double x=-C/B; + if (x<0) return 0; + else if (m1*x-f1<0) return 0; + else + { + double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd*dF; + G[0]=G0+lmd*dG; + return 1; + } + } + else + { + double delta=B*B-4*A*C; + if (delta<0) return 0; + else if (delta==0) + { + double x=-B/2/A; + if (x<0) return 0; + else if (m1*x-f1<0) return 0; + else + { + double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd*dF; + G[0]=G0+lmd*dG; + return 1; + } + } + else + { + int roots=0; + double x=(-B+sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else + { + double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd*dF; + G[0]=G0+lmd*dG; + roots++; + } + x=(-B-sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else + { + double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[roots]=F0+lmd*dF; + G[roots]=G0+lmd*dG; + roots++; + } + return roots; + } + } +}//FGFrom2 + +/* + function FGFrom2: solves the following equation system given m[2], f[2], k[2]. This is interpreted as + searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied. Functionally + this is the same as the version using norm[2], with m[] anf f[] normalized by norm[] beforehand so + that norm[] is no longer needed. However, k[2] cannot be computed from normalized m[2] so that it must + be specified explicitly. + + m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] > 0 + + F=F0+lmd*dF + G=G0+lmd*dG + + In: (F0, G0): search origin + (dF, dG): search direction + m[2], f[2], k[2]: coefficients in the first equation + Out: F[return value], G[return value]: solutions. + + Returns the number of solutions for (F, G). +*/ +int FGFrom2(double* F, double* G, double* lmd, double F0, double dF, double G0, double dG, double* m, double* f, int* k) +{ + double m1=m[0], m2=m[1], + f1=f[0], f2=f[1], + k1=k[0], k2=k[1]; + double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG), + B=2*m1*(f2-f1), + C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2; + //x is obtained by solving Axx+Bx+C=0 + if (fabs(A)<1e-6) //A==0 + { + double x=-C/B; + if (x<0) return 0; + else if (m1*x-f1<0) return 0; + else + { + lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd[0]*dF; + G[0]=G0+lmd[0]*dG; + return 1; + } + } + else + { + int roots=0; + double delta=B*B-4*A*C; + if (delta<0) return 0; + else if (delta==0) + { + double x=-B/2/A; + if (x<0) return 0; + else if (m1*x-f1<0) return 0; + else if ((m1*x-f1+f2)*m2<0) {} + else + { + lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd[0]*dF; + G[0]=G0+lmd[0]*dG; + roots++; + } + return roots; + } + else + { + int roots=0; + double x=(-B+sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else if ((m1*x-f1+f2)*m2<0) {} + else + { + lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[0]=F0+lmd[0]*dF; + G[0]=G0+lmd[0]*dG; + roots++; + } + x=(-B-sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else if ((m1*x-f1+f2)*m2<0) {} + else + { + lmd[roots]=(x*x-(F0+k1*G0))/(dF+k1*dG); + F[roots]=F0+lmd[roots]*dF; + G[roots]=G0+lmd[roots]*dG; + roots++; + } + return roots; + } + } +}//FGFrom2 + +/* + function FGFrom3: solves the following equation system given m[3], f[3], norm[3]. + + m[0](F+k[0]G)^0.5-f[0] m[1](F+k[1]G)^0.5-f[1] m[2](F+k[2]G)^0.5-f[2] + ------------------------ = ------------------------ = ------------------------ > 0 + norm[0] norm[1] norm[2] + + In: m[3], f[3], norm[3]: coefficients in the above + Out: F[return value], G[return value]: solutions. + + Returns the number of solutions for (F, G). +*/ +int FGFrom3(double* F, double* G, int* m, double* f, double* norm) +{ + double m1=m[0]/norm[0], m2=m[1]/norm[1], m3=m[2]/norm[2], + f1=f[0]/norm[0], f2=f[1]/norm[1], f3=f[2]/norm[2], + k1=m[0]*m[0]-1, k2=m[1]*m[1]-1, k3=m[2]*m[2]-1; + double h21=k2-k1, h31=k3-k1; //h21 and h31 + double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13' + double a=m2*m2*h13*h21-m3*m3*h12*h31; + if (a==0) + { + double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1), + b=h13*(f2-f1)-h12*(f3-f1); + x=x/(2*m1*b); + if (x<0) return 0; //x must the square root of something + else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0 + else + { + if (h21!=0) + { + G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21); + } + else + { + G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31); + } + F[0]=x*x-k1*G[0]; + return 1; + } + } + else + { + double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a; + a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a; + double A=h12, + B=2*m1*(f2-f1)-m2*m2*h21*a, + C=(f2-f1)*(f2-f1)-m2*m2*h21*b; + double delta=B*B-4*A*C; + if (delta<0) return 0; + else if (delta==0) + { + double x=-B/2/A; + if (x<0) return 0; //x must the square root of something + else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0 + else + { + G[0]=a*x+b; + F[0]=x*x-k1*G[0]; + return 1; + } + } + else + { + int roots=0; + double x=(-B+sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else + { + G[0]=a*x+b; + F[0]=x*x-k1*G[0]; + roots++; + } + x=(-B-sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else + { + G[roots]=a*x+b; + F[roots]=x*x-k1*G[roots]; + roots++; + } + return roots; + } + } +}//FGFrom3 + +/* + function FGFrom3: solves the following equation system given m[3], f[3], k[3]. Functionally this is + the same as the version using norm[3], with m[] anf f[] normalized by norm[] beforehand so that norm[] + is no longer needed. However, k[3] cannot be computed from normalized m[3] so that it must be + specified explicitly. + + m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] = m[2](F+k[2]G)^0.5-f[2] >0 + + In: m[3], f[3], k[3]: coefficients in the above + Out: F[return value], G[return value]: solutions. + + Returns the number of solutions for (F, G). +*/ +int FGFrom3(double* F, double* G, double* e, double* m, double* f, int* k) +{ + double m1=m[0], m2=m[1], m3=m[2], + f1=f[0], f2=f[1], f3=f[2], + k1=k[0], k2=k[1], k3=k[2]; + double h21=k2-k1, h31=k3-k1; //h21 and h31 + double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13' + double a=m2*m2*h13*h21-m3*m3*h12*h31; + if (a==0) + { + //a==0 implies two the e's are conjugates + double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1), + b=h13*(f2-f1)-h12*(f3-f1); + x=x/(2*m1*b); + if (x<0) return 0; //x must the square root of something + else if (m1*x-f1<-1e-6) return 0; //discarded because we are solving e1=e2=e3>0 + else if ((m1*x-f1+f2)*m2<0) return 0; + else if ((m1*x-f1+f3)*m3<0) return 0; + else + { + if (h21!=0) + { + G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21); + } + else + { + G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31); + } + F[0]=x*x-k1*G[0]; + double tmp=F[0]+G[0]*k[0]; testnn(tmp); + e[0]=m1*sqrt(tmp)-f[0]; + return 1; + } + } + else + { + double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a; + a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a; + double A=h12, + B=2*m1*(f2-f1)-m2*m2*h21*a, + C=(f2-f1)*(f2-f1)-m2*m2*h21*b; + double delta=B*B-4*A*C; + if (delta<0) return 0; + else if (delta==0) + { + int roots=0; + double x=-B/2/A; + if (x<0) return 0; //x must the square root of something + else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0 + else if ((m1*x-f1+f2)*m2<0) return 0; + else if ((m1*x-f1+f3)*m3<0) return 0; + else + { + G[0]=a*x+b; + F[0]=x*x-k1*G[0]; + double tmp=F[0]+G[0]*k[0]; testnn(tmp); + e[0]=m1*sqrt(tmp)-f[0]; + roots++; + } + return roots; + } + else + { + int roots=0; + double x=(-B+sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else if ((m1*x-f1+f2)*m2<0) {} + else if ((m1*x-f1+f3)*m3<0) {} + else + { + G[0]=a*x+b; + F[0]=x*x-k1*G[0]; + double tmp=F[0]+G[0]*k1; testnn(tmp); + e[0]=m1*sqrt(tmp)-f1; + roots++; + } + x=(-B-sqrt(delta))/2/A; + if (x<0) {} + else if (m1*x-f1<0) {} + else if ((m1*x-f1+f2)*m2<0) {} + else if ((m1*x-f1+f3)*m3<0) {} + else + { + G[roots]=a*x+b; + F[roots]=x*x-k1*G[roots]; + double tmp=F[roots]+G[roots]*k1; testnn(tmp); + e[roots]=m1*sqrt(tmp)-f1; + roots++; + } + return roots; + } + } +}//FGFrom3 + +/* + function FindNote: harmonic sinusoid tracking from a starting point in time-frequency plane forward + and backward. + + In: _t, _f: start time and frequency + frst, fren: tracking range, in frames + Spec: spectrogram + wid, offst: atom scale and hop size, must be consistent with Spec + settings: note match settings + brake: tracking termination threshold + Out: M, Fr, partials[M][Fr]: HS partials + + Returns 0 if tracking is done by FindNoteF(), 1 if tracking is done by FindNoteConst() +*/ +int FindNote(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings) +{ + double brake=0.02; + if (settings.delp==0) return FindNoteConst(_t, _f, M, Fr, partials, frst, fren, wid, offst, Spec, settings, brake*5); + + atom* part=new atom[(fren-frst)*settings.maxp]; + double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype}; + + int trst=floor((_t-wid/2)*1.0/offst+0.5); + if (trst<0) trst=0; + + TPolygon* R=new TPolygon(1024); R->N=0; + + cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst); + double f0=_f*wid; + cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + + settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true; + double B, starts=NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0, ds0); + + int k=0; + if (starts>0) + { + int startp=NMResultToAtoms(settings.maxp, part, trst*offst+wid/2, wid, results); + settings.pin0asanchor=false; + double* startvfp=new double[startp]; memcpy(startvfp, vfpp, sizeof(double)*startp); + k=startp; + k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, fren, wid, offst, Spec, settings, brake); + k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, frst-1, wid, offst, Spec, settings, brake); + delete[] startvfp; + } + delete R; delete[] x; + + AtomsToPartials(k, part, M, Fr, partials, offst); + delete[] part; + + // ReEst1(M, frst, fren, partials, WV->Data16[channel], wid, offst); + + return 0; +}//Findnote*/ + +/* + function FindNoteConst: constant-pitch harmonic sinusoid tracking + + In: _t, _f: start time and frequency + frst, fren: tracking range, in frames + Spec: spectrogram + wid, offst: atom scale and hop size, must be consistent with Spec + settings: note match settings + brake: tracking termination threshold + Out: M, Fr, partials[M][Fr]: HS partials + + Returns 1. +*/ +int FindNoteConst(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake) +{ + atom* part=new atom[(fren-frst)*settings.maxp]; + double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype}; + + //trst: the frame index to start tracking + int trst=floor((_t-wid/2)*1.0/offst+0.5), maxp=settings.maxp; + if (trst<0) trst=0; + + //find a note candidate for the starting frame at _t + TPolygon* R=new TPolygon(1024); R->N=0; + cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst); + double f0=_f*wid; + cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + + settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true; + double B, *starts=new double[maxp]; + NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0); + + //read the energy vector of this candidate HA to starts[]. starts[] will contain the highest single-frame + //energy of each partial during the tracking. + int P=0; while(P<maxp && f0*(P+1)*sqrt(1+B*((P+1)*(P+1)-1))<wid/2) P++; + for (int m=0; m<P; m++) starts[m]=results.vfp[m]*results.vfp[m]; + int atomcount=0; + + cdouble* ipw=new cdouble[maxp]; + + //find the ends of tracking by constant-pitch extension of the starting HA until + //at a frame at least half of the partials fall below starts[]*brake. + + //first find end of the note by forward extension from the frame at _t + int fr1=trst; + while (fr1<fren) + { + spec=Spec->Spec(fr1); + for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + double ls=0; + for (int m=0; m<P; m++) + { + double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1)); + int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB); + if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1; + ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2); + ls+=~ipw[m]; + if (starts[m]<~ipw[m]) starts[m]=~ipw[m]; + } + double sumstart=0, sumstop=0; + for (int m=0; m<P; m++) + { + if (~ipw[m]<starts[m]*brake) sumstop+=1;// sqrt(starts[m]); + sumstart+=1; //sqrt(starts[m]); + } + double lstop=sumstop/sumstart; + if (lstop>0.5) break; + + fr1++; + } + //note find the start of note by backward extension from the frame at _t + int fr0=trst-1; + while(fr0>frst) + { + spec=Spec->Spec(fr0); + for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + double ls=0; + for (int m=0; m<P; m++) + { + double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1)); + int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB); + if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1; + ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2); + ls+=~ipw[m]; + if (starts[m]<~ipw[m]) starts[m]=~ipw[m]; + } + + double sumstart=0, sumstop=0; + for (int m=0; m<P; m++) + { + if (~ipw[m]<starts[m]*brake) sumstop+=1; //sqrt(starts[m]); + sumstart+=1; //sqrt(starts[m]); + } + double lstop=sumstop/sumstart; + if (lstop>0.5) {fr0++; break;} + + fr0--; + } + + //now fr0 and fr1 are the first and last (excl.) frame indices + Fr=fr1-fr0; + cdouble* ipfr=new cdouble[Fr]; + double *as=new double[Fr*2], *phs=&as[Fr]; + cdouble** Allocate2(cdouble, Fr, wid/2+1, xx); + for (int fr=0; fr<Fr; fr++) + { + spec=Spec->Spec(fr0+fr); + for (int i=0; i<=wid/2; i++) xx[fr][i]=spec[i]; + } + + //reestimate partial frequencies, amplitudes and phase angles using all frames. In case of frequency estimation + //failure, the original frequency is used and all atoms of that partial are typed "atInfered". + for (int m=0; m<P; m++) + { + double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1)), dfm=(settings.delm<1)?settings.delm:1; + double errf=LSESinusoidMP(fm, fm-dfm, fm+dfm, xx, Fr, wid, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6); + // LSESinusoidMPC(fm, fm-1, fm+1, xx, Fr, wid, offst, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6); + atomtype type=(errf<0)?atInfered:atPeak; + for (int fr=0; fr<Fr; fr++) + { + double tfr=(fr0+fr)*offst+wid/2; + atom tmpatom={tfr, wid, fm/wid, as[fr], phs[fr], m+1, type, 0}; + part[atomcount++]=tmpatom; + } + } + + //Tag the atom at initial input (_t, _f) as anchor. + AtomsToPartials(atomcount, part, M, Fr, partials, offst); + partials[settings.pin0-1][trst-fr0].type=atAnchor; + delete[] ipfr; + delete[] ipw; + delete[] part; + delete R; delete[] x; delete[] as; DeAlloc2(xx); + delete[] starts; + return 1; +}//FindNoteConst + +/* + function FindNoteF: forward harmonic sinusoid tracking starting from a given harmonic atom until an + endpoint is detected or a search boundary is reached. + + In: starts: harmonic atom score of the start HA + startvfp[startp]: amplitudes of partials of start HA + R: F-G polygon of the start HA + frst, frst: frame index of the start and end frames of tracking. + Spec: spectrogram + wid, offst: atom scale and hop size, must be consistent with Spec + settings: note match settings + brake: tracking termination threshold. + Out: part[return value]: list of atoms tracked. + starts: maximal HA score of tracked frames. Its product with brake is used as the tracking threshold. + + Returns the number of atoms found. +*/ +int FindNoteF(atom* part, double& starts, TPolygon* R, int startp, double* startvfp, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake) +{ + settings.forcepin0=false, settings.pin0=0; + + int k=0, maxp=settings.maxp; + + TPolygon* RR=new TPolygon(1024, R); + int lastp=startp; + double *lastvfp=new double[settings.maxp]; memcpy(lastvfp, startvfp, sizeof(double)*startp); + + cdouble *x=new cdouble[wid/2+1]; + double *fpp=new double[maxp*4], *vfpp=&fpp[maxp], *pfpp=&fpp[maxp*2]; atomtype* ptype=(atomtype*)&fpp[maxp*3]; + NMResults results={fpp, vfpp, pfpp, ptype}; + int step=(fren>frst)?1:(-1); + for (int h=frst+step; (h-fren)*step<0; h+=step) + { + cmplx<QSPEC_FORMAT>* spec=Spec->Spec(h); + for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + double f0=0, B=0; + double tmp=NoteMatchStiff3(RR, f0, B, 1, &x, wid, 0, &settings, &results, lastp, lastvfp); + if (starts<tmp) starts=tmp; + if (tmp<starts*brake) break; //end condition: power drops by ??dB + + lastp=NMResultToAtoms(settings.maxp, &part[k], h*offst+wid/2, wid, results); k+=lastp; + memcpy(lastvfp, vfpp, sizeof(double)*lastp); + } + delete RR; delete[] lastvfp; delete[] x; delete[] fpp; + return k; +}//FindNoteF + +/* + function FindNoteFB: forward-backward harmonic sinusid tracking. + + In: frst, fren: index of start and end frames + Rst, Ren: F-G polygons of start and end harmonic atoms + vfpst[M], vfpen[M]: amplitude vectors of start and end harmonic atoms + Spec: spectrogram + wid, offst: atom scale and hop size, must be consistent with Spec + settings: note match settings + Out: partials[0:fren-frst][M], hosting tracked HS between frst and fren (inc). + + Returns 0 if successful, 1 if failure in pitch tracking stage (no feasible HA candidate at some frame). + On start partials[0:fren-frst][M] shall be prepared to receive fren-frst+1 harmonic atoms +*/ +int FindNoteFB(int frst, TPolygon* Rst, double* vfpst, int fren, TPolygon* Ren, double* vfpen, int M, atom** partials, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings) +{ //* + if (frst>fren) return -1; + int result=0; + if (settings.maxp>M) settings.maxp=M; + else M=settings.maxp; + int Fr=fren-frst+1; + double B, fpp[1024], delm=settings.delm, delp=settings.delp, iH2=settings.iH2; + double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; + int maxpitch=50; + + NMResults results={fpp, vfpp, pfpp, ptype}; + + cmplx<QSPEC_FORMAT>* spec; + + cdouble *x=new cdouble[wid/2+1]; + TPolygon **Ra=new TPolygon*[maxpitch], **Rb=new TPolygon*[maxpitch], ***R; memset(Ra, 0, sizeof(TPolygon*)*maxpitch), memset(Rb, 0, sizeof(TPolygon*)*maxpitch); + double *sca=new double[maxpitch], *scb=new double[maxpitch], **sc; sca[0]=scb[0]=0; + double** Allocate2(double, maxpitch, M, vfpa); memcpy(vfpa[0], vfpst, sizeof(double)*M); + double** Allocate2(double, maxpitch, M, vfpb); memcpy(vfpb[0], vfpen, sizeof(double)*M); + double*** vfp; + + double** Allocate2(double, Fr, wid/2, fps); + double** Allocate2(double, Fr, wid/2, vps); + int* pc=new int[Fr]; + + Ra[0]=new TPolygon(M*2+4, Rst); + Rb[0]=new TPolygon(M*2+4, Ren); + int pitchcounta=1, pitchcountb=1, pitchcount; + + //pitch tracking + int** Allocate2(int, Fr, maxpitch, prev); + int** Allocate2(int, Fr, maxpitch, pitches); + int* pitchres=new int[Fr]; + int fra=frst, frb=fren, fr; + while (fra<frb-1) + { + if (pitchcounta<pitchcountb){fra++; fr=fra; pitchcount=pitchcounta; vfp=&vfpa; sc=&sca; R=&Ra;} + else {frb--; fr=frb; pitchcount=pitchcountb; vfp=&vfpb; sc=&scb, R=&Rb;} + int fr_frst=fr-frst; + + int absfr=(partials[0][fr].t-wid/2)/offst; + spec=Spec->Spec(absfr); for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + pc[fr_frst]=QuickPeaks(fps[fr_frst], vps[fr_frst], wid, x, settings.M, settings.c, iH2, 0.0005); + NoteMatchStiff3FB(pitchcount, *R, *vfp, *sc, pitches[fr_frst], prev[fr_frst], pc[fr_frst], fps[fr_frst], vps[fr_frst], x, wid, maxpitch, &settings); + if (fr==fra) pitchcounta=pitchcount; + else pitchcountb=pitchcount; + if (pitchcount<=0) + {result=1; goto cleanup;} + } + { + //now fra=frb-1 + int fra_frst=fra-frst, frb_frst=frb-frst, maxpitcha=0, maxpitchb=0; + double maxs=0; + for (int i=0; i<pitchcounta; i++) + { + double f0a, f0b; + if (fra==frst) f0a=partials[0][frst].f*wid; + else + { + int peak0a=((__int16*)&pitches[fra_frst][i])[0], pin0a=((__int16*)&pitches[fra_frst][i])[1]; + f0a=fps[fra_frst][peak0a]/pin0a; + } + for (int j=0; j<pitchcountb; j++) + { + if (frb==fren) f0b=partials[0][fren].f*wid; + else + { + int peak0b=((__int16*)&pitches[frb_frst][j])[0], pin0b=((__int16*)&pitches[frb_frst][j])[1]; + f0b=fps[frb_frst][peak0b]/pin0b; + } + double pow2delp=pow(2, delp/12); + if (f0b<f0a*pow2delp+delm && f0b>f0a/pow2delp-delm) + { + double s=sca[i]+scb[j]+conta(M, vfpa[i], vfpb[j]); + if (maxs<s) maxs=s, maxpitcha=i, maxpitchb=j; + } + } + } + //get pitch tracking result + pitchres[fra_frst]=pitches[fra_frst][maxpitcha]; + for (int fr=fra; fr>frst; fr--) + { + maxpitcha=prev[fr-frst][maxpitcha]; + pitchres[fr-frst-1]=pitches[fr-frst-1][maxpitcha]; + } + pitchres[frb_frst]=pitches[frb_frst][maxpitchb]; + for (int fr=frb; fr<fren; fr++) + { + maxpitchb=prev[fr-frst][maxpitchb]; + pitchres[fr-frst+1]=pitches[fr-frst+1][maxpitchb]; + } + } +{ + double f0s[1024]; memset(f0s, 0, sizeof(double)*1024); + for (int i=frst+1; i<fren; i++) + { + int pitch=pitchres[i-frst]; + int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1]; + f0s[i]=fps[i-frst][peak0]/pin0; + } + f0s[frst]=sqrt(Rst->X[0]), f0s[fren]=sqrt(Ren->X[0]); + + //partial tracking + int lastp=0; while (lastp<M && vfpst[lastp]>0) lastp++; + double* lastvfp=new double[M]; memcpy(lastvfp, vfpst, sizeof(double)*M); + delete Ra[0]; Ra[0]=new TPolygon(M*2+4, Rst); + for (int h=frst+1; h<fren; h++) + { + int absfr=(partials[0][h].t-wid/2)/offst; + int h_frst=h-frst, pitch=pitchres[h_frst]; + int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1]; + spec=Spec->Spec(absfr); + double f0=fpp[0];//, B=Form11->BEdit->Text.ToDouble(); + //double deltaf=f0*(pow(2, settings.delp/12)-1); + for (int i=0; i<=wid/2; i++) x[i]=spec[i]; + memset(fpp, 0, sizeof(double)*1024); + //settings.epf0=deltaf, + settings.forcepin0=true; settings.pin0=pin0; f0=fps[h_frst][peak0]; settings.pin0asanchor=false; + if (NoteMatchStiff3(Ra[0], f0, B, pc[h_frst], fps[h_frst], vps[h_frst], 1, &x, wid, 0, &settings, &results, lastp, lastvfp)>0) + { + lastp=NMResultToPartials(M, h, partials, partials[0][h].t, wid, results); + memcpy(lastvfp, vfpp, sizeof(double)*lastp); + } + } + delete[] lastvfp; +} +cleanup: + delete[] x; + for (int i=0; i<pitchcounta; i++) delete Ra[i]; delete[] Ra; + for (int i=0; i<pitchcountb; i++) delete Rb[i]; delete[] Rb; + delete[] sca; delete[] scb; + DeAlloc2(vfpa); DeAlloc2(vfpb); + DeAlloc2(fps); DeAlloc2(vps); + DeAlloc2(pitches); DeAlloc2(prev); + delete[] pitchres; delete[] pc; + return result; //*/ +}//FindNoteFB + +/* + function GetResultsSingleFr: used by NoteMatchStiff3 to obtain final note match results after harmonic + grouping of peaks with rough frequency estimates, including a screening of found peaks based on shape + and consistency with other peak frequencies, reestimating sinusoid parameters using LSE, estimating + atoms without peaks by inferring their frequencies, and translating internal peak type to atom type. + + In: R0: F-G polygon of primitive (=rough estimates) partial frequencies + x[N]: spectrum + ps[P], fs[P]; partial indices and frequencies, may miss partials + rsrs[P]: peak shape factors, used for evaluating peak validity + psb[P]: peak type flags, related to atom::ptype. + settings: note match settings + numsam: number of partials to evaluate (=number of atoms to return) + Out: results: note match results as a NMResult structure + f0, B: fundamental frequency and stiffness coefficient + Rret: F-G polygon of reestimated set of partial frequencies + + Return the total energy of the harmonic atom. +*/ +double GetResultsSingleFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, cdouble* x, int N, int* psb, int numsam, NMSettings* settings, NMResults* results) +{ + Rret->N=0; + double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB; + int M=settings->M, maxp=settings->maxp; + double *fp=results->fp; memset(fp, 0, sizeof(double)*maxp); + + double result=0; + if (P>0) + { + double *vfp=results->vfp, *pfp=results->pfp; + atomtype *ptype=results->ptype; //memset(ptype, 0, sizeof(int)*maxp); + + double *f1=new double[(maxp+1)*4], *f2=&f1[maxp+1], *ft=&f2[maxp+1], *fdep=&ft[maxp+1], tmpa, cF, cG; + areaandcentroid(tmpa, cF, cG, R0->N, R0->X, R0->Y); + for (int p=1; p<=maxp; p++) ExFmStiff(f1[p], f2[p], p, R0->N, R0->X, R0->Y), ft[p]=p*sqrt(cF+(p*p-1)*cG); + //sort peaks by rsr and departure from model so that most reliable ones are found at the start + double* values=new double[P]; int* indices=new int[P]; + for (int i=0; i<P; i++) + { + values[i]=1e200; + int lp=ps[i]; double lf=fs[i]; + if (lf>=f1[lp] && lf<=f2[lp]) fdep[lp]=0; + else if (lf<f1[lp]) fdep[lp]=f1[lp]-lf; + else if (lf>f2[lp]) fdep[lp]=lf-f2[lp]; + double tmpv=(fdep[lp]>0.5)?(fdep[lp]-0.5)*2:0; + tmpv+=rsrs?rsrs[i]:0; + tmpv*=pow(lp, 0.2); + InsertInc(tmpv, i, values, indices, i+1); + } + + for (int i=0; i<P; i++) + { + int ind=indices[i]; + int lp=ps[ind]; //partial index + //Get LSE estimation of atoms + double lf=fs[ind], f1, f2, la, lph;//, lrsr=rsrs?rsrs[ind]:0 + if (lp==0 || lf==0) continue; + ExFmStiff(f1, f2, lp, R0->N, R0->X, R0->Y); + LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf); + //lrsr=PeakShapeC(lf, 1, N, &x, 6, M, c, iH2); + if (Rret->N>0) ExFmStiff(f1, f2, lp, Rret->N, Rret->X, Rret->Y); + if (Rret->N==0 || (lf>=f1 && lf<=f2)) + { + fp[lp-1]=lf; + vfp[lp-1]=la; + pfp[lp-1]=lph; + if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) ptype[lp-1]=atAnchor; //0 for anchor points + else ptype[lp-1]=atPeak; //1 for local maximum + //update R using found partails with amplitude>1 + if (Rret->N==0) InitializeR(Rret, lp, lf, delm, maxB); + else if (la>1) CutR(Rret, lp, lf, delm, true); + } + } + //* + for (int p=1; p<=maxp; p++) + { + if (fp[p-1]>0) + { + double f1, f2; + ExFmStiff(f1, f2, p, Rret->N, Rret->X, Rret->Y); + if (fp[p-1]<f1-0.3 || fp[p-1]>f2+0.3) + fp[p-1]=0; + } + }// */ + + + //estimate f0 and B + double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1; + areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y); + testnn(cF); f0=sqrt(cF); B=cG/cF; + + //Get LSE estimates for unfound partials + for (int i=0; i<numsam; i++) + { + if (fp[i]==0) //no peak is found for this partial in lcand + { + int m=i+1; + double tmp=cF+(m*m-1)*cG; testnn(tmp); + double lf=m*sqrt(tmp); + if (lf<N/2.1) + { + fp[i]=lf; + ptype[i]=atInfered; //2 for non peak + cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB); + vfp[i]=sqrt(r.x*r.x+r.y*r.y); + pfp[i]=Atan2(r.y, r.x);//*/ + } + } + } + if (f0>0) for (int i=0; i<numsam; i++) if (fp[i]>0) result+=vfp[i]*vfp[i]; + + delete[] f1; delete[] values; delete[] indices; + } + return result; +}//GetResultsSingleFr + +/* + function GetResultsMultiFr: the constant-pitch multi-frame version of GetResultsSingleFr. + + In: x[Fr][N]: spectrogram + ps[P], fs[P]; partial indices and frequencies, may miss partials + psb[P]: peak type flags, related to atom::ptype. + settings: note match settings + forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned") + numsam: number of partials to evaluate (=number of atoms to return) + Out: results[Fr]: note match results as Fr NMResult structures + f0, B: fundamental frequency and stiffness coefficient + Rret: F-G polygon of reestimated set of partial frequencies + + Return the total energy of the harmonic sinusoid. +*/ +double GetResultsMultiFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, int Fr, cdouble** x, int N, int offst, int* psb, int numsam, NMSettings* settings, NMResults* results, int forceinputlocalfr) +{ + Rret->N=0; + double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB;// *pinf=settings->pinf; + + int M=settings->M, maxp=settings->maxp, pincount=settings->pcount, *pin=settings->pin, *pinfr=settings->pinfr; +// double *fp=results->fp, *vfp=results->vfp, *pfp=results->pfp; +// int *ptype=results->ptype; + for (int fr=0; fr<Fr; fr++) memset(results[fr].fp, 0, sizeof(double)*maxp); + int* pclass=new int[maxp+1]; memset(pclass, 0, sizeof(int)*(maxp+1)); + for (int i=0; i<pincount; i++) if (pinfr[i]>=0) pclass[pin[i]]=1; + if (forceinputlocalfr>=0) pclass[settings->pin0]=1; + double result=0; + if (P>0) + { + double tmpa, cF, cG; + //found atoms + double *as=new double[Fr*2], *phs=&as[Fr]; + for (int i=P-1; i>=0; i--) + { + int lp=ps[i]; + double lf=fs[i]; + if (lp==0 || lf==0) continue; + + if (!pclass[lp]) + { + //Get LSE estimation of atoms + double startlf=lf; + LSESinusoidMP(lf, lf-1, lf+1, x, Fr, N, 3, M, c, iH2, as, phs, epf); + //LSESinusoidMPC(lf, lf-1, lf+1, x, Fr, N, offst, 3, M, c, iH2, as, phs, epf); + if (fabs(lf-startlf)>0.6) + { + lf=startlf; + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB); + as[fr]=abs(r); + phs[fr]=arg(r); + } + } + } + else //frequencies of local anchor atoms are not re-estimated + { + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB); + as[fr]=abs(r); + phs[fr]=arg(r); + } + } + + //LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf); + //fp[lp-1]=lf; vfp[lp-1]=la; pfp[lp-1]=lph; + for (int fr=0; fr<Fr; fr++) results[fr].fp[lp-1]=lf, results[fr].vfp[lp-1]=as[fr], results[fr].pfp[lp-1]=phs[fr]; + if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atAnchor; //0 for anchor points + else for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atPeak; //1 for local maximum + //update R using found partails with amplitude>1 + if (Rret->N==0) + { + //temporary treatment: +0.5 should be +rsr or something similar + InitializeR(Rret, lp, lf, delm+0.5, maxB); + areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y); + } + else //if (la>1) + { + CutR(Rret, lp, lf, delm+0.5, true); + areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y); + } + } + //estimate f0 and B + double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1; + areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y); + testnn(cF); f0=sqrt(cF); B=cG/cF; + + //Get LSE estimates for unfound partials + for (int i=0; i<numsam; i++) + { + if (results[0].fp[i]==0) //no peak is found for this partial in lcand + { + int m=i+1; + double tmp=cF+(m*m-1)*cG; testnn(tmp); + double lf=m*sqrt(tmp); + if (lf<N/2.1) + { + for (int fr=0; fr<Fr; fr++) + { + results[fr].fp[i]=lf, results[fr].ptype[i]=atInfered; //2 for non peak + int k1=ceil(lf-hB); if (k1<0) k1=0; + int k2=floor(lf+hB); if (k2>=N/2) k2=N/2-1; + cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, k1, k2); + results[fr].vfp[i]=abs(r); + results[fr].pfp[i]=arg(r); + } + } + } + } + delete[] as; + if (f0>0) for (int fr=0; fr<Fr; fr++) for (int i=0; i<numsam; i++) if (results[fr].fp[i]>0) result+=results[fr].vfp[i]*results[fr].vfp[i]; + result/=Fr; + } + delete[] pclass; + return result; +}//GetResultsMultiFr + +/* + function InitailizeR: initializes a F-G polygon with a fundamental frequency range and stiffness coefficient bound + + In: af, ef: centre and half width of the fundamental frequency range + maxB: maximal value of stiffness coefficient (the minimal is set to 0) + Out: R: the initialized F-G polygon. + + No reutrn value. +*/ +void InitializeR(TPolygon* R, double af, double ef, double maxB) +{ + R->N=4; + double *X=R->X, *Y=R->Y; + double g1=af-ef, g2=af+ef; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + X[0]=X[3]=g1, X[1]=X[2]=g2; + Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0; +}//InitializeR + +/* + function InitialzeR: initializes a F-G polygon with a frequency range for a given partial and + stiffness coefficient bound + + In: apind: partial index + af, ef: centre and half width of the frequency range of the apind-th partial + maxB; maximal value of stiffness coefficient (the minimal is set to 0) + Out: R: the initialized F-G polygon. + + No return value. +*/ +void InitializeR(TPolygon* R, int apind, double af, double ef, double maxB) +{ + R->N=4; + double *X=R->X, *Y=R->Y; + double k=apind*apind-1; + double g1=(af-ef)/apind, g2=(af+ef)/apind; + if (g1<0) g1=0; + g1=g1*g1, g2=g2*g2; + double kb1=1+k*maxB; + X[0]=g1/kb1, X[1]=g2/kb1, X[2]=g2, X[3]=g1; + Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0; +}//InitializeR + +/* + function maximalminimum: finds the point within a polygon that maximizes its minimal distance to the + sides. + + In: sx[N], sy[N]: x- and y-coordinates of vertices of a polygon + Out: (x, y): point within the polygon with maximal minimal distance to the sides + + Returns the maximial minimal distance. A circle centred at (x, y) with the return value as the radius + is the maximum inscribed circle of the polygon. +*/ +double maximalminimum(double& x, double& y, int N, double* sx, double* sy) +{ + //at the beginning let (x, y) be (sx[0], sy[0]), then the mininum distance d is 0. + double dm=0; + x=sx[0], y=sy[0]; + double *A=new double[N*3]; + double *B=&A[N], *C=&A[N*2]; + //calcualte equations of all sides A[k]x+B[k]y+C[k]=0, with signs adjusted so that for + // any (x, y) within the polygon, A[k]x+B[k]y+C[k]>0. A[k]^2+B[k]^2=1. + for (int n=0; n<N; n++) + { + double x1=sx[n], y1=sy[n], x2, y2, AA, BB, CC; + if (n+1!=N) x2=sx[n+1], y2=sy[n+1]; + else x2=sx[0], y2=sy[0]; + double dx=x1-x2, dy=y1-y2; + double ds=sqrt(dx*dx+dy*dy); + AA=dy/ds, BB=-dx/ds; + CC=-AA*x1-BB*y1; + //adjust signs + if (n+2<N) x2=sx[n+2], y2=sy[n+2]; + else x2=sx[n+2-N], y2=sy[n+2-N]; + if (AA*x2+BB*y2+CC<0) A[n]=-AA, B[n]=-BB, C[n]=-CC; + else A[n]=AA, B[n]=BB, C[n]=CC; + } + + //during the whole process (x, y) is always equal-distance to at least two sides, + // namely l1--(l1+1) and l2--(l2+1). there equations are A1x+B1y+C1=0 and A2x+B2y+C2=0, + // where A^2+B^2=1. + int l1=0, l2=N-1; + + double a, b; + b=A[l1]-A[l2], a=B[l2]-B[l1]; + double ds=sqrt(a*a+b*b); + a/=ds, b/=ds; + //the line (x+at, y+bt) passes (x, y), and points on this line are equal-distance to l1 and l2. + //along this line at point (x, y), we have dx=ads, dy=bds + //now find the signs so that dm increases as ds>0 + double ddmds=A[l1]*a+B[l1]*b; + if (ddmds<0) a=-a, b=-b, ddmds=-ddmds; + + while (true) + { + //now the vector starting from (x, y) pointing in (a, b) is equi-distance-to-l1-and-l2 and + // dm-increasing. actually at s from (x,y), d=dm+ddmds*s. + + //it is now guaranteed that the distance of (x, y) to l1 (=l2) is smaller than to any other + // sides. along direction (A, B) the distance of (x, y) to l1 (=l2) is increasing and + // the distance to at least one other sides is increasing, so that at some value of s the + // distances equal. we find the smallest s>0 where this happens. + int l3=-1; + double s=-1; + for (int n=0; n<N; n++) + { + if (n==l1 || n==l2) continue; + //distance of (x,y) to the side + double ldm=A[n]*x+B[n]*y+C[n]; //ldm>dm + double dldmds=A[n]*a+B[n]*b; + //so ld=ldm+lddmds*s, the equality is dm+ddmds*s=ldm+lddmds*s + if (ddmds-dldmds>0) + { + ds=(ldm-dm)/(ddmds-dldmds); + if (l3==-1) l3=n, s=ds; + else if (ds<s) l3=n, s=ds; + } + } + if (ddmds==0) s/=2; + x=x+a*s, y=y+b*s; + dm=A[l1]*x+B[l1]*y+C[l1]; +// Form1->Canvas->Ellipse(x-dm, y-dm, x+dm, y+dm); + //(x, y) is equal-distance to l1, l2 and l3 + //try use l3 to substitute l2 + b=A[l1]-A[l3], a=B[l3]-B[l1]; + ds=sqrt(a*a+b*b); + a/=ds, b/=ds; + ddmds=A[l1]*a+B[l1]*b; + if (ddmds<0) a=-a, b=-b, ddmds=-ddmds; + if (ddmds==0 || A[l2]*a+B[l2]*b>0) + { + l2=l3; + } + else //l1<-l3 fails, then try use l3 to substitute l2 + { + b=A[l3]-A[l2], a=B[l2]-B[l3]; + ds=sqrt(a*a+b*b); + a/=ds, b/=ds; + ddmds=A[l3]*a+B[l3]*b; + if (ddmds<0) a=-a, b=-b, ddmds=-ddmds; + if (ddmds==0 || A[l1]*a+B[l1]*b>0) + { + l1=l3; + } + else break; + } + } + + delete[] A; + return dm; +}//maximalminimum + +/* + function minimaxstiff: finds the point in polygon (N; F, G) that minimizes the maximal value of the + function + + | m[l]sqrt(F+(m[l]^2-1)*G)-f[l] | + e_l = | ----------------------------- | regarding l=0, ..., L-1 + | norm[l] | + + In: _m[L], _f[L], norm[L]: coefficients in the above + F[N], G[N]: vertices of a F-G polygon + Out: (F1, G1): the mini-maximum. + + Returns the minimized maximum value. + + Further reading: Wen X, "Estimating model parameters", Ch.3.2.6 from "Harmonic sinusoid modeling of + tonal music events", PhD thesis, University of London, 2007. +*/ +double minimaxstiff(double& F1, double& G1, int L, int* _m, double* _f, double* norm, int N, double* F, double* G) +{ + if (L==0 || N<=2) + { + F1=F[0], G1=G[0]; + return 0; + } + //normalizing + double* m=(double*)malloc(sizeof(double)*L*6);//new double[L*6]; + double* f=&m[L*2]; + int* k=(int*)&m[L*4]; + for (int l=0; l<L; l++) + { + k[2*l]=_m[l]*_m[l]-1; + k[2*l+1]=k[2*l]; + m[2*l]=_m[l]/norm[l]; + m[2*l+1]=-m[2*l]; + f[2*l]=_f[l]/norm[l]; + f[2*l+1]=-f[2*l]; + } + //From this point on the L distance functions with absolute value is replace by 2L distance functions + L*=2; + double* vmnl=new double[N*2]; + int* mnl=(int*)&vmnl[N]; +start: + //Initialize (F0, G0) to be the polygon vertex that has the minimal max_l(e_l) + // maxn: the vertex index + // maxl: the l that maximizes e_l at that vertex + // maxsg: the sign of e_l before taking the abs. value + int nc=-1, nd; + int l1=-1, l2=0, l3=0; + double vmax=0; + + for (int n=0; n<N; n++) + { + int lmax=-1; + double lvmax=0; + for (int l=0; l<L; l++) + { + double tmp=F[n]+k[l]*G[n]; testnn(tmp); + double e=m[l]*sqrt(tmp)-f[l]; + if (e>lvmax) lvmax=e, lmax=l; + } + mnl[n]=lmax, vmnl[n]=lvmax; + if (n==0) + { + vmax=lvmax, nc=n, l1=lmax; + } + else + { + if (lvmax<vmax) vmax=lvmax, nc=n, l1=lmax; + } + } + double F0=F[nc], G0=G[nc]; + + +// start searching the the minimal maximum from (F0, G0) +// +// Each searching step starts from (F0, G0), ends at (F1, G1) +// +// Starting conditions of one step: +// +// (F0, G0) can be +// (1)inside polygon R; +// (2)on one side of R (nc:(nc+1) gives the side) +// (3)a vertex of R (nc being the vertex index) +// +// The maximum at (F0, G0) can be +// (1)vmax=e1=e2=e3>...; (l1, l2, l3) +// (2)vmax=e1=e2>...; (l1, l2) +// (3)vmax=e1>.... (l1) +// +// More complication arise if we have more than 3 equal maxima, i.e. +// vmax=e1=e2=e3=e4>=.... CURRENTLY WE ASSUME THIS NEVER HAPPENS. +// +// These are also the ending conditions of one step. +// +// There are types of basic searching steps, i.e. +// +// (1) e1=e2 search: starting with e1=e2, search down the decreasing direction +// of e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1) +// =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about +// to go out of R. +// Arguments: l1, l2, (F0, G0, vmax) +// (2) e1!=e2 search: starting with e1=e2 and (F0, G0) being on one side, search +// down the side in the decreasing direction of both e1 and e1 until at point +// (F1, G1) there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or +// at a the search reaches a vertex of R. +// Arguments: l1, l2, dF, dG, (F0, G0, vmax) +// (3) e1 free search: starting with e1 being the only maximum, search down the decreasing +// direction of e1 until at point (F1, G1) there is another l2 so that +// e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about +// to go out of R. +// Arguments: l1, dF, dG, (F0, G0, vmax) +// (4) e1 side search: starting with e1 being the only maximum, search down the +// a side of R in the decreasing direction of e1 until at point (F1, G1) there +// is another l2 so that e1=e2, or until point (F1, G1) the search reaches +// a vertex. +// Arguments: l1, destimation vertex nd, (F0, G0, vmax) +// +// At the beginning of each searching step we check the conditions to see which +// basic step to perform, and provide the starting conditions. At the very start of +// the search, (F0, G0) is a vertex of R, e_l1 being the maximum at this point. +// + + int condpos=3; //inside, on side, vertex + int condmax=3; //triple max, duo max, solo max + int searchmode; + + bool minimax=false; //set to true when the minimal maximum is met + + int iter=L*2; + while (!minimax && iter>0) + { + iter--; + double m1=m[l1], m2=m[l2], m3=m[l3], k1=k[l1], k2=k[l2], k3=k[l3]; + double tmp, tmp1, tmp2, tmp3; + + switch (condmax) + { + case 1: + tmp=F0+k3*G0; testnn(tmp); + tmp3=sqrt(tmp); + case 2: + tmp=F0+k2*G0; testnn(tmp) + tmp2=sqrt(tmp); + case 3: + tmp=F0+k1*G0; testnn(tmp); + tmp1=sqrt(tmp); + } + double dF, dG; + int n0=(nc==0)?(N-1):(nc-1), n1=(nc==N-1)?0:(nc+1); + double x0, y0, x1, y1; + if (n0>=0) x0=F[nc]-F[n0], y0=G[nc]-G[n0]; + if (nc>=0) x1=F[n1]-F[nc], y1=G[n1]-G[nc]; + + if (condpos==1) //(F0, G0) being inside polygon + { + if (condmax==1) //e1=e2=e3 being the maximum + { + //vmax holds the maximum + //l1, l2, l3 + + //now choose a searching direction, either e1=e2 or e1=e3 or e2=e3. + // choose e1=e2 if (e3-e1) decreases along the decreasing direction of e1=e2 + // choose e1=e3 if (e2-e1) decreases along the decreasing direction of e1=e3 + // choose e2=e3 if (e1-e2) decreases along the decreasing directino of e2=e3 + // if no condition is satisfied, then (F0, G0) is the minimal maximum. + //calculate the decreasing direction of e1=e3 as (dF, dG) + double den=m1*m3*(k1-k3)/2; + dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, + dG=(m1*tmp3-m3*tmp1)/den; + //the negative gradient of e2-e1 is calculated as (gF, gG) + double gF=-(m2/tmp2-m1/tmp1)/2, + gG=-(k2*m2/tmp2-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0) //so that e2-e1 decreases in the decreasing direction of e1=e3 + { + l2=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e2=e3 as (dF, dG) + den=m2*m3*(k2-k3)/2; + dF=-(k2*m2*tmp3-k3*m3*tmp2)/den, + dG=(m2*tmp3-m3*tmp2)/den; + //calculate the negative gradient of e1-e2 as (gF, gG) + gF=-gF, gG=-gG; + if (dF*gF+dG*gG>0) //so that e1-e2 decreases in the decreasing direction of e2=e3 + { + l1=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e1=e2 as (dF, dG) + den=m1*m2*(k1-k2)/2; + dF=-(k1*m1*tmp2-k2*m2*tmp1)/den, + dG=(m1*tmp2-m2*tmp1)/den; + //calculate the negative gradient of (e3-e1) as (gF, gG) + gF=-(m3/tmp3-m1/tmp1)/2, + gG=-(k3*m3/tmp3-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0) //so that e3-e1 decreases in the decreasing direction of e1=e2 + { + searchmode=1; + } + else + { + F1=F0, G1=G0; + searchmode=0; //no search + minimax=true; //quit loop + } + } + } + } // + else if (condmax==2) //e1=e2 being the maximum + { + //vmax holds the maximum + //l1, l2 + searchmode=1; + } + else if (condmax==3) //e1 being the maximum + { + //the negative gradient of e1 + dF=-0.5*m1/tmp1; + dG=k1*dF; + searchmode=3; + } + } + else if (condpos==2) //(F0, G0) being on side nc:(nc+1) + { + //the vector nc->(nc+1) as (x1, y1) + if (condmax==1) //e1=e2=e3 being the maximum + { + //This case rarely happens. + + //First see if a e1=e2 search is possible + //calculate the decreasing direction of e1=e3 as (dF, dG) + double den=m1*m3*(k1-k3)/2; + double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den; + //the negative gradient of e2-e1 is calculated as (gF, gG) + double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3 + { //~~~~~~~~~~~~~and this direction points inward + l2=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e2=e3 as (dF, dG) + den=m2*m3*(k2-k3)/2; + dF=-(k2*m2*tmp3-k3*m3*tmp2)/den, + dG=(m2*tmp3-m3*tmp2)/den; + //calculate the negative gradient of e1-e2 as (gF, gG) + gF=-gF, gG=-gG; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3 + { + l1=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e1=e2 as (dF, dG) + den=m1*m2*(k1-k2)/2; + dF=-(k1*m1*tmp2-k2*m2*tmp1)/den, + dG=(m1*tmp2-m2*tmp1)/den; + //calculate the negative gradient of (e3-e1) as (gF, gG) + gF=-(m3/tmp3-m1/tmp1)/2, + gG=-(k3*m3/tmp3-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2 + { + searchmode=1; + } + else + { + //see the possibility of a e1!=e2 search + //calcualte the dot product of the gradients and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1), + d2=m2/2/tmp2*(x1+k2*y1), + d3=m3/2/tmp3*(x1+k3*y1); + //we can prove that if there is a direction pointing inward R in which + // e1, e2, e2 decrease, and another direction pointing outside R in + // which e1, e2, e3 decrease, then on one direction along the side + // all the three decrease. (Even more, this direction must be inside + // the <180 angle formed by the two directions.) + // + // On the contrary, if there is a direction + // in which all the three decrease, with two equal, it has to point + // outward for the program to get here. Then if along neither direction + // of side R can the three all descend, then there doesn't exist any + // direction inward R in which the three descend. In that case we + // have a minimal maximum at (F0, G0). + if (d1*d2<=0 || d1*d3<=0 || d2*d3<=0) //so that the three don't decrease in the same direction + { + F1=F0, G1=G0; + searchmode=0; //no search + minimax=true; //quit loop + } + else + { + if (d1>0) //so that d2>0, d3>0, all three decrease in the direction -(x1, y1) towards nc + { + if (d1>d2 && d1>d3) //e1 decreases fastest + { + l1=l3; //keep e2, e3 + } + else if (d2>=d1 && d2>d3) //e2 decreases fastest + { + l2=l3; //keep e1, e3 + } + else //d3>=d1 && d3>=d2, e3 decreases fastest + { + //keep e1, e2 + } + nd=nc; + } + else //d1<0, d2<0, d3<0, all three decrease in the direction (x1, y1) + { + if (d1<d2 && d1<d3) //e1 decreases fastest + { + l1=l3; + } + else if (d2<=d1 && d2<d3) //e2 decreases fastest + { + l2=l3; + } + else //d3<=d1 && d3<=d2 + { + //keep e1, e2 + } + nd=n1; + } + searchmode=2; + } + } + } + } + } + else if (condmax==2) //e1=e2 being the maximum + { + //first see if the decreasing direction of e1=e2 points to the inside + //calculate the decreasing direction of e1=e2 as (dF, dG) + double den=m1*m2*(k1-k2)/2; + dF=-(k1*m1*tmp2-k2*m2*tmp1)/den, + dG=(m1*tmp2-m2*tmp1)/den; + + if (x1*dG-y1*dF<0) //so that (dF, dG) points inward R + { + searchmode=1; + } + else + { + //calcualte the dot product of the gradients and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1), + d2=m2/2/tmp2*(x1+k2*y1); + if (d1*d2<=0) //so that along the side e1, e2 descend in opposite directions + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + else + { + if (d1>0) //so that both decrease in direction -(x1, y1) + nd=nc; + else + nd=n1; + searchmode=2; + } + } + } + else if (condmax==3) //e1 being the maximum + { + //calculate the negative gradient of e1 as (dF, dG) + dF=-0.5*m1/tmp1; + dG=k1*dF; + + if (x1*dG-y1*dF<0) //so the gradient points inward R + searchmode=3; + else + { + //calculate the dot product of the gradient and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1); + if (d1>0) //so that e1 decreases in direction -(x1, y1) + { + nd=nc; + searchmode=4; + } + else if (d1<0) + { + nd=n1; + searchmode=4; + } + else //so that e1 does not change along side nc:(nc+1) + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + } + } + } + else //condpos==3, (F0, G0) being vertex nc + { + //the vector nc->(nc+1) as (x1, y1) + //the vector (nc-1)->nc as (x0, y0) + + if (condmax==1) //e1=e2=e3 being the maximum + { + //This case rarely happens. + + //First see if a e1=e2 search is possible + //calculate the decreasing direction of e1=e3 as (dF, dG) + double den=m1*m3*(k1-k3)/2; + double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den; + //the negative gradient of e2-e1 is calculated as (gF, gG) + double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3 + { //~~~~~~~~~~~~~and this direction points inward + l2=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e2=e3 as (dF, dG) + den=m2*m3*(k2-k3)/2; + dF=-(k2*m2*tmp3-k3*m3*tmp2)/den, + dG=(m2*tmp3-m3*tmp2)/den; + //calculate the negative gradient of e1-e2 as (gF, gG) + gF=-gF, gG=-gG; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3 + { + l1=l3; + searchmode=1; + } + else + { + //calculate the decreasing direction of e1=e2 as (dF, dG) + den=m1*m2*(k1-k2)/2; + dF=-(k1*m1*tmp2-k2*m2*tmp1)/den, + dG=(m1*tmp2-m2*tmp1)/den; + //calculate the negative gradient of (e3-e1) as (gF, gG) + gF=-(m3/tmp3-m1/tmp1)/2, + gG=-(k3*m3/tmp3-k1*m1/tmp1)/2; + if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2 + { + searchmode=1; + } + else + { + //see the possibility of a e1!=e2 search + //calcualte the dot product of the gradients and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1), + d2=m2/2/tmp2*(x1+k2*y1), + d3=m3/2/tmp3*(x1+k3*y1); + //we can also prove that if there is a direction pointing inward R in which + // e1, e2, e2 decrease, and another direction pointing outside R in + // which e1, e2, e3 decrease, all the three decrease either on nc->(nc+1) + // direction along side nc:(nc+1), or on nc->(nc-1) direction along side + // nc:(nc-1). + // + // On the contrary, if there is a direction + // in which all the three decrease, with two equal, it has to point + // outward for the program to get here. Then if along neither direction + // of side R can the three all descend, then there doesn't exist any + // direction inward R in which the three descend. In that case we + // have a minimal maximum at (F0, G0). + if (d1<0 && d2<0 && d3<0) //so that all the three decrease in the direction (x1, y1) + { + if (d1<d2 && d1<d3) //e1 decreases fastest + { + l1=l3; + } + else if (d2<=d1 && d2<d3) //e2 decreases fastest + { + l2=l3; + } + else //d3<=d1 && d3<=d2 + { + //keep e1, e2 + } + nd=n1; + searchmode=2; + } + else + { + d1=m1/2/tmp1*(x0+k1*y0), + d2=m2/2/tmp2*(x0+k2*y0), + d3=m3/2/tmp3*(x0+k3*y0); + if (d1>0 && d2>0 && d3>0) //so that all the three decrease in the direction -(x0, y0) + { + if (d1>d2 && d1>d3) //e1 decreases fastest + { + l1=l3; //keep e2, e3 + } + else if (d2>=d1 && d2>d3) //e2 decreases fastest + { + l2=l3; //keep e1, e3 + } + else //d3>=d1 && d3>=d2, e3 decreases fastest + { + //keep e1, e2 + } + nd=n0; + searchmode=2; + } + else + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + } + } + } + } + } + else if (condmax==2) //e1=e2 being the maximum + { + //first see if the decreasing direction of e1=e2 points to the inside + //calculate the decreasing direction of e1=e2 as (dF, dG) + double den=m1*m2*(k1-k2)/2; + dF=-(k1*m1*tmp2-k2*m2*tmp1)/den, + dG=(m1*tmp2-m2*tmp1)/den; + + if (x1*dG-y1*dF<0 && x0*dG-y1*dF<0) //so that (dF, dG) points inward R + { + searchmode=1; + } + else + { + //calcualte the dot product of the gradients and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1), + d2=m2/2/tmp2*(x1+k2*y1); + if (d1<0 && d2<0) //so that along side nc:(nc+1) e1, e2 descend in direction (x1, y1) + { + nd=n1; + searchmode=2; + } + else + { + d1=m1/2/tmp1*(x0+k1*y0), + d2=m2/2/tmp2*(x0+k2*y0); + if (d1>0 && d2>0) //so that slong the side (nc-1):nc e1, e2 decend in direction -(x0, y0) + { + nd=n0; + searchmode=2; + } + else + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + } + } + } + else //condmax==3, e1 being the solo maximum + { + //calculate the negative gradient of e1 as (dF, dG) + dF=-0.5*m1/tmp1; + dG=k1*dF; + + if (x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so the gradient points inward R + searchmode=3; + else + { + //calculate the dot product of the gradient and (x1, y1) + double d1=m1/2/tmp1*(x1+k1*y1), + d0=-m1/2/tmp1*(x0+k1*y0); + if (d1<d0) //so that e1 decreases in direction (x1, y1) faster than in -(x0, y0) + { + if (d1<0) + { + nd=n1; + searchmode=4; + } + else + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + } + else //so that e1 decreases in direction -(x0, y0) faster than in (x1, y1) + { + if (d0<0) + { + nd=n0; + searchmode=4; + } + else + { + F1=F0, G1=G0; + searchmode=0; + minimax=true; + } + } + } + } + } + // the conditioning stage ends here + // the searching step starts here + if (searchmode==0) + {} + else if (searchmode==1) + { + //Search mode 1: e1=e2 search + // starting with e1=e2, search down the decreasing direction of + // e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1) + // =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about + // to go out of R. + //Arguments: l1, l2, (F0, G0, vmax) + double tmp=F0+k[l1]*G0; testnn(tmp); + double e1=m[l1]*sqrt(tmp)-f[l1]; + tmp=F0+k[l2]*G0; testnn(tmp); + double e2=m[l2]*sqrt(tmp)-f[l2]; + if (fabs(e1-e2)>1e-4) + { +// int kk=1; + goto start; + } + + //find intersections of e1=e2 with other ek's + l3=-1; + loopl3: + double e=-1; + for (int l=0; l<L; l++) + { + if (l==l1 || l==l2) continue; + double lF1[2], lG1[2], lE[2]; + double lm[3]={m[l1], m[l2], m[l]}; + double lf[3]={f[l1], f[l2], f[l]}; + int lk[3]={k[l1], k[l2], k[l]}; + int r=FGFrom3(lF1, lG1, lE, lm, lf, lk); + for (int i=0; i<r; i++) + { + if (lE[i]>e && lE[i]<vmax) l3=l, e=lE[i], F1=lF1[i], G1=lG1[i]; + } + } + //l3 shall be >=0 at this point + //find intersections of e1=e2 with polygon sides + if (l3<0) + { + ///int kkk=1; + goto loopl3; + } + nc=-1; + double F2, G2; + for (int n=0; n<N; n++) + { + int nn=(n==N-1)?0:(n+1); + double xn1=F[nn]-F[n], yn1=G[nn]-G[n], xn2=F1-F[n], yn2=G1-G[n]; + if (xn1*yn2-xn2*yn1>=0) //so that (F1, G1) is on or out of side n:(n+1) + { + nc=n; + break; + } + } + + if (nc<0) //(F1, G1) being inside R + { + vmax=e; + condpos=1; + condmax=1; + //l3 shall be >=0 at this point, so l1, l2, l3 are all ok + } + else //(F1, G1) being outside nc:(nc+1) //(F2, G2) being on side nc:(nc+1) + { + //find where e1=e2 crosses a side of R between (F0, G0) and (F1, G1) + double lF2[2], lG2[2], lmd[2]; + double lm[2]={m[l1], m[l2]}; + double lf[2]={f[l1], f[l2]}; + int lk[2]={k[l1], k[l2]}; + + int ncc=-1; + while (ncc<0) + { + n1=(nc==N-1)?0:(nc+1); + double xn1=F[n1]-F[nc], yn1=G[n1]-G[nc]; + + int r=FGFrom2(lF2, lG2, lmd, F[nc], xn1, G[nc], yn1, lm, lf, lk); + + bool once=false; + for (int i=0; i<r; i++) + { + double tmp=lF2[i]+k[l1]*lG2[i]; testnn(tmp); + double le=m[l1]*sqrt(tmp)-f[l1]; + if (le<=vmax) //this shall be satisfied once + { + once=true; + if (lmd[i]>=0 && lmd[i]<=1) //so that curve e1=e2 does cross the boundry within it + ncc=nc, F2=lF2[i], G2=lG2[i], e=le; + else if (lmd[i]>1) //so that the crossing point is out of vertex n1 + nc=n1; + else //lmd[i]<0, so that the crossing point is out of vertex nc-1 + nc=(nc==0)?(N-1):(nc-1); + } + } + if (!once) + { + //int kk=0; + goto start; + } + } + nc=ncc; + + vmax=e; + condpos=2; + if (F1==F2 && G1==G2) + condmax=1; + else + condmax=2; + F1=F2, G1=G2; + } + } + else if (searchmode==2) + { + // Search mode 2 (e1!=e2 search): starting with e1=e2, and a linear equation + // constraint, search down the decreasing direction until at point (F1, G1) + // there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or at point (F1, G1) + // the search is about to go out of R. + // Arguments: l1, l2, dF, dG, (F0, G0, vmax) + + //The searching direction (dF, dG) is always along some polygon side + // If conpos==2, it is along nc:(nc+1) + // If conpos==3, it is along nc:(nc+1) or (nc-1):nc + + // + + //first check whether e1>e2 or e2>e1 down (dF, dG) + dF=F[nd]-F0, dG=G[nd]-G0; + // + //the negative gradient of e2-e1 is calculated as (gF, gG) + double gF=-(m2/tmp2-m1/tmp1)/2, + gG=-(k2*m2/tmp2-k1*m1/tmp1)/2; + if (gF*dF+gG*dG>0) //e2-e1 decrease down direction (dF, dG), so that e1>e2 + {} + else //e1<e2 down (dF, dG) + { + int ll=l2; l2=l1; l1=ll; + m1=m[l1], m2=m[l2], k1=k[l1], k2=k[l2]; + tmp=F0+k1*G0; testnn(tmp); tmp1=sqrt(tmp); + tmp=F0+k2*G0; testnn(tmp); tmp2=sqrt(tmp); + } + //now e1>e2 down (dF, dG) from (F0, G0) + tmp=F[nd]+k1*G[nd]; testnn(tmp); + double e1=m1*sqrt(tmp)-f[l1]; + tmp=F[nd]+k2*G[nd]; testnn(tmp); + double e2=m2*sqrt(tmp)-f[l2], FEx, GEx, eEx; + int maxlmd=1; + if (e1>=e2) + { + // then e1 is larger than e2 for the whole searching interval + FEx=F[nd], GEx=G[nd]; + eEx=e1; + } + else // the they swap somewhere in between, now find it and make it maxlmd + { + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[l2]}; + double lf[2]={f[l1], f[l2]}; + int lk[2]={k[l1], k[l2]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + //process the results + if (r==2) //must be so + { + if (lmd[0]<lmd[1]) lmd[0]=lmd[1], FEx=lF1[1], GEx=lG1[1]; + else FEx=lF1[0], GEx=lG1[0]; + } + if (lmd[0]>0 && lmd[0]<1) //must be so + maxlmd=lmd[0]; + tmp=FEx+k1*GEx; testnn(tmp); + eEx=m1*sqrt(tmp)-f[l1]; + } + + l3=-1; +// int l4; + double e, llmd=maxlmd; + int *tpl=new int[L], ctpl=0; + for (int l=0; l<L; l++) + { + if (l==l1 || l==l2) continue; + tmp=FEx+k[l]*GEx; testnn(tmp); + double le=m[l]*sqrt(tmp)-f[l]; + if (le<eEx) + { + tpl[ctpl]=l; + ctpl++; + continue; + } + //solve for the equation system e1(F1, G1)=el(F1, G1), with (F1, G1) on nc:n1 + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[l]}; + double lf[2]={f[l1], f[l]}; + int lk[2]={k[l1], k[l]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + //process the results + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction + if (lmd[i]<llmd) + l3=l, F1=lF1[i], G1=lG1[i], llmd=lmd[i]; + } + } + if (ctpl==L-2) //so that at (FEx, GEx) e1 is maximal, equivalent to "l3<0" + {} + else + { + //at this point F1 and G1 must have valid values already + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + //test if e1 is maximal at (F1, G1) + bool lmax=true; + for (int tp=0; tp<ctpl; tp++) + { + tmp=F1+k[tpl[tp]]*G1; testnn(tmp); + double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]]; + if (le>e) {lmax=false; break;} + } + if (!lmax) + { + for (int tp=0; tp<ctpl; tp++) + { + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[tpl[tp]]}; + double lf[2]={f[l1], f[tpl[tp]]}; + int lk[2]={k[l1], k[tpl[tp]]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + //process the results + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction + if (lmd[i]<=llmd) + l3=tpl[tp], F1=lF1[i], G1=lG1[i], llmd=lmd[i]; + } + } + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + } + } + delete[] tpl; + if (l3>=0) //the solution is found in the right direction + { + vmax=e; + if (llmd==1) //(F1, G1) is a vertex of R + { + nc=nd; + condpos=3; + condmax=2; + l2=l3; + } + else + { + if (condpos==3 && nd==n0) nc=n0; + condpos=2; + condmax=2; + l2=l3; + } + } + else if (maxlmd<1) //so that e1=e2 remains maximal at maxlmd + { + if (condpos==3 && nd==n0) nc=n0; + condpos=2; + condmax=2; + //l1, l2 remain unchanged + F1=FEx, G1=GEx; + vmax=eEx; + } + else //so that e1 remains maximal at vertex nd + { + condpos=3; + condmax=3; + nc=nd; + F1=FEx, G1=GEx; //this shall equal to F0+dF, G0+dG + vmax=eEx; + } + } + else if (searchmode==3) + { + //Search mode 3 (e1 search): starting with e1 being the only maximum, search down + // the decreasing direction of e1 until at point (F1, G1) there is another l2 so + // that e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about + // to go out of R. + // + // Arguments: l1, dF, dG, (F0, G0, vmax) + // + + //first calculate the value of lmd from (F0, G0) to get out of R + double maxlmd=-1, FEx, GEx; + double fdggdf=-F0*dG+G0*dF; + nd=-1; + for (int n=0; n<N; n++) + { + if (condpos==2 && n==nc) continue; + if ((condpos==3 && n==nc) || n==n0) continue; + int nn=(n==N-1)?0:n+1; + double xn1=F[n], xn2=F[nn], yn1=G[n], yn2=G[nn]; + double test1=xn1*dG-yn1*dF+fdggdf, + test2=xn2*dG-yn2*dF+fdggdf; + if (test1*test2<=0) + { + //the two following are equivalent. we use the larger denominator. + FEx=(xn1*test2-xn2*test1)/(test2-test1); + GEx=(yn1*test2-yn2*test1)/(test2-test1); + if (fabs(dF)>fabs(dG)) maxlmd=(FEx-F0)/dF; + else maxlmd=(GEx-G0)/dG; + nd=n; + } + } + + //maxlmd must be >0 at this point. + l2=-1; + tmp=FEx+k[l1]*GEx; testnn(tmp); + double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=maxlmd; + int *tpl=new int[L], ctpl=0; + for (int l=0; l<L; l++) + { + if (l==l1) continue; + + //if el does not catch up with e1 at (FEx, GEx), then there is no hope this + // happens in between (F0, G0) and (FEx, GEx) + tmp=FEx+k[l]*GEx; testnn(tmp); + double le=m[l]*sqrt(tmp)-f[l]; + if (le<eEx) + { + tpl[ctpl]=l; + ctpl++; + continue; + } + //now the existence of (F1, G1) is guaranteed + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[l]}; + double lf[2]={f[l1], f[l]}; + int lk[2]={k[l1], k[l]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>maxlmd) continue; + if (lmd[i]<=llmd) llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i]; + } + } + if (ctpl==L-1) //e1 is maximal at eEx, equivalent to "l2<0" + {}//l2 must equal -1 at this point + else + { + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + //test if e1 is maximal at (F1, G1) + //e1 is already maximal of those l's not in tpl[ctpl] + bool lmax=true; + for (int tp=0; tp<ctpl; tp++) + { + tmp=F1+k[tpl[tp]]*G1; testnn(tmp); + double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]]; + if (le>e) {lmax=false; break;} + } + if (!lmax) + { + for (int tp=0; tp<ctpl; tp++) + { + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[tpl[tp]]}; + double lf[2]={f[l1], f[tpl[tp]]}; + int lk[2]={k[l1], k[tpl[tp]]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>maxlmd) continue; + if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i]; + } + } + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + } + } + delete[] tpl; + + if (l2>=0) //so that e1 meets some other ek during the search + { + vmax=e; //this has to equal m[l2]*sqrt(F1+k[l2]*G1)-f[l2] + if (vmax==eEx) + { + condpos=2; + condmax=2; + nc=nd; + } + else + { + condpos=1; + condmax=2; + } + } + else //l2==-1 + { + vmax=eEx; + F1=FEx, G1=GEx; + condpos=2; + condmax=3; + nc=nd; + } + } + else //searchmode==4 + { + //Search mode 4: a special case of search mode 3 in which the boundry constraint + // is simply 0<=lmd<=1. + dF=F[nd]-F0, dG=G[nd]-G0; + l2=-1; + int *tpl=new int[L], ctpl=0; + tmp=F[nd]+k[l1]*G[nd]; testnn(tmp); + double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=1; + for (int l=0; l<L; l++) + { + if (l==l1) continue; + //if el does not catch up with e1 at (F[nd], G[nd]), then there is no hope this + // happens in between (F0, G0) and vertex nd. + tmp=F[nd]+k[l]*G[nd]; testnn(tmp); + double le=m[l]*sqrt(tmp)-f[l]; + if (le<eEx) + { + tpl[ctpl]=l; + ctpl++; + continue; + } + //now the existence of (F1, G1) is guaranteed + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[l]}; + double lf[2]={f[l1], f[l]}; + int lk[2]={k[l1], k[l]}; + loop1: + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>1) continue; + if (llmd>=lmd[i]) + { + tmp=lF1[i]+k[l1]*lG1[i]; testnn(tmp); + double e1=m[l1]*sqrt(tmp)-f[l1]; + tmp=lF1[i]+k[l]*lG1[i]; testnn(tmp); + double e2=m[l]*sqrt(tmp)-f[l]; + if (fabs(e1-e2)>1e-4) + { + //int kk=0; + goto loop1; + } + llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i]; + } + } + } + if (ctpl==N-1) //so e1 is maximal at (F[nd], G[nd]) + {} + else + { + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + //test if e1 is maximal at (F1, G1) + bool lmax=true; + for (int tp=0; tp<ctpl; tp++) + { + tmp=F1+k[tpl[tp]]*G1; testnn(tmp); + double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]]; + if (le>e) {lmax=false; break;} + } + if (!lmax) + { + for (int tp=0; tp<ctpl; tp++) + { + double lF1[2], lG1[2], lmd[2]; + double lm[2]={m[l1], m[tpl[tp]]}; + double lf[2]={f[l1], f[tpl[tp]]}; + int lk[2]={k[l1], k[tpl[tp]]}; + int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk); + for (int i=0; i<r; i++) + { + if (lmd[i]<0 || lmd[i]>1) continue; + if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i]; + } + } +// e=m[l1]*sqrt(F1+k[l1]*G1)-f[l1]; + } + } + tmp=F1+k[l1]*G1; testnn(tmp); + e=m[l1]*sqrt(tmp)-f[l1]; + delete[] tpl; + if (l2>=0) + { + vmax=e; + if (vmax==eEx) + { + condpos=3; + condmax=2; + nc=nd; + } + else + { + condpos=2; + condmax=2; + //(F1, G1) lies on side n0:nc (nd=n0) or nc:n1 (nd=nc or nd=n1) + if (nd==n0) nc=n0; + //else nc=nc; + } + } + else //l2==-1, + { + vmax=eEx; + condpos=3; + condmax=3; + F1=F[nd], G1=G[nd]; + nc=nd; // + } + } + F0=F1, G0=G1; + + if (condmax==1 && ((l1^l2)==1 || (l1^l3)==1 || (l2^l3)==1)) + minimax=true; + else if (condmax==2 && ((l1^l2)==1)) + minimax=true; + else if (vmax<1e-6) + minimax=true; + } + + free(m);//delete[] m; + delete[] vmnl; + + return vmax; +}//minimaxstiff*/ + +/* + function NMResultToAtoms: converts the note-match result hosted in a NMResults structure, i.e. + parameters of a harmonic atom, to a list of atoms. The process retrieves atom parameters from low + partials until a required number of atoms have been retrieved or a non-positive frequency is + encountered (non-positive frequencies are returned by note-match to indicate high partials for which + no informative estimates can be obtained). + + In: results: the NMResults structure hosting the harmonic atom + M: number of atoms to request from &results + t: time of this harmonic atom + wid: scale of this harmonic atom with which the note-match was performed + Out: HP[return value]: list of atoms retrieved from $result. + + Returns the number of atoms retrieved. +*/ +int NMResultToAtoms(int M, atom* HP, int t, int wid, NMResults results) +{ + double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp; + atomtype* ptype=results.ptype; + for (int i=0; i<M; i++) + { + if (fpp[i]<=0) {memset(&HP[i], 0, sizeof(atom)*(M-i)); return i;} + HP[i].t=t, HP[i].s=wid, HP[i].f=fpp[i]/wid, HP[i].a=vfpp[i]; + HP[i].p=pfpp[i]; HP[i].type=ptype[i]; HP[i].pin=i+1; + } + return M; +}//NMResultToAtoms + +/* + function NMResultToPartials: reads atoms of a harmonic atom in a NMResult structure into HS partials. + + In: results: the NMResults structure hosting the harmonic atom + M: number of atoms to request from &results + fr: frame index of this harmonic atom + t: time of this harmonic atom + wid: scale of this harmonic atom with which the note-match was performed + Out: Partials[M][]: HS partials whose fr-th frame is updated to the harmonic partial read from $results + + Returns the number of partials retrieved. +*/ +int NMResultToPartials(int M, int fr, atom** Partials, int t, int wid, NMResults results) +{ + double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp; + atomtype* ptype=results.ptype; + for (int i=0; i<M; i++) + { + atom* part=&Partials[i][fr]; + if (fpp[i]<=0) {for (int j=i; j<M; j++) memset(&Partials[j][fr], 0, sizeof(atom)); return i;} + part->t=t, part->s=wid, part->f=fpp[i]/wid, part->a=vfpp[i]; + part->p=pfpp[i]; part->type=ptype[i]; part->pin=i+1; + } + return M; +}//NMResultToPartials + +/* + function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic + sinusoid from spectrogram if Fr>1. + + In: x[Fr][N/2+1]: spectrogram + fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes + N, offst: atom scale and hop size + R: initial F-G polygon constraint, optional + settings: note match settings + computes: pointer to a function that computes HA score, must be ds0 or ds1 + lastvfp[lastp]: amplitude of the previous harmonic atom + forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned") + Out: results: note match results + f0, B: fundamental frequency and stiffness coefficient + R: F-G polygon of harmonic atom + + Returns the total energy of HA or constant-pitch HS. +*/ +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps, + int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp, + double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr) +{ + double result=0; + + double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0, + delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2, + *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf, + int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr, + pcount=settings->pcount, M=settings->M; + +// int *ptype=results->ptype; + + //calculate the energy of the last frame (hp) + double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i]; + //fill frequency and amplitude buffers with 0 + //now determine numsam (the maximal number of partials) + bool inputpartial=(f0>0 && settings->pin0>0); + if (!inputpartial) + { + if (pcount>0) f0=pinf[0], pin0=pin[0]; + else f0=sqrt(R->X[0]), pin0=1; + } + int numsam=N*pin0/2.1/f0; + if (numsam>maxp) numsam=maxp; + //allocate buffer for candidate atoms + TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands); + //pcs[p] records the number of candidates for partial index p + //psb[p] = 1: anchor, 2: user input, 0: normal + int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1]; + memset(psb, 0, sizeof(int)*(numsam+1)*2); + //if R is not specified, initialize a trivial candidate with maxB + if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB); + //if R is, initialize a trivial candidate by extending R by delp + else cands[0][0]=new TTempAtom(R, delp, delp, minf0); + + int pm=0, pM=0; + int ind=1; pcs[0]=1; + //anchor partials: highest priority atoms, must have spectral peaks + for (int i=0; i<pcount; i++) + { + //skip out-of-scope atoms + if (pin[i]>=numsam) continue; + //skip user input atom + if (inputpartial && pin[i]==pin0) continue; + + void* dsparams; + if (computes==ds0) dsparams=&cands[ind-1][0]->s; + else + { + dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp}; + dsparams=¶ms; + } + + if (pinfr[i]>0) //local anchor + { + double ls=0; + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB); + ls+=~r; + } + ls=sqrt(ls/Fr); + cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm, ls, computes(ls, dsparams)); + } + else + { + //find the nearest peak to pinf[i] + while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--; + if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++; + cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm, vps[pm], computes(vps[pm], dsparams)); + } + delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++; + } + //harmonic atom grouping fails if anchors conflict + if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;} + + //user input partial: must have spectral peak + if (inputpartial) + { + //harmonic atom grouping fails if user partial is out of scope + if (pin0>numsam){f0=0; goto cleanup;} + + TTempAtom* lcand=cands[ind-1][0]; + //feasible frequency range for partial pin0 + double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y); + f1-=delm, f2+=delm; + if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1; + //forcepin0 being true means this partial have to be the peak nearest to f0 + bool inputfound=false; + if (forceinputlocalfr>=0) + { + int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1; + + cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12]; + for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB); + for (int k=1; k<k12-1; k++) + if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1]) + { + if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k; + } + if (pk>0) + { + double lf=k1+pk, la, lph, oldlf=lf; + LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf); + if (fabs(lf-oldlf)>0.6) + { + lf=oldlf; + cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB); + la=abs(r); lph=arg(r); + } + if (lf>f1 && lf<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm, la, computes(la, dsparams)); + pcs[ind]=1; + inputfound=true; + } + } + } + if (!inputfound && settings->forcepin0) + { //find the nearest peak to f0 + while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--; + if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++; + if (fps[pm]>f1 && fps[pm]<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm, vps[pm], computes(vps[pm], dsparams)); + pcs[ind]=1; + } + } + else if (!inputfound) + { + double epf0=settings->epf0; + //lpcs records number of candidates found for partial pin0 + int lpcs=0; + //lcoate peaks with the feasible frequency range, specified by f0 and epf0 + while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++; + while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--; + //add peaks as candidates + for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm, vps[j], computes(vps[j], dsparams)); + lpcs++; + } + pcs[ind]=lpcs; + } + //harmonic atom grouping fails if user partial is not found + if (pcs[ind]==0){f0=0; goto cleanup;} + else {delete lcand->R; lcand->R=0;} + psb[pin0]=2; ind++; + } + //normal partials (non-anchor, non-user) + //p: partial index + { + int p=0; + while (ind<=numsam) + { + p++; + //skip anchor or user input partials + if (psb[p]) continue; + //loop over all candidates + for (int i=0; i<pcs[ind-1]; i++) + { + //the ith candidate of last partial + TTempAtom* lcand=cands[ind-1][i]; + //lpcs records number of candidates found for partial p + int lpcs=0; + //the feasible frequency range for partial p + double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak + f1-=delm, f2+=delm; //allow a frequency error for the new partial + if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1; + //locate peaks within this range + while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++; + while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks + //add peaks as candidates + for (int j=pm; j<=pM; j++) + { + if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], computes(vps[j], dsparams)); + lpcs++; + if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;} + } + } + //if there is no peak found for partial p + if (lpcs==0) + { + cands[ind][pcs[ind]+lpcs]=lcand; lpcs++; + cands[ind-1][i]=0; + if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;} + } + else{delete lcand->R; lcand->R=0;} + + pcs[ind]+=lpcs; + } + ind++; + } + + //select the candidate with maximal s + int maxs=0; double vmax=cands[ind-1][0]->s; + for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s; + TTempAtom* lcand=cands[ind-1][maxs]; + + //get results + //read frequencies of found partials + int P=0; int* ps=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B + while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; P++;} lcand=lcand->Prev;} + //read R of candidate with maximal s as R0 + TPolygon* R0=cands[ind-1][maxs]->R; + + if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, 0, x[0], N, psb, numsam, settings, results); + else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, 0, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr); + + delete[] ps; delete[] fs; + } +cleanup: + for (int i=0; i<=numsam; i++) + for (int j=0; j<pcs[i]; j++) + delete cands[i][j]; + DeAlloc2(cands); + delete[] psb; + + return result; +}//NoteMatchStiff3 + +/* + function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic + sinusoid from spectrogram if Fr>1. This version uses residue-sinusoid ratio ("rsr") that measures how + "good" a spectral peak is in terms of its shape. peaks with large rsr[] is likely to be contaminated + and their frequency estimates are regarded unreliable. + + In: x[Fr][N/2+1]: spectrogram + N, offst: atom scale and hop size + fps[pc], vps[pc], rsr[pc]: primitive (rough) peak frequencies, amplitudes and shape factor + R: initial F-G polygon constraint, optional + settings: note match settings + computes: pointer to a function that computes HA score, must be ds0 or ds1 + lastvfp[lastp]: amplitude of the previous harmonic atom + forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned") + Out: results: note match results + f0, B: fundamental frequency and stiffness coefficient + R: F-G polygon of harmonic atom + + Returns the total energy of HA or constant-pitch HS. +*/ +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps, + double* rsr, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, + int lastp, double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr) +{ + double result=0; + + double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0, + delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2, + *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf, + int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr, + pcount=settings->pcount, M=settings->M; + +// int *ptype=results->ptype; + + //calculate the energy of the last frame (hp) + double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i]; + //fill frequency and amplitude buffers with 0 + //now determine numsam (the maximal number of partials) + bool inputpartial=(f0>0 && settings->pin0>0); + if (!inputpartial) + { + if (pcount>0) f0=pinf[0], pin0=pin[0]; + else f0=sqrt(R->X[0]), pin0=1; + } + int numsam=N*pin0/2.1/f0; + if (numsam>maxp) numsam=maxp; + //allocate buffer for candidate atoms + TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands); + //pcs[p] records the number of candidates for partial index p + //psb[p] = 1: anchor, 2: user input, 0: normal + int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1]; + memset(psb, 0, sizeof(int)*(numsam+1)*2); + //if R is not specified, initialize a trivial candidate with maxB + if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB); + //if R is, initialize a trivial candidate by extending R by delp + else cands[0][0]=new TTempAtom(R, delp, delp, minf0); + + int pm=0, pM=0; + int ind=1; pcs[0]=1; + //anchor partials: highest priority atoms, must have spectral peaks + for (int i=0; i<pcount; i++) + { + //skip out-of-scope atoms + if (pin[i]>=numsam) continue; + //skip user input atom + if (inputpartial && pin[i]==pin0) continue; + + void* dsparams; + if (computes==ds0) dsparams=&cands[ind-1][0]->s; + else + { + dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp}; + dsparams=¶ms; + } + + if (pinfr[i]>0) //local anchor + { + double ls=0, lrsr=PeakShapeC(pinf[i], 1, N, &x[pinfr[i]], hB*2, M, c, iH2); + for (int fr=0; fr<Fr; fr++) + { + cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB); + ls+=~r; + } + ls=sqrt(ls/Fr); + cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm+lrsr, ls, computes(ls, dsparams)); cands[ind][0]->rsr=lrsr; + } + else + { + //find the nearest peak to pinf[i] + while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--; + if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++; + cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm]; + } + delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++; + } + //harmonic atom grouping fails if anchors conflict + if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;} + + //user input partial: must have spectral peak + if (inputpartial) + { + //harmonic atom grouping fails if user partial is out of scope + if (pin0>numsam){f0=0; goto cleanup;} + + TTempAtom* lcand=cands[ind-1][0]; + //feasible frequency range for partial pin0 + double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y); + f1-=delm, f2+=delm; + if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1; + bool inputfound=false; + if (forceinputlocalfr>=0) + { + int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1; + + cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12]; + for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB); + for (int k=1; k<k12-1; k++) + if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1]) + { + if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k; + } + if (pk>0) + { + double lf=k1+pk, la, lph, oldlf=lf; + LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf); + if (fabs(lf-oldlf)>0.6) //by which we judge that the LS result is biased by interference + { + lf=oldlf; + cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB); + la=abs(r); lph=arg(r); + } + if (lf>f1 && lf<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + double lrsr=PeakShapeC(lf, 1, N, &x[forceinputlocalfr], hB*2, M, c, iH2); + cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm+lrsr, la, computes(la, dsparams)); cands[ind][0]->rsr=lrsr; + pcs[ind]=1; + inputfound=true; + } + } + } + //forcepin0 being true means this partial have to be the peak nearest to f0 + if (!inputfound && settings->forcepin0) + { //find the nearest peak to f0 + while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--; + if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++; + if (fps[pm]>f1 && fps[pm]<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm]; + pcs[ind]=1; + } + } + else if (!inputfound) + { + double epf0=settings->epf0; + //lpcs records number of candidates found for partial pin0 + int lpcs=0; + //lcoate peaks with the feasible frequency range, specified by f0 and epf0 + while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++; + while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--; + //add peaks as candidates + for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2) + { + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][lpcs]->rsr=rsr[j]; + lpcs++; + } + pcs[ind]=lpcs; + } + //harmonic atom grouping fails if user partial is not found + if (pcs[ind]==0){f0=0; goto cleanup;} + else {delete lcand->R; lcand->R=0;} + psb[pin0]=2; ind++; + } + //normal partials (non-anchor, non-user) + //p: partial index + { + int p=0; + while (ind<=numsam) + { + p++; + //skip anchor or user input partials + if (psb[p]) continue; + //loop over all candidates + for (int i=0; i<pcs[ind-1]; i++) + { + int tightpks=0; + //the ith candidate of last partial + TTempAtom* lcand=cands[ind-1][i]; + //lpcs records number of candidates found for partial p + int lpcs=0; + //the feasible frequency range for partial p + double tightf1, tightf2; ExFmStiff(tightf1, tightf2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak + double tightf=(tightf1+tightf2)*0.5, f1=tightf1-delm, f2=tightf2+delm; //allow a frequency error for the new partial + if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1; + //locate peaks within this range + while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++; + while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks + //add peaks as candidates + for (int j=pm; j<=pM; j++) + { + if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range + { + if ((fps[j]>tightf1 && fps[j]<tightf2) || fabs(fps[j]-tightf)<0.5) tightpks++; //indicates there are "tight" peaks found for this partial + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][pcs[ind]+lpcs]->rsr=rsr[j]; + lpcs++; + if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;} + } + } + //if there is no peak found for partial p. + if (!lpcs) + { + //use the lcand directly + //BUT UPDATE accumulative energy (acce) and s using induced partial + if (tightf<N/2-hB) + { + double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr); + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + double ls=computes(la, dsparams); + + lcand->acce+=la*la; + lcand->s=ls; + } + cands[ind][pcs[ind]+lpcs]=lcand; + lpcs++; + cands[ind-1][i]=0; + if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;} + } + //if there are peaks but no tight peak found for partial p + else if (!tightpks) + { + if (tightf<N/2-hB) + { + double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr); + void* dsparams; + if (computes==ds0) dsparams=&lcand->s; + else + { + dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp}; + dsparams=¶ms; + } + double ls=computes(la, dsparams); + TTempAtom* lnewcand=new TTempAtom(lcand, true); + lnewcand->f=0; lnewcand->acce+=la*la; lnewcand->s=ls; + cands[ind][pcs[ind]+lpcs]=lnewcand; + lpcs++; + if (pcs[ind]+lpcs>=MAX_CAND) + {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;} + } + delete lcand->R; lcand->R=0; + } + else{delete lcand->R; lcand->R=0;} + + pcs[ind]+=lpcs; + } + ind++; + } + + //select the candidate with maximal s + int maxs=0; double vmax=cands[ind-1][0]->s; + for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s; + TTempAtom* lcand=cands[ind-1][maxs]; + + //get results + //read frequencies of found partials + int P=0; int* ps=new int[maxp]; double* fs=new double[maxp*2], *rsrs=&fs[maxp]; //these are used for evaluating f0 and B + while (lcand){if (lcand->pind && lcand->f) {ps[P]=lcand->pind; fs[P]=lcand->f; rsrs[P]=lcand->rsr; P++;} lcand=lcand->Prev;} + //read R of candidate with maximal s as R0 + TPolygon* R0=cands[ind-1][maxs]->R; + + if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, rsrs, x[0], N, psb, numsam, settings, results); + else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, rsrs, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr); + + delete[] ps; delete[] fs; + } +cleanup: + for (int i=0; i<=numsam; i++) + for (int j=0; j<pcs[i]; j++) + delete cands[i][j]; + DeAlloc2(cands); + delete[] psb; + + return result; +}//NoteMatchStiff3 + +/* + function NoteMatchStiff3: wrapper function of the above that does peak picking and HA grouping (or + constant-pitch HS finding) in a single call. + + In: x[Fr][N/2+1]: spectrogram + N, offst: atom scale and hop size + R: initial F-G polygon constraint, optional + startfr, validfrrange: centre and half width, in frames, of the interval used for peak picking if Fr>1 + settings: note match settings + computes: pointer to a function that computes HA score, must be ds0 or ds1 + lastvfp[lastp]: amplitude of the previous harmonic atom + forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned") + Out: results: note match results + f0, B: fundamental frequency and stiffness coefficient + R: F-G polygon of harmonic atom + + Returns the total energy of HA or constant-pitch HS. +*/ +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int Fr, cdouble** x, int N, int offst, NMSettings* settings, + NMResults* results, int lastp, double* lastvfp, double (*computes)(double a, void* params), + bool forceinputlocalfr, int startfr, int validfrrange) +{ + //find spectral peaks + double *fps=(double*)malloc8(sizeof(double)*N*2*Fr), *vps=&fps[N/2*Fr], *rsr=&fps[N*Fr]; + int pc; + if (Fr>1) pc=QuickPeaks(fps, vps, Fr, N, x, startfr, validfrrange, settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr); + else pc=QuickPeaks(fps, vps, N, x[0], settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr); + //if no peaks are present, return 0, indicating empty hp + if (pc==0){free8(fps); return 0;} + + double result=NoteMatchStiff3(R, f0, B, pc, fps, vps, rsr, Fr, x, N, offst, settings, results, lastp, lastvfp, computes, forceinputlocalfr?startfr:-1); + free8(fps); + return result; +}//NoteMatchStiff3 + +/* + function NoteMatchStiff3: finds harmonic atom from spectrum given the pitch as a (partial index, + frequency) pair. This is used internally by NoteMatch3FB(). + + In: x[N/2+1]: spectrum + fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes + R: initial F-G polygon constraint, optional + pin0: partial index in the pitch specifier pair + peak0: index to frequency in fps[] in the pitch specifier pair + settings: note match settings + Out: vfp[]: partial amplitudes + R: F-G polygon of harmonic atom + + Returns the total energy of HA. +*/ +double NoteMatchStiff3(TPolygon* R, int peak0, int pin0, cdouble* x, int pc, double* fps, double* vps, int N, + NMSettings* settings, double* vfp, int** pitchind, int newpc) +{ + double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0, + delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2, + hB=settings->hB; + int maxp=settings->maxp, M=settings->M; // pcount=settings->pcount; + + double f0=fps[peak0]; + + //determine numsam + int numsam=N*pin0/2.1/f0; if (numsam>maxp) numsam=maxp; + if (pin0>=numsam) return 0; + //pcs[p] contains the number of candidates for partial index p + TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands); + int *pcs=new int[numsam+1]; memset(pcs, 0, sizeof(int)*(numsam+1)); + if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB); //initialization: trivial candidate with maxB + else cands[0][0]=new TTempAtom(R, delp, delp, minf0); //initialization: trivial candidate by expanding R + + cands[1][0]=new TTempAtom(cands[0][0], pin0, fps[peak0], delm, vps[peak0], vps[peak0]); cands[1][0]->tag[0]=peak0; + delete cands[0][0]->R; cands[0][0]->R=0; + + int pm=0, pM=0, ind=2, p=0; pcs[0]=pcs[1]=1; + + while (ind<=numsam) + { + p++; + if (p==pin0) continue; + for (int i=0; i<pcs[ind-1]; i++) + { + TTempAtom* lcand=cands[ind-1][i]; //the the ith candidate of last partial + int lpcs=0; + { + double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak + f1-=delm, f2+=delm; //allow a frequency error for the new partial + if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1; + while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++; + while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks + for (int j=pm; j<=pM; j++) + { + if (fps[j]>f1 && fps[j]<f2 && (p>pin0 || pitchind[j][p]<0)) + { + //this peak frequency falls in the frequency range + cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], lcand->s+vps[j]); cands[ind][pcs[ind]+lpcs]->tag[0]=j; lpcs++; + if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;} + } + } + if (lpcs==0) //there is no peak found for the partial i, the last level + { + cands[ind][pcs[ind]+lpcs]=lcand; lpcs++; //new TTempAtom(lcand, false); + cands[ind-1][i]=0; + if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;} + } + else + { + delete lcand->R; lcand->R=0; + } + } + pcs[ind]+=lpcs; + } + ind++; + } + + //select the candidate with maximal s + int maxs=0; double vmax=cands[ind-1][0]->s; + for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s; + TTempAtom* lcand=cands[ind-1][maxs]; + + //get fp, vfp, pfp, ptype + memset(vfp, 0, sizeof(double)*maxp); + + int P=0; int *ps=new int[maxp], *peaks=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B + + while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; peaks[P]=lcand->tag[0]; P++;} lcand=lcand->Prev;} + R->N=0; + + if (P>0) + { + for (int i=P-1; i>=0; i--) + { + int lp=ps[i]; + double lf=fs[i]; + vfp[lp-1]=vps[peaks[i]]; + if (R->N==0) InitializeR(R, lp, lf, delm, maxB); + else if (vfp[lp-1]>1) CutR(R, lp, lf, delm, true); + if (pitchind[peaks[i]][lp]>=0) {R->N=0; goto cleanup;} + else pitchind[peaks[i]][lp]=newpc; + } + + //estimate f0 and B + double tmpa, cF, cG; +// double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1; + areaandcentroid(tmpa, cF, cG, R->N, R->X, R->Y); + testnn(cF); f0=sqrt(cF); //B=cG/cF; + + + for (int i=0; i<numsam; i++) + { + if (vfp[i]==0) //no peak is found for this partial in lcand + { + int m=i+1; + double tmp=cF+(m*m-1)*cG; testnn(tmp); + double lf=m*sqrt(tmp); + if (lf<N/2.1) + { + cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB); + vfp[i]=-sqrt(r.x*r.x+r.y*r.y); +// vfp[i]=-IPWindowC(lf, xr, xi, N, M, c, iH2, lf-hB, lf+hB, 0); + } + } + } + } + +cleanup: + delete[] ps; delete[] fs; delete[] peaks; + for (int i=0; i<=numsam; i++) for (int j=0; j<pcs[i]; j++) delete cands[i][j]; + DeAlloc2(cands); delete[] pcs; + + double result=0; + if (f0>0) for (int i=0; i<numsam; i++) result+=vfp[i]*vfp[i]; + return result; +}//NoteMatchStiff3*/ + +/* + function NoteMatchStiff3FB: does one dynamic programming step in forward-background pitch tracking of + harmonic sinusoids. This is used internally by FindNoteFB(). + + In: R[pitchcount]: initial F-G polygons associated with pitch candidates at last frame + vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at last frame + sc[pitchcount]: accumulated scores associated with pitch candidates at last frame + fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes at this frame + maxpitch: maximal number of pitch candidates + x[wid/2+1]: spectrum + settings: note match settings + Out: pitchcount: number of pitch candidiate at this frame + newpitches[pitchcount]: pitch candidates at this frame, whose lower word is peak index, higher word is partial index + prev[pitchcount]: indices to predecessors of the pitch candidates at this frame + R[pitchcount]: F-G polygons associated with pitch candidates at this frame + vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at this frame + sc[pitchcount]: accumulated scores associated with pitch candidates at this frame + + No return value. +*/ +void NoteMatchStiff3FB(int& pitchcount, TPolygon**& R, double**& vfp, double*& sc, int* newpitches, int* prev, int pc, double* fps, double* vps, cdouble* x, int wid, int maxpitch, NMSettings* settings) +{ + int maxpin0=6; //this specifies the maximal value that a pitch candidate may have as partial index + double minf0=settings->minf0, delm=settings->delm, delp=settings->delp; + int maxp=settings->maxp; + + //pc is the number of peaks at this frame, maxp is the maximal number of partials in the model + //pitchind is initialized to a sequence of -1 + int** Allocate2(int, pc, maxp+1, pitchind); memset(pitchind[0], 0xff, sizeof(int)*pc*(maxp+1)); + + //extend F-G polygons to allow pitch variation + for (int i=0; i<pitchcount; i++) ExtendR(R[i], delp, delp, minf0); + + double *f1=new double[pitchcount], *f2=new double[pitchcount]; + int pm=0, newpc=0; + TPolygon** newR=new TPolygon*[maxpitch]; memset(newR, 0, sizeof(TPolygon*)*maxpitch); + double* newsc=new double[maxpitch]; memset(newsc, 0, sizeof(double)*maxpitch); + double** Allocate2(double, maxpitch, maxp, newvfp); + + for (int pin0=1; pin0<=maxpin0; pin0++) //pin0: the partial index of a candidate pitch + { + //find out the range [pm, pM) of indices into fps[], so that for a * in this range (pin0, fps[*]) may fall + //in the feasible pitch range succeeding one of the candidate pitches of the previous frame + for (int i=0; i<pitchcount; i++) {ExFmStiff(f1[i], f2[i], pin0, R[i]->N, R[i]->X, R[i]->Y); f1[i]-=delm, f2[i]+=delm;} + double f1a=f1[0], f2a=f2[0]; for (int i=1; i<pitchcount; i++) {if (f1a>f1[i]) f1a=f1[i]; if (f2a<f2[i]) f2a=f2[i];} + while (pm<pc-1 && fps[pm]<f1a) pm++; + int pM=pm; while (pM<pc-1 && fps[pM]<f2a) pM++; + + for (int p=pm; p<pM; p++) //loop through all peaks in this range + { + if (pitchind[p][pin0]>=0) continue; //skip this peak if it is already ... + int max; double maxs; TPolygon* lnewR=0; + + for (int i=0; i<pitchcount; i++) //loop through candidate pitches of the previous frame + { + if (fps[p]>f1[i] && fps[p]<f2[i]) //so that this peak is a feasible successor of the i-th candidate pitch of the previous frame + { + if (pitchind[p][pin0]<0) //if this peak has not been registered as a candidate pitch with pin0 being the partial index + { + lnewR=new TPolygon(maxp*2+4, R[i]); //create a F-G polygon for (this peak, pin0) pair + if (newpc==maxpitch) //maximal number of candidate pitches is reached + { + //delete the candidate with the lowest score without checking if this lowest score is below the score + //of the current pitch candidate (which is not yet computed). of course there is a risk that the new + //score is even lower so that $maxpitch buffered scores may not be the highest $maxpitch scores, but + //the (maxpitch-1) highest of the $maxpitch buffered scores should be the highest (maxpitch01) scores. + int minsc=0; for (int j=0; j<maxpitch; j++) if (newsc[j]<newsc[minsc]) minsc=j; + delete newR[minsc]; + if (minsc!=newpc-1) {newR[minsc]=newR[newpc-1]; memcpy(newvfp[minsc], newvfp[newpc-1], sizeof(double)*maxp); newsc[minsc]=newsc[newpc-1]; prev[minsc]=prev[newpc-1]; newpitches[minsc]=newpitches[newpc-1];} + } + else newpc++; + //try to find harmonic atom with this candidate pitch + if (NoteMatchStiff3(lnewR, p, pin0, x, pc, fps, vps, wid, settings, newvfp[newpc-1], pitchind, newpc-1)>0) + {//if successful, buffer its F-G polygon and save its score + newR[newpc-1]=lnewR; + max=i; maxs=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]); + } + else + {//if not, discard this candidate pitch + delete lnewR; lnewR=0; newpc--; + } + } + else //if this peak has already been registered as a candidate pitch with pin0 being the partial index + { + //compute it score as successor to the i-th candidate of the previous frame + double ls=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]); + //if the score is higher than mark the i-th candidate of previous frame as its predecessor + if (ls>maxs) maxs=ls, max=i; + } + } + } + if (lnewR) //i.e. a HA is found for this pitch candidate + { + ((__int16*)&newpitches[newpc-1])[0]=p; ((__int16*)&newpitches[newpc-1])[1]=pin0; + newsc[newpc-1]=maxs; //take note of its score + prev[newpc-1]=max; //take note of its predecessor + } + } + } + DeAlloc2(pitchind); + delete[] f1; delete[] f2; + + for (int i=0; i<pitchcount; i++) delete R[i]; delete[] R; R=newR; + for (int i=0; i<newpc; i++) for (int j=0; j<maxp; j++) if (newvfp[i][j]<0) newvfp[i][j]=-newvfp[i][j]; + DeAlloc2(vfp); vfp=newvfp; + delete[] sc; sc=newsc; + pitchcount=newpc; +}//NoteMatchStiff3FB + +/* + function PeakShapeC: residue-sinusoid-ratio for a given (hypothesis) sinusoid frequency + + In: x[Fr][N/2+1]: spectrogram + M, c[], iH2: cosine-family window specifiers + f: reference frequency, in bins + B: spectral truncation width + + Returns the residue-sinusoid-ratio. +*/ +double PeakShapeC(double f, int Fr, int N, cdouble** x, int B, int M, double* c, double iH2) +{ + cdouble* w=new cdouble[B]; + int fst=floor(f+1-B/2.0); + if (fst<0) fst=0; + if (fst+B>N/2) fst=N/2-B; + Window(w, f, N, M, c, fst, fst+B-1); + cdouble xx=0, ww=Inner(B, w, w); + double xw2=0; + for (int fr=0; fr<Fr; fr++) + xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]); + delete[] w; + if (xx.x==0) return 1; + return 1-xw2/(xx.x*ww.x); +}//PeakShapeC +//version using cmplx<float> as spectrogram data type +double PeakShapeC(double f, int Fr, int N, cfloat** x, int B, int M, double* c, double iH2) +{ + cdouble* w=new cdouble[B]; + int fst=floor(f+1-B/2.0); + if (fst<0) fst=0; + if (fst+B>N/2) fst=N/2-B; + Window(w, f, N, M, c, fst, fst+B-1); + cdouble xx=0, ww=Inner(B, w, w); + double xw2=0; + for (int fr=0; fr<Fr; fr++) + xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]); + delete[] w; + if (xx.x==0) return 1; + return 1-xw2/(xx.x*ww.x); +}//PeakShapeC + +/* + function QuickPeaks: finds rough peaks in the spectrum (peak picking) + + In: x[N/2+1]: spectrum + M, c[], iH2: cosine-family window function specifiers + mina: minimal amplitude to spot a spectral peak + [binst, binen): frequency range, in bins, to look for peaks + B: spectral truncation width + Out; f[return value], a[return value]: frequencies (in bins) and amplitudes of found peaks + rsr[return value]: residue-sinusoid-ratio of found peaks, optional + + Returns the number of peaks found. f[] and a[] must be allocated enough space before calling. +*/ +int QuickPeaks(double* f, double* a, int N, cdouble* x, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr) +{ + double hB=B*0.5; + if (binst<2) binst=2; + if (binst<hB) binst=hB; + if (binen<0) binen=N/2-1; + if (binen<0 || binen+hB>N/2-1) binen=N/2-1-hB; + double a0=~x[binst-1], a1=~x[binst], a2; + double minA=mina*mina*2/iH2; + int p=0, n=binst; + cdouble* w=new cdouble[B]; + while (n<binen) + { + a2=~x[n+1]; + if (a1>0 && a1>=a0 && a1>=a2) + { + if (a1>minA) + { + double A0=sqrt(a0), A1=sqrt(a1), A2=sqrt(a2); + f[p]=n+(A0-A2)/2/(A0+A2-2*A1); + int fst=floor(f[p]+1-hB); + Window(w, f[p], N, M, c, fst, fst+B-1); + double xw2=~Inner(B, &x[fst], w); + a[p]=sqrt(xw2)*iH2; + if (rsr) + { + cdouble xx=Inner(B, &x[fst], &x[fst]); + if (xx.x==0) rsr[p]=1; + else rsr[p]=1-xw2/xx.x*iH2; + } + p++; + } + } + a0=a1; + a1=a2; + n++; + } + delete[] w; + return p; +}//QuickPeaks + +/* + function QuickPeaks: finds rough peaks in the spectrogram (peak picking) for constant-frequency + sinusoids + + In: x[Fr][N/2+1]: spectrogram + fr0, r0: centre and half width of interval (in frames) to use for peak picking + M, c[], iH2: cosine-family window function specifiers + mina: minimal amplitude to spot a spectral peak + [binst, binen): frequency range, in bins, to look for peaks + B: spectral truncation width + Out; f[return value], a[return value]: frequencies (in bins) and summary amplitudes of found peaks + rsr[return value]: residue-sinusoid-ratio of found peaks, optional + + Returns the number of peaks found. f[] and a[] must be allocated enough space before calling. +*/ +int QuickPeaks(double* f, double* a, int Fr, int N, cdouble** x, int fr0, int r0, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr) +{ + double hB=B*0.5; + int hN=N/2; + if (binst<hB) binst=hB; + if (binen<0 || binen>hN-hB) binen=hN-hB; + double minA=mina*mina*2/iH2; + + double *a0s=new double[hN*3*r0], *a1s=&a0s[hN*r0], *a2s=&a0s[hN*2*r0]; + int *idx=new int[hN*r0], *frc=new int[hN*r0]; + int** Allocate2(int, (hN*r0), (r0*2+1), frs); + + int pc=0; + + int lr=0; + while (lr<=r0) + { + int fr[2]; //indices to frames to process for this lr + if (lr==0) fr[0]=fr0, fr[1]=-1; + else + { + fr[0]=fr0-lr, fr[1]=fr0+lr; + if (fr[1]>=Fr) fr[1]=-1; + if (fr[0]<0) {fr[0]=fr[1]; fr[1]=-1;} + } + for (int i=0; i<2; i++) + { + if (fr[i]>=0) + { + cdouble* xfr=x[fr[i]]; + for (int k=binst; k<binen; k++) + { + double a0=~xfr[k-1], a1=~xfr[k], a2=~xfr[k+1]; + if (a1>=a0 && a1>=a2) + { + if (a1>minA) + { + double A1=sqrt(a1), A2=sqrt(a2), A0=sqrt(a0); + double lf=k+(A0-A2)/2/(A0+A2-2*A1); + if (lr==0) //starting frame + { + f[pc]=lf; + a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2; + idx[pc]=pc; frs[pc][0]=fr[i]; frc[pc]=1; + pc++; + } + else + { + int closep, indexp=InsertInc(lf, f, pc, false); //find the closest peak ever found in previous (i.e. closer to fr0) frames + if (indexp==-1) indexp=pc, closep=pc-1; + else if (indexp-1>=0 && fabs(lf-f[indexp-1])<fabs(lf-f[indexp])) closep=indexp-1; + else closep=indexp; + + if (fabs(lf-f[closep])<0.2) //i.e. lf is very close to previous peak at fs[closep] + { + int idxp=idx[closep]; + a0s[idxp]+=a0, a1s[idxp]+=a1, a2s[idxp]+=a2; + frs[idxp][frc[idxp]]=fr[i]; frc[idxp]++; + double A0=sqrt(a0s[idxp]), A1=sqrt(a1s[idxp]), A2=sqrt(a2s[idxp]); + f[closep]=k+(A0-A2)/2/(A0+A2-2*A1); + } + else + { + memmove(&f[indexp+1], &f[indexp], sizeof(double)*(pc-indexp)); f[indexp]=lf; + memmove(&idx[indexp+1], &idx[indexp], sizeof(int)*(pc-indexp)); idx[indexp]=pc; + a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2, frs[pc][0]=fr[i], frc[pc]=1; + pc++; + } + } + } + } + } + } + } + lr++; + } + + cdouble* w=new cdouble[B]; + int* frused=new int[Fr]; + for (int p=0; p<pc; p++) + { + int fst=floor(f[p]+1-hB); + memset(frused, 0, sizeof(int)*Fr); + int idxp=idx[p]; + + Window(w, f[p], N, M, c, fst, fst+B-1); + double xw2=0, xw2r=0, xxr=0; + + for (int ifr=0; ifr<frc[idxp]; ifr++) + { + int fr=frs[idxp][ifr]; + frused[fr]=1; + xw2r+=~Inner(B, &x[fr][fst], w); + xxr+=Inner(B, &x[fr][fst], &x[fr][fst]).x; + } + xw2=xw2r; + for (int fr=0; fr<Fr; fr++) + if (!frused[fr]) xw2+=~Inner(B, &x[fr][fst], w); + a[p]=sqrt(xw2/Fr)*iH2; + if (xxr==0) rsr[p]=1; + else rsr[p]=1-xw2r/xxr*iH2; + } + delete[] w; + delete[] frused; + delete[] a0s; + delete[] idx; + delete[] frc; + DeAlloc2(frs); + return pc; +}//QuickPeaks + +/* + function ReEstHS1: reestimates a harmonic sinusoid by one multiplicative reestimation using phasor + multiplier + + In: partials[M][Fr]: HS partials + [frst, fren): frame (measurement point) range on which to do reestimation + Data16[]: waveform data + Out: partials[M][Fr]: updated HS partials + + No return value. +*/ +void ReEstHS1(int M, int Fr, int frst, int fren, atom** partials, __int16* Data16) +{ + double* fs=new double[Fr*3], *as=&fs[Fr], *phs=&fs[Fr*2]; + int wid=partials[0][0].s, offst=partials[0][1].t-partials[0][0].t; + int ldatastart=partials[0][0].t-wid/2; + __int16* ldata16=&Data16[ldatastart]; + for (int m=0; m<M; m++) + { + for (int fr=0; fr<Fr; fr++) {fs[fr]=partials[m][fr].f; if (fs[fr]<=0){delete[] fs; return;} as[fr]=partials[m][fr].a, phs[fr]=partials[m][fr].p;} + MultiplicativeUpdateF(fs, as, phs, ldata16, Fr, frst, fren, wid, offst); + for (int fr=0; fr<Fr; fr++) partials[m][fr].f=fs[fr], partials[m][fr].a=as[fr], partials[m][fr].p=phs[fr]; + } + delete[] fs; +}//ReEstHS1 + +/* + function ReEstHS1: wrapper function. + + In: HS: a harmonic sinusoid + Data16: its waveform data + Out: HS: updated harmonic sinusoid + + No return value. +*/ +void ReEstHS1(THS* HS, __int16* Data16) +{ + ReEstHS1(HS->M, HS->Fr, 0, HS->Fr, HS->Partials, Data16); +}//ReEstHS1 + +/* + function SortCandid: inserts a candid object into a listed of candid objects sorted first by f, then + (for identical f's) by df. + + In: cand: the candid object to insert to the list + cands[newN]: the sorted list of candid objects + Out: cands[newN+1]: the sorted list after the insertion + + Returns the index of $cand in the new list. +*/ +int SortCandid(candid cand, candid* cands, int newN) +{ + int lnN=newN-1; + while (lnN>=0 && cands[lnN].f>cand.f) lnN--; + while (lnN>=0 && cands[lnN].f==cand.f && cands[lnN].df>cand.df) lnN--; + lnN++; //now insert cantmp as cands[lnN] + memmove(&cands[lnN+1], &cands[lnN], sizeof(candid)*(newN-lnN)); + cands[lnN]=cand; + return lnN; +}//SortCandid + +/* + function SynthesisHS: synthesizes a harmonic sinusoid without aligning the phases + + In: partials[M][Fr]: HS partials + terminatetag: external termination flag. Function SynthesisHS() polls *terminatetag and exits with + 0 when it is set. + Out: [dst, den): time interval synthesized + xrec[den-dst]: resynthesized harmonic sinusoid + + Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In + the first case xrec is created anew with malloc8() and must be freed by caller using free8(). +*/ +double* SynthesisHS(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag) +{ + int wid=partials[0][0].s, hwid=wid/2; + double *as=(double*)malloc8(sizeof(double)*Fr*13); + double *fs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5], + *a3=&as[Fr*6], *a2=&as[Fr*7], *a1=&as[Fr*8], *a0=&as[Fr*9], *xs=&as[Fr*11]; + int* ixs=(int*)&as[Fr*12]; + + dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid; + double* xrec=(double*)malloc8(sizeof(double)*(den-dst)); + memset(xrec, 0, sizeof(double)*(den-dst)); + + for (int m=0; m<M; m++) + { + atom* part=partials[m]; bool fzero=false; + for (int fr=0; fr<Fr; fr++) + { + if (part[fr].f<=0){fzero=true; break;} + ixs[fr]=part[fr].t; + xs[fr]=part[fr].t; + as[fr]=part[fr].a*2; + fs[fr]=part[fr].f; + if (part[fr].type==atMuted) as[fr]=0; + } + if (fzero) break; + if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;} + + CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1); + CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1); + double ph=0, ph0=0; + for (int fr=0; fr<Fr-1; fr++) + { + part[fr].p=ph; + ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, true);) + if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;} + } + part[Fr-1].p=ph; + ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[Fr-2], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2], ph, true); + Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], ph0, true);) + } + free8(as); + return xrec; +}//SynthesisHS + +/* + function SynthesisHS: synthesizes a perfectly harmonic sinusoid without aligning the phases. + Frequencies of partials above the fundamental are not used in this synthesis process. + + In: partials[M][Fr]: HS partials + terminatetag: external termination flag. This function polls *terminatetag and exits with 0 when + it is set. + Out: [dst, den) time interval synthesized + xrec[den-dst]: resynthesized harmonic sinusoid + + Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In + the first case xrec is created anew with malloc8() and must be freed by caller using free8(). +*/ +double* SynthesisHS2(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag) +{ + int wid=partials[0][0].s, hwid=wid/2; + double *as=(double*)malloc8(sizeof(double)*Fr*7); //*as=new double[Fr*8], + double *xs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5]; + int *ixs=(int*)&as[Fr*6]; + double** a0=new double*[M*4], **a1=&a0[M], **a2=&a0[M*2], **a3=&a0[M*3]; + a0[0]=(double*)malloc8(sizeof(double)*M*Fr*4); //a0[0]=new double[M*Fr*4]; + for (int m=0; m<M; m++) a0[m]=&a0[0][m*Fr*4], a1[m]=&a0[m][Fr], a2[m]=&a0[m][Fr*2], a3[m]=&a0[m][Fr*3]; + + dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid; + double* xrec=(double*)malloc8(sizeof(double)*(den-dst)); + memset(xrec, 0, sizeof(double)*(den-dst)); + atom* part=partials[0]; + + for (int fr=0; fr<Fr; fr++) + { + xs[fr]=ixs[fr]=part[fr].t; + as[fr]=part[fr].f; + } + CubicSpline(Fr-1, f3, f2, f1, f0, xs, as, 1, 1); + + for (int m=0; m<M; m++) + { + part=partials[m]; + for (int fr=0; fr<Fr; fr++) + { + if (part[fr].f<=0){M=m; break;} + as[fr]=part[fr].a*2; + } + if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; return 0;} + if (m>=M) break; + + CubicSpline(Fr-1, a3[m], a2[m], a1[m], a0[m], xs, as, 1, 1); + } + + double *ph=(double*)malloc8(sizeof(double)*M*2), *ph0=&ph[M]; memset(ph, 0, sizeof(double)*M*2); + for (int fr=0; fr<Fr-1; fr++) + { +// double *la0=new double[M*4], *la1=&la0[M], *la2=&la0[M*2], *la3=&la0[M*3]; for (int m=0; m<M; m++) la0[m]=a0[m][fr], la1[m]=a1[m][fr], la2[m]=a2[m][fr], la3[m]=a3[m][fr], part[fr].p=ph[m]; Sinusoids(M, &xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], la3, la2, la1, la0, f3[fr], f2[fr], f1[fr], f0[fr], ph, true); delete[] la0; + for (int m=0; m<M; m++) ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[m][fr], a2[m][fr], a1[m][fr], a0[m][fr], (m+1)*f3[fr], (m+1)*f2[fr], (m+1)*f1[fr], (m+1)*f0[fr], ph[m], true);) + } + for (int m=0; m<M; m++) + { + part[Fr-1].p=ph[m]; + ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[Fr-2], a3[m][Fr-2], a2[m][Fr-2], a1[m][Fr-2], a0[m][Fr-2], (m+1)*f3[Fr-2], (m+1)*f2[Fr-2], (m+1)*f1[Fr-2], (m+1)*f0[Fr-2], ph[m], true); + Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[m][0], a2[m][0], a1[m][0], a0[m][0], (m+1)*f3[0], (m+1)*f2[0], (m+1)*f1[0], (m+1)*f0[0], ph0[m], true);) + if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; free8(ph); return 0;} + } + free8(as); free8(ph); free8(a0[0]); delete[] a0; + return xrec; +}//SynthesisHS2 + +/* + function SynthesisHSp: synthesizes a harmonic sinusoid with phase alignment + + In: partials[pm][pfr]: HS partials + startamp[pm][st_count]: onset amplifiers, optional + st_start, st_offst: start of and interval between onset amplifying points, optional + Out: [dst, den): time interval synthesized + xrec[den-dst]: resynthesized harmonic sinusoid + + Returns pointer to xrec. xrec is created anew with malloc8() and must be freed by caller with free8(). +*/ +double* SynthesisHSp(int pm, int pfr, atom** partials, int& dst, int& den, double** startamp, int st_start, int st_offst, int st_count) +{ + int wid=partials[0][0].s, hwid=wid/2, offst=partials[0][1].t-partials[0][0].t; + double *a1=new double[pfr*13]; + double *f1=&a1[pfr], *fa=&a1[pfr*2], *fb=&a1[pfr*3], *fc=&a1[pfr*4], *fd=&a1[pfr*5], + *aa=&a1[pfr*6], *ab=&a1[pfr*7], *ac=&a1[pfr*8], *ad=&a1[pfr*9], *p1=&a1[pfr*10], *xs=&a1[pfr*11]; + int* ixs=(int*)&a1[pfr*12]; + + dst=partials[0][0].t-hwid, den=partials[0][pfr-1].t+hwid; + double *xrec=(double*)malloc8(sizeof(double)*(den-dst)*2), *xrecm=&xrec[den-dst]; + memset(xrec, 0, sizeof(double)*(den-dst)); + + for (int p=0; p<pm; p++) + { + atom* part=partials[p]; + bool fzero=false; + for (int fr=0; fr<pfr; fr++) + { + if (part[fr].f<=0) + { + fzero=true; + break; + } + ixs[fr]=part[fr].t; + xs[fr]=part[fr].t; + a1[fr]=part[fr].a*2; + f1[fr]=part[fr].f; + p1[fr]=part[fr].p; + if (part[fr].type==atMuted) a1[fr]=0; + } + if (fzero) break; + + CubicSpline(pfr-1, fa, fb, fc, fd, xs, f1, 1, 1); + CubicSpline(pfr-1, aa, ab, ac, ad, xs, a1, 1, 1); + + for (int fr=0; fr<pfr-1; fr++) Sinusoid(&xrecm[ixs[fr]-dst], 0, offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], p1[fr], p1[fr+1], false); +// Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false); +// Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, aa[pfr-2], ab[pfr-2], ac[pfr-2], ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false); + Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, 0, 0, 0, ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false); + Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, 0, 0, 0, ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false); + + if (st_count) + { + double* amp=startamp[p]; + for (int c=0; c<st_count; c++) + { + double a1=amp[c], a2=(c+1<st_count)?amp[c+1]:1, da=(a2-a1)/st_offst; + int lst=ixs[0]-dst+st_start+c*st_offst; + double *lxrecm=&xrecm[lst]; + if (lst>0) for (int i=0; i<st_offst; i++) lxrecm[i]*=(a1+da*i); + else for (int i=-lst; i<st_offst; i++) lxrecm[i]*=(a1+da*i); + } + for (int i=0; i<ixs[0]-dst+st_start; i++) xrecm[i]*=amp[0]; + } + else + { + /* + for (int i=0; i<=hwid; i++) + { + double tmp=0.5+0.5*cos(M_PI*i/hwid); + xrecm[ixs[0]-dst-i]*=tmp; + xrecm[ixs[pfr-2]-dst+offst+i]*=tmp; + } */ + } + + for (int n=0; n<den-dst; n++) xrec[n]+=xrecm[n]; + } + + delete[] a1; + return xrec; +}//SynthesisHSp + +/* + function SynthesisHSp: wrapper function. + + In: HS: a harmonic sinusoid. + Out: [dst, den): time interval synthesized + xrec[den-dst]: resynthesized harmonic sinusoid + + Returns pointer to xrec, which is created anew with malloc8() and must be freed by caller using free8(). +*/ +double* SynthesisHSp(THS* HS, int& dst, int& den) +{ + return SynthesisHSp(HS->M, HS->Fr, HS->Partials, dst, den, HS->startamp, HS->st_start, HS->st_offst, HS->st_count); +}//SynthesisHSp +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hs.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,273 @@ +#ifndef hsH +#define hsH + +/* + hs.cpp - harmonic sinusoid model + + Further reading: Wen X. and M. Sandler, "Sinusoid modeling in a harmonic context," in Proc. DAFx'07, Bordeaux, 2007. +*/ + +//--------------------------------------------------------------------------- +#include <mem.h> +#include "arrayalloc.h" +#include "align8.h" +#include "fft.h" +#include "QuickSpec.h" +#include "TStream.h" + +#define ATOM_LOCALANCHOR 1 +#define HS_CONSTF 1 +#define MAX_CAND 64 +#define STIFF_B_MAX 0.01 + +enum atomtype //type flags of an HS atom +{ + atAnchor, //"anchor" is an atom whose validity is taken for granted, e.g. user input + atPeak, //an atom whose frequency is an actual spectral peak + atInfered, //an atom which has no spectral peak and whose frequency is inferred from other atoms + atMuted, //an atom which is being muted + atBuried //an atom which is buried in background noise +}; + +struct atom //an atom of a harmonic sinusoid +{ + double t; //time + double s; //scale + double f; //digital frequency + double a; //amplitude + double p; //phase angle + int pin; //partial index + atomtype type; //atom type + int tags; //additional info on atom type + float r1; +}; + +struct candid //candidate atom +{ + int p; //partial index + double f; //frequency + double df; //delta freqdel folk + double s; //score in partial-sum + int prev; //index of last partial f-df + int type; //0: f0, 1: local maximum, 2: not a maxixum + double ms; +}; + +struct dsparams1 //argument structure for calling ds1() +{ + double s; //score + double lastene; //energy of last frame + double currentacce; //energy of this frame, currently accumulated + int p; //partial index + int lastP; //number of efficient partials in last frame + double* lastvfp; //amplitude estimates of partials at last frame +}; + +struct NMResults //note match output structure +{ + double* fp; //frequencies, in bins + double* vfp; //amplitudes + double* pfp; //phase angles + atomtype* ptype; //atom types +}; + +struct NMSettings //note match algorithm settings +{ + double c[4]; //cosine-family window specifiers + int M; //ditto + double iH2; //ditto + double hB; //spectral truncation half width + int maxp; //maximal number of partials + double maxB; //stiffness coefficient upper bound + double epf; //frequency estimation error tolerance for LSE estimation + double epf0; //input frequency error bound for harmonic grouping + double delm; //frequency error bound for harmonic grouping + double delp; //pitch jump upper bound + double minf0; //minimal fundamental frequency + double maxf0; //maximal fundamental frequency + int pin0; //input partial index + bool forcepin0; //force the peak nearest to input frequency as an atom + bool pin0asanchor; //mark atom found near the input frequency as anchor + int pcount; //number of "pinned" (anchor) partials + int* pin; //partial indices of pinned partials + double* pinf; //frequencies of pinned partials + int* pinfr; //frame indices of pinned partials, used in multi-frame constant-pitch note match +}; + + +/* + stiffcandid is the harmonic atom class used internally by harmonic grouping and tracking routines. Literally + it means "candidate harmonic atoms on a stiff string model". The stiff string is the main harmonic model used + by me for describing frequency relations between partials of a harmonic sound. + + stiffcandid is superceded by TTempAtom class. +*/ + +class stiffcandid +{ +public: + int P; //number of partial estimates located + int* p; //partial indices of located partials, sizeof(int)*P + double* f; //frequencies of located partials, sizeof(double)*P + double s; //score in partial-sum + //{N; F, G}: the feasible polygonal area of (F, G) given the P partials, where F=F0*F0, G=F0*B + int N; + double* F; //sizeof(double)*N + double* G; //sizeof(double)*N + + stiffcandid(int Wid); //create empty with minimal info + stiffcandid(int Wid, int ap, double af, double errf); //create empty with pitch range + stiffcandid(int Wid, int ap, double af, double errf, double ds); //create with 1 atom + stiffcandid(stiffcandid* prev, int ap, double af, double errf, double ds); //create by updating with new atom + ~stiffcandid(); +}; + + +/* + THS is the data structure hosting a harmonic sinusoid representation. Its key members include the number + of partials, number of frames, and all its atoms (sinusoid parameters of all partials at measurement points). +*/ + +class THS +{ +public: + int Channel; //channel id: THS describes a harmonic sinusoid in single channel + int M; //number of partials + int Fr; //number of frames + atom** Partials; //atoms, [M][Fr] + + int* BufM[128]; //a buffer for algorithmic use + + int isconstf; //constant frequencies flag + + double** startamp;//onset amplifiers, optional + int st_start; //position of the first onset amplifying point + int st_offst; //interval of onset amplifying points + int st_count; //number of onset amplifying points + + //constructors and destructor + THS(); + THS(int aM, int aFr); + THS(THS* HS); + THS(THS* HS, double start, double end); + ~THS(); + + int StartPos(); //start position + int EndPos(); //end position + int EndPosEx(); //extended end position + int StdOffst(); //hop size + + void ClearBufM(); //free buffers registered in BufM[] + void Resize(int aM, int aFr, bool copydata=false, bool forcealloc=false); //change size (M or Fr) + + //I/O routines + int WriteHdrToStream(TStream* Stream); + int ReadHdrFromStream(TStream* Stream); + int WriteAtomToStream(TStream* Stream, atom* atm); + int ReadAtomFromStream(TStream* Stream, atom* atm); + int WriteToStream(TStream* Stream); + int ReadFromStream(TStream* Stream); +}; + + +/* + TPolygon is a polygon class. This class itself does not enforce convexness. However, when used for solving + the stiff string model, all polygons are convex, as they are the results of current a first convex polygon + by straight lines. + + For the convenience of computation, the sequence of vertices are arranged so that they come in clockwise + order starting from the leftmost (smallest x coordinate) vertex; in case two vertices are both leftmost + the upper (larger y coordinate) one is placed at the start and the lower one is placed at the end. +*/ + +class TPolygon +{ +public: + int N; //number of vertices (equals number of edges) + double* X; //x-coordinates of vertices + double* Y; //y-coordinates of vertices + TPolygon(int cap); + TPolygon(int cap, TPolygon* R); + ~TPolygon(); +}; + + +/* + TTempAtom is an atom class within the stiff-stirng harmonic atom context used internally by harmonic + grouping and tracking routines. Literally it means "a temporary atom", and truly it hosts most info + one expects of a potential atom. TTempAtom replaces stiffcandid class in harmonic sinusoid group and + tracking algorithms, due to the advanges that it carries atom information (frequency, amplitude, etc.) + as well as harmonic atom information (link to other atom, the F-G polygon), and is therefore both an + individual atom and a harmonic atom containing all atoms tracked down through the linked list Prev. +*/ + +class TTempAtom +{ +public: + int pind; //partial index + double f; //frequency + double a; //amplitude + double s; //scale + double rsr; //residue-sinusoid ratio + TTempAtom* Prev; //previous atom + TPolygon *R; //F-G polygon for harmonic group + union {double acce; int tag[2];}; + + TTempAtom(double af, double ef, double maxB); //create empty with frequency range + TTempAtom(int apin, double af, double ef, double maxB); //create empty with frequency range + TTempAtom(TPolygon* AR, double delf1, double delf2, double minf); //create empty with extended R + TTempAtom(int apind, double af, double ef, double aa, double as, double maxB); //create with one partial + TTempAtom(TTempAtom* APrev, bool DupR); //create duplicate + TTempAtom(TTempAtom* APrev, int apind, double af, double ef, double aa, double as, bool updateR=true); //duplicate and add one partial + ~TTempAtom(); +}; + +//--internal function-------------------------------------------------------- +double ds0(double, void*); //a score function, internal use only + +//--general polygon routines------------------------------------------------- +void areaandcentroid(double& A, double& cx, double& cy, int N, double* x, double* y); //compute area and centroid +void cutcvpoly(int& N, double* x, double* y, double A, double B, double C, bool protect=false); //sever polygon by line +double maximalminimum(double& x, double& y, int N, double* sx, double* sy); //maximum inscribed circle + +//--F-G polygon routines----------------------------------------------------- +void CutR(TPolygon* R, int apind, double af, double ef, bool protect=false); +void ExBStiff(double& Bmin, double& Bmax, int N, double* F, double* G); +void ExFmStiff(double& Fmin, double& Fmax, int m, int N, double* F, double* G); +void ExtendR(TPolygon* R, double delf1, double delf2, double minf); +void InitializeR(TPolygon* R, double af, double ef, double maxB); +void InitializeR(TPolygon* R, int apind, double af, double ef, double maxB); + +//--internal structure conversion routines----------------------------------- +int NMResultToAtoms(int M, atom* HP, int t, int wid, NMResults results); +int NMResultToPartials(int M, int fr, atom** Partials, int t, int wid, NMResults results); + +//--batch sinusoid estimation routines--------------------------------------- +double PeakShapeC(double f, int Fr, int N, cdouble** x, int B, int M, double* c, double iH2); +double PeakShapeC(double f, int Fr, int N, cfloat** x, int B, int M, double* c, double iH2); +int QuickPeaks(double* f, double* a, int N, cdouble* x, int M, double* c, double iH2, double mina, int binst=-1, int binen=-1, int B=5, double* rsr=0); +int QuickPeaks(double* f, double* a, int Fr, int N, cdouble** x, int fr0, int r0, int M, double* c, double iH2, double mina, int binst=-1, int binen=-1, int B=5, double* rsr=0); + +//--harmonic atom detection (harmonic grouping) routines--------------------- +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp, double* lastvfp, double (*computes)(double a, void* params)=ds0, int forceinputlocalfr=-1); //basic grouping without rsr +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps, double* rsr, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp, double* lastvfp, double (*computes)(double a, void* params)=ds0, int forceinputlocalfr=-1); //basic grouping with rsr +double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp, double* lastvfp, double (*deltas)(double a, void* params)=ds0, bool forceinputlocalfr=false, int startfr=-1, int validfrrange=0); //basic grouping with rsr - wrapper +double NoteMatchStiff3(TPolygon* R, int peak0, int pin0, cdouble* x, int pc, double* fps, double* vps, int N, NMSettings* settings, double* vfp, int** pitchind, int newpc); //grouping with given pitch, single-frame + +//--harmonic sinusoid tracking routines-------------------------------------- +int FindNote(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings); //harmonic sinusoid tracking (forward and backward tracking) +int FindNoteConst(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake); //constant-pitch harmonic sinusoid tracking +int FindNoteF(atom* part, double& starts, TPolygon* R, int startp, double* startvfp, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake); //forward harmonic sinusoid tracking +int FindNoteFB(int frst, TPolygon* Rst, double* vfpst, int fren, TPolygon* Ren, double* vfpen, int M, atom** partials, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings); //forward-backward harmonic sinusoid tracking +void NoteMatchStiff3FB(int& pitchcount, TPolygon**& R, double**& vfp, double*& sc, int* newpitches, int* prev, int pc, double* fps, double* vps, cdouble* x, int wid, int maxpitch, NMSettings* settings); //single DP step of FindNoteFB() + +//--harmonic sinusoid synthesis routines------------------------------------- +double* SynthesisHS(int pm, int pfr, atom** partials, int& dst, int& den, bool* terminatetag=0); +double* SynthesisHS2(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag=0); +double* SynthesisHSp(int pm, int pfr, atom** partials, int& dst, int& den, double** startamp=0, int st_start=0, int st_offst=0, int st_count=0); +double* SynthesisHSp(THS* HS, int& dst, int& den); + +//--other functions---------------------------------------------------------- +void ReEstHS1(THS* HS, __int16* Data16); //reestimation of HS + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hsedit.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,190 @@ +//--------------------------------------------------------------------------- + +#include "hsedit.h" +#include "splines.h" + +//--------------------------------------------------------------------------- + +/* + function DeFM: frequency de-modulation + + In: peakfr[npfr]: segmentation into FM cycles, peakfr[0]=0, peakfr[npfr-1]=Fr-1 + a1[Fr], f1[Fr]: sequence of amplitudes and frequencies + arec[Fr]: amplitude-based weights for frequency averaging + Out: a2[Fr], f2[Fr]: amplitude and frequency sequance after demodulation + + No return value. +*/ +void DeFM(double* a2, double* f2, double* a1, double* f1, double* arec, int npfr, int* peakfr) +{ + double *frs=new double[npfr*12], *a=&frs[npfr], *f=&frs[npfr*2], + *aa=&frs[npfr*3], *ab=&frs[npfr*4], *ac=&frs[npfr*5], *ad=&frs[npfr*6], + *fa=&frs[npfr*7], *fb=&frs[npfr*8], *fc=&frs[npfr*9], *fd=&frs[npfr*10]; + a[0]=a1[0], f[0]=f1[0], frs[0]=peakfr[0]; + for (int i=1; i<npfr-1; i++) + { + a[i]=f[i]=frs[i]=0; double lrec=0; + for (int fr=peakfr[i-1]; fr<peakfr[i+1]; fr++) + a[i]+=a1[fr]*a1[fr], f[i]+=f1[fr]*arec[fr], frs[i]+=fr*arec[fr], lrec+=arec[fr]; + a[i]=sqrt(a[i]/(peakfr[i+1]-peakfr[i-1])), f[i]/=lrec, frs[i]/=lrec; + } + a[npfr-1]=a1[peakfr[npfr-1]], f[npfr-1]=f1[peakfr[npfr-1]], frs[npfr-1]=peakfr[npfr-1]; + CubicSpline(npfr-1, aa, ab, ac, ad, frs, a, 1, 1, a2); + CubicSpline(npfr-1, fa, fb, fc, fd, frs, f, 1, 1, f2); + delete[] frs; +}//DeFM + +/* + function DFMSeg: segments HS frames into FM cycles + + In: partials[M][Fr]: HS partials + Out: peakfr[npfr]: segmentation, peakfr[0]=0, peakfr[npfr-1]=Fr-1. + arec[Fr]: total amplitudes of frames + + No return value. +*/ +void DFMSeg(double* arec, int& npfr, int* peakfr, int M, int Fr, atom** partials) +{ + double *frec=new double[Fr]; + memset(arec, 0, sizeof(double)*Fr); memset(frec, 0, sizeof(double)*Fr); + for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) {double la=partials[m][fr].a; la=la*la; arec[fr]+=la; frec[fr]+=partials[m][fr].f/(m+1)*la;} + for (int fr=0; fr<Fr; fr++) frec[fr]=frec[fr]/arec[fr]; + peakfr[0]=0; npfr=1; + for (int fr=1; fr<Fr-1; fr++) + { + if ((frec[fr]<frec[fr-1] && frec[fr]<frec[fr+1]) || (frec[fr]>frec[fr-1] && frec[fr]>frec[fr+1])) + { + peakfr[npfr]=fr; + if (peakfr[npfr]-peakfr[npfr-1]>2) npfr++; + } + } + peakfr[npfr++]=Fr-1; + delete[] frec; +}//DFMSeg + +/* + function HSAM: harmonic sinusoid amplitude modulation + + In: SrcHS: source harmonic sinusoid + dep: modulation depth + fre: modulator frequency + ph: modulator phase + Out: HS: destination harmonic sinusoid + + No reutrn value. +*/ +void HSAM(THS* HS, THS* SrcHS, double dep, double fre, double ph) +{ + double omg=M_PI*2*fre; + for (int m=0; m<HS->M; m++) + for (int fr=0; fr<HS->Fr; fr++) + HS->Partials[m][fr].a=SrcHS->Partials[m][fr].a*(1+dep*cos(omg*SrcHS->Partials[m][fr].t+ph)); +}//HSAM + +/* + function HSFM: harmonic sinusoid frequency modulation + + In: SrcHS: source harmonic sinusoid + a: modulation extent, in semitones + fre: modulator frequency + ph: modulator phase + Out: HS: destination harmonic sinusoid + + No reutrn value. +*/ +void HSFM(THS* HS, THS* SrcHS, double a, double freq, double ph) +{ + double omg=M_PI*2*freq, pa=pow(2, a/12.0)-1; + for (int m=0; m<HS->M; m++) + for (int fr=0; fr<HS->Fr; fr++) + HS->Partials[m][fr].f=SrcHS->Partials[m][fr].f*(1+pa*cos(omg*SrcHS->Partials[m][fr].t+ph)); +}//HSFM + +/* + function HSFM_SF: harmonic sinusoid frequency modulation with source-filter model + + In: SrcHS: source harmonic sinusoid + a: modulation extent, in semitones + fre: modulator frequency + ph: modulator phase + SF: source-filter model + Out: HS: destination harmonic sinusoid + + No reutrn value. +*/ +void HSFM_SF(THS* HS, THS* SrcHS, double a, double freq, double ph, TSF* SF) +{ + double omg=M_PI*2*freq, pa=pow(2, a/12.0)-1; + for (int m=0; m<HS->M; m++) for (int fr=0; fr<HS->Fr; fr++) + { + double f0=SrcHS->Partials[m][fr].f; + double f1=f0*(1+pa*cos(omg*SrcHS->Partials[m][fr].t+ph)); + HS->Partials[m][fr].f=f1; + HS->Partials[m][fr].a=SrcHS->Partials[m][fr].a*exp(SF->LogAF(f1)-SF->LogAF(f0)); + } +}//HSFM_SF + +/* + function: HSPitchShift: harmonic sinusoid pitch shifting + + In: SrcHS: source harmonic sinusoid + ps12: amount of pitch shift, in semitones + Out: HS: destination harmonic sinusoid + + No return value. +*/ +void HSPitchShift(THS* HS, THS* SrcHS, double ps12) +{ + double pa=pow(2, ps12/12.0); + for (int m=0; m<HS->M; m++) for (int fr=0; fr<HS->Fr; fr++) HS->Partials[m][fr].f=SrcHS->Partials[m][fr].f*pa; +}//HSPitchShift + +/* + function ReFM: frequency re-modulation + + In: partials[M][Fr]: HS partials + amount: relative modulation depth after remodulation + rate: relateive modulation rate after remodulation + SF: a source-filter model, optional + Out: partials2[M][Fr]: remodulated HS partials. Must be allocated before calling. + + No return value. +*/ +void ReFM(int M, int Fr, atom** partials, atom** partials2, double amount, double rate, TSF* SF) +{ + double *arec=new double[Fr]; int *peakfr=new int[Fr], npfr; + DFMSeg(arec, npfr, peakfr, M, Fr, partials); + + double *a1=new double[Fr*8]; + double *f1=&a1[Fr], *a2=&a1[Fr*3], *f2=&a1[Fr*4], *da=&a1[Fr*5], *df=&a1[Fr*6]; + + for (int m=0; m<M; m++) + { + atom *part=partials[m], *part2=partials2[m]; bool fzero=false; + for (int fr=0; fr<Fr; fr++) + { + if (part[fr].f<=0){fzero=true; break;} + a1[fr]=part[fr].a*2; + f1[fr]=part[fr].f; + } + if (fzero){part2[0].f=0; break;} + DeFM(a2, f2, a1, f1, arec, npfr, peakfr); + for (int i=0; i<Fr; i++) da[i]=a1[i]-a2[i], df[i]=f1[i]-f2[i]; + for (int fr=0; fr<Fr; fr++) + { + double frd=fr/rate; int dfrd=floor(frd); frd-=dfrd; + double lda=0, ldf=0; + if (dfrd<Fr-1) lda=da[dfrd]*(1-frd)+da[dfrd+1]*frd, ldf=df[dfrd]*(1-frd)+df[dfrd+1]*frd; + else if (dfrd==Fr-1) lda=da[dfrd]*(1-frd), ldf=df[dfrd]*(1-frd); + part2[fr].f=f2[fr]=f2[fr]+ldf*amount; + if (SF) part2[fr].a=part[fr].a*exp(SF->LogAF(part2[fr].f)-SF->LogAF(part[fr].f)); + else part2[fr].a=(a2[fr]+lda*amount)*0.5; + } + } + delete[] a1; + delete[] arec; delete[] peakfr; +}//ReFM + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hsedit.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,23 @@ +#ifndef hseditH +#define hseditH + + +/* + hsedit.cpp - harmonic sinusoid audio editing routines. +*/ + +#include "hs.h" +#include "hssf.h" + +//--tool procedures---------------------------------------------------------- +void DeFM(double* a2, double* f2, double* a1, double* f1, double* arec, int npfr, int* peakfr); +void DFMSeg(double* arec, int& npfr, int* peakfr, int M, int Fr, atom** partials); +void ReFM(int M, int Fr, atom** partials, atom** partials2, double amount=1, double rate=1, TSF* SF=0); + +//--HS editing sample routines----------------------------------------------- +void HSAM(THS* HS, THS* SrcHS, double dep, double fre, double ph); +void HSFM(THS* HS, THS* SrcHS, double a, double freq, double ph); +void HSFM_SF(THS* HS, THS* SrcHS, double a, double freq, double ph, TSF* SF); +void HSPitchShift(THS* HS, THS* SrcHS, double ps12); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hssf.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,1156 @@ +//--------------------------------------------------------------------------- + + +#include <math.h> +#include <mem.h> +#include "hssf.h" +#include "arrayalloc.h" +#include "Matrix.h" +#include "vibrato.h" + +//--------------------------------------------------------------------------- +//method TSF::TSF: default constructor. +TSF::TSF() +{ + memset(this, 0, sizeof(TSF)); +}//TSF + +//method TSF::~TSF: default destructor. +TSF::~TSF() +{ + if (h) DeAlloc2(h); + if (b) DeAlloc2(b); + delete[] lp; + delete[] F0; + free(avgh); + free(avgb); +}//~TSF + +/* + method TSF::AllocateL: allocates or reallocates storage space whose size depends on L. This uses an + external L value for allocation and updates L itself. +*/ +void TSF::AllocateL(int AnL) +{ + L=AnL; + delete[] F0; F0=new double[(L+2)*6]; + F0C=&F0[L+2], F0D=&F0[(L+2)*2], logA0C=&F0[(L+2)*3], logA0=&F0[(L+2)*4], logA0D=&F0[(L+2)*5]; +}//AllocateL + +/* + method TSF::AllocateP: allocates or reallocates storage space whose size depends on P, using the + interal P. +*/ +void TSF::AllocateP() +{ + delete[] lp; + lp=new double[P+2]; +}//AllocateP + +/* + method TSF::AllocateSF: allocates or reallocates storage space for source-filter coefficients. +*/ +void TSF::AllocateSF() +{ + if (h) DeAlloc2(h); int k=1.0/F; Allocate2(double, L, (k+2), h); memset(h[0], 0, sizeof(double)*(L)*(k+2)); + if (b) DeAlloc2(b); Allocate2(double, L, M, b); memset(b[0], 0, sizeof(double)*(L)*M); + avgh=(double*)realloc(avgh, sizeof(double)*(k+2)); avgb=(double*)realloc(avgb, sizeof(double)*M); +}//AllocateSF + +/* + method TSF::Duplicate: copies the complete contents from another SF object + + In: SF: source object +*/ +void TSF::Duplicate(TSF& SF) +{ + memcpy(this, &SF, sizeof(TSF)); lp=F0=avgh=avgb=0, h=b=0; + AllocateL(L); memcpy(F0, SF.F0, sizeof(double)*(L+2)*6); + AllocateP(); memcpy(lp, SF.lp, sizeof(double)*(P+2)); + AllocateSF(); int SFL=ceil(lp[P-1])-ceil(lp[0]); + 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); + memcpy(avgh, SF.avgh, sizeof(double)*(K+2)); memcpy(avgb, SF.avgb, sizeof(double)*M); +}//Duplicate + +/* + method TSF::LoadFromFileHandle: reads SF object from file +*/ +void TSF::LoadFromFileHandle(FILE* f) +{ + fread(&M, sizeof(int), 1, f); + fread(&L, sizeof(int), 1, f); + fread(&P, sizeof(int), 1, f); + fread(&offst, sizeof(double), 1, f); + AllocateL(L); AllocateP(); + fread(F0C, sizeof(double), L, f); + fread(logA0C, sizeof(double), L, f); + fread(logA0D, sizeof(double), L, f); + fread(lp, sizeof(double), P, f); + LoadSFFromFileHandle(f); +}//LoadFromFileHandle + +/* + method TSF::LoadSFFromFileHandle: reads SF coefficients from file +*/ +void TSF::LoadSFFromFileHandle(FILE* f) +{ + fread(&K, sizeof(int), 1, f); + fread(&FScaleMode, sizeof(int), 1, f); + fread(&F, sizeof(double), 1, f); + fread(&Fs, sizeof(double), 1, f); + AllocateSF(); + for (int l=0; l<L; l++) fread(b[l], sizeof(double), M, f); + for (int l=0; l<L; l++) fread(h[l], sizeof(double), K+2, f); + fread(avgb, sizeof(double), M, f); + fread(avgh, sizeof(double), K+2, f); +}//LoadSFFromFileHandle + +/* + method TSF::LogAF: average filter response + + In: f: frequency + + Returns the average response of the filter model at f +*/ +double TSF::LogAF(double f) +{ + if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700); + int k=floor(f/F); + double f_plus=f/F-k; + if (k<K+1) return avgh[k]*(1-f_plus)+avgh[k+1]*f_plus; + else return avgh[K+1]; +}//LogAF + +/* + method TSF::LogAF: filter response + + In: f: frequency + fr: frame index + + Returns the response of the filter model at f for frame fr +*/ +double TSF::LogAF(double f, int fr) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1; + if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700); + int k=floor(f/F); + double f_plus=f/F-k; + if (k<K+1) return h[l][k]*(1-f_plus)+h[l][k+1]*f_plus; + else return h[l][K+1]; +}//LogAF + +/* + method TSF::LogAS: source response + + In: m: partial index + fr: frame index + + Returns response of the source model for partial m at frame fr +*/ +double TSF::LogAS(int m, int fr) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1; + return b[l][m]; +}//LogAS + +/* + method TSF::ReAllocateL: reallocates storage space whose size depends on L, and transfer the contents. + This uses an external L value for allocation but does not update L itself. +*/ +void TSF::ReAllocateL(int newL) +{ + 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]; + memcpy(logA0C, &F0[(L+2)*3], sizeof(double)*(L+2)); + memcpy(logA0D, &F0[(L+2)*5], sizeof(double)*(L+2)); + memcpy(F0D, &F0[(L+2)*2], sizeof(double)*(L+2)); + memcpy(F0C, &F0[L+2], sizeof(double)*(L+2)); + memcpy(newF0, F0, sizeof(double)*(L+2)); + delete[] F0; + F0=newF0; +}//ReAllocateL + +/* + method TSF::SaveSFToFileHandle: writes SF coefficients to file +*/ +void TSF::SaveSFToFileHandle(FILE* f) +{ + fwrite(&K, sizeof(int), 1, f); + fwrite(&FScaleMode, sizeof(int), 1, f); + fwrite(&F, sizeof(double), 1, f); + fwrite(&Fs, sizeof(double), 1, f); + for (int l=0; l<L; l++) fwrite(b[l], sizeof(double), M, f); + for (int l=0; l<L; l++) fwrite(h[l], sizeof(double), K+2, f); + fwrite(avgb, sizeof(double), M, f); + fwrite(avgh, sizeof(double), K+2, f); +}//SaveSFToFileHandle + +/* + method TSF::SaveToFile: save SF object to file + + In: filename: full path of destination file +*/ +void TSF::SaveToFile(char* filename) +{ + FILE* file; + if ((file=fopen(filename, "wb"))!=NULL) + { + SaveToFileHandle(file); fclose(file); + } +}//SaveToFile + +/* + method TSF::SaveToFileHandle: writes SF object to file +*/ +void TSF::SaveToFileHandle(FILE* f) +{ + fwrite(&M, sizeof(int), 1, f); + fwrite(&L, sizeof(int), 1, f); + fwrite(&P, sizeof(int), 1, f); + fwrite(&offst, sizeof(double), 1, f); + fwrite(F0C, sizeof(double), L, f); + fwrite(logA0C, sizeof(double), L, f); + fwrite(logA0D, sizeof(double), L, f); + fwrite(lp, sizeof(double), P, f); + SaveSFToFileHandle(f); +}//SaveToFileHandle + +/* + method TSF::ShiftFilterByDB: adds a given number of dBs to the filter model. + + In: dB: amount to add. +*/ +void TSF::ShiftFilterByDB(double dB) +{ + for (int l=0; l<L; l++) for (int k=0; k<K+2; k++) h[l][k]+=dB; + for (int k=0; k<K+2; k++) avgh[k]+=dB; +}//ShiftFilterByDB + +//--------------------------------------------------------------------------- +//functions + + +/* + function AnalyzeSF: wrapper function. + + In: HS: a harmonic sinusoid + sps: sampling rate ("samples per second") + offst: hop size, the interval between adjacent harmonic atoms of HS + SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation) + SFF: filter response sampling interval + SFScale: set if filter sampling uses mel scale + SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach + Out: SF: a TSF object estimated from HS. + cyclefrcount: number of cycles + cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle + + No return value. +*/ +void AnalyzeSF(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, double offst, int* cyclefrcount, + int SFMode, double SFF, int SFFScale, double SFtheta) +{ + AnalyzeSF_1(HS, SF, sps, offst); + AnalyzeSF_2(HS, SF, cyclefrs, cyclefs, sps, cyclefrcount, SFMode, SFF, SFFScale, SFtheta); +}//AnalyzeSF + +/* + function AnalyzeSF_1: first stage of source-filter model estimation, in which the duration of a HS is + segmented into what is equivalent to "cycles" in vibrato analysis. + + In: HS: a harmonic sinusoid + sps: sampling rate ("samples per second") + offst: hop size, the interval between adjacent harmonic atoms of HS + Out: SF: a TSF object partially updated, particularly lp[P]. + + No return value. +*/ +void AnalyzeSF_1(THS& HS, TSF& SF, double sps, double offst) +{ + int M=HS.M, Fr=HS.Fr; + if (M<=0 || Fr<=0) return; + atom** Partials=HS.Partials; + + //basic descriptor: offst + SF.offst=offst; + + //demodulating pitch + //Basic descriptor: L + SF.AllocateL(Fr); + + double* A0=SF.logA0; + //find the amplitude and pitch tracks + SF.F0max=0, SF.F0min=1; + for (int fr=0; fr<Fr; fr++) + { + double suma2=0, suma2p=0; + for (int m=0; m<M; m++) + { + double a=Partials[m][fr].a, p=Partials[m][fr].f/(m+1); + if (p<=0) break; + suma2+=a*a, suma2p+=a*a*p; + } + SF.F0[fr]=suma2p/suma2; + if (SF.F0max<SF.F0[fr]) SF.F0max=SF.F0[fr]; + if (SF.F0min>SF.F0[fr]) SF.F0min=SF.F0[fr]; + A0[fr]=suma2; + } + + //Basic descriptor: M + SF.M=0.45/SF.F0max; + if (SF.M>M) SF.M=M; + + //rough estimation of rate + double* f0d=new double[Fr-1]; for (int i=0; i<Fr-1; i++) f0d[i]=SF.F0[i+1]-SF.F0[i]; + RateAndReg(SF.rate, SF.regularity, f0d, 0, Fr-2, 8, sps, offst); + delete[] f0d; + //find peaks of the pitch track + int* peakfr=new int[Fr/2]; + double periodinframe=sps/(SF.rate*offst), *dummy=(double*)0xFFFFFFFF; + FindPeaks(peakfr, SF.P, SF.F0, Fr, periodinframe, dummy); + //Basic descriptor: lp[] + SF.AllocateP(); + for (int p=0; p<SF.P; p++) + { + double startfr; QIE(&SF.F0[peakfr[p]], startfr); SF.lp[p]=startfr+peakfr[p]; + } + delete[] peakfr; +}//AnalyzeSF_1 + +/* + function AnalyzeSF_2: second stage of source-filter model estimation, in which the HS is demodulated + and its source-filter model is estimated. + + In: HS: a harmonic sinusoid + SF: a TSF object with valid segmentation data (lp[P]) of HS + sps: sampling rate ("samples per second") + SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation) + SFF: filter response sampling interval + SFScale: set if filter sampling uses mel scale + SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach + Out: SF: the TSF object fully estimated. + cyclefrcount: number of cycles + cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle + + No return value. +*/ +void AnalyzeSF_2(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, int* cyclefrcount, + int SFMode, double SFF, int SFFScale, double SFtheta) +{ + int M=HS.M, Fr=HS.Fr; + if (M<=0 || Fr<=0) return; + atom** Partials=HS.Partials; + + if (SF.P>=1) //de-modulation + { + //Basic descriptor: F0C[] + DeFM(SF.F0C, SF.F0, SF.logA0, SF.P, SF.lp, Fr, SF.F0Overall, *cyclefrcount, cyclefrs, cyclefs); + //Basic descriptor: A0C[] + double *A0C=SF.logA0C, *logA0C=SF.logA0C, *logA0D=SF.logA0D, *logA0=SF.logA0; + DeAM(A0C, SF.logA0, SF.P, SF.lp, Fr); + for (int fr=0; fr<Fr; fr++) logA0C[fr]=log(A0C[fr]); + //Basic descriptor: logA0D[] + int hM=M/2; + for (int fr=0; fr<Fr; fr++) + { + double loga0d=0; + for (int m=0; m<hM; m++) loga0d+=log(Partials[m][fr].a); + logA0[fr]=loga0d/hM; + logA0D[fr]=logA0[fr]-logA0C[fr]; + } + } + //Basic descriptor: F0D[] + SF.F0Cmax=0; SF.F0Dmax=-1; SF.F0Cmin=1; SF.F0Dmin=1; + for (int fr=0; fr<Fr; fr++) + { + if (SF.F0Cmax<SF.F0C[fr]) SF.F0Cmax=SF.F0C[fr]; + if (SF.F0Cmin>SF.F0C[fr]) SF.F0Cmin=SF.F0C[fr]; + SF.F0D[fr]=SF.F0[fr]-SF.F0C[fr]; + } + + //Basic descriptor: b, h + //Source-filter modeling + { + SF.F=SFF; SF.Fs=sps; SF.AllocateSF(); + int M=SF.M; double **h=SF.h, **b=SF.b;// *avgh=SF.avgh, *avgb=SF.avgb; + + double *logA0C=useA0?SF.logA0:SF.logA0C; + if (SFMode==0){ + S_F_FB(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, sps);} + else if (SFMode==1){ + S_F_SV(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, SFtheta, sps);} + + SF.FScaleMode=SFFScale; +// int K=SF.K, effL=ceil(SF.lp[SF.P-1])-ceil(SF.lp[0]); +// 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;} +// 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;} + } +}//AnalyzeSF_2 + +/* + function I3: integrates the product of 3 linear functions (0, a*)-(T, b*) (*=1, 2 or 3) over (0, T). + See "further reading", pp.12 eq.(37). + + In: T, a1, a2, a3, b1, b2, b3 + + Returns the definite integral. +*/ +double I3(double T, double a1, double a2, double a3, double b1, double b2, double b3) +{ + return T*(a1*(a2*(3*a3+b3)+b2*(a3+b3))+b1*(a2*(a3+b3)+b2*(a3+3*b3)))/12; +}//I3 + +/* + function P_Ax: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic + coefficient of filter coefficients x[L][K+2] towards the totoal variation. See "further reading", + pp.8, (29a). + + In: x[L][K+2]: filter coefficients + f[L][M]: partial frequencies + F: filter response sampling interval + theta: balancing factor of amplitude and frequency variations + Out: d[L][K+2]: Ax. + + Returns inner product of x[][] and d[][]. +*/ +double P_Ax(double** d, int L, int M, int K, double** x, double** f, double F, double theta) +{ + double **dFtr=d; + Alloc2(L, K+2, dSrc); + double DelFtr=P2_DelFtr(dFtr, L, K, x, F); + double DelSrc=P3_DelSrc(dSrc, L, M, K, x, f, F); + Multiply(L, K+2, dFtr, dFtr, (1-theta)/F); + MultiAdd(L, K+2, d, dFtr, dSrc, theta); + DeAlloc2(dSrc); + return DelFtr*(1-theta)/F+DelSrc*theta; +}//P_Ax + +/* + function P_Ax_cf: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic + coefficient of time-unvarying filter coefficients x[K+2] towards totoal source variation. + + In: x[K+2]: filter coefficients + f[L][M]: partial frequencies + F: filter response sampling interval + Out: d[K+2]: Ax. + + Returns inner product of x[] and d[]. +*/ +double P_Ax_cf(double* d, int L, int M, int K, double* x, double** f, double F) +{ + double xAx=0; + for (int l=0; l<L; l++) memset(d, 0, sizeof(double)*(K+2)); + for (int l=0; l<L-1; l++) + for (int m=0; m<M; m++) + { + double f0f=f[l][m]/F, f1f=f[l+1][m]/F; + if (f0f<=0 || f1f<=0) continue; + int k0=floor(f0f), k1=floor(f1f); + double f0=f0f-k0, f1=f1f-k1; + double g0=1-f0, g1=1-f1; + double A=-x[k1]*g1-x[k1+1]*f1+x[k0]*g0+x[k0+1]*f0; + d[k1]-=2*A*g1; + d[k1+1]-=2*A*f1; + d[k0]+=2*A*g0; + d[k0+1]+=2*A*f0; + xAx+=A*A; + } + return xAx; +}//P_Ax_cf + +/* + function P1_b: internal procedure P1 of slow-variation SF estimator. See "further reading", pp.8. + + In: a[L][M], f[L][M]: partial amplitudes and frequencies + F: filter response sampling interval + theta: balancing factor of amplitude and frequency variations + Out: b[L][K+2]: linear coefficients of filter coefficients h[L][K+2] towards the total variation. + + No return value. +*/ +void P1_b(double** b, int L, int M, int K, double** a, double** f, double F, double theta) +{ + for (int l=0; l<L; l++) memset(b[l], 0, sizeof(double)*(K+2)); + for (int l=0; l<L-1; l++) + { + for (int m=0; m<M; m++) + { + double f0f=f[l][m]/F, f1f=f[l+1][m]/F; + if (f0f<=0 || f1f<=0) continue; + int k0=floor(f0f), k1=floor(f1f); + double f0=f0f-k0, f1=f1f-k1; + double g0=1-f0, g1=1-f1; + double delta=a[l+1][m]-a[l][m]; + b[l+1][k1]+=g1*delta, b[l+1][k1+1]+=f1*delta; + b[l][k0]-=g0*delta, b[l][k0+1]-=f0*delta; + } + } + Multiply(L, K+2, b, b, theta); +}//P1_b + +/* + function P1_b_cf: internal procedure P1 of slow-variation SF estimator, constant-filter version. + + In: a[L][M], f[L][M]: partial amplitudes and frequencies + F: filter response sampling interval + Out: b[K+2]: linear coefficients of filter coefficients h[K+2] towards total source variation. + + No return value. +*/ +void P1_b_cf(double* b, int L, int M, int K, double** a, double** f, double F) +{ + memset(b, 0, sizeof(double)*(K+2)); + for (int l=0; l<L-1; l++) + { + for (int m=0; m<M; m++) + { + double f0f=f[l][m]/F, f1f=f[l+1][m]/F; + if (f0f<=0 || f1f<=0) continue; + int k0=floor(f0f), k1=floor(f1f); + double f0=f0f-k0, f1=f1f-k1; + double g0=1-f0, g1=1-f1; + double delta=a[l+1][m]-a[l][m]; + b[k1]+=g1*delta, b[k1+1]+=f1*delta; + b[k0]-=g0*delta, b[k0+1]-=f0*delta; + } + } + Multiply(K+2, b, b, 2); +}//P1_b_cf + +/* + function P2_DelFtr: internal procedure P2 of slow-variation SF estimator. See "further reading", pp.8. + + In: x[L][K+2]: filter coefficients + F: filter response sampling interval + Out: d[L][K+2]: derivative of total filter variation against filter coefficients. + + Returns the inner product of x[][] and d[][]. +*/ +double P2_DelFtr(double** d, int L, int K, double** x, double F) +{ + double xAx=0; + for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2)); + for (int l=0; l<L-1; l++) + for (int k=0; k<K+1; k++) + { + try{ + double A0=x[l+1][k]-x[l][k], A1=x[l+1][k+1]-x[l][k+1]; + d[l+1][k]+=(2*A0+A1); + d[l][k]-=(2*A0+A1); + d[l+1][k+1]+=(2*A1+A0); + d[l][k+1]-=(2*A1+A0); + xAx+=A0*A0+A1*A1+A0*A1; + } + catch(...){} + } + Multiply(L, K+2, d, d, F/3); + return xAx*F/3; +}//P2_DelFtr + +/* + function P3_DelSec: internal procedure P3 of slow-variation SF estimator. See "further reading", pp.9. + + In: x[L][K+2]: filter coefficients + f[L][M]: partial frequencies + F: filter response sampling interval + Out: d[L][K+2]: derivative of total source variation against filter coefficients. + + Returns the inner product of x[][] and d[][]. +*/ +double P3_DelSrc(double** d, int L, int M, int K, double** x, double** f, double F) +{ + double xAx=0; + for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2)); + for (int l=0; l<L-1; l++) + for (int m=0; m<M; m++) + { + double f0f=f[l][m]/F, f1f=f[l+1][m]/F; + if (f0f<=0 || f1f<=0) continue; + int k0=floor(f0f), k1=floor(f1f); + double f0=f0f-k0, f1=f1f-k1; + double g0=1-f0, g1=1-f1; + double A=-x[l+1][k1]*g1-x[l+1][k1+1]*f1+x[l][k0]*g0+x[l][k0+1]*f0; + d[l+1][k1]-=2*A*g1; + d[l+1][k1+1]-=2*A*f1; + d[l][k0]+=2*A*g0; + d[l][k0+1]+=2*A*f0; + xAx+=A*A; + } + return xAx; +}//P3_DelSec + +/* + function S_F_b: computes source coefficients given amplitudes and filter coefficients + + In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1]. + lp[P]: temporal segmentation points, in frames. + logA0C[Fr]: amplitude carrier + h[L][K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]). + F: filter response sampling interval + FScaleMode: set if mel scale is used as frequency scale + Fs: sampling frequency, effective only when FScaleMode is set. + Out: b[L][M]: source coefficients for L frames starting from ceil(lp[0]). + avgb[L]: average source coefficents for these L frames + + No return value. +*/ +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) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int L=lpp-lp0; + for (int l=0; l<L; l++) + { + int fr=lp0+l; + double loga0=logA0C[fr]; + for (int m=0; m<M; m++) + { + double f=Partials[m][fr].f; + if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700); + if (f>0) + { + double loga=log(Partials[m][fr].a); + loga-=loga0; + double ff=f/F; + int klm=floor(ff); + double f0=ff-klm; + double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0; + b[l][m]=loga-hlm; + } + else b[l][m]=-100; + } + } + // 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;} + 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;} +}//S_F_b + +/* + function S_F_b: wrapper function + + In: Partials: HS partials + SF: TSF object containing valid filter coefficients + Out: SF: TSF object with source coefficients updated + + No return value. +*/ +void S_F_b(TSF& SF, atom** Partials) +{ + 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); +}//S_F_b + +/* + function S_F_b: filter-bank method for estimating source-filter model. This is a wrapper function of + SF_FB() that transfers and scales values, etc. + + In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1]. + lp[P]: temporal segmentation points, in frames. + logA0C[Fr]: amplitude carrier + F: filter response sampling interval + FScaleMode: set if mel scale is used as frequency scale + Fs: sampling frequency, effective only when FScaleMode is set. + Out: K + h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + + Returns 0. +*/ +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) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int L=lpp-lp0; + Alloc2(L, M, a); Alloc2(L, M, f); + double fmax=0; + for (int l=0; l<L; l++) + { + int fr=lp0+l; + double ia0c=exp(-logA0C[fr]); + for (int m=0; m<M; m++) + { + f[l][m]=Partials[m][fr].f; + if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700); + if (f[l][m]>0) a[l][m]=Partials[m][fr].a*ia0c; //log(Partials[m][fr].a); + else a[l][m]=0; + if (fmax<f[l][m]) fmax=f[l][m]; + } + } + K=floor(fmax/F); + + + SF_FB(h[0], M, K, &a[0], &f[0], F, 1); + for (int l=1; l<L-1; l++) SF_FB(h[l], M, K, &a[l], &f[l], F, 0); + SF_FB(h[L-1], M, K, &a[L-1], &f[L-1], F, -1); + + for (int l=0; l<L; l++) + for (int m=0; m<M; m++) + { + double ff=f[l][m]/F; + if (ff<=0) b[l][m]=-100; + else + { + int klm=floor(ff); + double f0=ff-klm; + double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0; + b[l][m]=log(a[l][m])-hlm; + } + } + + 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;} + 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;} + + DeAlloc2(a); DeAlloc2(f); + return 0; +}//S_F_b + +/* + function S_F_SV: slow-variation method for estimating source-filter model. This is a wrapper function + of SF_SV() that transfers and scales values, etc. + + In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1]. + lp[P]: temporal segmentation points, in frames. + logA0C[Fr]: amplitude carrier + theta: balancing factor of amplitude and frequency variations + F: filter response sampling interval + FScaleMode: set if mel scale is used as frequency scale + Fs: sampling frequency, effective only when FScaleMode is set. + Out: K + h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + Returns 0. +*/ +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) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int L=lpp-lp0; + Alloc2(L, M, loga); Alloc2(L, M, f); + double fmax=0; + for (int l=0; l<L; l++) + { + int fr=lp0+l; + for (int m=0; m<M; m++) + { + f[l][m]=Partials[m][fr].f; + if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700); + if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a); + else loga[l][m]=-100; + if (fmax<f[l][m]) fmax=f[l][m]; + } + } + K=floor(fmax/F); + + for (int l=0; l<L; l++){double loga0=logA0C[lp0+l]; for (int m=0; m<M; m++) loga[l][m]-=loga0;} + + SF_SV(h, L, M, K, loga, f, F, theta, 1e-6, L*(K+2)); + + for (int l=0; l<L; l++) + for (int m=0; m<M; m++) + { + double ff=f[l][m]/F; + if (ff<=0) continue; + int klm=floor(ff); + double f0=ff-klm; + double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0; + b[l][m]=loga[l][m]-hlm; + } + + 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;} + 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;} + + DeAlloc2(loga); DeAlloc2(f); + return 0; +}//S_F_SV + +/* + function S_F_SV_cf: slow-variation method for estimating source-(constant)filter model. This is a + wrapper function of SF_SV_cf() that transfers and scales values, as well as converting results to + source filter format used by the vibrato class TVo. + + In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1]. + lp[P]: temporal segmentation points, in frames. + A0C[Fr]: amplitude carrier (linear, not dB) + F: filter response sampling interval + FScaleMode: set if mel scale is used as frequency scale + Fs: sampling frequency, effective only when FScaleMode is set. + Out: K + h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages + LogAF[afres/2]: TVo-format filter response + LogAS[M]: TVo-format source response + + Returns 0. +*/ +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) +{ + int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]); + int L=lpp-lp0; + Alloc2(L, M, loga); Alloc2(L, M, f); + double fmax=0; + for (int l=0; l<L; l++) + { + int fr=lp0+l; + for (int m=0; m<M; m++) + { + try{ + f[l][m]=Partials[m][fr].f; + } + catch(...) + { + m=m; + } + if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700); + if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a); + else loga[l][m]=-100; + if (fmax<f[l][m]) fmax=f[l][m]; + } + } + K=floor(fmax/F); + + SF_SV_cf(h[0], b, L, M, K, loga, f, F, 1e-6, K+2); + for (int l=0; l<L; l++) + { + if (l>0) memcpy(h[l], h[0], sizeof(double)*(K+2)); + double loga0=log(A0C[l]); + for (int m=0; m<M; m++) b[l][m]-=loga0; + } + + double* lh=new double[K+2]; memset(lh, 0, sizeof(double)*(K+2)); + for (int k=0; k<K+2; k++) + { + for (int l=0; l<L; l++) lh[k]+=h[l][k]; + lh[k]/=L; + } + for (int i=0; i<afres/2; i++) + { + double freq=i*1.0/afres; + if (FScaleMode) freq=log(1+freq*Fs/700)/log(1+Fs/700); + int k=floor(freq/F); + double f_plus=freq/F-k; + if (k<K+1) LogAF[i]=lh[k]*(1-f_plus)+lh[k+1]*f_plus; + else LogAF[i]=lh[K+1]; + } + delete[] lh; + + for (int m=0; m<M; m++) + { + int ccount=0; + for (int fr=lp0; fr<lpp; fr++) + { + double f=Partials[m][fr].f*afres; + if (f<=0) continue; + int intf=floor(f); + LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf)); + ccount++; + } + if (ccount>0) LogAS[m]/=ccount; + else LogAS[m]=0; + } + + DeAlloc2(loga); DeAlloc2(f); + return 0; +}//S_F_SV_cf + +/* + function SF_FB: computes filter coefficients with filter-bank method: single frame. See "further + reading", pp.12, P5. + + In: al[][M], fl[][M]: partial amplitudes and frequencies, al[0] and fl[0] are of current frame + F: filter response sampling interval + LMode: 1: current frame is the first frame; -1: the last frame; 0: neither first nor last frame + Out: hl[K+2]: filter coefficients integrated at current frame + + Returns K. +*/ +int SF_FB(double* hl, int M, int K, double** al, double** fl, double F, int LMode) +{ + memset(hl, 0, sizeof(double)*(K+2)); + double* norm=new double[K+2]; memset(norm, 0, sizeof(double)*(K+2)); + for (int m=0; m<M; m++) + { + int dfsgn, k, K1; + if (LMode!=1) + { + if (fl[0][m]-fl[-1][m]==0) + { + double a0, wk, wk_, a1; + int k=floor(fl[0][m]/F); + a0=al[-1][m], a1=al[0][m]; + wk=1-(fl[0][m]/F-k), wk_=1-wk; + double dh=(a0+2*a1)/6.0; + hl[k]+=wk*dh; //I3(1, wk, 0, a0, wk, 1, a1); + hl[k+1]+=wk_*dh;//I3(1, wk_, 0, a0, wk_, 1, a1); + norm[k]+=wk*0.5;//I3(1, wk, 0, 1, wk, 1, 1); + norm[k+1]+=wk_*0.5;//I3(1, wk_, 0, 1, wk_, 1, 1); + } + else + { + double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_; + dfsgn=Sign(fl[0][m]-fl[-1][m]); + if (dfsgn>0) K1=ceil(fl[-1][m]/F); else K1=floor(fl[-1][m]/F); + t0=-1; k=K1; + if (fl[-1][m]>0 && fl[0][m]>0) do + { + t1=-1+(k*F-fl[-1][m])/(fl[0][m]-fl[-1][m]); + if (t1>0) t1=0; + + if (t0==-1){a0=al[-1][m], u0=0, w0k=1-fabs(fl[-1][m]/F-k); w0k_=1-w0k;} + else{a0=al[0][m]+t0*(al[0][m]-al[-1][m]), u0=1+t0, w0k=0, w0k_=1;} + if (t1==0){a1=al[0][m], u1=1, w1k=1-fabs(fl[0][m]/F-k); w1k_=1-w1k;} + else{a1=al[0][m]+t1*(al[0][m]-al[-1][m]), u1=1+t1, w1k=1, w1k_=0;} + + hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1); + hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1); + norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1); + norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1); + + t0=t1; + k+=dfsgn; + } + while (t1<0); + } + } + + if (LMode!=-1) + { + double a0, wk, wk_, a1; + if (fl[1][m]-fl[0][m]==0) + { + int k=floor(fl[0][m]/F); + a0=al[0][m], a1=al[1][m]; + wk=1-fabs(fl[0][m]/F-k), wk_=1-wk; + double dh=(2*a0+a1)/6.0; + hl[k]+=wk*dh; //I3(1, wk, 1, a0, wk, 0, a1); + hl[k+1]+=wk_*dh;//I3(1, wk_, 1, a0, wk_, 0, a1); + norm[k]+=wk*0.5;//I3(1, wk, 1, 1, wk, 0, 1); + norm[k+1]+=wk_*0.5;//I3(1, wk_, 1, 1, wk_, 0, 1); + } + else + { + double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_; + dfsgn=Sign(fl[1][m]-fl[0][m]); + if (dfsgn>0) K1=ceil(fl[0][m]/F); else K1=floor(fl[0][m]/F); + t0=0; k=K1; + if (fl[0][m]>0 && fl[1][m]>0) do + { + try{ + t1=(k*F-fl[0][m])/(fl[1][m]-fl[0][m]);} + catch(...){} + if (t1>1) t1=1; + + if (t0==0){a0=al[0][m], u0=1, w0k=1-fabs(fl[0][m]/F-k); w0k_=1-w0k;} + else{a0=al[0][m]+t0*(al[1][m]-al[0][m]), u0=1-t0, w0k=1, w0k_=0;} + if (t1==1){a1=al[1][m], u1=0, w1k=1-fabs(fl[1][m]/F-k); w1k_=1-w1k;} + else{a1=al[0][m]+t1*(al[1][m]-al[0][m]), u1=1-t1, w1k=0, w1k_=1;} + + hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1); + hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1); + norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1); + norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1); + + t0=t1; + k+=dfsgn; + } + while (t1<1); + } + } + } + + int mink=0; while (mink<=K+1 && norm[mink]==0) mink++; + int maxk=K+1; while (maxk>=0 && norm[maxk]==0) maxk--; + int lastk=mink, nextk=-1; + for (int k=mink; k<=maxk; k++) + { + if (k==nextk) lastk=k; + else if (norm[k]!=0) hl[k]=log(hl[k]/norm[k]), lastk=k; + else + { + if (k>nextk){nextk=k+1; while (norm[nextk]==0) nextk++; hl[nextk]=log(hl[nextk]/norm[nextk]);} + hl[k]=(hl[lastk]*(nextk-k)+hl[nextk]*(k-lastk))/(nextk-lastk); + } + } + for (int k=0; k<mink; k++) hl[k]=hl[mink]; + for (int k=maxk+1; k<K+2; k++) hl[k]=hl[maxk]; + + delete[] norm; + return K; +}//SF_FB + +/* + function SF_SV: CG method for estimation source-filter model using slow-variation criterion. See + "further reading", pp.9, boxed algorithm P4. + + In: a[L][M], f[L][M]: partial amplitudes and frequencies + F: filter response sampling interval + theta: balancing factor of amplitude and frequency variations + ep: convergence test threshold + maxiter: maximal number of iterates + Out: h[L][K+2]: filter coefficients + + Returns K. +*/ +int SF_SV(double** h, int L, int M, int K, double** a, double** f, double F, double theta, double ep, int maxiter) +{ + Alloc2(L*7, K+2, b); + double **Ax=&b[L], **d=&Ax[L], **r=&d[L], **q=&r[L]; + for (int l=0; l<L; l++) memset(h[l], 0, sizeof(double)*(K+2)); + //calcualte b + P1_b(b, L, M, K, a, f, F, theta); + //calculate Ah + //double hAh= + P_Ax(Ax, L, M, K, h, f, F, theta); + //double **t_Ax=&b[L*5], **t_h=&b[L*6], epdel=1e-10, t_err=0; + + //double bh=Inner(L, K+2, b, h); //debug + //double Del=0.5*hAh-bh; //debug + + MultiAdd(L, K+2, r, b, Ax, -1); + Copy(L, K+2, d, r); + double delta=Inner(L, K+2, r, r); + ep=ep*delta; + + int iter=0; + while(iter<maxiter && delta>ep) + { + P_Ax(q, L, M, K, d, f, F, theta); + /*debug point: watch if hAh-bh keeps decreasing + double lastdel=Del; + hAh=P_Ax(t_Ax, L, M, K, h, f, F, theta); + bh=Inner(L, K+2, b, h); + Del=hAh-bh; + if (Del>lastdel) + {lastdel=Del;} //set breakpoint here*/ + double alpha=delta/Inner(L, K+2, d, q); + MultiAdd(L, K+2, h, h, d, alpha); + if (iter%50==0 && false){P_Ax(Ax, L, M, K, h, f, F, theta); MultiAdd(L, K+2, r, b, Ax, -1);} + else MultiAdd(L, K+2, r, r, q, -alpha); + double deltanew=Inner(L, K+2, r, r); + MultiAdd(L, K+2, d, r, d, deltanew/delta); + delta=deltanew; + iter++; + } + + DeAlloc2(b); + return K; +}//SF_SV + +/* + function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion. + + In: a[L][M], f[L][M]: partial amplitudes and frequencies + F: filter response sampling interval + ep: convergence test threshold + maxiter: maximal number of iterates + Out: h[K+2]: filter coefficients + + Returns K. +*/ +int SF_SV_cf(double* h, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter) +{ + double* b=new double[(K+2)*7]; + double *Ax=&b[K+2], *d=&Ax[K+2], *r=&d[K+2], *q=&r[K+2]; + + //calcualte b + P1_b_cf(b, L, M, K, a, f, F); + //calculate Ah + //double hAh= + P_Ax_cf(Ax, L, M, K, h, f, F); + + MultiAdd(K+2, r, b, Ax, -1); + Copy(K+2, d, r); + double delta=Inner(K+2, r, r); + ep=ep*delta; + + int iter=0; + while(iter<maxiter && delta>ep) + { + P_Ax_cf(q, L, M, K, d, f, F); + double alpha=delta/Inner(K+2, d, q); + MultiAdd(K+2, h, h, d, alpha); + if (iter%50==0 && false){P_Ax_cf(Ax, L, M, K, h, f, F); MultiAdd(K+2, r, b, Ax, -1);} + else MultiAdd(K+2, r, r, q, -alpha); + double deltanew=Inner(K+2, r, r); + MultiAdd(K+2, d, r, d, deltanew/delta); + delta=deltanew; + iter++; + } + + delete[] b; + return K; +}//SF_SV_cf + +/* + function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion. + + In: a[L][M], f[L][M]: partial amplitudes and frequencies + F: filter response sampling interval + ep: convergence test threshold + maxiter: maximal number of iterates + Out: h[K+2]: filter coefficients + b[L][M]: source coefficients + + No reutrn value. +*/ +void SF_SV_cf(double* h, double** b, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter) +{ + memset(h, 0, sizeof(double)*(K+2)); + SF_SV_cf(h, L, M, K, a, f, F, ep, maxiter); + for (int l=0; l<L; l++) + for (int m=0; m<M; m++) + { + double ff=f[l][m]/F; + if (ff<=0) continue; + int klm=floor(ff); + double f0=ff-klm; + double hlm=h[klm]*(1-f0)+h[klm+1]*f0; + b[l][m]=a[l][m]-hlm; + } +}//SF_SV_cf + +/* + function Sign: sign function + + In: x: a floating-point number + + Returns 0 if x=0, 1 if x>0, -1 if x<0. +*/ +int Sign(double x) +{ + if (x==0) return 0; + else if (x>0) return 1; + else return -1; +}//Sign + +/* + function SynthesizeSF: synthesizes a harmonic sinusoid representation from a TSF object. + + In: SF: a TSF object + sps: sampling rate ("samples per second") + Out: HS: teh synthesized HS. + + No return value. +*/ +void SynthesizeSF(THS* HS, TSF* SF, double sps) +{ + int t0=HS->Partials[0][0].t, s0=HS->Partials[0][0].s, L=SF->L, M=SF->M; + double *F0C=SF->F0C, *F0D=SF->F0D, *F0=SF->F0, offst=SF->offst; + double *logA0C=useA0?SF->logA0:SF->logA0C; + HS->Resize(M, L); + + atom** Partials=HS->Partials; + + for (int l=0; l<L; l++) + { + double tl=t0+offst*l; + F0[l]=F0C[l]+F0D[l]; + for (int m=0; m<M; m++) + { + Partials[m][l].s=s0; + Partials[m][l].t=tl; + Partials[m][l].f=(m+1)*F0[l]; + + if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].a=0; + else Partials[m][l].a=exp(logA0C[l]+SF->LogAS(m,l)+SF->LogAF(Partials[m][l].f,l)); + } + } +}//SynthesizeSF + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hssf.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,118 @@ +#ifndef hssfH +#define hssfH + +/* + hssf.cpp - source-filter modeling for harmonic sinusoids + + Further reading: Wen X. and M. Sandler, "Source-filter modeling in sinusoid domain," in Proc. AES 126th + Convention, Munich, 2009. +*/ + + +#include <stdio.h> +#include "hs.h" + +//--------------------------------------------------------------------------- +const double Amel=1127.0104803341574386544633680278; +const bool useA0=true; //if true, use A0D+A0C instead of A0C in S-F decomposition as pre-normalizer + + +/* + TSF is the class implementing source-filter model for harmonic sinusoids. TSF shares the basic framework + of the vibrato description class TVo, but implements a more compact source-filter representation. It does + not go into detailed vibrato analysis such as extraction modulator shape. + + An analysis/synthesis cycle converts THS to TSF and back. +*/ + +struct TSF +{ + //basic characteristics + int M; //number of partials + int L; //number of frames + int P; //number of segmentation points + double offst; //hop size + double* F0C; //[0:L-1] pitch carrier + double* F0D; //[0:L-1] pitch modulator + double* logA0C; //[0:L-1] amplitude carreir + double* logA0D; //[0:L-1] amplitude modulator + + double* lp; //[0:P-1] peak positions + + double F; //filter: band with (linear or mel) associated to each b[][] + double Fs; //sampling frequency + int FScaleMode; //linear or mel + int K; //number of filter bands + double** b; //[0:L-1][0:M-1] single-frame source, in dB + double** h; //[0:L-1][0:K+1] single-frame filter, in dB + double* avgb; //[0:M-1] average source + double* avgh; //[0:K+1] average filter + + //other properties + + double rate; //vibrato rate + double regularity;//vibrato regularity + double F0max; //maximal fundamental frequency + double F0min; //minimal fundamental frequency + double F0Cmax; //maximal fundamental carrier frequency + double F0Cmin; //minimal fundamental carrier frequency + double F0Overall; //overall average fundamental frequency + double F0Dmax; //maximal fundamental modulator frequency + double F0Dmin; //minimal fundamental modulator frequency + double* F0; //[0:L-1] fundamental frequency + double* logA0; //[0:L-1] log amplitude + + TSF(); + ~TSF(); + + //copy function + void Duplicate(TSF& SF); + + //output routines + double LogAF(double f); + double LogAF(double f, int fr); + double LogAS(int m, int fr); + + //memory handling routines + void AllocateL(int AnL); + void ReAllocateL(int newL); + void AllocateP(); + void AllocateSF(); + + //I/O routines + void SaveSFToFileHandle(FILE* f); + void SaveToFileHandle(FILE* f); + void LoadSFFromFileHandle(FILE* f); + void LoadFromFileHandle(FILE* f); + void SaveToFile(char* filename); + + //other member functions + void ShiftFilterByDB(double dB); +}; + +//--tool procedures---------------------------------------------------------- +int Sign(double); + +//--general source-filter model routines------------------------------------- +void S_F_b(TSF& SF, atom** Partials); + +//--slow-variation SF estimation routines------------------------------------ +double P2_DelFtr(double** d, int L, int K, double** x, double F); +double P3_DelSrc(double** d, int L, int M, int K, double** x, double** f, double F); +int SF_SV(double** h, int L, int M, int K, double** a, double** f, double F, double theta, double ep, int maxiter); +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=0.005, int FScaleMode=0, double theta=0.5, double Fs=44100); +void SF_SV_cf(double* h, double** b, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter); +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=0.005, int FScaleMode=0, double Fs=44100); + +//--filter-bank SF estimation routines--------------------------------------- +int SF_FB(double* hl, int M, int K, double** al, double** fl, double F, int LMode); +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=0.005, int FScaleMode=0, double Fs=44100); + +//--source-filter analysis and synthesis routines---------------------------- +void AnalyzeSF_1(THS& HS, TSF& SF, double sps, double offst); +void AnalyzeSF_2(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, int* cyclefrcount=0, int SFMode=0, double SFF=0.01, int SFFScale=0, double SFtheta=0); +void AnalyzeSF(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, double offst, int* cyclefrcount=0, int SFMode=0, double SFF=0.01, int SFFScale=0, double SFtheta=0); +void SynthesizeSF(THS* HS, TSF* SF, double sps); + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/multires.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,849 @@ +//--------------------------------------------------------------------------- + + +#include <math.h> +#include "multires.h" +#include "arrayalloc.h" +#include "procedures.h" + +//--------------------------------------------------------------------------- + +//function xlogx(x): returns x*log(x) +inline double xlogx(double x) +{ + if (x==0) return 0; + else return x*log(x); +}//xlogx + +//macro NORMAL_: a normalization step used for tiling +#define NORMAL_(A, a) A=a*(A+log(a)); +// #define NORMAL_(A, a) A=a*a*A; +// #define NORMAL_(A, a) A=sqrt(a)*A; + +/* + function DoCutSpectrogramSquare: find optimal tiling of a square. This is a recursive procedure. + + In: Specs[0][1][N], Specs[1][2][N/2], ..., Specs[log2(N)][N][1], multiresolution power spectrogram + e[Res]: total power of each level, e[i] equals the sum of Specs[i][][] + NN: maximal tile height + Out: cuts[N-1]: the tiling result + ents[Res] + + Returns the entropy of the output tiling. +*/ +double DoCutSpectrogramSquare(int* cuts, double*** Specs, double* e, int N, int NN, bool Norm, double* ents) +{ + double result; + int Res=log2(N)+1; + + if (N==1) // 1*1 only(no cuts), returns the sample function. + { + double sp00; + if (e[0]!=0) sp00=Specs[0][0][0]/e[0]; else sp00=0; + ents[0]=xlogx(sp00); + return ents[0]; + } + else if (N==2) + { + double sp00, sp01, sp10, sp11; + if (e[0]!=0) sp00=Specs[0][0][0]/e[0], sp01=Specs[0][0][1]/e[0]; else sp00=sp01=0; + if (e[1]!=0) sp10=Specs[1][0][0]/e[1], sp11=Specs[1][1][0]/e[1]; else sp10=sp11=0; + double ent0=xlogx(sp00)+xlogx(sp01); + double ent1=xlogx(sp10)+xlogx(sp11); + if (ent0<ent1) + { + cuts[0]=1; + ents[0]=0, ents[1]=ent1; + } + else + { + cuts[0]=0; + ents[0]=ent0, ents[1]=0; + } + } + else + { + int* tmpcuts=new int[N-2]; + int *lcuts, *rcuts; + double ***lSpecs, ***rSpecs, *el, *er, ent0, ent1, a; + double *entl0=new double[Res-1], *entr0=new double[Res-1], + *entl1=new double[Res-1], *entr1=new double[Res-1]; + //vertical cuts: l->left half, r->right half + if (N<=NN) + { + lcuts=&cuts[1], rcuts=&cuts[N/2]; + VSplitSpecs(N, Specs, lSpecs, rSpecs); + el=new double[Res-1], er=new double[Res-1]; + memset(el, 0, sizeof(double)*(Res-1)); memset(er, 0, sizeof(double)*(Res-1)); + if (Norm) + { + //normalization + for (int i=0, Fr=1, n=N/2; i<Res-1; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) for (int k=0; k<n; k++) + el[i]+=lSpecs[i][j][k], er[i]+=rSpecs[i][j][k]; + } + else + for (int i=0; i<Res-1; i++) el[i]=er[i]=1; + + DoCutSpectrogramSquare(lcuts, lSpecs, el, N/2, NN, Norm, entl1); + DoCutSpectrogramSquare(rcuts, rSpecs, er, N/2, NN, Norm, entr1); + + ent1=0; + + for (int i=0; i<Res-1; i++) + { + if (e[i]!=0) + { + a=el[i]/e[i]; if (a>0) {NORMAL_(entl1[i], a);} else entl1[i]=0; ent1=ent1+entl1[i]; + a=er[i]/e[i]; if (a>0) {NORMAL_(entr1[i], a);} else entr1[i]=0; ent1=ent1+entr1[i]; + } + else + entl1[i]=entr1[i]=0; + } + + DeAlloc2(lSpecs); DeAlloc2(rSpecs); + delete[] el; delete[] er; + + } + //horizontal cuts: l->lower half, r->upper half + lcuts=tmpcuts, rcuts=&tmpcuts[N/2-1]; + HSplitSpecs(N, Specs, lSpecs, rSpecs); + el=new double[Res-1], er=new double[Res-1]; + memset(el, 0, sizeof(double)*(Res-1)); memset(er, 0, sizeof(double)*(Res-1)); + if (Norm) + { + //normalization + for (int i=0, Fr=1, n=N/2; i<Res-1; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) for (int k=0; k<n; k++) + el[i]+=lSpecs[i][j][k], er[i]+=rSpecs[i][j][k]; + } + else + for (int i=0; i<Res-1; i++) el[i]=er[i]=1; + + DoCutSpectrogramSquare(lcuts, lSpecs, el, N/2, NN, Norm, entl0); + DoCutSpectrogramSquare(rcuts, rSpecs, er, N/2, NN, Norm, entr0); + + ent0=0; + + if (Norm) + for (int i=0; i<Res-1; i++) + { + if (e[i]!=0) + { + a=el[i]/e[i]; if (a>0) {NORMAL_(entl0[i], a);} else entl0[i]=0; ent0=ent0+entl0[i]; + a=er[i]/e[i]; if (a>0) {NORMAL_(entr0[i], a);} else entr0[i]=0; ent0=ent0+entr0[i]; + } + else + entl0[i]=entr0[i]=0; + } + + DeAlloc2(lSpecs); DeAlloc2(rSpecs); + delete[] el; delete[] er; + + if (N<=NN && ent0<ent1) + { + cuts[0]=1; + result=ent1; + for (int i=0; i<Res-1; i++) + { + ents[i+1]=entl1[i]+entr1[i]; + } + ents[0]=0; + } + else + { + memcpy(&cuts[1], tmpcuts, sizeof(int)*(N-2)); + cuts[0]=0; + result=ent0; + for (int i=0; i<Res-1; i++) + { + ents[i]=entl0[i]+entr0[i]; + } + ents[Res-1]=0; + } + + delete[] tmpcuts; + delete[] entl0; delete[] entl1; delete[] entr0; delete[] entr1; + } + + return result; +}//DoCutSpectrogramSquare + +/* + function DoMixSpectrogramSquare: renders a composite spectrogram on a pixel grid. This is a recursive + procedure. + + In: Specs[0][1][N], Specs[1][2][N/2], Specs[2][4][N/4], ..., Specs[][N][1]: multiresolution power + spectrogram + cuts[N-1]: tiling + X, Y: dimensions of pixel grid to render + Out: Spec[X][Y]: pixel grid rendered to represent the given spectrograms and tiling + + No return value; +*/ +void DoMixSpectrogramSquare(double** Spec, int* cuts, double*** Specs, int N, bool Norm, int X=0, int Y=0) +{ + if (X==0 && Y==0) X=Y=N; + + if (N==1) + { + double value=Specs[0][0][0];//sqrt(Specs[0][0][0]); + value=value; + for (int x=0; x<X; x++) for (int y=0; y<Y; y++) Spec[x][y]=value; + } + else + { + double* e; + int Res; + + if (Norm) + { + //normalization + Res=log2(N)+1; + e=new double[Res]; + memset(e, 0, sizeof(double)*Res); + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + e[i]+=Specs[i][j][k]; + double em=e[0]; + for (int i=1; i<Res; i++) + { + if (e[i]>em) e[i]=em/e[i]; + else e[i]=1; + if (e[i]>em) em=e[i]; + } e[0]=1; + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[1]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]*=e[i]; + } + } + + double **lSpec, **rSpec, ***lSpecs, ***rSpecs; + if (cuts[0]) //1: vertical split + { + VSplitSpecs(N, Specs, lSpecs, rSpecs); + VSplitSpec(X, Y, Spec, lSpec, rSpec); + DoMixSpectrogramSquare(lSpec, &cuts[1], lSpecs, N/2, Norm, X/2, Y); + DoMixSpectrogramSquare(rSpec, &cuts[N/2], rSpecs, N/2, Norm, X/2, Y); + } + else //0: horizontal split + { + HSplitSpecs(N, Specs, lSpecs, rSpecs); + HSplitSpec(X, Y, Spec, lSpec, rSpec); + DoMixSpectrogramSquare(lSpec, &cuts[1], lSpecs, N/2, Norm, X, Y/2); + DoMixSpectrogramSquare(rSpec, &cuts[N/2], rSpecs, N/2, Norm, X, Y/2); + } + + if (Norm) + { + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[1]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]/=e[i]; + } + delete[] e; + } + + delete[] lSpec; delete[] rSpec; DeAlloc2(lSpecs); DeAlloc2(rSpecs); + } +}//DoMixSpectrogramSquare + +/* + function DoMixSpectrogramSquare: retrieves a composite spectrogram as a vector. This is a recursive + procedure. + + In: Specs[0][1][N], Specs[1][2][N/2], Specs[2][4][N/4], ..., Specs[][N][1]: multiresolution power + spectrogram + cuts[N-1]: tiling + Out: Spec[N]: composite spectrogram sampled fron Specs according to tiling cut[] + + No return value; +*/ +void DoMixSpectrogramSquare(double* Spec, int* cuts, double*** Specs, int N, bool Norm) +{ +// if (X==0 && Y==0) X=Y=N; + + if (N==1) + Spec[0]=Specs[0][0][0];//sqrt(Specs[0][0][0]); + else + { + double* e; + int Res; + + //Norm=false; + if (Norm) + { + //normalization + Res=log2(N)+1; + e=new double[Res]; + memset(e, 0, sizeof(double)*Res); + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + e[i]+=Specs[i][j][k]; + double em=e[0]; + for (int i=1; i<Res; i++) + { + if (e[i]>em) e[i]=em/e[i]; + else e[i]=1; + if (e[i]>em) em=e[i]; + } e[0]=1; + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[i]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]*=e[i]; + } + } + + double ***lSpecs, ***rSpecs; + if (cuts[0]) //1: vertical split + { + VSplitSpecs(N, Specs, lSpecs, rSpecs); + DoMixSpectrogramSquare(Spec, &cuts[1], lSpecs, N/2, Norm); + DoMixSpectrogramSquare(&Spec[N/2], &cuts[N/2], rSpecs, N/2, Norm); + } + else //0: horizontal split + { + HSplitSpecs(N, Specs, lSpecs, rSpecs); + DoMixSpectrogramSquare(Spec, &cuts[1], lSpecs, N/2, Norm); + DoMixSpectrogramSquare(&Spec[N/2], &cuts[N/2], rSpecs, N/2, Norm); + } + + if (Norm) + { + for (int i=0, Fr=1, n=N; i<Res; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[1]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]/=e[i]; + } + delete[] e; + } + + DeAlloc2(lSpecs); DeAlloc2(rSpecs); + } +}//DoMixSpectrogramSquare + +//--------------------------------------------------------------------------- +/* + function HSplitSpec: split a spectrogram horizontally into lower and upper halves. + + In: Spec[X][Y]: spectrogram to split + Out: lSpec[X][Y/2], uSpec[X][Y/2]: the two half spectrograms + + No return value. Both lSpec and uSpec are allocated anew. The caller is responsible to free these + buffers. +*/ +void HSplitSpec(int X, int Y, double** Spec, double**& lSpec, double**& uSpec) +{ + lSpec=new double*[X], uSpec=new double*[X]; + for (int i=0; i<X; i++) + lSpec[i]=Spec[i], uSpec[i]=&Spec[i][Y/2]; +}//HSplitSpec + +/* + function HSplitSpecs: split a multiresolution spectrogram horizontally into lower and upper halves + + A full spectrogram array is given in log2(N)+1 spectrograms, with the base spec of 1*N, 1st octave of + 2*(N/2), ..., last octave of N*1. When this array is split into two spectrogram arrays horizontally, + the last spec (with the highest time resolution). Each of the two new arrays is given in log2(N) + spectrograms. + + In: Specs[nRes+1][][]: multiresolution spectrogram + Out: lSpecs[nRes][][], uSpecs[nRes][][], the two half multiresolution spectrograms + + This function allocates two 2nd order arrays of double*, which the caller is responsible to free. +*/ +void HSplitSpecs(int N, double*** Specs, double***& lSpecs, double***& uSpecs) +{ + int nRes=log2(N); // new number of resolutions + lSpecs=new double**[nRes], uSpecs=new double**[nRes]; + lSpecs[0]=new double*[nRes*N/2], uSpecs[0]=new double*[nRes*N/2]; for (int i=1; i<nRes; i++) lSpecs[i]=&lSpecs[0][i*N/2], uSpecs[i]=&uSpecs[0][i*N/2]; + for (int i=0, Fr=1, n=N; i<nRes; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) lSpecs[i][j]=Specs[i][j], uSpecs[i][j]=&Specs[i][j][n/2]; +}//HSplitSpecs + +//--------------------------------------------------------------------------- +/* + function MixSpectrogramSquare: find composite spectrogram from multiresolution spectrogram,output as + pixel grid + + In: Specs[0][1][N], Specs[1][2][N/2], ..., Specs[Res-1][N][1], multiresolution spectrogram + Out: Spec[N][N]: composite spectrogram as pixel grid (N time redundant) + cuts[N-1] (optional): tiling + + Returns entropy. +*/ +double MixSpectrogramSquare(double** Spec, double*** Specs, int N, int Res, bool Norm, bool NormMix, int* cuts=0) +{ + int tRes=log2(N)+1; + if (Res==0) Res=tRes; + int NN=1<<(Res-1); + bool createcuts=(cuts==0); + if (createcuts) cuts=new int[N]; + double* e=new double[tRes], *ents=new double[tRes]; + //normalization + memset(e, 0, sizeof(double)*Res); + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + e[i]+=Specs[i][j][k]; + + if (!Norm) + { + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[i]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]/=e[i]; + } +// for (int i=0; i<Res; i++) e[i]=1; + } + + double result=DoCutSpectrogramSquare(cuts, Specs, e, N, NN, Norm, ents); + delete[] ents; + + if (!Norm) + { + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[i]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]*=e[i]; + } + } + DoMixSpectrogramSquare(Spec, cuts, Specs, N, NormMix, N, N); + + delete[] e; + if (createcuts) delete[] cuts; + return result; +}//MixSpectrogramSquare + +//--------------------------------------------------------------------------- +/* + function MixSpectrogramSquare: find composite spectrogram from multiresolution spectrogram,output as + vectors + + In: Specs[0][1][N], Specs[1][2][N/2], ..., Specs[Res-1][N][1], multiresolution spectrogram + Out: spl[N-1], Spec[N]: composite spectrogram as tiling and value vectors + + Returns entropy. +*/ +double MixSpectrogramSquare(int* spl, double* Spec, double*** Specs, int N, int Res, bool Norm, bool NormMix) +{ + int tRes=log2(N)+1; + if (Res==0) Res=tRes; + int NN=1<<(Res-1); + + double* e=new double[tRes], *ents=new double[tRes]; + + //normalization + memset(e, 0, sizeof(double)*Res); + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + e[i]+=Specs[i][j][k]; + + if (!Norm) + { + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[i]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]/=e[i]; + } +// for (int i=0; i<Res; i++) e[i]=1; + } + + double result=DoCutSpectrogramSquare(spl, Specs, e, N, NN, Norm, ents); + delete[] ents; + if (!Norm) + { + for (int i=0, Fr=1, n=N; i<tRes; i++, Fr*=2, n/=2) + { + if (e[i]!=0 && e[i]!=1) + for (int j=0; j<Fr; j++) + for (int k=0; k<n; k++) + Specs[i][j][k]*=e[i]; + } + } + DoMixSpectrogramSquare(Spec, spl, Specs, N, NormMix); + return result; +}//MixSpectrogramSquare + +//--------------------------------------------------------------------------- +/* + function MixSpectrogramBlock: obtain the composite spectrogram of a waveform block as pixel grid. + + This method deals with the effective duration of WID/2 samples of a frame of WID samples. The maximal + frame width is WID, minimal frame width is wid. The spectrum with frame width WID (base) is given in + lSpecs[0][0], the spectra with frame width WID/2 (1st octave) is given in lSpecs[1][0] & lSpecs[1][1], + etc. + + The output Spec[WID/wid][WID] is a spectrogram sampled from lSpecs, with a redundancy factor WID/wid. + The sampling is optimized so that a maximal entropy is achieved globally. This maximal entropy is + returned. + + In: Specs[0][1][WID], Specs[1][2][WID/2], ..., Specs[Res-1][WID/wid][wid], multiresolution spectrogram + Out: Spec[WID/wid][WID], composite spectrogram as pixel grid + cuts[wid][WID/wid-1] (optional), tilings of the wid squares the block is divided into. + + Returns entropy. +*/ +double MixSpectrogramBlock(double** Spec, double*** Specs, int WID, int wid, bool norm, bool normmix, int** cuts=0) +{ + int N=WID/wid, Res=log2(WID/wid)+1; + double result=0, **lSpec=new double*[N], ***lSpecs=new double**[Res]; + lSpecs[0]=new double*[Res*N]; for (int i=1; i<Res; i++) lSpecs[i]=&lSpecs[0][i*N]; + + bool createcuts=(cuts==0); + if (createcuts) + { + cuts=new int*[wid]; + memset(cuts, 0, sizeof(int*)*wid); + } + + for (int i=0; i<wid; i++) + { + for (int j=0; j<N; j++) + lSpec[j]=&Spec[j][i*N]; + for (int j=0, n=N, Fr=1; j<Res; j++, n/=2, Fr*=2) + { + for (int k=0; k<Fr; k++) + lSpecs[j][k]=&Specs[j][k][i*n]; + } + + int Log2N=log2(N); + if (i==0) + result+=MixSpectrogramSquare(lSpec, lSpecs, N, Log2N>2?(log2(N)-1):2, norm, normmix, cuts[i]); + else if (i==1) + result+=MixSpectrogramSquare(lSpec, lSpecs, N, Log2N>1?Log2N:2, norm, normmix, cuts[i]); + else + result+=MixSpectrogramSquare(lSpec, lSpecs, N, Log2N+1, norm, normmix, cuts[i]); + } + delete[] lSpec; + DeAlloc2(lSpecs); + if (createcuts) delete[] cuts; + return result; +}//MixSpectrogramBlock + +/* + function MixSpectrogramBlock: obtain the composite spectrogram of a waveform block as vectors. + + In: Specs[0][1][WID], Specs[1][2][WID/2], ..., Specs[Res-1][WID/wid][wid], multiresolution spectrogram + Out: spl[WID], Spec[WID], composite spectrogram as tiling and value vectors. Each of the vectors is + made up of $wid subvectors, each subvector pair describing a N*N block of the composite + spectrogram. + + Returns entropy. +*/ +double MixSpectrogramBlock(int* spl, double* Spec, double*** Specs, int WID, int wid, bool norm, bool normmix) +{ + int N=WID/wid, Res=log2(WID/wid)+1, *lspl; + double result=0, *lSpec, ***lSpecs=new double**[Res]; + lSpecs[0]=new double*[Res*N]; for (int i=1; i<Res; i++) lSpecs[i]=&lSpecs[0][i*N]; + + for (int i=0; i<wid; i++) + { + lspl=&spl[i*N]; + lSpec=&Spec[i*N]; + for (int j=0, n=N, Fr=1; j<Res; j++, n/=2, Fr*=2) + { + for (int k=0; k<Fr; k++) + lSpecs[j][k]=&Specs[j][k][i*n]; + } + int Log2N=log2(N); + /* + if (i==0) + result+=MixSpectrogramSquare(lspl, lSpec, lSpecs, N, Log2N>2?(log2(N)-1):2, norm, normmix); + else if (i==1) + result+=MixSpectrogramSquare(lspl, lSpec, lSpecs, N, Log2N>1?Log2N:2, norm, normmix); + else */ + result+=MixSpectrogramSquare(lspl, lSpec, lSpecs, N, Log2N+1, norm, normmix); + + } + DeAlloc2(lSpecs); + + return result; +}//MixSpectrogramBlock + + +//--------------------------------------------------------------------------- +/* + Functions names as ...Block2(...) implement the same functions as the above directly without + explicitly dividing the multiresolution spectrogram into square blocks. +*/ + +/* + function DoCutSpectrogramBlock2: find optimal tiling for a block + + In: Specs[R0][x0:x0+x-1][Y0:Y0+Y-1], [R0+1][2x0:2x0+2x-1][Y0/2:Y0/2+Y/2-1],..., + Specs[R0+?][Nx0:Nx0+Nx-1][Y0/N:Y0/N+Y/N-1], multiresolution spectrogram + Out: spl[Y-1], tiling of this block + + Returns entropy. +*/ +double DoCutSpectrogramBlock2(int* spl, double*** Specs, int Y, int R0, int x0, int Y0, int N, double& ene) +{ + double ent=0; + if (Y>N) //N=WID/wid, the actual square size + { + spl[0]=0; + double ene1, ene2; + ent+=DoCutSpectrogramBlock2(&spl[1], Specs, Y/2, R0, x0, Y0, N, ene1); + ent+=DoCutSpectrogramBlock2(&spl[Y/2], Specs, Y/2, R0, x0, Y0+Y/2, N, ene2); + ene=ene1+ene2; + } + else if (N==1) + { + double tmp=Specs[R0][x0][Y0]; + ene=tmp; + ent=xlogx(tmp); + } + else //Y==N, the square case + { + double enel, ener, enet, eneb, entl, entr, entt, entb; + int* tmpspl=new int[Y]; + entl=DoCutSpectrogramBlock2(&spl[1], Specs, Y/2, R0+1, 2*x0, Y0/2, N/2, enel); + entr=DoCutSpectrogramBlock2(&spl[Y/2], Specs, Y/2, R0+1, 2*x0+1, Y0/2, N/2, ener); + entb=DoCutSpectrogramBlock2(&tmpspl[1], Specs, Y/2, R0, x0, Y0, N/2, eneb); + entt=DoCutSpectrogramBlock2(&tmpspl[Y/2], Specs, Y/2, R0, x0, Y0+Y/2, N/2, enet); + double ene0=enet+eneb, ene1=enel+ener, ent0=entt+entb, ent1=entl+entr; + //normalize + double eneres=1-(ene0+ene1)/2, norment0, norment1; + //double a0=1/(ene0+eneres), a1=1/(ene1+eneres); + //quasi-global normalization + norment0=(ent0-ene0*log(ene0+eneres))/(ene0+eneres), norment1=(ent1-ene1*log(ene1+eneres))/(ene1+eneres); + //local normalization + //if (ene0>0) norment0=ent0/ene0-log(ene0); else norment0=0; if (ene1>0) norment1=ent1/ene1-log(ene1); else norment1=0; + if (norment1<norment0) + { + spl[0]=0; + ent=ent0, ene=ene0; + memcpy(&spl[1], &tmpspl[1], sizeof(int)*(Y-2)); + } + else + { + spl[0]=1; + ent=ent1, ene=ene1; + } + } + return ent; +}//DoCutSpectrogramBlock2 + +/* + function DoMixSpectrogramBlock2: sampling multiresolution spectrogram according to given tiling + + In: Specs[R0][x0:x0+x-1][Y0:Y0+Y-1], [R0+1][2x0:2x0+2x-1][Y0/2:Y0/2+Y/2-1],..., + Specs[R0+?][Nx0:Nx0+Nx-1][Y0/N:Y0/N+Y/N-1], multiresolution spectrogram + spl[Y-1]; tiling of this block + Out: Spec[Y], composite spectrogram as value vector + + Returns 0. +*/ +double DoMixSpectrogramBlock2(int* spl, double* Spec, double*** Specs, int Y, int R0, int x0, int Y0, bool normmix, int res, double* e) +{ + if (Y==1) + { + Spec[0]=Specs[R0][x0][Y0]*e[0]; + } + else + { + double le[32]; + if (normmix && Y<(1<<res)) + { + for (int i=0, j=1, k=Y; i<res; i++, j*=2, k/=2) + { + double lle=0; + for (int fr=0; fr<j; fr++) for (int n=0; n<k; n++) lle+=Specs[i+R0][x0+fr][Y0+n]*Specs[i+R0][x0+fr][Y0+n]; + lle=sqrt(lle)*e[i]; + if (i==0) le[0]=lle; + else if (lle>le[0]*2) le[i]=e[i]*le[0]*2/lle; + else le[i]=e[i]; + } + le[0]=e[0]; + } + else + { + memcpy(le, e, sizeof(double)*res); + } + + if (spl[0]==0) + { + int newres; + if (Y>=(1<<res)) newres=res; + else newres=res-1; + DoMixSpectrogramBlock2(&spl[1], Spec, Specs, Y/2, R0, x0, Y0, normmix, newres, le); + DoMixSpectrogramBlock2(&spl[Y/2], &Spec[Y/2], Specs, Y/2, R0, x0, Y0+Y/2, normmix, newres, le); + } + else + { + DoMixSpectrogramBlock2(&spl[1], Spec, Specs, Y/2, R0+1, x0*2, Y0/2, normmix, res-1, &le[1]); + DoMixSpectrogramBlock2(&spl[Y/2], &Spec[Y/2], Specs, Y/2, R0+1, x0*2+1, Y0/2, normmix, res-1, &le[1]); + } + } + return 0; +}//DoMixSpectrogramBlock2 + +/* + function MixSpectrogramBlock2: obtain the composite spectrogram of a waveform block as vectors. + + In: Specs[0][1][WID], Specs[1][2][WID/2], ..., Specs[Res-1][WID/wid][wid], multiresolution spectrogram + Out: spl[WID], Spec[WID], composite spectrogram as tiling and value vectors. Each of the + vectors is made up of $wid subvectors, each subvector pair describing a N*N block of the + composite spectrogram. + + Returns entropy. +*/ +double MixSpectrogramBlock2(int* spl, double* Spec, double*** Specs, int WID, int wid, bool normmix) +{ + double ene[32]; + //find the total energy and normalize + for (int i=0, Fr=1, Wid=WID; Wid>=wid; i++, Fr*=2, Wid/=2) + { + double lene=0; + for (int fr=0; fr<Fr; fr++) for (int k=0; k<Wid; k++) lene+=Specs[i][fr][k]*Specs[i][fr][k]; + ene[i]=lene; + if (lene!=0) + { + double ilene=1.0/lene; + for (int fr=0; fr<Fr; fr++) for (int k=0; k<Wid; k++) Specs[i][fr][k]=Specs[i][fr][k]*Specs[i][fr][k]*ilene; + } + } + + double result=DoCutSpectrogramBlock2(spl, Specs, WID, 0, 0, 0, WID/wid, ene[31]); + + //de-normalize + for (int i=0, Fr=1, Wid=WID; Wid>=wid; i++, Fr*=2, Wid/=2) + { + double lene=ene[i]; + if (lene!=0) + for (int fr=0; fr<Fr; fr++) for (int k=0; k<Wid; k++) Specs[i][fr][k]=sqrt(Specs[i][fr][k]*lene); + } + + double e[32]; for (int i=0; i<32; i++) e[i]=1; + DoMixSpectrogramBlock2(spl, Spec, Specs, WID, 0, 0, 0, normmix, log2(WID/wid)+1, e); + return result; +}//MixSpectrogramBlock2 + +//--------------------------------------------------------------------------- +/* + function MixSpectrogram: obtain composite spectrogram from multiresolutin spectrogram as pixel grid + + This method deals with Fr (base) frames of WID samples. Each base frame may be divided into 2 1st- + octave frames, 4 2nd-octave frames, ..., etc. The spectrogram calculated on base frame is given in + Specs[0] (Fr frames); that of 1st octave is given in Specs[1] (2*Fr frames); etc. The method resamples + the spectrograms of different frame width into a single spectrogram so that the entropy is maximized + globally. + + The output Spec is a spectrogram of apparent resolution WID at hop size wid. It is a redundant + representation, with equal values occupying blocks of size WID/wid. + + In: Specs[0][Fr][WID], Specs[1][Fr*2][WID/2], ..., Specs[Res-1] [Fr*(WID/wid)][wid], multiresolution + spectrogram + Out: Spec[Fr*(WID/wid)][WID], composite spectrogram as pixel grid + cuts[Fr][wid][N=Wid/wid], tilings of small square blocks + Returns 0. +*/ +double MixSpectrogram(double** Spec, double*** Specs, int Fr, int WID, int wid, bool norm, bool normmix, int*** cuts) +{ + //totally Fr frames of WID samples + //each frame is divided into wid VERTICAL parts + bool createcuts=(cuts==0); + if (createcuts) + { + cuts=new int**[Fr]; + memset(cuts, 0, sizeof(int**)*Fr); + } + int Res=log2(WID/wid)+1; + double*** lSpecs=new double**[Res]; + for (int i=0; i<Fr; i++) + { + for (int j=0, nfr=1; j<Res; j++, nfr*=2) lSpecs[j]=&Specs[j][i*nfr]; + MixSpectrogramBlock(&Spec[i*WID/wid], lSpecs, WID, wid, norm, normmix, cuts[i]); + } + + delete[] lSpecs; + if (createcuts) delete[] cuts; + return 0; +}//MixSpectrogram + +/* + function MixSpectrogram: obtain composite spectrogram from multiresolutin spectrogram as vectors + + In: Specs[0][Fr][WID], Specs[1][Fr*2][WID/2], ..., Specs[Res-1] [Fr*(WID/wid)][wid], multiresolution + spectrogram + Out: spl[Fr][WID], Spec[Fr][WID], composite spectrogram as tiling and value vectors by frame. + + Returns 0. +*/ +double MixSpectrogram(int** spl, double** Spec, double*** Specs, int Fr, int WID, int wid, bool norm, bool normmix) +{ + //totally Fr frames of WID samples + //each frame is divided into wid VERTICAL parts + int Res=log2(WID/wid)+1; + double*** lSpecs=new double**[Res]; + for (int i=0; i<Fr; i++) + { + for (int j=0, nfr=1; j<Res; j++, nfr*=2) lSpecs[j]=&Specs[j][i*nfr]; + MixSpectrogramBlock(spl[i], Spec[i], lSpecs, WID, wid, norm, normmix); +// MixSpectrogramBlock2(spl[i], Spec[i], lSpecs, WID, wid, norm); + } + + delete[] lSpecs; + return 0; +}//MixSpectrogram + +//--------------------------------------------------------------------------- +/* + function VSplitSpec: split a spectrogram vertically into left and right halves. + + In: Spec[X][Y]: spectrogram to split + Out: lSpec[X][Y/2], rSpec[X][Y/2]: the two half spectrograms + + No return value. Both lSpec and rSpec are allocated anew. The caller is responsible to free these + buffers. +*/ +void VSplitSpec(int X, int Y, double** Spec, double**& lSpec, double**& rSpec) +{ + lSpec=new double*[X/2], rSpec=new double*[X/2]; + for(int i=0; i<X/2; i++) + lSpec[i]=Spec[i], rSpec[i]=Spec[i+X/2]; +}//VSplitSpec + +/* + function VSplitSpecs: split a multiresolution spectrogram vertically into left and right halves + + A full spectrogram array is given in log2(N)+1 spectrograms, with the base spec of 1*N, 1st octave of + 2*(N/2), ..., last octave of N*1. When this array is split into two spectrogram arrays horizontally, + the last spec (with the highest time resolution). Each of the two new arrays is given in log2(N) + spectrograms. + + In: Specs[nRes+1][][]: multiresolution spectrogram + Out: lSpecs[nRes][][], rSpecs[nRes][][], the two half multiresolution spectrograms + + This function allocates two 2nd order arrays of double*, which the caller is responsible to free. +*/ +void VSplitSpecs(int N, double*** Specs, double***& lSpecs, double***& rSpecs) +{ + int nRes=log2(N); // new number of resolutions + lSpecs=new double**[nRes], rSpecs=new double**[nRes]; + lSpecs[0]=new double*[nRes*N/2], rSpecs[0]=new double*[nRes*N/2]; for (int i=1; i<nRes; i++) lSpecs[i]=&lSpecs[0][i*N/2], rSpecs[i]=&rSpecs[0][i*N/2]; + for (int i=0, Fr=1; i<nRes; i++, Fr*=2) + for (int j=0; j<Fr; j++) + lSpecs[i][j]=Specs[i+1][j], rSpecs[i][j]=Specs[i+1][j+Fr]; +}//VSplitSpecs + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/multires.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,33 @@ +#ifndef multiresH +#define multiresH + +/* + multires.cpp - composite spectrogram routines + + This unit deals with basis selection from multiresolution Fourier bases set. + + A tiling of an area of the T-F plane is represented as a sequence of 0's and 1's, each 0 represents + a horizontal split-in-half, each 1 represents a vertical split-in-half. The sequence is made up of + three pieces: 1) the first entry giving the direction of primary split; 2) the tiling of the upper + or left half after the primary split; and 3) the tiling of the lower or right half. + + A composite spectrogram is represented as a tiling and a vector of amplitude (or square amplitude) + values, each representing the energy within a tile. If the primary split is horizontal, then the + first/second half of the amplitude or power vector represents the the upper/lower half of the + composite spectrogram; if the primary splits is vertical, then the first/second half of the amplitude + or power vector represents the left/right half of the composite spectrogram. + + Further reading: Wen X. and M. Sandler, "Composite spectrogram using multiple fourier transforms," + IET Signal Processing, 3(1):51-63, 2009. +*/ + +//--memory re-indexing routines for internal use----------------------------- +void HSplitSpec(int X, int Y, double** Spec, double**& lSpec, double**& uSpec); +void HSplitSpecs(int N, double*** Specs, double***& lSpecs, double***& uSpecs); +void VSplitSpec(int X, int Y, double** Spec, double**& lSpec, double**& rSpec); +void VSplitSpecs(int N, double*** Specs, double***& lSpecs, double***& rSpecs); + +//--composite spectrogram algorithms----------------------------------------- +double MixSpectrogram(double** Spec, double*** Specs, int Fr, int WID, int wid, bool norm=true, bool normmix=true, int*** cuts=0); +double MixSpectrogram(int** spl, double** Spec, double*** Specs, int Fr, int WID, int wid, bool norm=true, bool normmix=true); +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/opt.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,1332 @@ +//--------------------------------------------------------------------------- + + +#include <math.h> +#include <memory.h> +#include <stdlib.h> +#include "opt.h" +#include "matrix.h" + +//--------------------------------------------------------------------------- +//macro nsign: judges if two double-precision floating pointer numbers have different signs +#define nsign(x1, x2) (0x80&(((char*)&x1)[7]^((char*)&x2)[7])) + +//--------------------------------------------------------------------------- +/* + function GradientMax: gradient method maximizing F(x) + + In: function F(x) and its derivative dF(x) + start[dim]: initial value of x + ep: a stopping threshold + maxiter: maximal number of iterates + Out: start[dim]: the final x + + Returnx max F(final x) +*/ +double GradientMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*), + int dim, double* start, double ep, int maxiter) +{ + double oldG, G=F(dim, start, params), *dG=new double[dim], *X=new double[dim]; + + int iter=0; + do + { + oldG=G; + + dF(dG, dim, start, params); + Search1Dmaxfrom(params, F, dim, start, dG, 1e-2, 1e-6, 1e6, X, &G, ep); + memcpy(start, X, sizeof(double)*dim); + + iter++; + } + while (iter<maxiter && G>oldG*(1+ep)); + return G; +}//GradientMax + +/* + function GradientOneMax: gradient method maximizing F(x), which searches $dim dimensions one after + another instead of taking the gradient descent direction. + + In: function F(x) and its derivative dF(x) + start[dim]: initial value of x + ep: a stopping threshold + maxiter: maximal number of iterates + Out: start[dim]: the final x + + Returnx max F(final x) +*/ +double GradientOneMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*), + int dim, double* start, double ep, int maxiter) +{ + double oldG, G=F(dim, start, params), *dG=new double[dim], *X=new double[dim]; + + int iter=0; + do + { + oldG=G; + for (int d=0; d<dim; d++) + { + dF(dG, dim, start, params); + for (int i=0; i<dim; i++) if (d!=i) dG[i]=0; + if (dG[d]!=0) + { + Search1Dmaxfrom(params, F, dim, start, dG, 1e-2, 1e-6, 1e6, X, &G, ep); + memcpy(start, X, sizeof(double)*dim); + } + } + iter++; + } + while (iter<maxiter && G>oldG*(1+ep)); + return G; +}//GradientOneMax + +//--------------------------------------------------------------------------- +/* + function mindenormaldouble: returns the minimal denormal double-precision floating point number. +*/ +double mindenormaldouble() +{ + double result=0; + *((unsigned char*)&result)=1; + return result; +}//mindenormaldouble + +/* + function minnormaldouble: returns the minimal normal double-precision floating point number. +*/ +double minnormaldouble() +{ + double result=0; + ((unsigned char*)&result)[6]=16; + return result; +}//minnormaldouble + +/* + //function nextdouble: returns the next double value after x in the direction of dir +*/ +double nextdouble(double x, double dir) +{ + if (x==0) + { + if (dir<0) return -mindenormaldouble(); + else return mindenormaldouble(); + } + else if ((x>0)^(dir>0)) *((__int64*)&x)-=1; + else *((__int64*)&x)+=1; + return x; +}//nextdouble + +/* + function Newton: Newton method for finding the root of y(x) + + In: function y(x) and its derivative y'(x) + params: additional argument for calling y and y' + x0: inital value of x + epx, epy: stopping thresholds on x and y respectively + maxiter: maximal number of iterates + Out: x0: the root + + Returns 0 if successful. +*/ +double Newton(double& x0, double (*y)(double, void*), double (*y1)(double, void*), void* params, double epx, int maxiter, double epy) +{ + double y0=y(x0, params), x2, y2, dx, dy, x3, y3; + if (y0==0) return 0; + + int iter=0; + while (iter<maxiter) + { + dy=y1(x0, params); + if (dy==0) return -1; + x2=x0-y0/dy; + y2=y(x2, params); + while (fabs(y2)>fabs(y0)) + { + x2=(x2+x0)/2; + y2=y(x2, params); + } + if (y2==0) + { + x0=x2; + return 0; + } + dx=fabs(x0-x2); + if (dx==0) + { + if (fabs(y0)<epy) return 0; + else return -1; + } + else if (dx<epx) + { + if nsign(y2, y0) + { + x0=x2; + return dx; + } + else + { + x3=2*x2-x0; + y3=y(x3, params); + if (y3==0) + { + x0=x3; + return 0; + } + if nsign(y2, y3) + { + x0=x2; + return dx; + } + else if (fabs(y3)<fabs(y2)) + { + x2=x3, y2=y3; + } + } + } + x0=x2; + y0=y2; + iter++; + } + if (fabs(y0)<epy) return 0; + else return -1; +}//Newton + +/* + function Newton: Newton method for finding the root of y(x), for use when it's more efficient for y' + and y to be calculated in one function call + + In: function dy(x) that computes y and y' at x + params: additional argument for calling dy + yshift: the byte shift in params where dy(x) put the value of y(x) as double type + x0: inital value of x + epx, epy: stopping thresholds on x and y respectively + maxiter: maximal number of iterates + Out: x0: the root + + Returns 0 if successful. +*/ +int Newton(double& x0, double (*dy)(double, void*), void* params, int yshift, double epx, int maxiter, double epy, double xmin, double xmax) +{ + double y0, x2, y2, dx, dy0, dy2, dy3, x3, y3; + dy0=dy(x0, params); + y0=*(double*)((char*)params+yshift); + if (y0==0) return 0; + + int iter=0; + while (iter<maxiter) + { + if (dy0==0) return -1; + dx=-y0/dy0; + + x2=x0+dx; + dy2=dy(x2, params); + y2=*(double*)((char*)params+yshift); + //make sure the iteration yields a y closer to 0 + while (fabs(y2)>fabs(y0)) + { + dx/=2; + x2=x0+dx; + dy2=dy(x2, params); + y2=*(double*)((char*)params+yshift); + } + //the above may fail due to precision problem, i.e. x0+dx=x0 + if (x2==x0) + { + if (fabs(y0)<epy) return 0; + else + { + x2=nextdouble(x0, dx); + dy2=dy(x2, params); + y2=*(double*)((char*)params+yshift); + if (y2==0) + { + x0=x2; + return 0; + } + else + { + dx=x2-x0; + while (y2==y0) + { + dx*=2; + x2=x0+dx; + dy2=dy(x2, params); + y2=*(double*)((char*)params+yshift); + } + if nsign(y2, y0) + return fabs(dx); + else if (fabs(y2)>fabs(y0)) + return -1; + } + } + } + else if (y2==0) + { + x0=x2; + return 0; + } + else + { + double fabsdx=fabs(dx); + if (fabsdx<epx) + { + if nsign(y2, y0) + { + x0=x2; + return fabsdx; + } + else + { + x3=x2+dx; + dy3=dy(x3, params); + y3=*(double*)((char*)params+yshift); + if (y3==0){x0=x3; return 0;} + if nsign(y2, y3) + { + x0=x2; + return dx; + } + else if (fabs(y3)<fabs(y2)) + { + x2=x3, y2=y3, dy2=dy3; + } + } + } + } + if (x2<xmin || x2>xmax) return -1; + x0=x2; + y0=y2; + dy0=dy2; + iter++; + } + if (fabs(y0)<epy) return 0; + else return -1; +}//Newton + +/* + function init_Newton_1d: finds an initial x and interval (xa, xb) from which to search for an extremum + + On calling, xa<x0<xb, y(x0)>=y(xa), y(x0)>=y(xb) (or y(x0)<=y(xa), y(x0)<=y(xb)). + On return, y(x0)>=y(xa), y(x0)>=y(xb) (or y(x0)<=y(xa), y(x0)<=y(xb)), and either y'(xa)>0, y'(xb)<0 + (or y'(xa)<0, y'(xb)>0), or xb-xa<epx. + + In: function dy(x) that computes y and y' at x + params: additional argument for calling dy + yshift: the byte shift in params where dy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx: tolerable error of extremum + Out: improved initial value x0 and search range (xa, xb) + + Returns xb-xa if successful, -0.5 or -0.6 if y(x0) is not bigger(smaller) than both y(xa) and y(xb) +*/ +double init_Newton_1d(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx) +{ + double dy0, dya, dyb, y0, ya, yb; + dy0=dy(x0, params); y0=*(double*)((char*)params+yshift); + dya=dy(xa, params); ya=*(double*)((char*)params+yshift); + dyb=dy(xb, params); yb=*(double*)((char*)params+yshift); + if (y0>=ya && y0>=yb) + { + //look for xa<x0<xb, ya<y0>yb, so that dya>0, dyb<0 + while ((dya<=0 || dyb>=0) && xb-xa>=epx) + { + double x1=(x0+xa)/2; + double dy1=dy(x1, params); + double y1=*(double*)((char*)params+yshift); + double x2=(x0+xb)/2; + double dy2=dy(x2, params); + double y2=*(double*)((char*)params+yshift); + if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;} + } + return xb-xa; + } + else if (y0<=ya && y0<=yb) + { + //look for xa<x0<xb, ya>y0<yb, so that dya<0, dyb>0 + while ((dya>=0 || dyb<=0) && xb-xa>=epx) + { + double x1=(x0+xa)/2; + double dy1=dy(x1, params); + double y1=*(double*)((char*)params+yshift); + double x2=(x0+xb)/2; + double dy2=dy(x2, params); + double y2=*(double*)((char*)params+yshift); + if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;} + } + return xb-xa; + } + else//meaning y(x0) is not bigger(smaller) than both y(xa) and y(xb) + { + if (y0<=yb) return -0.5; //ya<=y0<=yb + if (y0<=ya) return -0.6; //ya>=y0>=yb + } + return -0; +}//init_Newton_1d + +/* + function init_Newton_1d_max: finds an initial x and interval (xa, xb) from which to search for an + maximum. + + On calling xa<x0<xb, and it is assumed that y(x0)>=y(xa), y(x0)>=y(xb). If the conditions on y are met, + it find a new set of xa<x0<xb, so that y(x0)>y(xa), y(x0)>=y(xb), y'(xa)>0, y'(xb)<0, or xb-xa<epx, + and returns xb-xa>0; otherwise it returns a negative value showing the initial order of y(x0), y(xa) + and y(xb) as + -0.5 for ya<=y0<yb + -0.6 for yb<=y0<ya + -0.7 for y0<ya<yb + -0.8 for y0<yb<=ya + + In: function dy(x) that computes y and y' at x + params: additional argument for calling dy + yshift: the byte shift in params where dy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx: tolerable error of extremum + Out: improved initial value x0 and search range (xa, xb) + + Returns xb-xa if successful, a negative value as above if not. +*/ +double init_Newton_1d_max(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx) +{ + double dy0, dya, dyb, y0, ya, yb; + dy0=dy(x0, params); y0=*(double*)((char*)params+yshift); + dya=dy(xa, params); ya=*(double*)((char*)params+yshift); + dyb=dy(xb, params); yb=*(double*)((char*)params+yshift); + if (y0>=ya && y0>=yb) + { + //look for xa<x0<xb, ya<y0>yb, so that dya>0, dyb<0 + while ((dya<=0 || dyb>=0) && xb-xa>=epx) + { + double x1=(x0+xa)/2; + double dy1=dy(x1, params); + double y1=*(double*)((char*)params+yshift); + double x2=(x0+xb)/2; + double dy2=dy(x2, params); + double y2=*(double*)((char*)params+yshift); + if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;} + } + return xb-xa; + } + else//meaning y(x0) is not bigger than both y(xa) and y(xb) + { + if (y0<yb && y0>=ya) return -0.5; //ya<=y0<yb + if (y0<ya && y0>=yb) return -0.6; //ya>y0>=yb + if (ya<yb) return -0.7; //y0<ya<yb + return -0.8; //y0<yb<=ya + } +}//init_Newton_1d_max + +/* + function init_Newton_1d_min: finds an initial x and interval (xa, xb) from which to search for an + manimum. + + On calling xa<x0<xb, and it is assumed that y(x0)<=y(xa), y(x0)<=y(xb). If the conditions on y are met, + it find a new set of xa<x0<xb, so that y(x0)<y(xa), y(x0)<=y(xb), y'(xa)<0, y'(xb)>0, or xb-xa<epx, + and returns xb-xa>0; otherwise it returns a negative value showing the initial order of y(x0), y(xa) + and y(xb) as + -0.5 for ya<=y0<yb + -0.6 for yb<=y0<ya + -0.7 for y0>yb>ya + -0.8 for y0>ya>=yb + + In: function dy(x) that computes y and y' at x + params: additional argument for calling dy + yshift: the byte shift in params where dy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx: tolerable error of extremum + Out: improved initial value x0 and search range (xa, xb) + + Returns xb-xa if successful, a negative value as above if not. +*/ +double init_Newton_1d_min(double& x0, double& xa, double& xb, double (*dy)(double, void*), void* params, int yshift, double epx) +{ + double dy0, dya, dyb, y0, ya, yb; + dy0=dy(x0, params); y0=*(double*)((char*)params+yshift); + dya=dy(xa, params); ya=*(double*)((char*)params+yshift); + dyb=dy(xb, params); yb=*(double*)((char*)params+yshift); + if (y0<=ya && y0<=yb) + { + //look for xa<x0<xb, ya>y0<yb, so that dya<0, dyb>0 + while ((dya>=0 || dyb<=0) && xb-xa>=epx) + { + double x1=(x0+xa)/2; + double dy1=dy(x1, params); + double y1=*(double*)((char*)params+yshift); + double x2=(x0+xb)/2; + double dy2=dy(x2, params); + double y2=*(double*)((char*)params+yshift); + if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2;} + } + return xb-xa; + } + else//meaning y(x0) is not bigger than both y(xa) and y(xb) + { + if (y0<yb && y0>=ya) return -0.5; //ya<=y0<yb + if (y0<ya && y0>=yb) return -0.6; //ya>y0>=yb + if (ya<yb) return -0.7; //ya<yb<y0 + return -0.8; //yb<=ya<y0 + } +}//init_Newton_1d_min + +/* + function Newton_1d_max: Newton method for maximizing y(x). On calling y(x0)>=y(xa), y(x0)>=y(xb), + y'(xa)>0, y'(xb)<0 + + In: function ddy(x) that computes y, y' and y" at x + params: additional argument for calling ddy + y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type + yshift: the byte shift in params where ddy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx, epdy: stopping threshold for x and y', respectively + maxiter: maximal number of iterates + Out: x0: the maximum + + Returns the error bound for the maximum x0, or + -2 if y(*) do not satisfy input conditions + -3 if y'(*) do not satisfy input conditions +*/ +double Newton_1d_max(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epdy, bool checkinput) +{ + double x1, y1, x2, y2, dx, dy1, dy2, dy3, x3, y3, ddy1, ddy2, ddy3; + double y0, ya, yb, dy0, dya, dyb, ddy0; + if (xb-xa<epx) {return xb-xa;} + ddy(xa, params); + dya=*(double*)((char*)params+y1shift), ya=*(double*)((char*)params+yshift); + ddy(xb, params); + dyb=*(double*)((char*)params+y1shift), yb=*(double*)((char*)params+yshift); + ddy0=ddy(x0, params); + dy0=*(double*)((char*)params+y1shift), y0=*(double*)((char*)params+yshift); + + if (checkinput) + { + if (y0>=ya && y0>=yb) + { + if (dya<=0 || dyb>=0) return -3; + } + else return -2; + } + + if (dy0==0) return 0; + + int iter=0; + while (iter<maxiter) + { + if (ddy0>=0) //newton method not applicable + { + do + { + x1=(x0+xa)/2; + ddy1=ddy(x1, params); + dy1=*(double*)((char*)params+y1shift), y1=*(double*)((char*)params+yshift); + x2=(x0+xb)/2; + ddy2=ddy(x2, params); + dy2=*(double*)((char*)params+y1shift), y2=*(double*)((char*)params+yshift); + if (y0>=y1 && y0>=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1>=y0 && y1>=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1, ddy0=ddy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;} + } + while(ddy0>=0 && (dya<=0 || dyb>=0) && xb-xa>=epx); + if (xb-xa<epx) return xb-xa; + } + if (fabs(dy0)<epdy) return 0; + //x2: Newton point + //dx is the same direction as dy0 if ddy0<0 + dx=-dy0/ddy0; + x2=x0+dx; + if (x2==x0) return 0; + + //make sure x2 lies in [xa, xb] + if (x2<xa) {x2=(xa+x0)/2; dx=x2-x0;} + else if (x2>xb) {x2=(xb+x0)/2; dx=x2-x0;} + ddy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + + //make sure the iteration yields a larger y + if (y2<y0) + { + while (y2<y0 && dx>epx) + { + dx/=2; + x2=x0+dx; + ddy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + } + if (y2<y0) return fabs(dx); + } + + //new x2 lies between xa and xb and y2>=y0 + dy2=*(double*)((char*)params+y1shift); + if (fabs(dy2)<epdy){x0=x2; return 0;} + + double fabsdx=fabs(dx); + if (fabsdx<epx) + { + if nsign(dy0, dy2){x0=x2; return fabsdx;} + x3=x2+dx; + ddy3=ddy(x3, params); + dy3=*(double*)((char*)params+y1shift), y3=*(double*)((char*)params+yshift); + if (dy3==0) {x0=x3; return 0;} + if (y2>=y3) {x0=x2; return fabsdx;} + else x2=x3, y2=y3, dy2=dy3, ddy2=ddy3; + } + + if (x0<x2) xa=x0, ya=y0, dya=dy0; + else xb=x0, yb=y0, dyb=dy0; + x0=x2, y0=y2, dy0=dy2, ddy0=ddy2; + iter++; + } + if (fabs(dy0)<epdy) return 0; + else return -1; +}//Newton_1d_max + +/* + function Newton_1d_max: Newton method for maximizing y(x). On calling y(x0)<=y(xa), y(x0)<=y(xb), + y'(xa)<0, y'(xb)>0 + + In: function ddy(x) that computes y, y' and y" at x + params: additional argument for calling ddy + y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type + yshift: the byte shift in params where ddy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx, epdy: stopping threshold for x and y', respectively + maxiter: maximal number of iterates + Out: x0: the maximum + + Returns the error bound for the maximum x0, or + -2 if y(*) do not satisfy input conditions + -3 if y'(*) do not satisfy input conditions +*/ +double Newton_1d_min(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epdy, bool checkinput) +{ + double x1, y1, x2, y2, dx, dy1, dy2, dy3, x3, y3, ddy1, ddy2, ddy3; + double y0, ya, yb, dy0, dya, dyb, ddy0; + if (xb-xa<epx) {return xb-xa;} + ddy(xa, params); + dya=*(double*)((char*)params+y1shift), ya=*(double*)((char*)params+yshift); + ddy(xb, params); + dyb=*(double*)((char*)params+y1shift), yb=*(double*)((char*)params+yshift); + ddy0=ddy(x0, params); + dy0=*(double*)((char*)params+y1shift), y0=*(double*)((char*)params+yshift); + + if (checkinput) + { + if (y0<=ya && y0<=yb) + { + if (dya>=0 || dyb<=0) return -3; + } + else return -2; + } + + if (dy0==0) return 0; + + int iter=0; + while (iter<maxiter) + { + if (ddy0<=0) //newton method not applicable + { + do + { + x1=(x0+xa)/2; + ddy1=ddy(x1, params); + dy1=*(double*)((char*)params+y1shift), y1=*(double*)((char*)params+yshift); + x2=(x0+xb)/2; + ddy2=ddy(x2, params); + dy2=*(double*)((char*)params+y1shift), y2=*(double*)((char*)params+yshift); + if (y0<=y1 && y0<=y2) xa=x1, ya=y1, dya=dy1, xb=x2, yb=y2, dyb=dy2; + else if (y1<=y0 && y1<=y2) {xb=x0, yb=y0, dyb=dy0; x0=x1, y0=y1, dy0=dy1, ddy0=ddy1;} + else {xa=x0, ya=y0, dya=dy0; x0=x2, y0=y2, dy0=dy2, ddy0=ddy2;} + } + while((dya>=0 || dyb<=0) && xb-xa>=epx); + } + else + { + //Newton point + //dx is the same direction as dy0 if ddy0<0 + if (fabs(dy0)<epdy) return 0; + dx=-dy0/ddy0; + x2=x0+dx; + if (x2==x0) return 0; + //make sure x2 lies in [xa, xb] + if (x2<xa) {x2=(xa+x0)/2; dx=x2-x0;} + else if (x2>xb) {x2=(xb+x0)/2; dx=x2-x0;} + ddy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + //make sure the iteration yields a larger y + if (y2>y0) + { + while (y2>y0) + { + dx/=2; + x2=x0+dx; + ddy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + } + if (x2==x0) return 0; //dy0<0 but all y(x+(ep)dx)<0 + } + dy2=*(double*)((char*)params+y1shift); + if (fabs(dy2)<epdy) + { + x0=x2; + return 0; + } + else + { + double fabsdx=fabs(dx); + if (fabsdx<epx) + { + if nsign(dy2, dy0) + { + x0=x2; + return fabsdx; + } + else + { + x3=x2+dx; + ddy3=ddy(x3, params); + dy3=*(double*)((char*)params+y1shift), y3=*(double*)((char*)params+yshift); + if (dy3==0) {x0=x3; return 0;} + if nsign(dy2, dy3) + { + x0=x2; + return dx; + } + else if (y3<y2) + { + x2=x3, y2=y3, dy2=dy3, ddy2=ddy3; + } + } + } + } + if (x0<x2) xa=x0, ya=y0, dya=dy0; + else xb=x0, yb=y0, dyb=dy0; + + x0=x2, y0=y2, dy0=dy2, ddy0=ddy2; + } + + iter++; + } + if (fabs(dy0)<epdy) return 0; + else return -1; +}//Newton_1d_min + +/* + function Newton1dmax: uses init_Newton_1d_max and Newton_1d_max to search for a local maximum of y. + + By default ddy and dy use the same parameter structure and returns the lower order derivatives at + specific offsets within that structure. However, since the same params is used, it's possible the same + variable may take different offsets when returned from ddy() and dy(). + + On calling xa<x0<xb, and it is assumed that y0>=ya, y0>=yb. If the conditions on y are not met, it + returns a negative value showing the order of y0, ya and yb, i.e. + -0.5 for ya<=y0<yb + -0.6 for yb<=y0<ya + -0.7 for y0<ya<yb + -0.8 for y0<yb<=ya + + In: function ddy(x) that computes y, y' and y" at x + function dy(x) that computes y and y' at x + params: additional argument for calling ddy and dy + y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type + yshift: the byte shift in params where ddy(x) put the value of y(x) as double type + dyshift: the byte shift in params where dy(x) put the value of y(x) as double type + x0: inital value of x + (xa, xb): initial search range + epx, epdy: stopping threshold for x and y', respectively + maxiter: maximal number of iterates + Out: x0: the maximum + + Returns the error bound for maximum x0, or a negative value if the initial point does not satisfy + relevent assumptions. +*/ +double Newton1dmax(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double (*dy)(double, void*), int dyshift, double epx, int maxiter, double epdy, bool checkinput) +{ + double ep=init_Newton_1d_max(x0, xa, xb, dy, params, dyshift, epx); + if (ep>epx) ep=Newton_1d_max(x0, xa, xb, ddy, params, y1shift, yshift, epx, maxiter, epdy, true); + return ep; +}//Newton1dmax + +//function Newton1dmin: same as Newton1dmax, in the other direction. +double Newton1dmin(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double (*dy)(double, void*), int dyshift, double epx, int maxiter, double epdy, bool checkinput) +{ + double ep=init_Newton_1d_min(x0, xa, xb, dy, params, dyshift, epx); + if (ep>epx) ep=Newton_1d_min(x0, xa, xb, ddy, params, y1shift, yshift, epx, maxiter, epdy, true); + return ep; +}//Newton1dmin + +/* + function NewtonMax: looks for the maximum of y within [x0, x1] using Newton method. + + In: function ddy(x) that computes y, y' and y" at x + params: additional argument for calling ddy + y1shift: the byte shift in params where ddy(x) put the value of y'(x) as double type + yshift: the byte shift in params where ddy(x) put the value of y(x) as double type + (x0, x1): initial search range + epx, epy: stopping threshold for x and y, respectively + maxiter: maximal number of iterates + Out: maximum x0 + + Returns an error bound on maximum x0 if successful, a negative value if not. +*/ +double NewtonMax(double& x0, double x1, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double epx, int maxiter, double epy) +{ + double y0, x2, y2, dx, dy0, dy2, dy3, x3, y3; + dy0=ddy(x0, params); + y0=*(double*)((char*)params+yshift); + if (y0==0) return 0; + + int iter=0; + while (iter<maxiter) + { + if (dy0==0) return -1; + dx=-y0/dy0; + + x2=x0+dx; + dy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + //make sure the iteration yields a y closer to 0 + while (fabs(y2)>fabs(y0)) + { + dx/=2; + x2=x0+dx; + dy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + } + //the above may fail due to precision problem, i.e. x0+dx=x0 + if (x2==x0) + { + if (fabs(y0)<epy) return 0; + else + { + x2=nextdouble(x0, dx); + dy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + if (y2==0) + { + x0=x2; + return 0; + } + else + { + dx=x2-x0; + while (y2==y0) + { + dx*=2; + x2=x0+dx; + dy2=ddy(x2, params); + y2=*(double*)((char*)params+yshift); + } + if nsign(y2, y0) + return fabs(dx); + else if (fabs(y2)>fabs(y0)) + return -1; + } + } + } + else if (y2==0) + { + x0=x2; + return 0; + } + else + { + double fabsdx=fabs(dx); + if (fabsdx<epx) + { + if nsign(y2, y0) + { + x0=x2; + return fabsdx; + } + else + { + x3=x2+dx; + dy3=ddy(x3, params); + y3=*(double*)((char*)params+yshift); + if (y3==0){x0=x3; return 0;} + if nsign(y2, y3) + { + x0=x2; + return dx; + } + else if (fabs(y3)<fabs(y2)) + { + x2=x3, y2=y3, dy2=dy3; + } + } + } + } + x0=x2; + y0=y2; + dy0=dy2; + iter++; + } + if (fabs(y0)<epy) return 0; + else return -1; +}//NewtonMax + +//--------------------------------------------------------------------------- +/* + function rootsearchhalf: searches for the root of y(x) in (x1, x2) by half-split method. On calling + y(x1) and y(x2) must not have the same sign, or -1 is returned signaling a failure. + + In: function y(x), + params: additional arguments for calling y + (x1, x2): inital range + epx: stopping threshold on x + maxiter: maximal number of iterates + Out: root x1 + + Returns a positive error estimate if a root is found, -1 if not. +*/ +double rootsearchhalf(double&x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter) +{ + if (x1>x2) {double tmp=x1; x1=x2; x2=tmp;} + double y1=y(x1, params), y3, x3, ep2=epx*2; + if (y1==0) return 0; + else + { + double y2=y(x2, params); + if (y2==0) + { + x1=x2; + return 0; + } + else if nsign(y1, y2) //y1 and y2 have different signs + { + int iter=0; + while (x2-x1>ep2 && iter<maxiter) + { + x3=(x1+x2)/2; + y3=y(x3, params); + if (y3==0) + { + x1=x3; + return 0; + } + else + { + if nsign(y1, y3) x2=x3, y2=y3; + else x1=x3, y1=y3; + } + iter++; + } + x1=(x1+x2)/2; + return x2-x1; + } + else + return -1; + } +}//rootsearchhalf + +//--------------------------------------------------------------------------- +/* + function rootsearchsecant: searches for the root of y(x) in (x1, x2) by secant method. + + In: function y(x), + params: additional arguments for calling y + (x1, x2): inital range + epx: stopping threshold on x + maxiter: maximal number of iterates + Out: root x1 + + Returns a positive error estimate if a root is found, -1 if not. +*/ +double rootsearchsecant(double& x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter) +{ + double y1=y(x1, params), y2=y(x2, params), y3, x3, dx, x0, y0; + if (y1==0) return 0; + if (y2==0) + { + x1=x2; + return 0; + } + if (fabs(y1)>fabs(y2)) + { + x3=x1, y3=y1; + } + else + { + x3=x2, y3=y2; + x2=x1, y2=y1; + } + + int iter=0; + while (iter<maxiter) + { + x1=(y3*x2-y2*x3)/(y3-y2); + y1=y(x1, params); + + dx=fabs(x1-x2); + if (dx<epx) + { + if nsign(y1, y2) + { + return dx; + } + else + { + x0=2*x1-x2; + y0=y(x0, params); + if nsign(y0, y1) + { + return dx; + } + } + } + x3=x2, y3=y2; + x2=x1, y2=y1; + iter++; + } + return -1; +}//rootsearchsecant + +/* + function rootsearchbisecant: searches for the root of y(x) in (x1, x2) by bi-secant method. On calling + given that y(x1) and y(x2) have different signs. The bound interval (x1, x2) converges to 0, as + compared to standared secant method. + + In: function y(x), + params: additional arguments for calling y + (x1, x2): inital range + epx: stopping threshold on x + maxiter: maximal number of iterates + Out: root x1 + + Returns an estimated error bound if a root is found, a negative value if not. +*/ +double rootsearchbisecant(double&x1, double x2, double (*y)(double, void*), void* params, double epx, int maxiter) +{ + if (x1>x2) {double tmp=x1; x1=x2; x2=tmp;} + double y1=y(x1, params), y3, y4, dy2, dy3, x3, x4, ep2=epx*2; + if (y1==0) return 0; + else + { + double y2=y(x2, params); + if (y2==0) + { + x1=x2; + return 0; + } + else if nsign(y1, y2) //y1 and y2 have different signs + { + int iter=0; + while (x2-x1>ep2 && iter<maxiter) + { + dy2=y1-y2; + x3=(y1*x2-y2*x1)/dy2; + y3=y(x3, params); + if (y3==0) + { + x1=x3; + return 0; + } + else + { + if nsign(y1, y3) + { + dy3=y3-y2; + if (dy3==0 || nsign(dy3, dy2)) x2=x3, y2=y3; + else + { + x4=(y3*x2-y2*x3)/dy3; //x4<x3 + if (x4<=x1) x2=x3, y2=y3; + else + { + y4=y(x4, params); + if (y4==0) + { + x1=x4; + return 0; + } + else + { + if nsign(y4, y3) x1=x4, y1=y4, x2=x3, y2=y3; + else x2=x4, y2=y4; + } + } + } + } + else + { + dy3=y1-y3; + if (dy3==0 || nsign(dy3, dy2)) x1=x3, y1=y3; + else + { + x4=(y1*x3-y3*x1)/dy3; //x4>x3 + if (x4>=x2) x1=x3, y1=y3; + else + { + y4=y(x4, params); + if (y4==0) + { + x1=x4; + return 0; + } + else + { + if nsign(y4, y3) x1=x3, y1=y3, x2=x4, y2=y4; + else x1=x4, y1=y4; + } + } + } + } + } + iter++; + } + x1=(x1+x2)/2; + return x2-x1; + } + else + return -1; + } +}//rootsearchbisecant + +//--------------------------------------------------------------------------- +/* + function Search1Dmax: 1-dimensional maximum search within interval [inf, sup] using 0.618 method + + In: function F(x), + params: additional arguments for calling F + (inf, sup): inital range + epx: stopping threshold on x + maxiter: maximal number of iterates + convcheck: specifies if convexity is checked at each interate, false recommended. + Out: x: the maximum + maxresult: the maximal value, if specified + + Returns an estimated error bound of x if successful, a negative value if not. +*/ +double Search1Dmax(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult, double ep, int maxiter, bool convcheck) +{ + double v0618=(sqrt(5.0)-1)/2, iv0618=1-v0618; + double l=sup-inf; + double startsup=sup, startinf=inf, startl=l; + double x1=inf+l*(1-v0618); + double x2=inf+l*v0618; + double result; + + double v0=F(inf, params); + double v1=F(x1, params); + double v2=F(x2, params); + double v3=F(sup, params); + int iter=0; + + if (v1<v0 && v1<v3 && v2<v0 && v2<v3) goto return1; + if (convcheck && (v1<v0*v0618+v3*iv0618 || v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + + while (iter<maxiter) + { + if (l<startl*ep) break; + if (v1>v2) + { + sup=x2, v3=v2; + l=sup-inf; + x2=x1, v2=v1; + x1=inf+l*(1-v0618); + v1=F(x1, params); + if (convcheck && (v1<v0*v0618+v3*iv0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + } + else + { + inf=x1, v0=v1; + l=sup-inf; + x1=x2, v1=v2; + x2=inf+l*v0618; + v2=F(x2, params); + if (convcheck && (v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + } + iter++; + } + if (v1>v2) {x=x1; if (maxresult) maxresult[0]=v1; result=x1-inf;} + else {x=x2; if (maxresult) maxresult[0]=v2; result=sup-x2;} + return result; + +return1: //where at some stage v(.) fails to be convex + if (v0>=v1 && v0>=v2 && v0>=v3){x=inf; if (maxresult) maxresult[0]=v0; result=(x==startinf)?(-1):(sup-x1);} + else if (v1>=v0 && v1>=v2 && v1>=v3){x=x1; if (maxresult) maxresult[0]=v1; result=x1-inf;} + else if (v2>=v0 && v2>=v1 && v2>=v3){x=x2; if (maxresult) maxresult[0]=v2; result=sup-x2;} + else {x=sup; if (maxresult) maxresult[0]=v3; result=(x==startsup)?(-1):(x2-inf);} + return result; +}//Search1Dmax + +/* + function Search1DmaxEx: 1-dimensional maximum search using Search1Dmax, but allows setting a $len + argument so that a pre-location of the maximum is performed by sampling (inf, sup) at interval $len, + so that Search1Dmax will work on an interval no longer than 2*len. + + In: function F(x), + params: additional arguments for calling F + (inf, sup): inital range + len: rough sampling interval for pre-locating maximum + epx: stopping threshold on x + maxiter: maximal number of iterates + convcheck: specifies if convexity is checked at each interate, false recommended. + Out: x: the maximum + maxresult: the maximal value, if specified + + Returns an estimated error bound of x if successful, a negative value if not. +*/ +double Search1DmaxEx(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult, double ep, int maxiter, double len) +{ + int c=ceil((sup-inf)/len+1); + if (c<8) c=8; + double* vs=new double[c+1], step=(sup-inf)/c; + for (int i=0; i<=c; i++) vs[i]=F(inf+step*i, params); + int max=0; for (int i=1; i<=c; i++) if (vs[max]<vs[i]) max=i; + *maxresult=vs[max]; delete[] vs; + if (max>0 && max<c) return Search1Dmax(x, params, F, inf+step*(max-1), inf+step*(max+1), maxresult, ep, maxiter); + else if (max==0) + { + if (step<=ep) {x=inf; return ep;} + else return Search1DmaxEx(x, params, F, inf, inf+step, maxresult, ep, maxiter, len); + } + else + { + if (step<=ep) {x=sup; return ep;} + else return Search1DmaxEx(x, params, F, sup-step, sup, maxresult, ep, maxiter, len); + } +}//Search1DmaxEx + +/* + function Search1Dmaxfrom: 1-dimensional maximum search from start[] towards start+direct[]. The + function tries to locate an interval ("step") for which the 4-point convexity rule confirms the + existence of a convex maximum. + + In: function F(x) + params: additional arguments for calling F + start[dim]: initial x[] + direct[dim]: search direction + step: initial step size + minstep: minimal step size in 4-point scheme between x0 and x3 + maxstep: maximal step size in 4-point scheme between x0 and x3 + Out: opt[dim]: the maximum, if specified + max: maximal value, if specified + + Returns x>0 that locally maximizes F(start+x*direct) + or x|-x>0 that maximizes F(start-x*direct) at calculated points if convex check fails. +*/ +double Search1Dmaxfrom(void* params, double (*F)(int, double*, void*), int dim, double* start, double* direct, + double step, double minstep, double maxstep, double* opt, double* max, double ep, int maxiter, bool convcheck) +{ + bool record=true; + if (!opt) + { + record=false; + opt=new double[dim]; + } + + double v0618=(sqrt(5.0)-1)/2, iv0618=1-v0618; + double x3=step, x0=0, x1=-1, x2=-1, l=x3-x0; + + //prepares x0~x3, v0~v3, x0=0, v2>v3 + memcpy(opt, start, sizeof(double)*dim); + double v0=F(dim, opt, params); + MultiAdd(dim, opt, start, direct, x3); + double v1, v2, v3=F(dim, opt, params); + if (v0>=v3) + { + x1=x0+l*iv0618; + MultiAdd(dim, opt, start, direct, x1); + v1=F(dim, opt, params); + x2=x0+l*v0618; + MultiAdd(dim, opt, start, direct, x2); + v2=F(dim, opt, params); + while (l>minstep && v0>v1) + { + x3=x2, v3=v2; + x2=x1, v2=v1; + l=x3-x0; + x1=x0+l*iv0618; + MultiAdd(dim, opt, start, direct, x1); + v1=F(dim, opt, params); + } + if (l<=minstep) + { + if (max) max[0]=v0; + if (!record) delete[] opt; + return x0; + } + } + else//v0<=v3 + { + do + { + x1=x2, v1=v2; + x2=x3, v2=v3; + x3+=l*v0618; + l=x3-x0; + MultiAdd(dim, opt, start, direct, x3); + v3=F(dim, opt, params); + } while (v2<v3 && x3-x0<maxstep); + if (l>=maxstep) //increasing all the way to maxstep + { + if (max) max[0]=v3; + if (!record) delete opt; + return x3; + } + else if (x1<0) + { + x1=x0+l*iv0618; + MultiAdd(dim, opt, start, direct, x1); + v1=F(dim, opt, params); + } + } + + double oldl=l; + + int iter=0; + + if (convcheck && (v1<v0*v0618+v3*iv0618 || v2<v0*iv0618+v3*v0618 + || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + + while (iter<maxiter) + { + if (l<oldl*ep) break; + if (v1>v2) + { + x3=x2, v3=v2; + l=x3-x0; + x2=x1, v2=v1; + x1=x0+l*iv0618; + MultiAdd(dim, opt, start, direct, x1); + v1=F(dim, opt, params); + if (convcheck && (v1<v0*v0618+v3*iv0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + } + else + { + x0=x1, v0=v1; + l=x3-x0; + x1=x2, v1=v2; + x2=x0+l*v0618; + MultiAdd(dim, opt, start, direct, x2); + v2=F(dim, opt, params); + if (convcheck && (v2<v0*iv0618+v3*v0618 || v1<v0*iv0618+v2*v0618 || v2<v1*v0618+v3*iv0618)) goto return1; + } + iter++; + } + + if (!record) delete[] opt; + + if (max) max[0]=v1; + return x1; + +return1: + if (!record) delete[] opt; + if (v1>v0) v0=v1,x0=x1; if (v2>v0) v0=v2, x0=x2; if (v3>v0) v0=v3, x0=x3; + if (max) max[0]=v0; + return -x0; +}//Search1Dmaxfrom + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/opt.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,31 @@ +#ifndef optH +#define optH + +/* + opt.cpp - optimization routines +*/ + +//--tool procedures---------------------------------------------------------- +double nextdouble(double x, double dir); //returns next double-precision floating pointer number + +//--Gradient method---------------------------------------------------------- +double GradientMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*), int dim, double* start, double ep=1e-6, int maxiter=100); +double GradientOneMax(void* params, double (*F)(int, double*, void*), void (*dF)(double*, int, double*, void*), int dim, double* start, double ep, int maxiter); + +//--Newton methods------------------------------------------------------------ +double Newton(double& x0, double (*y)(double, void*), double (*y1)(double, void*), void* params=0, double epx=1e-6, int maxiter=100, double epy=1e-256); +int Newton(double& x0, double (*dy)(double, void*), void* params, int yshift, double epx=1e-6, int maxiter=100, double epy=1e-256, double xmin=-1e308, double xmax=1e308); +double Newton1dmax(double& x0, double xa, double xb, double (*ddy)(double, void*), void* params, int y1shift, int yshift, double (*dy)(double, void*), int dyshift, double epx=1e-6, int maxiter=100, double epdy=1e-128, bool checkinput=true); +double Newton2D(double& x1, double& x2, double (*dy)(double&, double&, void*), double (*ddy)(double&, double&, double&, void*), void* params=0, double epx1=1e-6, double epx2=1e-6, int maxiter=100, double epdy=1e-256); + +//--root search routines----------------------------------------------------- +double rootsearchhalf(double&x1, double x2, double (*y)(double, void*), void* params=0, double epx=1e-6, int maxiter=100); +double rootsearchsecant(double& x1, double x2, double (*y)(double, void*), void* params=0, double epx=1e-6, int maxiter=100); +double rootsearchbisecant(double&x1, double x2, double (*y)(double, void*), void* params=0, double epx=1e-6, int maxiter=100); + +//--1-D maximum search routines---------------------------------------------- +double Search1Dmax(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult=0, double ep=1e-6, int maxiter=100, bool convcheck=false); +double Search1DmaxEx(double& x, void* params, double (*F)(double, void*), double inf, double sup, double* maxresult=0, double ep=1e-6, int maxiter=100, double len=1); +double Search1Dmaxfrom(void* params, double (*F)(int, double*, void*), int dim, double* start, double* direct, double step=1e-2, double minstep=1e-6, double maxstep=1e6, double* opt=0, double* max=0, double ep=1e-6, int maxiter=100, bool convcheck=false); + +#endif
--- /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 + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/procedures.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,236 @@ +#ifndef proceduresH +#define proceduresH + +/* + procedures.cpp - this file collects miscellaneous structures and functions. Not all of these are + referenced somewhere else. +*/ + + +//--------------------------------------------------------------------------- +#include <memory.h> +#include <stdlib.h> +#include <values.h> +#include "fft.h" +#include "windowfunctions.h" + +/* + macro testnn: non-negative test. This macro throws out an exception if the value of x is negative. +*/ +#ifndef testnn +#define testnn(x) {try{if (x<0) throw("testnn");}catch(...){x=0;}} +#endif + +/* + macro AllocateGMM and ReleaseGMM: allocates and frees buffers that hosts descriptors of a Gaussian- + mixture model. +*/ +#define AllocateGMM(_MIXS, _DIM, _P, _M, _DEV) \ + double* _P=new double[_MIXS]; double** _M=new double*[_MIXS]; double ** _DEV=new double*[_MIXS]; \ + _M[0]=new double[(_MIXS)*(_DIM)]; _DEV[0]=new double[(_MIXS)*(_DIM)]; \ + for (int _MIX=1; _MIX<_MIXS; _MIX++) _M[_MIX]=&_M[0][_MIX*(_DIM)], _DEV[_MIX]=&_DEV[0][_MIX*(_DIM)]; +#define ReleaseGMM(_P, _M, _DEV) \ + delete[] _P; if (_M) {delete[] _M[0]; delete[] _M;} if (_DEV) {delete[] _DEV[0]; delete[] _DEV;} + + +//--------------------------------------------------------------------------- +/* + Tick count tool (stop watch) is made up of a TickClock struct and three macros that uses this + structure as augument. + + Use of TickClock: declare the instance directly. Use StartClock() and StopClock() to start and stop + tick counting. Each time the clock is stopped the interval between last start and stop operations is + added to run-time t. Clear the recorded run-time with ResetClock(). +*/ +struct TickClock //tick counter struct +{ + double t; //accumulated run time + __int64 Ticks; //tick count recorded at start trigger + __int64 Ticks2; //tick count recorded at stop trigger +}; +#define ResetClock(AClock) {AClock.t=0;} +#define StartClock(AClock) {AClock.Ticks=GetTickCount();} +#define StopClock(AClock) {AClock.Ticks2=GetTickCount(); AClock.t+=0.001*(AClock.Ticks2-AClock.Ticks);} + +//--------------------------------------------------------------------------- + +struct fftparams //buffers used by FFT routines +{ + double* Amp; //amplitude output buffer + double* Arg; //phase angle output buffer + cdouble* w; //twiddle factor buffer + cdouble* x; //data buffer +}; + +struct TGMM //Gaussian mixture model +{ +public: + bool mcs; + int mixs; //number of mixtures + int dim; //data dimension + double* p; //mixture weights + double** m; //mixture component centres + double** dev; //diagonal mixture component correlation + double acct; + TGMM(); + ~TGMM(); +}; + +struct TTFSpan //a rectanguar area in the T-F plane +{ + int T1; //start time + int T2; //finish time + double F1; //lowest frequency + double F2; //highest frequency +}; + +struct TSpecPeak //spectral peak +{ + int t; //time in samples + int tres; //time resolution + double f; //digital frequency + double fres; //frequency resolution + double df; //derivative of f + double am; //amplitude + double ph; //phase angle +}; + +/* + TSpecTrack is a class that maintains a list of TSpecPeak objects that form a spectral track. It models + a sinusid track in sinusoid modeling. +*/ +class TSpecTrack +{ +private: + int Capacity; //number of peaks the current storage space is allocated to host +public: + int t1; //time of first peak + int t2; //time of last peak + double fmin; //minimum peak frequency + double fmax; //maximum peak frequency + + int Count; //number of peaks in the track + TSpecPeak* Peaks; //peaks in the track, ordered by time + + TSpecTrack(int ACapacity=50); + ~TSpecTrack(); + + void InsertPeak(TSpecPeak& APeak, int index); + int LocatePeak(TSpecPeak& APeak); + int Add(TSpecPeak& APeak); +}; + +/* + TTFSpans is a class that maintains a list of TFSpans. This is used to mark selected areas in the time- + frequency plane for further processing. +*/ +class TTFSpans +{ +public: + int Count; //number of spans + int Capacity; //number of spans the current storage space is allocated to host + TTFSpan* Items; //the hosted spans + + TTFSpans(); + ~TTFSpans(); + void Add(TTFSpan& ATFSpan); + void Clear(); + int Delete(int Index); +}; + +double ACPower(double* data, int Count, void*); +void Add(double* dest, double* source, int Count); +void Add(double* out, double* addend, double* adder, int count); +void ApplyWindow(double* OutBuffer, double* Buffer, double* Weights, int Size); +double Avg(double*, int, void*); +void AvgFilter(double* dataout, double* data, int Count, int HWid); +int BitInv(int value, int order); +void CalculateSpectrogram(double* data, int Count, int start, int end, int Wid, int Offst, double* Window=0, double** Spec=0, double** Ph=0, double amp=1, bool half=true); +void Conv(double* out, int N1, double* in1, int N2, double* in2); +void DCT( double* output, double* input, int N); +void IDCT( double* output, double* input, int N); +double DCAmplitude(double*, int, void*); +double DCPower(double*, int, void*); +double ddIPHann(double, void*); +void DeDC(double* data, int Count, int HWid); +void DeDC_static(double* data, int Count); +void DoubleToInt(void* out, int BytesPerSample, double* in, int Count); +void DoubleToIntAdd(void* out, int BytesPerSample, double* in, int Count); +double DPower(double* data, int Count, void*); +double Energy(double* data, int Count); +double ExpOnsetFilter(double* dataout, double* data, int Count, double Tr, double Ta); +double ExpOnsetFilter_balanced(double* dataout, double* data, int Count, double Tr, double Ta, int bal=5); +double ExtractLinearComponent(double* dataout, double* data, int Count); +void FFTConv(double* dest, double* source1, int size1, double* source2, int size2, int zero=0, double* pre_buffer=NULL, double* post_buffer=NULL); +void FFTConv(unsigned char* dest, unsigned char* source1, int bps, int size1, double* source2, int size2, int zero=0, unsigned char* pre_buffer=NULL, unsigned char* post_buffer=NULL); +void FFTConv(double* dest, double* source1, int size1, double* source2, int size2, double* pre_buffer=NULL); +void FFTFilter(double* dataout, double* data, int Count, int Wid, int On, int Off); +void FFTFilterOLA(double* dataout, double* data, int Count, int Wid, int On, int Off, double* pre_buffer=NULL); +void FFTFilterOLA(unsigned char* dataout, unsigned char* data, int BytesPerSample, int Count, int Wid, int On, int Off, unsigned char* pre_buffer=NULL); +void FFTFilterOLA(double* dataout, double* data, int Count, double* amp, double* ph, int Wid, double* pre_buffer=NULL); +double FFTMask(double* dataout, double* data, int Count, int Wid, double DigiOn, double DigiOff, double maskcoef=1); +int FindInc(double value, double* data, int Count); +double Gaussian(int Dim, double* Vector, double* Mean, double* Dev, bool StartFrom1); +void HalfDCT(double* data, int Count, double* CC, int order); +double Hamming(double f, double T); +double Hann(double x, double N); +double HannSq(double x, double N); +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=1e-6); +double I0(double x); +int InsertDec(double value, double* data, int Count); +int InsertDec(int value, int* data, int Count); +int InsertDec(double value, int index, double* data, int* indices, int Count); +int InsertInc(void* value, void** data, int Count, int (*Compare)(void*, void*)); +int InsertInc(double value, double* data, int Count, bool doinsert=true); +int InsertInc(double value, int index, double* data, int* indices, int Count); +int InsertInc(double value, double index, double* data, double* indices, int Count); +int InsertInc(double value, int* data, int Count, bool doinsert=true); +int InsertIncApp(double value, double* data, int Count); +double InstantFreq(int k, int hwid, cdouble* x, int mode=1); +void InstantFreq(double* freqspec, int hwid, cdouble* x, int mode=1); +void IntToDouble(double* out, void* in, int BytesPerSample, int Count); +double IPHannC(double f, cdouble* x, int N, int K1, int K2); +double IPHannC(double f, void* params); +double IPHannCP(double f, void* params); +void lse(double* x, double* y, int Count, double& a, double& b); +void memdoubleadd(double* dest, double* source, int count); +void MFCC(int FrameWidth, int NumBands, int Ceps_Order, double* Data, double* Bands, double* C, double* Amps, cdouble* W, cdouble* X); +double* MFCCPrepareBands(int NumberOfBands, int SamplesPerSec, int NumberOfBins); +void Multi(double* data, int count, double mul); +void Multi(double* out, double* in, int count, double mul); +void MultiAdd(double* out, double* in, double* adder, int count, double mul); +int NearestPeak(double* data, int count, int anindex); +double NegativeExp(double* x, double* y, int Count, double& lmd, int sample=1, double step=0.05, double eps=1e-6, int maxiter=50); +double NL(double* data, int Count, int Wid); +double Normalize(double* data, int Count, double Maxi=1); +double Normalize2(double* data, int Count, double Norm=1); +double nextdouble(double x, double dir); +double PhaseSpan(double* data, int Count, void*); +void PolyFit(int P, double* a, int N, double* x, double* y); +void Pow(double* data, int Count, double ex); +//int prevcand(candid** cands, int p, int n, int ap); +int Rectify(double* data, int Count, double th=0); +double Res(double in, double mod); +double Romberg(int n, double(*f)(double, void*), double a, double b, void* params=0); +double Romberg(double(*f)(double, void*), double a, double b, void* params=0, int maxiter=50, double ep=1e-6); +double sinca(double x); +double sincd_unn(double x, int N); +double SmoothPhase(double* Arg, int Count, int mpi=2); +//int SortCandid(candid cand, candid* cands, int newN); +double StiffB(double f0, double fm, int m); +double StiffFm(double f0, int m, double B); +double StiffF0(double fm, int m, double B); +double StiffM(double f0, double fm, double B); +void TFFilter(double* data, double* dataout, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst=0); +void TFFilter(void* data, void* dataout, int BytesPerSample, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst); +void TFMask(double* data, double* dataout, int Count, int Wid, TTFSpans* Spans, double masklevel, WindowType wt, double windp, int Sps, int TOffst); +void TFMask(void* data, void* dataout, int BytesPerSec, int Count, int Wid, TTFSpans* Spans, double masklevel, WindowType wt, double windp, int Sps, int TOffst); +//double dIPHannC(double f, void* params); +//void ddsIPWindowStiff(double& df0f0, double& df0B, double& dBB, double& df0, double& dB, double& y, double f0, double B, cdouble* x, int N, int M, double* c, double iH2, double hB, int maxp); +//void dsIPWindowStiff(double& df0, double& dB, double& y, double f0, double B, cdouble* x, int N, int M, double* c, double iH2, double hB, int maxp); +//double IPWindowStiff(double f0, double B, cdouble* x, int N, int M, double* c, double iH2, double* ffp, double* vfp, double* pfp, double hB, int maxp); +//double sIPWindowStiff(double f0, double B, cdouble* x, int N, int M, double* c, double iH2, double* ffp, double* vfp, double* pfp, double hB, int maxp); +//double sIPWindowStiff(double f0, double B, cdouble* x, int N, int M, double* c, double iH2, double hB, int maxp); +//double IPWindowMulti(double* f, int ss, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/splines.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,247 @@ +//--------------------------------------------------------------------------- + + +#include "splines.h" + +//--------------------------------------------------------------------------- +/* + function tridiagonal: solves linear system A[N][N]x[N]=d[N] where A is tridiagonal + + In: tridiagonal matrix A[N][N] gives as three vectors - lower subdiagonal + a[1:N-1], main diagonal b[0:N-1], upper subdiagonal c[0:N-2] + vector d[N] + Out: vector d[N] now containing x[N] + + No return value. +*/ +void tridiagonal(int N, double* a, double* b, double* c, double* d) +{ + for (int k=1; k<N; k++) + { + double m=a[k]/b[k-1]; + b[k]=b[k]-m*c[k-1]; + d[k]=d[k]-m*d[k-1]; + } + d[N-1]=d[N-1]/b[N-1]; + for (int k=N-2; k>=0; k--) + d[k]=(d[k]-c[k]*d[k+1])/b[k]; +}//tridiagonal + +/* + function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline + + In: x[N+1], y[N+1]: knots + bordermode: boundary mode of cubic spline + bordermode=0: natural: y''(x0)=y''(x_N)=0 + bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1)) + bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point + mode: spline format + mode=0: spline defined on the l'th piece as y(x)=a[l]x^3+b[l]x^2+c[l]x+d[l] + mode=1: spline defined on the l'th piece as y(x)=a[l]t^3+b[l]t^2+c[l]t+d[l], t=x-x[l] + Out: a[N],b[N],c[N],d[N]: piecewise trinomial coefficients + data[]: cubic spline computed for [xstart:xinterval:xend], if specified + No return value. On start d must be allocated no less than N+1 storage units. +*/ +void CubicSpline(int N, double* a, double* b, double* c, double* d, double* x, double* y, int bordermode, int mode, double* data, double xinterval, double xstart, double xend) +{ + double *h=new double[N]; + for (int i=0; i<N; i++) h[i]=x[i+1]-x[i]; + + for (int i=0; i<N-1; i++) + { + a[i]=h[i]; + b[i]=2*(h[i]+h[i+1]); + c[i]=h[i+1]; + d[i+1]=6*((y[i+2]-y[i+1])/h[i+1]-(y[i+1]-y[i])/h[i]); + } + a[0]=0; c[N-2]=0; + + if (bordermode==1) + { + b[0]+=h[0]; + b[N-2]+=h[N-1]; + } + else if (bordermode==2) + { + b[0]+=2*h[0]; c[0]-=h[0]; + b[N-2]+=2*h[N-1]; a[N-2]-=h[N-1]; + } + + tridiagonal(N-1, a, b, c, &d[1]); + + if (bordermode==0) + { + d[0]=0; d[N]=0; + } + else if (bordermode==1) + { + d[0]=d[1]; d[N]=d[N-1]; + } + else if (bordermode==2) + { + d[0]=2*d[1]-d[2]; + d[N]=2*d[N-1]-d[N-2]; + } + + for (int i=0; i<N; i++) + { + double zi=d[i], zi1=d[i+1], xi=x[i], xi1=x[i+1], hi=h[i]; + double co1=y[i+1]/hi-hi*zi1/6, co2=y[i]/hi-hi*zi/6; + if (mode==0) + { + a[i]=(zi1-zi)/6/hi; + b[i]=(-xi*zi1+xi1*zi)/2/hi; + c[i]=(zi1*xi*xi-zi*xi1*xi1)/2/hi+co1-co2; + d[i]=(-zi1*xi*xi*xi+zi*xi1*xi1*xi1)/6/hi-co1*xi+co2*xi1; + } + else if (mode==1) + { + a[i]=(zi1-zi)/6/hi; + b[i]=zi/2; + c[i]=-zi*hi/2+co1-co2; + d[i]=zi*hi*hi/6+co2*hi; + } + zi=zi1; + } + delete[] h; + if (data) + { + if (xend<xstart) xend=x[N], xstart=x[0]; + int Count=(xend-xstart)/xinterval+1; + for (int i=0, n=0; i<Count; i++) + { + double xx=xstart+i*xinterval; + while (n<N-1 && xx>x[n+1]) n++; + if (mode==1) xx=xx-x[n]; + data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n])); + } + } +}//CubicSpline + +/* + function CubicSpline: computing piece-wise trinomial coefficients of a cubic spline with uniformly placed knots + + In: y[N+1]: spline values at knots (0, h, 2h, ..., Nh) + bordermode: boundary mode of cubic spline + bordermode=0: natural: y''(x0)=y''(x_N)=0 + bordermode=1: quadratic runout: y''(x0)=y''(x1), y''(x_N)=y''(x_(N-1)) + bordermode=2: cubic runout: y''(x0)=2y''(x1)-y''(x2), the same at the end point + mode: spline format + mode=0: spline defined on the l'th piece as y(x)=a[l]x^3+b[l]x^2+c[l]x+d[l] + mode=1: spline defined on the l'th piece as y(x)=a[l]t^3+b[l]t^2+c[l]t+d[l], t=x-x[l] + Out: a[N],b[N],c[N],d[N]: piecewise trinomial coefficients + data[]: cubic spline computed for [xstart:xinterval:xend], if specified + No return value. On start d must be allocated no less than N+1 storage units. +*/ +void CubicSpline(int N, double* a, double* b, double* c, double* d, double h, double* y, int bordermode, int mode, double* data, double xinterval, double xstart, double xend) +{ + for (int i=0; i<N-1; i++) + { + a[i]=h; + b[i]=4*h; + c[i]=h; + d[i+1]=6*((y[i+2]-y[i+1])/h-(y[i+1]-y[i])/h); + } + a[0]=0; c[N-2]=0; + + if (bordermode==1) + { + b[0]+=h; + b[N-2]+=h; + } + else if (bordermode==2) + { + b[0]+=2*h; c[0]-=h; + b[N-2]+=2*h; a[N-2]-=h; + } + + tridiagonal(N-1, a, b, c, &d[1]); + + if (bordermode==0) + { + d[0]=0; d[N]=0; + } + else if (bordermode==1) + { + d[0]=d[1]; d[N]=d[N-1]; + } + else if (bordermode==2) + { + d[0]=2*d[1]-d[2]; + d[N]=2*d[N-1]-d[N-2]; + } + + for (int i=0; i<N; i++) + { + double zi=d[i], zi1=d[i+1]; + if (mode==0) + { + double co1=y[i+1]/h-h*zi1/6, co2=y[i]/h-h*zi/6; + a[i]=(zi1-zi)/6/h; + b[i]=(-i*zi1+(i+1)*zi)/2; + c[i]=(zi1*i*i*h-zi*(i+1)*h*(i+1))/2+co1-co2; + d[i]=(-zi1*i*h*i*h*i+zi*(i+1)*h*(i+1)*h*(i+1))/6-co1*i*h+co2*(i+1)*h; + } + else if (mode==1) + { + a[i]=(zi1-zi)/6/h; + b[i]=zi/2; + c[i]=-(zi*2+zi1)*h/6+(y[i+1]-y[i])/h; + d[i]=y[i]; + } + zi=zi1; + } + if (data) + { + if (xend<xstart) xend=N*h, xstart=0; + int Count=(xend-xstart)/xinterval+1; + for (int i=0, n=0; i<Count; i++) + { + double xx=xstart+i*xinterval; + while (n<N-1 && xx>(n+1)*h) n++; + if (mode==1) xx=xx-n*h; + data[i]=d[n]+xx*(c[n]+xx*(b[n]+xx*a[n])); + } + } +}//CubicSpline + +/* + function Smooth_Interpolate: smoothly interpolate/extrapolate P-piece spline from P-1 values. The + interpolation scheme is chosen according to P: + P-1=1: constant + P-1=2: linear function + P-1=3: quadratic function + P-1>=4: cubic spline + + In: xind[P-1], x[P-1]: knot points + Out: y[N]: smoothly interpolated signal, y(xind[p])=x[p], p=0, ..., P-2. + + No return value. +*/ +void Smooth_Interpolate(double* y, int N, int P, double* x, double* xind) +{ + if (P==2) + { + for (int n=0; n<N; n++) y[n]=x[0]; + } + else if (P==3) + { + double alp=(x[1]-x[0])/(xind[1]-xind[0]); + for (int n=0; n<N; n++) y[n]=x[0]+(n-xind[0])*alp; + } + else if (P==4) + { + double x0=xind[0], x1=xind[1], x2=xind[2], y0=x[0], y1=x[1], y2=x[2]; + double ix0_12=y0/((x0-x1)*(x0-x2)), ix1_02=y1/((x1-x0)*(x1-x2)), ix2_01=y2/((x2-x0)*(x2-x1)); + double a=ix0_12+ix1_02+ix2_01, + b=-(x1+x2)*ix0_12-(x0+x2)*ix1_02-(x0+x1)*ix2_01, + c=x1*x2*ix0_12+x0*x2*ix1_02+x0*x1*ix2_01; + for (int n=0; n<N; n++) y[n]=c+n*(b+n*a); + } + else + { + double *fa=new double[P*4], *fb=&fa[P], *fc=&fa[P*2], *fd=&fa[P*3]; + CubicSpline(P-2, fa, fb, fc, fd, xind, x, 0, 1, y, 1, 0, N-1); + delete[] fa; + } +}//Smooth_Interpolate
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/splines.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,16 @@ +#ifndef splinesH +#define splinesH + +/* + splines.cpp - spline interpolation routines +*/ + +//--cubic spline construction routines--------------------------------------- +void CubicSpline(int N, double* a, double* b, double* c, double* d, double* x, double* y, int bordermode, int mode=0, double* data=0, double xinterval=1, double xstart=0, double xend=-1); +void CubicSpline(int N, double* a, double* b, double* c, double* d, double h, double* y, int bordermode, int mode=0, double* data=0, double xinterval=1, double xstart=0, double xend=-1); + +//--smooth interpolation----------------------------------------------------- +void Smooth_Interpolate(double* y, int N, int P, double* x, double* xind); + + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/vibrato.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,1301 @@ +//--------------------------------------------------------------------------- + + +#include <math.h> +#include <mem.h> +#include "vibrato.h" +#include "arrayalloc.h" +#include "fft.h" +#include "hssf.h" +#include "Matrix.h" +#include "splines.h" +#include "xcomplex.h" +//--------------------------------------------------------------------------- +//method TVo::TVo: default constructor +TVo::TVo() +{ + memset(this, 0, sizeof(TVo)); + afres=8192; +}//TVo + +//method TVo::~TVo: default destructor +TVo::~TVo() +{ + free(peakfr); + delete[] lp; + delete[] F0; + if (fxr) DeAlloc2(fxr); + if (fres) DeAlloc2(fres); + if (LogASp) DeAlloc2(LogASp); +}//~TVo + +/* + method TVo::AllocateL: allocates or reallocates storage space whose size depends on L. This uses an + external L value for allocation and updates L itself. +*/ +void TVo::AllocateL(int AnL) +{ + L=AnL; + delete[] F0; + F0=new double[(L+2)*4]; + F0C=&F0[L+2], F0D=&F0[(L+2)*2], A0C=&F0[(L+2)*3]; +}//AllocateL + +/* + method TVo::AllocateP: allocates or reallocates storage space whose size depends on P, using the + interal P. +*/ +void TVo::AllocateP() +{ + peakfr=(int*)realloc(peakfr, sizeof(int)*P); + delete[] lp; lp=new double[(P+2)*3]; + Dp=&lp[P+2]; + Kp=(int*)&lp[(P+2)*2]; + if (LogASp) DeAlloc2(LogASp); Allocate2(double, P, M, LogASp); +}//AllocateP + +/* + method TVo::AllocatePK: allocates or reallocates storage space whose size depends on both P and K. +*/ +void TVo::AllocatePK() +{ + if (fxr) DeAlloc2(fxr); + Allocate2(double, P, 2*K+1, fxr); + memset(fxr[0], 0, sizeof(double)*P*(2*K+1)); + if (fres) DeAlloc2(fres); + Allocate2(double, P, K, fres); +}//AllocatePK + +/* + method TVo::GetOldP: returns the number of "equivalent" cycles of the whole duration. Although P-1 + cycles are delineated by lp[P], the parts before lp[0] and after lp[P-1] are also a part of the + harmonic sinusoid being modeled and therefore should be taken into account when necessary. +*/ +double TVo::GetOldP() +{ + return P-1+lp[0]/(lp[1]-lp[0])+(L-lp[P-1])/(lp[P-1]-lp[P-2]); +}//GetOldP + +/* + methodTVo::LoadFromFileHandle: reads TVo object from a file stream. +*/ +void TVo::LoadFromFileHandle(FILE* file) +{ + fread(&M, sizeof(int), 1, file); + fread(&L, sizeof(int), 1, file); + fread(&P, sizeof(int), 1, file); + fread(&K, sizeof(int), 1, file); + fread(&h, sizeof(double), 1, file); + fread(&afres, sizeof(int), 1, file); + AllocateL(L); AllocateP(); AllocatePK(); + fread(F0C, sizeof(double), L, file); + fread(A0C, sizeof(double), L, file); + fread(LogAF, sizeof(double), 4096, file); + fread(peakfr, sizeof(int), P, file); + fread(lp, sizeof(double), P, file); + fread(Dp, sizeof(double), P, file); + fread(Kp, sizeof(int), P, file); + fread(fxr[0], sizeof(double), P*(2*K+1), file); + fread(LogASp[0], sizeof(double), P*M, file); +}//LoadFromFileHandle + +/* + method TVo::ReAllocateL: reallocates storage space whose size depends on L, and transfer the contents. + This uses an external L value for allocation but does not update L itself. +*/ +void TVo::ReAllocateL(int newL) +{ + double* newF0=new double[(newL+2)*4]; + F0C=&newF0[newL+2], F0D=&newF0[(newL+2)*2], A0C=&newF0[(newL+2)*3]; + memcpy(A0C, &F0[(L+2)*3], sizeof(double)*(L+2)); + memcpy(F0D, &F0[(L+2)*2], sizeof(double)*(L+2)); + memcpy(F0C, &F0[L+2], sizeof(double)*(L+2)); + memcpy(newF0, F0, sizeof(double)*(L+2)); + delete[] F0; + F0=newF0; +}//ReAllocateL + +/* + method TVo::SaveToFile: saves a TVo object to a file. + + In: filename: path to the destination file. +*/ +void TVo::SaveToFile(char* filename) +{ + FILE* file=fopen(filename, "wb"); + if (file) + { + SaveToFileHandle(file); + fclose(file); + } + else + { + throw(strcat((char*)"Cannot open file ", filename)); + } +}//SaveToFile + +/* + method TVo::SaveToFileHandle: saves a TVo object to an open file stream. +*/ +void TVo::SaveToFileHandle(FILE* file) +{ + fwrite(&M, sizeof(int), 1, file); + fwrite(&L, sizeof(int), 1, file); + fwrite(&P, sizeof(int), 1, file); + fwrite(&K, sizeof(int), 1, file); + fwrite(&h, sizeof(double), 1, file); + fwrite(&afres, sizeof(int), 1, file); + fwrite(F0C, sizeof(double), L, file); + fwrite(A0C, sizeof(double), L, file); + fwrite(LogAF, sizeof(double), 4096, file); + fwrite(peakfr, sizeof(int), P, file); + fwrite(lp, sizeof(double), P, file); + fwrite(Dp, sizeof(double), P, file); + fwrite(Kp, sizeof(int), P, file); + fwrite(fxr[0], sizeof(double), P*(2*K+1), file); + fwrite(LogASp[0], sizeof(double), P*M, file); +}//SaveToFileHandle + +//--------------------------------------------------------------------------- +//functions + + +/* + function AnalyzeV: calculates all basic and non-basic descriptors of a vibrato from a harmonic + sinusoid + + In: HS: harmonic sinusoid to compute vibrato representation from + sps: sampling rate + h: hop size + SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation) + SFF: filter response sampling interval + SFScale: set if filter sampling uses mel scale + SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach + Out: V: a TVo object hosting vibrato descriptors (vibrato representation) of HS + cyclefrcount: number of cycles between cycle boundaries, equals V.P-1. + cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: time (in frames) and F0 centroid of cycles. These are + knots from which V.F0C[] is interpolated. + peaky[V.P]: shape score of cycle peaks + + No return value. +*/ +void AnalyzeV(THS& HS, TVo& V, double*& peaky, double*& cyclefrs, double*& cyclefs, double sps, double h, int* cyclefrcount, + int SFMode, double SFF, int SFFScale, double SFtheta) +{ + int M=HS.M, Fr=HS.Fr; + if (M<=0 || Fr<=0) return; + atom** Partials=HS.Partials; + + //basic descriptor: h + V.h=h; + + //demodulating pitch + //Basic descriptor: L + V.AllocateL(Fr); + + double* AA=new double[Fr]; + //find the pitch track + V.F0max=0, V.F0min=1; + for (int fr=0; fr<Fr; fr++) + { + double suma2=0, suma2p=0; + for (int m=0; m<M; m++) + { + double a=Partials[m][fr].a, p=Partials[m][fr].f/(m+1); + if (p<=0) break; + suma2+=a*a, suma2p+=a*a*p; + } + V.F0[fr]=suma2p/suma2; + if (V.F0max<V.F0[fr]) V.F0max=V.F0[fr]; + if (V.F0min>V.F0[fr]) V.F0min=V.F0[fr]; + AA[fr]=suma2; + } + + //Basic descriptor: M + V.M=0.45/V.F0max; + if (V.M>M) V.M=M; + + //rough estimation of rate + double* f0d=new double[Fr-1]; for (int i=0; i<Fr-1; i++) f0d[i]=V.F0[i+1]-V.F0[i]; + RateAndReg(V.rate, V.regularity, f0d, 0, Fr-2, 8, sps, h); + delete[] f0d; + + //find peaks of the pitch track + free(V.peakfr); V.peakfr=(int*)malloc(sizeof(int)*Fr/2); + double periodinframe=sps/(V.rate*h); + FindPeaks(V.peakfr, V.P, V.F0, Fr, periodinframe, peaky); + if (V.P>=1) //de-modulation + { + //Basic descriptor: F0C[] + DeFM(V.F0C, V.F0, AA, V.P, V.peakfr, Fr, V.F0Overall, *cyclefrcount, cyclefrs, cyclefs, true); + //Basic descriptor: A0C[] + DeAM(V.A0C, AA, V.P, V.peakfr, Fr); + } + + V.F0Cmax=0; V.F0Dmax=-1; V.F0Cmin=1; V.F0Dmin=1; + for (int fr=0; fr<Fr; fr++) + { + if (V.F0Cmax<V.F0C[fr]) V.F0Cmax=V.F0C[fr]; + if (V.F0Cmin>V.F0C[fr]) V.F0Cmin=V.F0C[fr]; + V.F0D[fr]=V.F0[fr]-V.F0C[fr]; + } + + V.D=0; int cyclestart=(V.P>=2)?V.peakfr[0]:0, cycleend=(V.P>=2)?V.peakfr[V.P-1]:Fr; + for (int fr=cyclestart; fr<cycleend; fr++) + { + double lf0d=V.F0D[fr]; + if (V.F0Dmax<lf0d) V.F0Dmax=lf0d; + if (V.F0Dmin>lf0d) V.F0Dmin=lf0d; + V.D+=lf0d*lf0d; + } + V.D=sqrt(V.D/(cycleend-cyclestart)); + delete[] AA; + + //Calculate rate and regularity + //find the section used for calculating regularity and rate + { + int frst=0, fren=Fr-1; + while (frst<fren && V.F0D[frst]*V.F0D[frst+1]>0) frst++; + while (fren>frst && V.F0D[fren]*V.F0D[fren-1]>0) fren--; + + RateAndReg(V.rate, V.regularity, V.F0D, frst, fren, 8, sps, h); + } + + //Basic descriptor: P + //Basic descriptor: peakfr[] + periodinframe=sps/(V.rate*h); + FindPeaks(V.peakfr, V.P, V.F0D, Fr, periodinframe, peaky); + + //Basic descriptor: lp[] + V.AllocateP(); + for (int p=0; p<V.P; p++) + { + double startfr; QIE(&V.F0D[V.peakfr[p]], startfr); V.lp[p]=startfr+V.peakfr[p]; + } + + //Basic descriptor: LogAF[] + //Source-filter modeling + if (SFMode==0) + S_F(V.afres, V.LogAF, V.LogAS, Fr, M, Partials, V.A0C, V.lp, V.P, V.F0Overall); + else + S_F_SV(V.afres, V.LogAF, V.LogAS, Fr, M, Partials, V.A0C, V.lp, V.P, SFF, SFFScale, SFtheta, sps); + + + //the alternative: find K only + V.K=50; + if (V.K>periodinframe/2) V.K=periodinframe/2; + + //single-cycle analyses + //Basic descriptor: Dp[] + V.AllocatePK(); + for (int p=1; p<V.P; p++) + { + //int frst=ceil(V.lp[p-1]), freninc=floor(V.lp[p]); + int frst=V.peakfr[p-1], freninc=V.peakfr[p]; + V.Dp[p]=0; for (int fr=frst; fr<=freninc; fr++) V.Dp[p]+=V.F0D[fr]*V.F0D[fr]; V.Dp[p]=sqrt(V.Dp[p]/(freninc-frst+1)); + double* f0e=new double[freninc-frst+1]; + for (int fr=0; fr<=freninc-frst; fr++) f0e[fr]=V.F0D[frst+fr]/V.Dp[p]; + V.Kp[p]=V.K; FS_QR(V.Kp[p], V.fxr[p], &f0e[0], freninc-frst, V.lp[p]-V.lp[p-1], V.lp[p-1]-frst, V.fres[p]); + delete[] f0e; + + memset(V.LogASp[p], 0, sizeof(double)*V.M); + for (int m=0; m<V.M; m++) + { + int ccount=0; + for (int fr=frst; fr<=freninc; fr++) + { + double f=Partials[m][fr].f*V.afres; + if (f<=0) continue; + int intf=floor(f); + V.LogASp[p][m]=V.LogASp[p][m]+log(Partials[m][fr].a/V.A0C[fr])-(V.LogAF[intf]*(intf+1-f)+V.LogAF[intf+1]*(f-intf)); + ccount++; + } + if (ccount>0) V.LogASp[p][m]/=ccount; + else V.LogASp[p][m]=0; + } + } + memset(V.FXR, 0, sizeof(double)*2*V.K); memset(V.FRes, 0, sizeof(double)*V.K); + double sumdp=0; for (int p=1; p<V.P; p++) sumdp+=V.Dp[p], V.FXR[0]+=V.fxr[p][0]*V.Dp[p], V.FRes[0]+=V.fres[p][0]*V.Dp[p]; V.FXR[0]/=sumdp, V.FRes[0]/=sumdp; + for (int k=1; k<V.K; k++) + { + double sumdp=0; + for (int p=1; p<V.P; p++) + { + if (k<V.Kp[p]) + { + V.FXR[2*k-1]+=V.fxr[p][2*k-1]*V.Dp[p]; + V.FXR[2*k]+=V.fxr[p][2*k]*V.Dp[p]; + V.FRes[k]+=V.fres[p][k]*V.Dp[p]; + sumdp+=V.Dp[p]; + } + } + V.FXR[2*k-1]/=sumdp, V.FXR[2*k]/=sumdp, V.FRes[k]/=sumdp; + } +}//AnalyzeV + +/* + function copeak: measures the similarity of F0 between frst and fren to a cosine cycle 0~2pi. + + In: F0[peakfr[frst]:peakfr[fren]] + periodinframe: period of the cosine reference, in frames + + Returns the correlation coefficient. +*/ +double copeak(double* F0, int* peakfr, int frst, int fren, double periodinframe) +{ + double ef0=0, rfs=0, rff=0, rss=0; int frcount=0; + for (int fr=peakfr[frst]; fr<=peakfr[fren]; fr++) ef0+=F0[fr], frcount++; ef0/=frcount; + for (int fr=peakfr[frst]; fr<=peakfr[fren]; fr++) + { + double s=cos(2*M_PI*(fr-peakfr[frst])/periodinframe), f=F0[fr]; + rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s; + } + return rfs/sqrt(rff*rss); +}//copeak + +/* + function CorrectFS: time-shifts Fourier series so that maximum is reached at 0. This is done by + finding the actual peak numerically then shifting it to 0. + + In: c[2K-1]: Fourier series coefficients, in the order of 0r, 1i, 1r, 2i, 2r, ..., so that the series + is expanded as c[0]+c[1]sinx+c[2]cosx+c[3]sin2x+c[4]cos2x+...c[2K-2]cos(K-1)x + epx: a small positive value for convergence test for finding the peak + maxiter: maximal number of iterates for finding the peak + Out: c[2K-1]: updated Fourier series coefficients. + + No return value. +*/ +void CorrectFS(int K, double* c, double epx=1e-8, int maxiter=10) +{ + double y=0, kb=0, kka=0, th2pi=0, dth2pi=0, my, mth2pi; + epx=2*M_PI*epx; + int iter=0; + for (int k=1; k<K; k++) y+=c[2*k], kb+=k*c[2*k-1], kka+=k*k*c[2*k]; my=y; + dth2pi=(kka>0)?(kb/kka):(kb*0.1); + while (iter<maxiter && fabs(dth2pi)>epx) + { + iter++; + th2pi+=dth2pi; + kb=kka=y=0; + for (int k=1; k<K; k++) + { + double cc=cos(k*th2pi), ss=sin(k*th2pi); + y+=(c[2*k]*cc+c[2*k-1]*ss); + kb+=k*(-c[2*k]*ss+c[2*k-1]*cc); + kka+=k*k*(c[2*k]*cc+c[2*k-1]*ss); + } + if (y>my) my=y, mth2pi=th2pi; + dth2pi=(kka>0)?(kb/kka):(kb*0.1); + } + if (iter>=maxiter || th2pi!=mth2pi) + { + th2pi=mth2pi; + } + for (int k=1; k<K; k++) + { + double cc=cos(k*th2pi), ss=sin(k*th2pi); + double tmp=c[2*k]*cc+c[2*k-1]*ss; + c[2*k-1]=-c[2*k]*ss+c[2*k-1]*cc; c[2*k]=tmp; + } +}//CorrectFS + +/* + function DeAM: amplitude demodulation based on given segmentation into cycles + + In: A1[Fr]: original amplitude + peakfr[npfr]: cycle boundaries (literally, "frame indices of frequency modulation peaks") + Out: A2[Fr]: demodulated amplitude + + No return value. +*/ +void DeAM(double* A2, double* A1, int npfr, int* peakfr, int Fr) +{ + if (npfr==1) {memcpy(A2, A1, sizeof(double)*Fr); return;} + double *frs=new double[npfr*2], *a=&frs[npfr]; + for (int i=0; i<npfr-1; i++) + { + int frst=peakfr[i], fren=peakfr[i+1]; + a[i]=(A1[frst]+A1[fren])*0.5; + frs[i]=(frst*A1[frst]+fren*A1[fren])*0.5; + for (int fr=frst+1; fr<fren; fr++) a[i]+=A1[fr], frs[i]+=fr*A1[fr]; + frs[i]/=a[i]; a[i]=log(sqrt(a[i]/(fren-frst))); + } + if (npfr==2) + { + A2[0]=exp(a[0]); + for (int fr=1; fr<Fr; fr++) A2[fr]=A2[0]; + } + else if (npfr==3) + { + double alp=(a[1]-a[0])/(frs[1]-frs[0]); + for (int fr=0; fr<Fr; fr++) A2[fr]=exp(a[0]+(fr-frs[0])*alp); + } + else if (npfr==4) + { + double x0=frs[0], x1=frs[1], x2=frs[2], y0=log(a[0]), y1=log(a[1]), y2=log(a[2]); + double ix0_12=y0/((x0-x1)*(x0-x2)), ix1_02=y1/((x1-x0)*(x1-x2)), ix2_01=y2/((x2-x0)*(x2-x1)); + double aa=ix0_12+ix1_02+ix2_01, + b=-(x1+x2)*ix0_12-(x0+x2)*ix1_02-(x0+x1)*ix2_01, + c=x1*x2*ix0_12+x0*x2*ix1_02+x0*x1*ix2_01; + for (int fr=0; fr<Fr; fr++) A2[fr]=exp(c+fr*(b+fr*aa)); + } + else + { + double *fa=new double[npfr*4], *fb=&fa[npfr], *fc=&fa[npfr*2], *fd=&fa[npfr*3]; + CubicSpline(npfr-2, fa, fb, fc, fd, frs, a, 0, 1, A2, 1, 0, Fr-1); + for (int fr=0; fr<Fr; fr++) A2[fr]=exp(A2[fr]); + delete[] fa; + } + delete[] frs; +}//DeAM + +/* + function DeAM: wrapper function using floating-point cycle boundaries + + In: A1[Fr]: original amplitude + lp[npfr]: cycle boundaries + Out: A2[Fr]: demodulated amplitude + + No return value. +*/ +void DeAM(double* A2, double* A1, int npfr, double* lp, int Fr) +{ + int* peakfr=new int[npfr]; + for (int i=0; i<npfr; i++) peakfr[i]=ceil(lp[i]); + DeAM(A2, A1, npfr, peakfr, Fr); + delete[] peakfr; +}//DeAM + +/* + function DeFM: frequency demodulation based on given segmentation into cycles + + In: f1[Fr], AA[Fr]: original frequency and partial-independent amplitude + peakfr[npfr]: cycle boundaries (literally, "frame indices of frequency modulation peaks") + furthursmoothing: specifies if a second round demodulation is to be performed for more smooth + carrier + Out: f2[Fr]: demodulated frequency + f[fcount], frs[fcount]: frequency and time (in frames) of carrier knots, optional + fct: frequency centroid + + No return value. +*/ +void DeFM(double* f2, double* f1, double* AA, int npfr, int* peakfr, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing) +{ + double ac=0; + fct=0; + + if (npfr==1) + { + memcpy(f2, f1, sizeof(double)*Fr); + for (int fr=0; fr<Fr; fr++) ac+=AA[fr], fct+=f1[fr]*AA[fr]; fct/=ac; + return; + } + + bool localfrs=(frs==(double*)0xFFFFFFFF), localf=(f==(double*)0xFFFFFFFF); + if (!localfrs) delete[] frs; + if (!localf) delete[] f; + frs=new double[npfr]; f=new double[npfr]; + for (int i=0; i<npfr-1; i++) + { + int frst=peakfr[i], fren=peakfr[i+1]; + f[i]=(f1[frst]*AA[frst]+f1[fren]*AA[fren])*0.5; + frs[i]=(frst*AA[frst]+fren*AA[fren])*0.5; + double lrec=(AA[frst]+AA[fren])*0.5; + for (int fr=frst+1; fr<fren; fr++) f[i]+=f1[fr]*AA[fr], frs[i]+=fr*AA[fr], lrec+=AA[fr]; + ac+=lrec; + fct+=f[i]; + f[i]/=lrec, frs[i]/=lrec; + } + fct/=ac; + + if (furthersmoothing) + { + //further smoothing + int newnpfr=1; + for (int pfr=1; pfr<npfr-2; pfr++) + { + if ((f[pfr]>f[pfr-1] && f[pfr]>f[pfr+1]) || (f[pfr]<f[pfr-1] && f[pfr]<f[pfr+1])) + { + //this is a peak or valley + if ((pfr+1<npfr-2 && f[pfr+1]>f[pfr] && f[pfr+1]>f[pfr+2]) || (f[pfr+1]<f[pfr] && f[pfr+1]<f[pfr+2])) + { + frs[newnpfr]=0.5*(frs[pfr]+frs[pfr+1]), f[newnpfr]=0.5*(f[pfr]+f[pfr+1]); + newnpfr++; + } + } + else + { + frs[newnpfr]=frs[pfr], f[newnpfr]=f[pfr]; + newnpfr++; + } + } + frs[newnpfr]=frs[npfr-2], f[newnpfr]=f[npfr-2]; newnpfr++; newnpfr++; + npfr=newnpfr; + } + + Smooth_Interpolate(f2, Fr, npfr, f, frs); + fcount=npfr-1; + + if (localfrs) delete[] frs; + if (localf) delete[] f; +}//DeFM + +/* + function DeFM: wrapper function using floating-point cycle boundaries + + In: f1[Fr], AA[Fr]: original frequency and partial-independent amplitude + lp[npfr]: cycle boundaries + furthursmoothing: specifies if a second round demodulation is to be performed for more smooth + carrier + Out: f2[Fr]: demodulated frequency + f[fcount], frs[fcount]: frequency and time (in frames) of carrier knots, optional + fct: frequency centroid + + No return value. +*/ +void DeFM(double* f2, double* f1, double* AA, int npfr, double* lp, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing) +{ + int* peakfr=new int[npfr]; + for (int i=0; i<npfr; i++) peakfr[i]=ceil(lp[i]); + DeFM(f2, f1, AA, npfr, peakfr, Fr, fct, fcount, frs, f, furthersmoothing); + delete[] peakfr; +}//DeFM + +/* + function FindPeaks: find modulation peaks of signal F0[] roughly periodical at $periodinframe + + In: F0[Fr]: modulated signal + periodinframe: reference modulation period + Out: npfr, peakfr[npfr]: modulation peaks + peaky[npfr]: shape score for the peaks measuring their similarity to cosine peak, optional + + No return value. +*/ +void FindPeaks(int* peakfr, int& npfr, double* F0, int Fr, double periodinframe, double*& peaky) +{ + npfr=0; + for (int fr=1; fr<Fr-1; fr++) + { + if (F0[fr]>F0[fr-1] && F0[fr]>F0[fr+1]) + { + peakfr[npfr++]=fr; + } + } + + bool localpeaky=(peaky==(double*)0xFFFFFFFF); if (!localpeaky) delete[] peaky; + + peaky=new double[npfr]; + for (int pfr=0; pfr<npfr; pfr++) + { + double rfs=0, rff=0, rss=0; + if (peakfr[pfr]>periodinframe/2) + { + double ef0=0; int frcount=0; for (int fr=peakfr[pfr]-periodinframe/2; fr<peakfr[pfr]; fr++) ef0+=F0[fr], frcount++; ef0/=frcount; + for (int fr=peakfr[pfr]-periodinframe/2; fr<peakfr[pfr]; fr++) + { + double s=cos(2*M_PI*(fr-peakfr[pfr])/periodinframe), f=F0[fr]; + rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s; + } + } + if (peakfr[pfr]<Fr-periodinframe/2) + { + double ef0=0; int frcount=0; for (int fr=peakfr[pfr]; fr<peakfr[pfr]+periodinframe/2; fr++) ef0+=F0[fr], frcount++; ef0/=frcount; + for (int fr=peakfr[pfr]; fr<peakfr[pfr]+periodinframe/2; fr++) + { + double s=cos(2*M_PI*(fr-peakfr[pfr])/periodinframe), f=F0[fr]; + rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s; + } + } + peaky[pfr]=rfs/sqrt(rff*rss); + } + int pst=0; for (int pfr=1; pfr<npfr; pfr++) if (peaky[pst]<peaky[pfr]) pst=pfr; +// return; + //* + int pen=pst+1, pfr=pst+1; + while (pfr<npfr) + { + int impfr=-1; + while (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*0.5) pfr++; + if (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*1.5) + { + double mpfr=peaky[pfr]+copeak(F0, peakfr, pen-1, pfr, periodinframe); + impfr=pfr; pfr++; + while (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*1.5) + { + double mp=peaky[pfr]+copeak(F0, peakfr, pen-1, pfr, periodinframe); + if (mp>mpfr) impfr=pfr, mpfr=mp; + pfr++; + } + } + else + { + while (pfr<npfr) + { + if (peaky[pfr]>0) {impfr=pfr; break;} + pfr++; + } + } + if (impfr>=0) peakfr[pen++]=peakfr[impfr]; + } + pst--; pfr=pst; + while (pfr>=0) + { + int impfr=-1; + while (pfr>=0 && peakfr[pfr]>peakfr[pst+1]-periodinframe*0.5) pfr--; + if (pfr>=0 && peakfr[pfr]>=peakfr[pst+1]-periodinframe*1.5) + { + double mpfr=peaky[pfr]+copeak(F0, peakfr, pfr, pst+1, periodinframe); + impfr=pfr; pfr--; + while (pfr>=0 && peakfr[pfr]>=peakfr[pst+1]-periodinframe*1.5) + { + double mp=peaky[pfr]+copeak(F0, peakfr, pfr, pst+1, periodinframe); + if (mp>mpfr) impfr=pfr, mpfr=mp; + pfr--; + } + } + else + { + while (pfr>=0) + { + if (peaky[pfr]>0) {impfr=pfr; break;} + pfr--; + } + } + if (impfr>=0) peakfr[pst--]=peakfr[impfr]; + } + pst++; + npfr=pen-pst; + memmove(peakfr, &peakfr[pst], sizeof(int)*npfr); //*/ + if (localpeaky) delete[] peaky; +}//FindPeaks + +/* + function FS: Fourier series decomposition + + In: data[frst:fren]: signal to decompose + period: period of Fourier series decomposition + shift: amount of original shift of Fourier components (from frst to frst-shift) + K: number of Fourier components requested + Out: K: number of Fourier components returned + FXR[K], FXI[K]: real and imaginary parts of Fourier series coefficients + FRES[K]: decomposition residues + + No return value. +*/ +void FS(int& K, double* FXR, double* FXI, double* FRES, double* data, int frst, int fren, double period, double shift) +{ + if (K>period/2) K=period/2; + int len=fren-frst+1; + double omg0=2*M_PI/period, ilen=1.0/len; + + //for (int fr=frst; fr<=fren; fr++) data[fr]=cos(2*M_PI*fr/period+1); + + double* res=new double[len]; memcpy(res, &data[frst], sizeof(double)*len); + double* rec=new double[len]; + double resene=0; for (int i=0; i<len; i++) resene+=res[i]*res[i]; double ene=resene; + + for (int k=0; k<K; k++) + { +// double eer=0, eei=0; + double xr=0, xi=0, omg=k*omg0; + for (int fr=frst; fr<=fren; fr++) + { + double ph=omg*(fr-shift); + double c=cos(ph), s=sin(ph); + xr+=data[fr]*c, xi+=data[fr]*s; +// eer+=c*c, eei+=s*s; + } + xr*=ilen, xi*=ilen; +// if (eer>0) xr/=eer; +// if (eei>0) xi/=eei; + FXR[k]=xr, FXI[k]=xi; + double lresene=0; + for (int fr=frst; fr<=fren; fr++) + { + double ph=omg*(fr-shift); +// rec[fr-frst]=xr*cos(ph)+xi*sin(ph); + if (k==0) rec[fr-frst]=xr*cos(ph)+xi*sin(ph); + else rec[fr-frst]=2*(xr*cos(ph)+xi*sin(ph)); + res[fr-frst]-=rec[fr-frst]; + lresene+=res[fr-frst]*res[fr-frst]; + } + resene=lresene; + FRES[k]=resene/ene; + } + delete[] rec; + delete[] res; +}//FS + +/* + function FS_QR: Fourier series decomposition with QR orthogonalization. Since the Fourier series is + applied on finite-length discrate signal, the Fourier components are no longer orthogonal to each + other. A decreasing residue can be guaranteed by applying QR (or any other) orthogonalization process + to the Fourier components before decomposition. + + In: data[0:Fr]: signal to decompose + period: period of Fourier series decomposition + shift: amount of original shift of Fourier components (from 0 to -shift) + K: number of Fourier components requested + Out: K: number of Fourier components returned + FXR[2K-1]: Fourier series coefficients, in the order of 0r, 1i, 1r, 2i, 2r, .... + FRES[K]: decomposition residues, optional + + No return value. +*/ +void FS_QR(int& K, double* FXR, double* data, int Fr, double period, double shift, double* FRES) +{ + int len=Fr+1; + if (K>period/2) K=period/2; + if (K>Fr/2) K=Fr/2; + if (K>len/2) K=len/2; + if (K<=0) return; + memset(FXR, 0, sizeof(double)*(2*K-1)); + + double omg0=2*M_PI/period, ene, resene; + + if (FRES) + { + ene=0; for (int i=0; i<len; i++) ene+=data[i]*data[i]; resene=ene; + } + + /*debug only + double *res=new double[len*4], *rec=&res[len]; + memcpy(res, data, sizeof(double)*len); memset(rec, 0, sizeof(double)*len); + double resene2=resene;//*/ + + int NK=K*2-1; + + Alloc2(len, NK, A); Alloc2(len, len, Q); Alloc2(len, NK, R); + for (int fr=0; fr<=Fr; fr++) A[fr][0]=1; + for (int k=1; k<(NK+1)/2; k++) + { + double omg=k*omg0; + for (int fr=0; fr<=Fr; fr++){double ph=omg*(fr-shift); A[fr][2*k-1]=2*sin(ph), A[fr][2*k]=2*cos(ph);} + } + QR_householder(len, NK, A, Q, R);/////////////////// + GIUT(NK, R); + for (int k=0; k<NK; k++) + { + double xr=0; + for (int fr=0; fr<=Fr; fr++) + { + double c=Q[fr][k]; + xr+=data[fr]*c; + } + for (int kk=0; kk<=k; kk++) FXR[kk]+=xr*R[kk][k]; + /*debug only + double lresene=0; + for (int fr=0; fr<=Fr; fr++) + { + rec[fr]=0; for (int kk=0; kk<=k; kk++) rec[fr]+=FXR[kk]*A[fr][kk]; + res[fr]=data[fr]-rec[fr]; + lresene+=res[fr]*res[fr]; + } + resene2=lresene; */ + + if (FRES) + { + resene-=xr*xr; + if (k%2==0) FRES[k/2]=resene/ene; + } + } + /* debug only + delete[] res; //*/ + DeAlloc2(A); DeAlloc2(Q); DeAlloc2(R); +}//FS_QR + +/* + function InterpolateV: adjusts modulation rate to $rate times the current value. Since TVo is based on + individual cycles, this operation involves finding new cycle boundaries and recomputing other single- + cycle descriptors by interpolation. This is used for time stretching and cycle rate adjustment. + + In: V: a TVo object to adjust + newP: number of total cycles after adjustment. + rate: amount of adjustment. + Out: another TVo object with new cycle boundaries and single-cycle descriptors interpoated from V + + Returns pointer to the output TVo. This function does not affect the content in V. +*/ +TVo* InterpolateV(double newP, double rate, TVo& V) +{ + double Pst=-V.lp[0]/(V.lp[1]-V.lp[0]); + double oldP=V.P-1+(V.L-V.lp[V.P-1])/(V.lp[V.P-1]-V.lp[V.P-2])-Pst; + + TVo* newV=new TVo; + memcpy(newV, &V, sizeof(TVo)); + + newV->F0C=0; newV->A0C=0; newV->peakfr=0; newV->Dp=0; newV->lp=0, newV->Kp=0; + newV->fxr=0; newV->LogASp=0; newV->F0=0; newV->F0D=0; newV->fres=0; + + int inewP=ceil(newP+Pst); + newV->P=inewP; + + newV->peakfr=new int[inewP]; + newV->AllocateP(); newV->AllocatePK(); + + newV->lp[0]=V.lp[0]/rate; + newV->peakfr[0]=floor(newV->lp[0]+0.5); + for (int p=1; p<inewP; p++) + { + double rp=1+(p-1)*(oldP-2+Pst)/(newP-2+Pst); + int ip=floor(rp); rp=rp-ip; + if (ip+1>=V.P) ip=V.P-1, rp=0; + if (fabs(rp)<1e-16) + { + newV->lp[p]=newV->lp[p-1]+(V.lp[ip]-V.lp[ip-1])/rate; + newV->Dp[p]=V.Dp[ip]; + newV->Kp[p]=V.Kp[ip]; + memcpy(newV->fxr[p], V.fxr[ip], sizeof(double)*2*newV->Kp[p]); + memcpy(newV->LogASp[p], V.LogASp[ip], sizeof(double)*V.M); + } + else + { + double fv1=1.0/(V.lp[ip]-V.lp[ip-1]), fv2=1.0/(V.lp[ip+1]-V.lp[ip]); + double fv=rate*(fv1*(1-rp)+fv2*rp); + newV->lp[p]=newV->lp[p-1]+1.0/fv; + newV->Dp[p]=V.Dp[ip]*(1-rp)+V.Dp[ip+1]*rp; + int minK=V.Kp[ip]; if (minK>V.Kp[ip+1]) minK=V.Kp[ip+1]; + for (int k=0; k<2*minK-1; k++) newV->fxr[p][k]=V.fxr[ip][k]*(1-rp)+V.fxr[ip+1][k]*rp; + for (int m=0; m<V.M; m++) newV->LogASp[p][m]=V.LogASp[ip][m]*(1-rp)+V.LogASp[ip+1][m]*rp; + newV->Kp[p]=minK; + } + newV->peakfr[p]=floor(newV->lp[p]+0.5); + memset(newV->fres[0], 0, sizeof(double)*newV->P*V.K); + } + int newL=newV->lp[inewP-1]+(newP+Pst-(inewP-1))*(V.lp[V.P-1]-V.lp[V.P-2])/rate+0.5; + newV->AllocateL(newL); + + //F0C and A0C + if (V.L!=newL) + { + for (int l=0; l<newL; l++) + { + double rl=1.0*l*(V.L-1)/(newL-1); + int il=floor(rl); rl-=il; + newV->F0C[l]=V.F0C[il]*(1-rl)+V.F0C[il+1]*rl; + newV->A0C[l]=V.A0C[il]*(1-rl)+V.A0C[il+1]*rl; + } + } + else + { + memcpy(newV->F0C, V.F0C, sizeof(double)*V.L); + memcpy(newV->A0C, V.A0C, sizeof(double)*V.L); +// memcpy(newV->F0, V.F0, sizeof(double)*V.L); +// memcpy(newV->F0D, V.F0D, sizeof(double)*V.L); + } + return newV; +}//InterpoalteV + +//vibrato rate boundaries, in Hz +#define MAXMOD 15.0 +#define MINMOD 3.0 + +/* + function RateAndReg: evaluates modulation rate and regularity + + In: data[frst:fren]: modulated signal, time measured in frames + padrate: padding rate used for inverse FFT + sps: sampling frequency + offst: hop size + Out: rate, regularity: rate and regularity + + No return value. +*/ +void RateAndReg(double& rate, double& regularity, double* data, int frst, int fren, int padrate, double sps, double offst) +{ + rate=0; regularity=0; + //find the section used for calculating regularity and rate + int frlen=fren-frst+1, frlenp=1<<(int(log2(fren))+2), frlenpp=frlenp*padrate; + double *lf=new double[frlenpp*4]; + cdouble *w=(cdouble*)&lf[frlenpp]; + cdouble *x=(cdouble*)&lf[frlenpp*2]; + SetTwiddleFactors(frlenp, w); + + memset(lf, 0, sizeof(double)*frlenp); + memcpy(lf, &data[frst], sizeof(double)*frlen); +// for (int i=0; i<frlen; i++) lf[i]=data[frst+i]-edata; + + double* rdata=new double[50]; + for (int i=0; i<50; i++) + { + rdata[i]=0; for (int j=0; j<fren-i; j++) rdata[i]+=data[j]*data[j+i]; + } + + + RFFTC(lf, NULL, NULL, log2(frlenp), w, x, 0); +// xr[0]=0; + for (int i=0; i<frlenp/2; i++) {x[i].x=x[i].x*x[i].x+x[i].y*x[i].y; x[i].y=0;} + memset(&x[frlenp/2], 0, sizeof(cdouble)*(frlenpp-frlenp/2)); + + SetTwiddleFactors(frlenpp, w); + CIFFTR(x, log2(frlenpp), w, lf); + + delete[] rdata; + + double minpeak=sps/(offst*MAXMOD), maxpeak=sps/(offst*MINMOD); + + //find 2nd highest peak at interval padrate + int imaxlf=padrate*minpeak; + while (imaxlf<frlenpp && imaxlf<padrate*maxpeak && lf[imaxlf]<=lf[imaxlf-padrate]) imaxlf+=padrate; + double maxlf=lf[imaxlf]; + for (int i=imaxlf/padrate; i<frlen/2; i++) if (maxlf<lf[i*padrate]) maxlf=lf[i*padrate], imaxlf=i*padrate; + //locate the peak at interval 1 + int ii=imaxlf; for (int i=ii-padrate+1; i<ii+padrate; i++) if (maxlf<lf[i]) maxlf=lf[i], imaxlf=i; + + double a_=0.5*(lf[imaxlf-1]+lf[imaxlf+1])-maxlf, b_=0.5*(lf[imaxlf+1]-lf[imaxlf-1]); + double period=imaxlf-0.5*b_/a_; + period=period/padrate; //period in frames + rate=sps/(period*offst); //rate in hz + regularity=(maxlf-0.25*b_*b_/a_)/lf[0]*(fren-frst)/(fren-frst-period); + + delete[] lf; +}//RateAndReg + +/* + function RegularizeV: synthesize a regular vibrato from the basic descriptors. + + In: V: a TVo object hosting vibrato descriptors + sps: sampling rate + h: hop size + Out: HS: a harmonic sinusoid + + No return value. +*/ +void RegularizeV(THS& HS, TVo& V, double sps, double h) +{ + int K=V.Kp[1]; for (int p=2; p<V.P; p++) if (K>V.Kp[p]) K=V.Kp[p]; + double D=0, *fxr=new double[2*K-1]; + memset(fxr, 0, sizeof(double)*(2*K-1)); + for (int p=1; p<V.P; p++) + { + D+=V.Dp[p]; for (int k=0; k<2*K-1; k++) fxr[k]+=V.fxr[p][k]*V.Dp[p]; + } + //uncomment the following line if re-normalization needed + //double alfa=fxr[0]*fxr[0]; for (int k=1; k<2*K-1; k++) alfa+=2*fxr[k]*fxr[k]; alfa=sqrt(alfa); D=alfa; + for (int k=0; k<2*K-1; k++) fxr[k]/=D; + D/=(V.P-1); + V.D=D; + + atom** Partials=HS.Partials; + for (int l=0; l<V.L; l++) + { + double theta=V.rate*l*V.h/sps;//-0.25; + //f0(theta) + double f0=fxr[0]; for (int k=1; k<K; k++) f0=f0+2*(fxr[2*k-1]*sin(2*M_PI*k*theta)+fxr[2*k]*cos(2*M_PI*k*theta)); + V.F0D[l]=D*f0; V.F0[l]=V.F0C[l]+V.F0D[l]; + for (int m=0; m<V.M; m++) + { + if (Partials[m][l].t<0) Partials[m][l].t=Partials[0][l].t; + Partials[m][l].f=(m+1)*V.F0[l]; + if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].f=-1, Partials[m][l].a=0; + else Partials[m][l].a=V.A0C[l]*exp(V.LogAS[m]+V.LogAF[int(Partials[m][l].f*V.afres)]); + } + } + + delete[] fxr; +}//RegularizeV + +/* + function QIE: computes extremum using quadratic interpolation + + In: y[-1:1]: three points to interpolate from. It is assumed y[0] is larger (or smaller) than both + y[-1] and y[1]. + + Returns the extremum of y. +*/ +double QIE(double* y) +{ + double a=0.5*(y[1]+y[-1])-y[0], b=0.5*(y[1]-y[-1]); + return y[0]-0.25*b*b/a; +}//QIE + +/* + function QIE: computes extremum using quadratic interpolation + + In: y[-1:1]: three points to interpolate from. It is assumed y[0] is larger (or smaller) than both + y[-1] and y[1]. + Out: x: the argument value that yields extremum of y(x) + + Returns the extremum of y. +*/ +double QIE(double* y, double& x) +{ + double a=0.5*(y[1]+y[-1])-y[0], b=0.5*(y[1]-y[-1]); + x=-0.5*b/a; + return y[0]-0.25*b*b/a; +}//QIE + +/* + function S_F: original source-filter estimation used in AES124. + + In: afres: filter response resolution + Partials[M][Fr]: HS partials + A0C[Fr]: partial-independent amplitude carrier + lp[P]: cycle boundaries + F0Overall: average fundamental frequency + Out: LogAF[afres/2]: filter response + LogAS[M]: source amplitudes + + Returns 0. +*/ +double S_F(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F0Overall) +{ + //SOURCE-FILTER estimation routines + //derivative of log A(f), f is sampled at 1/8192 (5hz), averaged over (-15hz, 15hz) + double *DAF=new double[afres], *NAF=&DAF[afres/2]; + memset(DAF, 0, sizeof(double)*afres); + double avgwidth1, avgwidth2; + for (int fr=lp[1]; fr<lp[P-1]; fr++) for (int m=0; m<M; m++) + { + if (Partials[m][fr].f>0 && Partials[m][fr-1].f>0 && Partials[m][fr+1].f>0) + { + double f_1=Partials[m][fr-1].f, f0=Partials[m][fr].f, f1=Partials[m][fr+1].f, a_1=Partials[m][fr-1].a/A0C[fr-1], a1=Partials[m][fr+1].a/A0C[fr+1]; + if ((f0-f_1)*(f0-f1)<0) + { + double dlogaf_df=(log(a1)-log(a_1))/(f1-f_1); //calculate derivative of log + + //this derivative contributes within a frequency interval of |f1-f_1| + if (f1<f_1) {double tmp=f1; f1=f_1; f_1=tmp;} + int startbin=ceil(f_1*afres), endbin=floor(f1*afres); + + if (startbin<0) startbin=0; + avgwidth1=(f0-f_1)*afres, avgwidth2=(f1-f0)*afres; + for (int i=startbin; i<=endbin; i++) + { + double fi=i-f0*afres, w; + if (fi<0) w=1-fabs(fi)/avgwidth1; else w=1-fabs(fi)/avgwidth2; + DAF[i]+=dlogaf_df*w, NAF[i]+=w; + } + } + } + } + for (int i=0; i<afres/2; i++) if (NAF[i]>0) DAF[i]=DAF[i]/NAF[i]; //else NAF[i]=0; + int i=0, nbands=0, bandsstart[100], bandsendinc[100]; double mininaf=3; + while (i<afres/2) + { + while (i<afres/2 && NAF[i]<mininaf) i++; if (i>=afres/2) break; + bandsstart[nbands]=i; + while (i<afres/2 && NAF[i]>0) i++; if (i>=afres/2) {bandsendinc[nbands++]=i-1; break;} + if (NAF[i]<=0) while (NAF[i]<mininaf) i--; + bandsendinc[nbands++]=i++; + } + + //integrate DAF over the fundamental band, with AF(F0Overall)=1 + i=F0Overall*afres; + double theta=F0Overall*afres-i; + LogAF[i]=-0.5*theta*(DAF[i]+DAF[i]*(1-theta)+DAF[i+1]*theta)/afres; i--; + while (i>=bandsstart[0]){LogAF[i]=LogAF[i+1]-0.5*(DAF[i]+DAF[i+1])/afres; i--;} + i=F0Overall*afres+1; + LogAF[i]=0.5*(1-theta)*(DAF[i]+DAF[i-1]*(1-theta)+DAF[i]*theta)/afres; i++; + while (i<=bandsendinc[0]){LogAF[i]=LogAF[i-1]+0.5*(DAF[i]+DAF[i-1])/afres; i++;} + + int band=1; + while (band<nbands) + { + //integrate DAF over band-th band, first ignore the blank band + LogAF[bandsstart[band]]=LogAF[bandsendinc[band-1]]; + for (int i=bandsstart[band]+1; i<=bandsendinc[band]; i++) LogAF[i]=LogAF[i-1]+0.5*(DAF[i]+DAF[i-1])/afres; + //then look for the shift required between bands + double cshift=0; int ccount=0; + for (int fr=lp[1]; fr<lp[P-1]; fr++) + { + double f=Partials[band-1][fr].f*afres; + if (bandsendinc[band-1]>bandsstart[band-1] && (f<bandsstart[band-1] || f>bandsendinc[band-1])) continue; + int intf=floor(f); + double logaflast=LogAF[intf]+(DAF[intf]+0.5*(DAF[intf+1]-DAF[intf])*(f-intf))*(f-intf)/afres; + f=Partials[band][fr].f*afres; + if (bandsendinc[band]>bandsstart[band] && (f<bandsstart[band] || f>bandsendinc[band])) continue; + intf=floor(f); + double logafthis=LogAF[intf]+(DAF[intf]+0.5*(DAF[intf+1]-DAF[intf])*(f-intf))*(f-intf)/afres; + cshift=cshift+log(Partials[band][fr].a*(band+1)/(Partials[band-1][fr].a*band))+logaflast-logafthis; + ccount++; + } + cshift/=ccount; + //apply the shift + for (int i=bandsstart[band]; i<=bandsendinc[band]; i++) LogAF[i]+=cshift; + //make up the blank band + int lastend=bandsendinc[band-1], thisstart=bandsstart[band]; + for (int i=lastend+1; i<thisstart; i++) LogAF[i]=LogAF[lastend]+cshift*(i-lastend)/(thisstart-lastend); + band++; + } + delete[] DAF; + + int tmpband=bandsstart[0]; for (int i=0; i<tmpband; i++) LogAF[i]=LogAF[tmpband]; + tmpband=bandsendinc[nbands-1]; for (int i=tmpband; i<afres/2; i++) LogAF[i]=LogAF[tmpband]; + + //Here LogAF contains the filter model, new get the source model + memset(LogAS, 0, sizeof(double)*100); + for (int m=0; m<M; m++) + { + int ccount=0; + for (int fr=lp[1]; fr<lp[P-1]; fr++) + { + double f=Partials[m][fr].f*afres; + if (f<=0) continue; + int intf=floor(f); + LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf)); + ccount++; + } + if (ccount>0) LogAS[m]/=ccount; + else LogAS[m]=0; + } + return 0; +}//S_F + +/* + function S_F_SV: slow-variation source-filter estimation used in AES126, adapted from TSF to TVo. + + In: afres: filter response resolution + Partials[M][Fr]: HS partials + A0C[Fr]: partial-independent amplitude carrier + lp[P]: cycle boundaries + F: filter response sampling interval + FScaleMode: set if filter sampling uses mel scale + theta: balancing factor of amplitude and frequency variations, needed for SV approach + Out: LogAF[afres/2]: filter response + LogAS[M]: source amplitudes + + Returns 0. +*/ +double S_F_SV(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F, int FScaleMode, double theta, double Fs) +{ + int lp0=lp[1], lpp=lp[P-1]; + int L=lpp-lp0; + Alloc2(L, M, loga); Alloc2(L, M, f); + double fmax=0; + for (int l=0; l<L; l++) + { + int fr=lp0+l; + for (int m=0; m<M; m++) + { + f[l][m]=Partials[m][fr].f; + if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700); + if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a); + else loga[l][m]=-100; + if (fmax<f[l][m]) fmax=f[l][m]; + } + } + int K=floor(fmax/F); + + Alloc2(L, K+2, h); memset(h[0], 0, sizeof(double)*L*(K+2)); + + SF_SV(h, L, M, K, loga, f, F, theta, 1e-6, L*(K+2)); + + for (int k=0; k<K+2; k++) + { + for (int l=1; l<L; l++) h[0][k]+=h[l][k]; + h[0][k]/=L; + } + + for (int i=0; i<afres/2; i++) + { + double freq=i*1.0/afres; + int k=floor(freq/F); + double f_plus=freq/F-k; + if (k<K+1) LogAF[i]=h[0][k]*(1-f_plus)+h[0][k+1]*f_plus; + else LogAF[i]=h[0][K+1]; + } + + memset(LogAS, 0, sizeof(double)*100); + for (int m=0; m<M; m++) + { + int ccount=0; + for (int fr=lp[1]; fr<lp[P-1]; fr++) + { + double f=Partials[m][fr].f*afres; + if (f<=0) continue; + int intf=floor(f); + LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf)); + ccount++; + } + if (ccount>0) LogAS[m]/=ccount; + else LogAS[m]=0; + } + + DeAlloc2(loga); DeAlloc2(f); DeAlloc2(h); + return 0; +}//S_F_SV + +/* + function SynthesizeV: synthesizes a harmonic sinusoid from vibrato descriptors + + In: V: a TVo object hosting vibrato descriptors + sps: sampling rate + UseK: maximal number of Fourier series components to use to construct frequency modulator + Out: HS: a harmonic sinusoid + + No return value. +*/ +void SynthesizeV(THS* HS, TVo* V, double sps, int UseK) +{ + int HSt0=HS->Partials[0][0].t, HSs0=HS->Partials[0][0].s, L=V->L, M=V->M, + P=V->P, *Kp=V->Kp, K=V->K, afres=V->afres; + double **fxr=V->fxr, *lp=V->lp, **LogASp=V->LogASp, *LogAF=V->LogAF, *Dp=V->Dp, + *F0C=V->F0C, *F0D=V->F0D, *F0=V->F0, *A0C=V->A0C, h=V->h; + HS->Resize(M, L); + double *gamp=new double[4*P], *gamm=&gamp[P]; + + Alloc2(P, K*2, newfxr); + int* newKp=new int[P]; + for (int p=1; p<P; p++) + { + memcpy(newfxr[p], fxr[p], sizeof(double)*2*K); + newKp[p]=Kp[p]; if (UseK>=2 && newKp[p]>UseK) newKp[p]=UseK; + CorrectFS(newKp[p], newfxr[p]); + } + + double f0, f1; + for (int p=1; p<P-1; p++) + { + if (p==1) + { + f0=newfxr[p][0]; for (int k=1; k<newKp[p]; k++) f0=f0+2*newfxr[p][2*k]; + } + else f0=f1; + f1=newfxr[p+1][0]; for (int k=1; k<newKp[p+1]; k++) f1=f1+2*newfxr[p+1][2*k]; + gamp[p]=(Dp[p+1]*f1-Dp[p]*f0)/2; gamm[p+1]=-gamp[p]; //additive correction + } + gamm[1]=-gamp[1], gamp[P-1]=-gamm[P-1]; //additive correction + atom** Partials=HS->Partials; + for (int p=0; p<P; p++) + { + double l0, lrange; + int lst, len, ilp; + if (p==0) lst=0, len=ceil(lp[0]), ilp=1; + else if (p==P-1) lst=ceil(lp[p-1]), len=L, ilp=p-1; + else lst=ceil(lp[p-1]), len=ceil(lp[p]), ilp=p; + l0=lp[ilp-1], lrange=lp[ilp]-l0; + for (int l=lst; l<len; l++) + { + Partials[0][l].s=HSs0; + Partials[0][l].t=HSt0+h*l; + double theta=(l-l0)/lrange; + double f0=newfxr[ilp][0]; for (int k=1; k<newKp[ilp]-1; k++) f0=f0+2*(newfxr[ilp][2*k-1]*sin(2*M_PI*k*theta)+newfxr[ilp][2*k]*cos(2*M_PI*k*theta)); + F0D[l]=f0*Dp[ilp]+gamm[ilp]*(1-theta)+gamp[ilp]*theta; //additive correction +// V.F0D[l]=f0*V.Dp[lp]; //no correction + F0[l]=F0C[l]+F0D[l]; + for (int m=0; m<M; m++) + { + Partials[m][l].s=HSs0; + Partials[m][l].t=Partials[0][l].t; + Partials[m][l].f=(m+1)*F0[l]; + if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].a=0; + else if (ilp==1) Partials[m][l].a=A0C[l]*exp(LogASp[1][m]*(1-0.5*theta)+LogASp[2][m]*0.5*theta+LogAF[int(Partials[m][l].f*afres)]); + else if (ilp==P-1) Partials[m][l].a=A0C[l]*exp(LogASp[ilp][m]*0.5*(1+theta)+LogASp[ilp-1][m]*0.5*(1-theta)+LogAF[int(Partials[m][l].f*afres)]); + else +// Partials[m][l].a=A0C[l]*exp(LogASp[ilp][m]*0.5+LogASp[ilp-1][m]*0.5*(1-theta)+LogASp[ilp+1][m]*0.5*theta+LogAF[int(Partials[m][l].f*afres)]); + Partials[m][l].a=A0C[l]*exp(LogASp[p][m]+LogAF[int(Partials[m][l].f*afres)]); + } + } + } + delete[] gamp; + DeAlloc2(newfxr); + delete[] newKp; +// VibratoDemoForm->Label13->Caption=L; +}//SynthesizeV + + + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/vibrato.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,99 @@ +#ifndef vibratoH +#define vibratoH + + +/* + vibrato.cpp - vibrato analysis and synthesis using harmonic sinusoids + + Further reading: Wen X. and M. Sandler, "Analysis and synthesis of audio vibrato using harmonic sinusoids," + in Proc. AES 124th Convention, Amsterdam, 2008. +*/ + +#include <stdio.h> +#include "hs.h" +//--------------------------------------------------------------------------- + +/* + TVo is the data structure hosting descriptors of a vibrato representation of a harmonic sinusoid. Its + basic framework is shared by the TSF object which hosts a more elaborate source-filter model than TVo, + but does not look into vibrato-specific features such as modulator shape. + + An analysis/sythesis cycle converts THS to TVo and back. +*/ +struct TVo +{ + //basic characteristics + int M; //number of partials + int L; //number of frames + int P; //number of F0 peaks + double h; //hop size + double* F0C; //[0:L-1] pitch carrier + double* A0C; //[0:L-1] amplitude carreir + int afres; //filter model bins + double LogAF[4096]; //[0:afres-1] filter model + + int* peakfr; //[0:P-1] peak positions, in frames + double* lp; //[0:P-1] peak positions, refined to floating-point values in frames + double* Dp; //[1:P-1] single-cycle rms of F0D + int* Kp; //[1:P-1] order of single-cycle modulator shape descriptor + double** fxr; //[1:P-1][0:2K] single-cycle modulator shape coefficients - cosine, sine, cosine, ... + double** LogASp; //[1:P-1][0:M-1] single-cycle source model + int K; + + //other properties + double rate; //vibrato rate + double regularity;//vibrato regularity + double F0max; //maximal fundamental frequency + double F0min; //minimal fundamental frequency + double F0Cmax; //maximal fundamental carrier frequency + double F0Cmin; //minimal fundamental carrier frequency + double F0Overall; //overall average fundamental frequency + double F0Dmax; //maximal fundamental modulator frequency + double F0Dmin; //minimal fundamental modulator frequency + double* F0; //[0:L-1] pitch + double* F0D; //[0:L-1] pitch modulator + double D; //rms of F0D, + double LogAS[100];//[0:M-1] source model + double FXR[100]; //average cycle modulator shape coefficients + double FRes[50]; //average modulator residues + double** fres; //[1:P-1][0:K-1] single-cycle modulator residues + + TVo(); + ~TVo(); + void AllocateL(int AnL); + void ReAllocateL(int newL); + void AllocateP(); + void AllocatePK(); + void SaveToFile(char* filename); + void SaveToFileHandle(FILE* file); + void LoadFromFileHandle(FILE* file); + double GetOldP(); +}; + +//--tool procedures---------------------------------------------------------- +double QIE(double* y); +double QIE(double* y, double& x); + +//--demodulation routines---------------------------------------------------- +void DeFM(double* f2, double* f1, double* AA, int npfr, int* peakfr, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing=false); +void DeFM(double* f2, double* f1, double* AA, int npfr, double* lp, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing=false); +void DeAM(double* A2, double* A1, int npfr, int* peakfr, int Fr); +void DeAM(double* A2, double* A1, int npfr, double* lp, int Fr); + +//--source-filter analysis routine------------------------------------------- +double S_F(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F0Overall); +double S_F_SV(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F=0.005, int FScaleMode=0, double theta=0.5, double sps=44100); + +//--vibrato analysis and synthesis routines---------------------------------- +void AnalyzeV(THS& HS, TVo& V, double*& peaky, double*& cyclefrs, double*& cyclefs, double sps, double h, int* cyclefrcount=0, + int SFMode=0, double SFF=0.01, int SFFScale=0, double SFtheta=0); +void RegularizeV(THS& HS, TVo& V, double sps, double h); +void SynthesizeV(THS* HS, TVo* V, double sps, int UseK=0); +TVo* InterpolateV(double newP, double rate, TVo& V); + +//--other functions---------------------------------------------------------- +void FindPeaks(int* peakfr, int& npfr, double* F0, int Fr, double periodinframe, double*& peaky); +void FS_QR(int& K, double* FXR, double* data, int Fr, double period, double shift, double* FRES); +void RateAndReg(double& rate, double& regularity, double* data, int frst, int fren, int padrate, double sps, double offst); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/wavelet.cpp Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,1018 @@ +//--------------------------------------------------------------------------- + +#include <math.h> +#include <mem.h> +#include "wavelet.h" +#include "matrix.h" + +//--------------------------------------------------------------------------- +/* + function csqrt: real implementation of complex square root z=sqrt(x) + + In: xr and xi: real and imaginary parts of x + Out: zr and zi: real and imaginary parts of z=sqrt(x) + + No return value. +*/ +void csqrt(double& zr, double& zi, double xr, double xi) +{ + if (xi==0) + if (xr>=0) zr=sqrt(xr), zi=0; + else zi=sqrt(-xr), zr=0; + else + { + double xm=sqrt(xr*xr+xi*xi); + double ri=sqrt((xm-xr)/2); + zr=xi/2/ri; + zi=ri; + } +}//csqrt + +/* + function Daubechies: calculates the Daubechies filter of a given order p + + In: filter order p + Out: h[2p]: the 2p FIR coefficients + + No reutrn value. The calculated filters are minimum phase, which means the energy concentrates at the + beginning. This is usually used for reconstruction. On the contrary, for wavelet analysis the filter + is mirrored. +*/ +void Daubechies(int p, double* h) +{ +//initialize h(z) + double r01=pow(2, -p-p+1.5); + + h[0]=1; + for (int i=1; i<=p; i++) + { + h[i]=h[i-1]*(p+1-i)/i; + } + + //construct polynomial p + double *P=new double[p], *rp=new double[p], *ip=new double[p]; + + P[p-1]=1; + double r02=1; + for (int i=p-1; i>0; i--) + { + double rate=(i+1-1.0)/(p-2.0+i+1); + P[i-1]=P[i]*rate; + r02/=rate; + } + Roots(p-1, P, rp, ip); + for (int i=0; i<p-1; i++) + { + //current length of h is p+1+i, h[0:p+i] + if (i<p-2 && rp[i]==rp[i+1] && ip[i]==-ip[i+1]) + { + double ar=rp[i], ai=ip[i]; + double bkr=-2*ar+1, bki=-2*ai, ckr=4*(ar*ar-ai*ai-ar), cki=4*(2*ar*ai-ai), dlr, dli; + csqrt(dlr, dli, ckr, cki); + double akr=bkr+dlr, aki=bki+dli; + if (akr*akr+aki*aki>1) akr=bkr-dlr, aki=bki-dli; + double ak1=-2*akr, ak2=akr*akr+aki*aki; + h[p+2+i]=ak2*h[p+i]; + h[p+1+i]=ak2*h[p-1+i]+ak1*h[p+i]; + for (int j=p+i; j>1; j--) h[j]=h[j]+ak1*h[j-1]+ak2*h[j-2]; + h[1]=h[1]+ak1*h[0]; + r02/=ak2; + i++; + } + else //real root of P + { + double ak, bk=-(2*rp[i]-1), delk=4*rp[i]*(rp[i]-1); + if (bk>0) ak=bk-sqrt(delk); + else ak=bk+sqrt(delk); + r02/=ak; + h[p+1+i]=-ak*h[p+i]; + for (int j=p+i; j>0; j--) h[j]=h[j]-ak*h[j-1]; + } + } + delete[] P; delete[] rp; delete[] ip; + r01=r01*sqrt(r02); + for (int i=0; i<p*2; i++) h[i]*=r01; +}//Daubechies + +/* + Periodic wavelet decomposition and reconstruction routines + + The wavelet transform of an N-point sequence is arranged in "interleaved" format + as another N-point sequance. Level 1 details are found at N/2 points 1, 3, 5, ..., + N-1; level 2 details are found at N/4 points 2, 6, ..., N-2; level 3 details are + found at N/8 points 4, 12, ..., N-4; etc. +*/ + +/* + function dwtpqmf: in this implementation h and g are the same as reconstruction qmf filters. In fact + the actual filters used are their mirrors and filter origin are aligned to the ends of the real + filters, which turn out to be the starts of h and g. + + The inverse transform is idwtp(), the same as inversing dwtp(). + + In: in[Count]: waveform data + h[M], g[M]: quadratic mirror filter pair + N: maximal time resolution + Count=kN, N=2^lN being the reciprocal of the most detailed frequency scale, i.e. + N=1 for no transforming at all, N=2 for dividing into approx. and detail, + N=4 for dividing into approx+detail(approx+detial), etc. + Count*2/N=2k gives the smallest length to be convolved with h and g. + Out: out[N], the wavelet transform, arranged in interleaved format. + + Returns maximal atom length (should equal N). +*/ +int dwtpqmf(double* in, int Count, int N, double* h, double* g, int M, double* out) +{ + double* tmp=new double[Count]; + double *tmpa=tmp, *tmpla=in; + int C=Count, L=0, n=1; + +A: + { + //C: signal length at current layer + //L: layer index, 0 being most detailed + //n: atom length on current layer, equals 2^L. + //C*n=(C<<L)=Count + double* tmpd=&tmpa[C/2]; + for (int i=0; i<C; i+=2) + { + int i2=i/2; + tmpa[i2]=tmpla[i]*h[0]; + tmpd[i2]=tmpla[i]*g[0]; + for (int j=1; j<M; j++) + { + if (i+j<C) + { + tmpa[i2]+=tmpla[i+j]*h[j]; + tmpd[i2]+=tmpla[i+j]*g[j]; + } + else + { + tmpa[i2]+=tmpla[i+j-C]*h[j]; + tmpd[i2]+=tmpla[i+j-C]*g[j]; + } + } + } + for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i]; + for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i]; + n*=2; + if (n<N) + { + tmpla=tmpa; + tmpa=tmpd; + L++; + C/=2; + goto A; + } + } + delete[] tmp; + return n; +}//dwtpqmf + +/* + function dwtp: in this implementation h and g can be different from mirrored reconstruction filters, + i.e. the biorthogonal transform. h[0] and g[0] are aligned at the ends of the filters, i.e. h[-M+1:0], + g[-M+1:0]. + + In: in[Count]: waveform data + h[-M+1:0], g[-M+1:0]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], the wavelet transform, arranged in interleaved format. + + Returns maximal atom length (should equal N). +*/ +int dwtp(double* in, int Count, int N, double* h, double* g, int M, double* out) +{ + double* tmp=new double[Count]; + double *tmpa=tmp, *tmpla=in; + int C=Count, L=0, n=1; + +A: + { + double* tmpd=&tmpa[C/2]; + for (int i=0; i<C; i+=2) + { + int i2=i/2; + tmpa[i2]=tmpla[i]*h[0]; + tmpd[i2]=tmpla[i]*g[0]; + for (int j=-1; j>-M; j--) + { + if (i-j<C) + { + tmpa[i2]+=tmpla[i-j]*h[j]; + tmpd[i2]+=tmpla[i-j]*g[j]; + } + else + { + tmpa[i2]+=tmpla[i-j-C]*h[j]; + tmpd[i2]+=tmpla[i-j-C]*g[j]; + } + } + } + for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i]; + for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i]; + n*=2; + if (n<N) + { + tmpla=tmpa; + tmpa=tmpd; + L++; + C/=2; + goto A; + } + } + delete[] tmp; + return n; +}//dwtp + +/* + function dwtp: in this implementation h and g can be different size. h[0] and g[0] are aligned at the + ends of the filters, i.e. h[-Mh+1:0], g[-Mg+1:0]. + + In: in[Count]: waveform data + h[-Mh+1:0], g[-Mg+1:0]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], the wavelet transform, arranged in interleaved format. + + Returns maximal atom length (should equal N). +*/ +int dwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out) +{ + double* tmp=new double[Count]; + double *tmpa=tmp, *tmpla=in; + int C=Count, L=0, n=1; + +A: + { + double* tmpd=&tmpa[C/2]; + for (int i=0; i<C; i+=2) + { + int i2=i/2; + tmpa[i2]=tmpla[i]*h[0]; + tmpd[i2]=tmpla[i]*g[0]; + for (int j=-1; j>-Mh; j--) + { + if (i-j<C) + { + tmpa[i2]+=tmpla[i-j]*h[j]; + } + else + { + tmpa[i2]+=tmpla[i-j-C]*h[j]; + } + } + for (int j=-1; j>-Mg; j--) + { + if (i-j<C) + { + tmpd[i2]+=tmpla[i-j]*g[j]; + } + else + { + tmpd[i2]+=tmpla[i-j-C]*g[j]; + } + } + } + for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i]; + for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i]; + n*=2; + if (n<N) + { + tmpla=tmpa; + tmpa=tmpd; + L++; + C/=2; + goto A; + } + } + delete[] tmp; + return n; +}//dwtp + +/* + function dwtp: in this implementation h and g can be arbitrarily located: h from $sh to $eh, g from + $sg to $eg. + + In: in[Count]: waveform data + h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], the wavelet transform, arranged in interleaved format. + + Returns maximal atom length (should equal N). +*/ +int dwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out) +{ + double* tmp=new double[Count]; + double *tmpa=tmp, *tmpla=in; + int C=Count, L=0, n=1; + +A: + { + double* tmpd=&tmpa[C/2]; + for (int i=0; i<C; i+=2) + { + int i2=i/2; + tmpa[i2]=0;//tmpla[i]*h[0]; + tmpd[i2]=0;//tmpla[i]*g[0]; + for (int j=sh; j<=eh; j++) + { + int ind=i-j; + if (ind>=C) tmpa[i2]+=tmpla[ind-C]*h[j]; + else if (ind<0) tmpa[i2]+=tmpla[ind+C]*h[j]; + else tmpa[i2]+=tmpla[ind]*h[j]; + } + for (int j=sg; j<=eg; j++) + { + int ind=i-j; + if (ind>=C) tmpd[i2]+=tmpla[i-j-C]*g[j]; + else if (ind<0) tmpd[i2]+=tmpla[i-j+C]*g[j]; + else tmpd[i2]+=tmpla[i-j]*g[j]; + } + } + for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i]; + for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i]; + n*=2; + if (n<N) + { + tmpla=tmpa; + tmpa=tmpd; + L++; + C/=2; + goto A; + } + } + delete[] tmp; + return n; +}//dwtp + +/* + function idwtp: periodic wavelet reconstruction by qmf + + In: in[Count]: wavelet transform in interleaved format + h[M], g[M]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], waveform data (detail level 0). + + No return value. +*/ +void idwtp(double* in, int Count, int N, double* h, double* g, int M, double* out) +{ + double* tmp=new double[Count]; + memcpy(out, in, sizeof(double)*Count); + int n=N, C=Count/N, L=log2(N)-1; + while (n>1) + { + memset(tmp, 0, sizeof(double)*C*2); + //2k<<L being the approx, (2k+1)<<L being the detail + for (int i=0; i<C; i++) + { + for (int j=0; j<M; j++) + { + if (i*2+j<C*2) + { + tmp[i*2+j]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j]; + } + else + { + tmp[i*2+j-C*2]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j]; + } + } + } + for (int i=0; i<C*2; i++) out[i<<L]=tmp[i]; + n/=2; + C*=2; + L--; + } + delete[] tmp; +}//idwtp + +/* + function idwtp: in which h and g can have different length. + + In: in[Count]: wavelet transform in interleaved format + h[Mh], g[Mg]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], waveform data (detail level 0). + + No return value. +*/ +void idwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out) +{ + double* tmp=new double[Count]; + memcpy(out, in, sizeof(double)*Count); + int n=N, C=Count/N, L=log2(N)-1; + while (n>1) + { + memset(tmp, 0, sizeof(double)*C*2); + //2k<<L being the approx, (2k+1)<<L being the detail + for (int i=0; i<C; i++) + { + for (int j=0; j<Mh; j++) + { + int ind=i*2+j+(Mg-Mh)/2; + if (ind>=C*2) + { + tmp[ind-C*2]+=out[(2*i)<<L]*h[j]; + } + else if (ind<0) + { + tmp[ind+C*2]+=out[(2*i)<<L]*h[j]; + } + else + { + tmp[ind]+=out[(2*i)<<L]*h[j]; + } + } + } + for (int i=0; i<C; i++) + { + for (int j=0; j<Mg; j++) + { + int ind=i*2+j+(Mh-Mg)/2; + if (ind>=C*2) + { + tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j]; + } + else if (ind<0) + { + tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j]; + } + else + { + tmp[ind]+=out[(2*i+1)<<L]*g[j]; + } + } + } + for (int i=0; i<C*2; i++) out[i<<L]=tmp[i]; + n/=2; + C*=2; + L--; + } + delete[] tmp; +}//idwtp + +/* + function idwtp: in which h and g can be arbitrarily located: h from $sh to $eh, g from $sg to $eg + + In: in[Count]: wavelet transform in interleaved format + h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair + N: maximal time resolution + Out: out[N], waveform data (detail level 0). + + No return value. +*/ +void idwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out) +{ + double* tmp=new double[Count]; + memcpy(out, in, sizeof(double)*Count); + int n=N, C=Count/N, L=log2(N)-1; + while (n>1) + { + memset(tmp, 0, sizeof(double)*C*2); + //2k<<L being the approx, (2k+1)<<L being the detail + for (int i=0; i<C; i++) + { + for (int j=sh; j<=eh; j++) + { + int ind=i*2+j; + if (ind>=C*2) tmp[ind-C*2]+=out[(2*i)<<L]*h[j]; + else if (ind<0) tmp[ind+C*2]+=out[(2*i)<<L]*h[j]; + else tmp[ind]+=out[(2*i)<<L]*h[j]; + } + } + for (int i=0; i<C; i++) + { + for (int j=sg; j<=eg; j++) + { + int ind=i*2+j; + if (ind>=C*2) tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j]; + else if (ind<0) tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j]; + else tmp[ind]+=out[(2*i+1)<<L]*g[j]; + } + } + for (int i=0; i<C*2; i++) out[i<<L]=tmp[i]; + n/=2; + C*=2; + L--; + } + delete[] tmp; +}//idwtp + +//--------------------------------------------------------------------------- + +/* + Spline biorthogonal wavelet routines. + + Further reading: "Calculation of biorthogonal spline wavelets.pdf" +*/ + +//function Cmb: combination number C(n, k) (n>=k>=0) +int Cmb(int n, int k) +{ + if (k>n/2) k=n-k; + int c=1; + for (int i=1; i<=k; i++) c=c*(n+1-i)/i; + return c; +} + +/* + function splinewl: computes spline biorthogonal wavelet filters. This version of splinewl calcualtes + the positive-time half of h and hm coefficients only. + + p1 and p2 must have the same parity. If p1 is even, p1 coefficients will be returned in h1; if p1 is + odd, p1-1 coefficients will be returned in h1. + + Actual length of h is p1+1, of hm is p1+2p2-1. only a half of each is kept. + p even: h[0:p1/2] <- [p1/2:p1], hm[0:p1/2+p2-1] <- [p1/2+p2-1:p1+2p2-2] + p odd: h[0:(p1-1)/2] <- [(p1+1)/2:p1], hm[0:(p1-3)/2+p2] <- [(p1-1)/2+p2:p1+2p2-2] + i.e. h[0:hp1] <- [p1-hp1:p1], hm[0:hp1+p2-1] <- [p1-hp1-1+p2:p1+2p2-2] + + In: p1, p2: specify vanishing moments of h and hm + Out: h[] and hm[] as specified above. + + No return value. +*/ +void splinewl(int p1, int p2, double* h, double* hm) +{ + int hp1=p1/2, hp2=p2/2; + int q=(p1+p2)/2; + h[hp1]=sqrt(2.0)*pow(2, -p1); +// h1[hp1]=1; + for (int i=1, j=hp1-1; i<=hp1; i++, j--) + { + h[j]=h[j+1]*(p1+1-i)/i; + } + + double *_hh1=new double[p2+1], *_hh2=new double[2*q]; + double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q]; + + hh1[hp2]=sqrt(2.0)*pow(2, -p2); + for (int i=1, j=hp2-1; i<=hp2; i++, j--) + { + hh1[j]=hh1[j+1]*(p2+1-i)/i; + } + if (p2%2) //p2 is odd + { + for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i]; + } + else //p2 even + { + for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i]; + } + + memset(_hh2, 0, sizeof(double)*2*q); + for (int n=1-q; n<=q-1; n++) + { + int k=abs(n); + int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n); //CC=1.0*C(q-1+k, k)*C(2*k, k-n) + for (; k<=q-1; k++) + { + hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k); + CC1=CC1*(q+k)/(k+1); + CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n)); + } + hh2[n]*=pow(-1, n); + } + + //hh1[hp2-p2:hp2], hh2[1-q:q-1] + //h2=conv(hh1, hh2), but the positive half only + memset(hm, 0, sizeof(double)*(hp1+p2)); + for (int i=hp2-p2; i<=hp2; i++) + for (int j=1-q; j<=q-1; j++) + { + if (i+j>=0 && i+j<hp1+p2) + hm[i+j]+=hh1[i]*hh2[j]; + } + + delete[] _hh1; + delete[] _hh2; +}//splinewl + + +/* + function splinewl: calculates the analysis and reconstruction filter pairs of spline biorthogonal + wavelet (h, g) and (hm, gm). h has the size p1+1, hm has the size p1+2p2-1. + + If p1+1 is odd, then all four filters are symmetric; if not, then h and hm are symmetric, while g and + gm are anti-symmetric. + + The concatenation of filters h with hm (or g with gm) introduces a time shift of p1+p2-1, which is the + return value multiplied by -1. + + If normmode==1, the results are normalized so that ||h||^2=||g||^2=1; + if normmode==2, the results are normalized so that ||hm||^2=||gm||^2=1, + if normmode==3, the results are normalized so that ||h||^2==||g||^2=||hm||^2=||gm||^2. + + If a *points* buffer is specified, the function returns the starting and ending + positions (inclusive) of h, hm, g, and gm, in the order of (hs, he, hms, hme, + gs, ge, gms, gme), as ps[0]~ps[7]. + + In: p1 and p2, specify vanishing moments of h and hm, respectively. + normmode: mode for normalization + Out: h[p1+1], g[p1+1], hm[p1+2p2-1], gm[p1+2p2-1], points[8] (optional) + + Returns -p1-p2+1. +*/ +int splinewl(int p1, int p2, double* h, double* hm, double* g, double* gm, int normmode, int* points) +{ + int lf=p1+1, lb=p1+2*p2-1; + int hlf=lf/2, hlb=lb/2; + + double *h1=&h[hlf], *h2=&hm[hlb]; + int hp1=p1/2, hp2=p2/2; + int q=(p1+p2)/2; + h1[hp1]=sqrt(2.0)*pow(2, -p1); +// h1[hp1]=2*pow(2, -p1); + for (int i=1, j=hp1-1; i<=hp1; i++, j--) h1[j]=h1[j+1]*(p1+1-i)/i; + + double *_hh1=new double[p2+1+2*q]; + double *_hh2=&_hh1[p2+1]; + double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q]; + hh1[hp2]=sqrt(2.0)*pow(2, -p2); +// hh1[hp2]=pow(2, -p2); + for (int i=1, j=hp2-1; i<=hp2; i++, j--) hh1[j]=hh1[j+1]*(p2+1-i)/i; + if (p2%2) for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i]; + else for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i]; + memset(_hh2, 0, sizeof(double)*2*q); + for (int n=1-q; n<=q-1; n++) + { + int k=abs(n); + int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n); //CC=1.0*C(q-1+k, k)*C(2*k, k-n) + for (int k=abs(n); k<=q-1; k++) + { + hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k); + CC1=CC1*(q+k)/(k+1); + CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n)); + } + hh2[n]*=pow(-1, n); + } + //hh1[hp2-p2:hp2], hh2[1-q:q-1] + //h2=conv(hh1, hh2), but the positive half only + memset(h2, 0, sizeof(double)*(hp1+p2)); + for (int i=hp2-p2; i<=hp2; i++) for (int j=1-q; j<=q-1; j++) + if (i+j>=0 && i+j<hp1+p2) h2[i+j]+=hh1[i]*hh2[j]; + delete[] _hh1; + + int hs, he, hms, hme, gs, ge, gms, gme; + if (lf%2) + { + hs=-hlf, he=hlf, hms=-hlb, hme=hlb; + gs=-hlb+1, ge=hlb+1, gms=-hlf-1, gme=hlf-1; + } + else + { + hs=-hlf, he=hlf-1, hms=-hlb+1, hme=hlb; + gs=-hlb, ge=hlb-1, gms=-hlf+1, gme=hlf; + } + + if (lf%2) + { + for (int i=1; i<=hlf; i++) h1[-i]=h1[i]; + for (int i=1; i<=hlb; i++) h2[-i]=h2[i]; + double* _g=&g[hlb-1], *_gm=&gm[hlf+1]; + for (int i=gs; i<=ge; i++) _g[i]=(i%2)?h2[1-i]:-h2[1-i]; + for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?h1[-1-i]:-h1[-1-i]; + } + else + { + for (int i=0; i<hlf; i++) h1[-i-1]=h1[i]; + for (int i=0; i<hlb; i++) h2[-i-1]=h2[i]; + h2=&h2[-1]; + double *_g=&g[hlb], *_gm=&gm[hlf-1]; + for (int i=gs; i<=ge; i++) _g[i]=(i%2)?-h2[-i]:h2[-i]; + for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?-h1[-i]:h1[-i]; + } + + if (normmode) + { + double sumh=0; for (int i=0; i<=he-hs; i++) sumh+=h[i]*h[i]; + double sumhm=0; for (int i=0; i<=hme-hms; i++) sumhm+=hm[i]*hm[i]; + if (normmode==1) + { + double rr=sqrt(sumh); + for (int i=0; i<=hme-hms; i++) hm[i]*=rr; + rr=1.0/rr; + for (int i=0; i<=he-hs; i++) h[i]*=rr; + rr=sqrt(sumhm); + for (int i=0; i<=gme-gms; i++) gm[i]*=rr; + rr=1.0/rr; + for (int i=0; i<=ge-gs; i++) g[i]*=rr; + } + else if (normmode==2) + { + double rr=sqrt(sumh); + for (int i=0; i<=ge-gs; i++) g[i]*=rr; + rr=1.0/rr; + for (int i=0; i<=gme-gms; i++) gm[i]*=rr; + rr=sqrt(sumhm); + for (int i=0; i<=he-hs; i++) h[i]*=rr; + rr=1.0/rr; + for (int i=0; i<=hme-hms; i++) hm[i]*=rr; + } + else if (normmode==3) + { + double rr=pow(sumh/sumhm, 0.25); + for (int i=0; i<=hme-hms; i++) hm[i]*=rr; + for (int i=0; i<=ge-gs; i++) g[i]*=rr; + rr=1.0/rr; + for (int i=0; i<=he-hs; i++) h[i]*=rr; + for (int i=0; i<=gme-gms; i++) gm[i]*=rr; + } + } + + if (points) + { + points[0]=hs, points[1]=he, points[2]=hms, points[3]=hme; + points[4]=gs, points[5]=ge, points[6]=gms, points[7]=gme; + } + return -p1-p2+1; +}//splinewl + +//--------------------------------------------------------------------------- +/* + function wavpacqmf: calculate pseudo local cosine transforms using wavelet packet + + In: data[Count], Count=fr*WID, waveform data + WID: largest scale, equals 2^ORDER + wid: smallest scale, euqals 2^order + h[M], g[M]: quadratic mirror filter pair, fr>2*M + Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid] + + No return value. +*/ +void wavpacqmf(double*** spec, double* data, int Count, int WID, int wid, int M, double* h, double* g) +{ + int fr=Count/WID, ORDER=log2(WID), order=log2(wid); + double* _data1=new double[Count*2]; + double *data1=_data1, *data2=&_data1[Count]; + //the qmf always filters data1 and puts the results to data2 + memcpy(data1, data, sizeof(double)*Count); + int l=0, C=fr*WID, FR=1; + double *ldata, *ldataa, *ldatad; + while (l<ORDER) + { + //qmf + for (int f=0; f<FR; f++) + { + ldata=&data1[f*C]; + if (f%2==0) + ldataa=&data2[f*C], ldatad=&data2[f*C+C/2]; + else + ldatad=&data2[f*C], ldataa=&data2[f*C+C/2]; + + memset(&data2[f*C], 0, sizeof(double)*C); + for (int i=0; i<C; i+=2) + { + int i2=i/2; + ldataa[i2]=ldata[i]*h[0]; + ldatad[i2]=ldata[i]*g[0]; + for (int j=1; j<M; j++) + { + if (i+j<C) + { + ldataa[i2]+=ldata[i+j]*h[j]; + ldatad[i2]+=ldata[i+j]*g[j]; + } + else + { + ldataa[i2]+=ldata[i+j-C]*h[j]; + ldatad[i2]+=ldata[i+j-C]*g[j]; + } + } + } + } + double *tmp=data2; data2=data1; data1=tmp; + l++; + C=(C>>1); + FR=(FR<<1); + if (l>=order) + { + for (int f=0; f<FR; f++) + for(int i=0; i<C; i++) + spec[ORDER-l][i][f]=data1[f*C+i]; + } + } + + delete[] _data1; +}//wavpacqmf + +/* + function iwavpacqmf: inverse transform of wavpacqmf + + In: spec[Fr][Wid], Fr>M*2 + h[M], g[M], quadratic mirror filter pair + Out: data[Fr*Wid] + + No return value. +*/ +void iwavpacqmf(double* data, double** spec, int Fr, int Wid, int M, double* h, double* g) +{ + int Count=Fr*Wid, Order=log2(Wid); + double* _data1=new double[Count]; + double *data1, *data2, *ldata, *ldataa, *ldatad; + if (Order%2) data1=_data1, data2=data; + else data1=data, data2=_data1; + //data pass to buffer + for (int i=0, t=0; i<Wid; i++) + for (int j=0; j<Fr; j++) + data1[t++]=spec[j][i]; + + int l=Order; + int C=Fr; + int K=Wid/2; + while (l>0) + { + memset(data2, 0, sizeof(double)*Count); + for (int k=0; k<K; k++) + { + if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C]; + else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C]; + ldata=&data2[2*k*C]; + //qmf + for (int i=0; i<C; i++) + { + for (int j=0; j<M; j++) + { + if (i*2+j<C*2) + { + ldata[i*2+j]+=ldataa[i]*h[j]+ldatad[i]*g[j]; + } + else + { + ldata[i*2+j-C*2]+=ldataa[i]*h[j]+ldatad[i]*g[j]; + } + } + } + } + + double *tmp=data2; data2=data1; data1=tmp; + l--; + C=(C<<1); + K=(K>>1); + } + delete[] _data1; +}//iwavpacqmf + +/* + function wavpac: calculate pseudo local cosine transforms using wavelet packet, + + In: data[Count], Count=fr*WID, waveform data + WID: largest scale, equals 2^ORDER + wid: smallest scale, euqals 2^order + h[hs:he-1], g[gs:ge-1]: filter pair + Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid] + + No return value. +*/ +void wavpac(double*** spec, double* data, int Count, int WID, int wid, double* h, int hs, int he, double* g, int gs, int ge) +{ + int fr=Count/WID, ORDER=log2(WID), order=log2(wid); + double* _data1=new double[Count*2]; + double *data1=_data1, *data2=&_data1[Count]; + //the qmf always filters data1 and puts the results to data2 + memcpy(data1, data, sizeof(double)*Count); + int l=0, C=fr*WID, FR=1; + double *ldata, *ldataa, *ldatad; + while (l<ORDER) + { + //qmf + for (int f=0; f<FR; f++) + { + ldata=&data1[f*C]; + if (f%2==0) + ldataa=&data2[f*C], ldatad=&data2[f*C+C/2]; + else + ldatad=&data2[f*C], ldataa=&data2[f*C+C/2]; + + memset(&data2[f*C], 0, sizeof(double)*C); + for (int i=0; i<C; i+=2) + { + int i2=i/2; + ldataa[i2]=0;//ldata[i]*h[0]; + ldatad[i2]=0;//ldata[i]*g[0]; + for (int j=hs; j<=he; j++) + { + int ind=i-j; + if (ind>=C) + { + ldataa[i2]+=ldata[ind-C]*h[j]; + } + else if (ind<0) + { + ldataa[i2]+=ldata[ind+C]*h[j]; + } + else + { + ldataa[i2]+=ldata[ind]*h[j]; + } + } + for (int j=gs; j<=ge; j++) + { + int ind=i-j; + if (ind>=C) + { + ldatad[i2]+=ldata[ind-C]*g[j]; + } + else if (ind<0) + { + ldatad[i2]+=ldata[ind+C]*g[j]; + } + else + { + ldatad[i2]+=ldata[ind]*g[j]; + } + } + } + } + double *tmp=data2; data2=data1; data1=tmp; + l++; + C=(C>>1); + FR=(FR<<1); + if (l>=order) + { + for (int f=0; f<FR; f++) + for(int i=0; i<C; i++) + spec[ORDER-l][i][f]=data1[f*C+i]; + } + } + + delete[] _data1; +}//wavpac + +/* + function iwavpac: inverse transform of wavpac + + In: spec[Fr][Wid] + h[hs:he-1], g[gs:ge-1], reconstruction filter pair + Out: data[Fr*Wid] + + No return value. +*/ +void iwavpac(double* data, double** spec, int Fr, int Wid, double* h, int hs, int he, double* g, int gs, int ge) +{ + int Count=Fr*Wid, Order=log2(Wid); + double* _data1=new double[Count]; + double *data1, *data2, *ldata, *ldataa, *ldatad; + if (Order%2) data1=_data1, data2=data; + else data1=data, data2=_data1; + //data pass to buffer + for (int i=0, t=0; i<Wid; i++) + for (int j=0; j<Fr; j++) + data1[t++]=spec[j][i]; + + int l=Order; + int C=Fr; + int K=Wid/2; + while (l>0) + { + memset(data2, 0, sizeof(double)*Count); + for (int k=0; k<K; k++) + { + if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C]; + else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C]; + ldata=&data2[2*k*C]; + //qmf + for (int i=0; i<C; i++) + { + for (int j=hs; j<=he; j++) + { + int ind=i*2+j; + if (ind>=C*2) + { + ldata[ind-C*2]+=ldataa[i]*h[j]; + } + else if (ind<0) + { + ldata[ind+C*2]+=ldataa[i]*h[j]; + } + else + { + ldata[ind]+=ldataa[i]*h[j]; + } + } + for (int j=gs; j<=ge; j++) + { + int ind=i*2+j; + if (ind>=C*2) + { + ldata[ind-C*2]+=ldatad[i]*g[j]; + } + else if (ind<0) + { + ldata[ind+C*2]+=ldatad[i]*g[j]; + } + else + { + ldata[ind]+=ldatad[i]*g[j]; + } + } + } + } + + double *tmp=data2; data2=data1; data1=tmp; + l--; + C=(C<<1); + K=(K>>1); + } + delete[] _data1; +}//iwavpac
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/wavelet.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,28 @@ +#ifndef waveletH +#define waveletH + +/* + wavelet.cpp - wavelet routines +*/ + +//--wavelet filter routines-------------------------------------------------- +void Daubechies(int p, double* h); //computes Daubechies filter +void splinewl(int p1, int p2, double* h1, double* h2); //compute spline biorthogonal wavelet filter +int splinewl(int p1, int p2, double* h, double* hm, double* g, double* gm, int normmode=0, int* points=0);//compute spline biorthogonal wavelet filter + +//--periodic DWT and IDWT---------------------------------------------------- +int dwtpqmf(double* in, int Count, int N, double* h, double* g, int M, double* out); +int dwtp(double* in, int Count, int N, double* h, double* g, int M, double* out); +int dwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out); +int dwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out); +void idwtp(double* in, int Count, int N, double* h, double* g, int M, double* out); +void idwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out); +void idwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out); + +//--pseudo local cosine with wavelet packet---------------------------------- +void wavpacqmf(double*** spec, double* data, int Count, int WID, int wid, int M, double* h, double* g); +void wavpac(double*** spec, double* data, int Count, int WID, int wid, double* h, int hs, int he, double* g, int gs, int ge); +void iwavpacqmf(double* data, double** spec, int Fr, int Wid, int M, double* h, double* g); +void iwavpac(double* data, double** spec, int Fr, int Wid, double* h, int hs, int he, double* g, int gs, int ge); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/xcomplex.h Tue Oct 05 10:45:57 2010 +0100 @@ -0,0 +1,118 @@ +#ifndef XCOMPLEX +#define XCOMPLEX + + +/* + xcomplex.h - Xue's complex number class. + + This classes is modelled after standard c++ complex class, with a few additional methods. Unused + standard member and non-member functions/operators may not have been implemented. +*/ + +#include <math.h> + +//* +template <class T> class cmplx +{ +public: +//standard members + T x; + T y; + cmplx(){} + cmplx(const T& c, const T& d) {x=c; y=d;} + cmplx(const T& c) {x=c; y=0;} + template<class X> cmplx(const cmplx<X>& c){x=c.x, y=c.y;} + + T real(){return x;} + T imag(){return y;} + + cmplx<T>& operator=(const T& c){x=c; y=0; return *this;} + template<class X> cmplx<T>& operator+=(const X& c){x+=c; y=0; return *this;} + template<class X> cmplx<T>& operator-=(const X& c){x-=c; y=0; return *this;} + template<class X> cmplx<T>& operator*=(const X& c){x*=c; y*=c; return *this;} + template<class X> cmplx<T>& operator/=(const X& c){x/=c; y/=c; return *this;} + template<class X> cmplx<T>& operator=(const cmplx<X>& c){x=c.x; y=c.y; return* this;} + + template<class X> cmplx<T>& operator+=(const cmplx<X>& c){x+=c.x; y+=c.y; return *this;} + template<class X> cmplx<T>& operator-=(const cmplx<X>& c){x-=c.x; y-=c.y; return *this;} + template<class X> cmplx<T>& operator*=(const cmplx<X>& c){T tmpx=x*c.x-y*c.y; y=x*c.y+y*c.x; x=tmpx; return *this;} + template<class X> cmplx<T>& operator/=(const cmplx<X>& c){if (c.y==0){x/=c.x; y/=c.x;} else if (c.x==0){T tmpx=y/c.y; y=-x/c.y; x=tmpx;} else {T xm=c.x*c.x+c.y*c.y; T tmpx=(c.x*x+c.y*y)/xm; y=(y*c.x-x*c.y)/xm; x=tmpx;} return *this;} + +//non-standard members + //operator~: square of absolute value + T operator~() {return x*x+y*y;} + //operator*: complex conjugate + cmplx<T> operator*(){cmplx<T> result; result.x=x; result.y=-y; return result;} + //operator^: multiplicaiton with the complex conjugate of the argument + cmplx<T> operator^(const cmplx &b){cmplx<T> result; result.x=x*b.x+y*b.y; result.y=y*b.x-x*b.y; return result;} + //cinv: complex reciprocal + cmplx<T> cinv(){cmplx<T> result; if (y==0) result.x=1/x, result.y=0; else if (x==0) result.y=-1/y, result.x=0; else{T xm=x*x+y*y; result.x=x/xm; result.y=-y/xm;} return result;} + //rotate: rotate by an angle + cmplx<T>& rotate(const T ph) {double s=sin(ph), c=cos(ph), tmpx; tmpx=x*c-y*s; y=x*s+y*c; x=tmpx; return *this;} +}; //*/ +//standard Non-member Operators +template<class Ta, class Tb> cmplx<Ta> operator+(const cmplx<Ta>& a, const cmplx<Tb>& b){cmplx<Ta> result=a; result+=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator+(const cmplx<Ta>& a, Tb& b){cmplx<Ta> result=a; result+=b; return result;} +template<class T> cmplx<T> operator+(T a, const cmplx<T>& b){cmplx<T> result=a; result+=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator-(const cmplx<Ta>& a, const cmplx<Tb>& b){cmplx<Ta> result=a; result-=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator-(const cmplx<Ta>& a, Tb& b){cmplx<Ta> result=a; result-=b; return result;} +template<class T> cmplx<T> operator-(T a, const cmplx<T>& b){cmplx<T> result=a; result-=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator*(const cmplx<Ta>& a, const cmplx<Tb>& b){cmplx<Ta> result=a; result*=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator*(const cmplx<Ta>& a, Tb& b){cmplx<Ta> result=a; result*=b; return result;} +template<class T> cmplx<T> operator*(T& a, const cmplx<T>& b){cmplx<T> result=b; result*=a; return result;} +template<class Ta, class Tb> cmplx<Ta> operator/(const cmplx<Ta>& a, const cmplx<Tb>& b){cmplx<Ta> result=a; result/=b; return result;} +template<class Ta, class Tb> cmplx<Ta> operator/(const cmplx<Ta>& a, Tb& b){cmplx<Ta> result=a; result/=b; return result;} +template<class T> cmplx<T> operator/(T a, const cmplx<T>& b){cmplx<T> result=a; result/=b; return result;} +template<class T> cmplx<T> operator+(const cmplx<T>& a){return a;} +template<class T> cmplx<T> operator-(const cmplx<T>& a){cmplx<T> result; result.x=-a.x; result.y=-a.y; return result;} +template<class Ta, class Tb> bool operator==(const cmplx<Ta>& a, const cmplx<Tb>& b){return (a.x==b.x && a.y==b.y);} +template<class Ta, class Tb> bool operator==(const cmplx<Ta>& a, Tb b){return (a.x==b && a.y==0);} +template<class Ta, class Tb> bool operator==(Ta a, const cmplx<Tb>& b){return (a==b.x && 0==b.y);} +template<class Ta, class Tb> bool operator!=(const cmplx<Ta>& a, const cmplx<Tb>& b){return (a.x!=b.x || a.y!=b.y);} +template<class Ta, class Tb> bool operator!=(const cmplx<Ta>& a, Tb b){return (a.x!=b || a.y!=0);} +template<class Ta, class Tb> bool operator!=(Ta a, const cmplx<Tb>& b){return (a!=b.x || 0!=b.y);} +/* +template <class T, class charT, class traits> basic_istream<charT, traits>& operator>>(istream&, complex<T>&); +template <class T, class charT, class traits> basic_ostream<charT, traits>& operator<<(ostream&, const complex<T>&); +*/ +//Values +template<class T> T real(const cmplx<T>& a){return a.x;} +template<class T> T imag(const cmplx<T>& a){return a.y;} +template<class T> T abs(const cmplx<T>& a){return sqrt(a.x*a.x+a.y*a.y);} +template<class T> T fabs(const cmplx<T>& a){return sqrt(a.x*a.x+a.y*a.y);} +template<class T> T arg(const cmplx<T>& a){return (a.x==0 && a.y==0)?0:atan2(a.y, a.x);} +template<class T> T norm(const cmplx<T>& a){return a.x*a.x+a.y*a.y;} +template<class T> cmplx<T> conj(const cmplx<T>& a){cmplx<T> result; result.x=a.x; result.y=-a.y; return result;} +template<class T> cmplx<T> polar(const T& r, const T& theta){cmplx<T> result; result.x=r*cos(theta); result.y=r*sin(theta); return result;} +//Transcendentals +/* +template<class T> cmplx<T> cos (const cmplx<T>&); +template<class T> cmplx<T> cosh (const cmplx<T>&); +*/ +template<class T> cmplx<T> exp(const cmplx<T>& a){return polar(exp(a.x), a.y);} +template<class T> cmplx<T> log(const cmplx<T>& a){cmplx<T> result; result.x=0.5*log(norm(a)); result.y=arg(a); return result;} +/* +template<class T> cmplx<T> log10 (const cmplx<T>&); +template<class T> cmplx<T> pow (const cmplx<T>&, int); +*/ +template<class T> cmplx<T> pow(const cmplx<T>& a, T& e){cmplx<T> result; T rad=abs(a); T angle=arg(a); return polar(rad, angle*e);} +/* +template<class T> cmplx<T> pow (const cmplx<T>&, const cmplx<T>&); +template<class T> cmplx<T> pow (const T&, const cmplx<T>&); +template<class T> cmplx<T> sin (const cmplx<T>&); +template<class T> cmplx<T> sinh (const cmplx<T>&); +*/ +template<class T> cmplx<T> sqrt(const cmplx<T>& a){cmplx<T> result; if (a.y==0) {if (a.x>=0){result.x=sqrt(a.x); result.y=0;} else{result.y=sqrt(-a.x); result.x=0;}} else {result.y=sqrt((sqrt(a.x*a.x+a.y*a.y)-a.x)*0.5); result.x=0.5*a.y/result.y;} return result;} +/* +template<class T> cmplx<T> tan (const cmplx<T>&); +template<class T> cmplx<T> tanh (const cmplx<T>&); +*/ + +//non-standard non-member functions +//template operator^: multiplying one complex number with the complex conjugate of another +template<class T> cmplx<T> operator^(const cmplx<T>& a, const cmplx<T>& b){cmplx<T> result=a; result^=b; return result;} + +typedef cmplx<double> cdouble; +typedef cmplx<float> cfloat; + +#endif