Mercurial > hg > x
changeset 2:fc19d45615d1
* Make all file names lower-case to avoid case ambiguity
(some includes differed in case from the filenames they were
trying to include). Also replace MinGW-specific mem.h with
string.h
| author | Chris Cannam |
|---|---|
| date | Tue, 05 Oct 2010 11:04:40 +0100 |
| parents | 6422640a802f |
| children | 42c078b19e9a |
| files | Matrix.cpp Matrix.h New Text Document.txt QuickSpec.cpp QuickSpec.h SinEst.cpp SinEst.h SinSyn.cpp SinSyn.h TStream.h WindowFunctions.cpp WindowFunctions.h fft.cpp hs.cpp hs.h hssf.cpp matrix.cpp matrix.h procedures.cpp quickspec.cpp quickspec.h sinest.cpp sinest.h sinsyn.cpp sinsyn.h tstream.h vibrato.cpp wavelet.cpp windowfunctions.cpp windowfunctions.h |
| diffstat | 30 files changed, 8730 insertions(+), 8731 deletions(-) [+] |
line wrap: on
line diff
--- a/Matrix.cpp Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,2245 +0,0 @@ -//--------------------------------------------------------------------------- -#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);} - - -
--- a/Matrix.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,407 +0,0 @@ -#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
--- a/New Text Document.txt Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -New Text Document.txt \ No newline at end of file
--- a/QuickSpec.cpp Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,349 +0,0 @@ -//--------------------------------------------------------------------------- - -#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 - -
--- a/QuickSpec.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,158 +0,0 @@ -#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
--- a/SinEst.cpp Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,3970 +0,0 @@ -//--------------------------------------------------------------------------- - -#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
--- a/SinEst.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,346 +0,0 @@ -#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
--- a/SinSyn.cpp Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,776 +0,0 @@ -//--------------------------------------------------------------------------- - - -#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
--- a/SinSyn.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,33 +0,0 @@ -#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
--- a/TStream.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,23 +0,0 @@ -#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
--- a/WindowFunctions.cpp Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,377 +0,0 @@ -//--------------------------------------------------------------------------- -#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
--- a/WindowFunctions.h Tue Oct 05 10:45:57 2010 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,33 +0,0 @@ -#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
--- a/fft.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/fft.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -1,6 +1,6 @@ //--------------------------------------------------------------------------- -#include <mem.h> +#include <string.h> #include <stdlib.h> #include "fft.h"
--- a/hs.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/hs.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -4,8 +4,8 @@ #include "align8.h" #include "arrayalloc.h" #include "procedures.h" -#include "SinEst.h" -#include "SinSyn.h" +#include "sinest.h" +#include "sinsyn.h" #include "splines.h" #include "xcomplex.h" #ifdef I
--- a/hs.h Tue Oct 05 10:45:57 2010 +0100 +++ b/hs.h Tue Oct 05 11:04:40 2010 +0100 @@ -8,12 +8,12 @@ */ //--------------------------------------------------------------------------- -#include <mem.h> +#include <string.h> #include "arrayalloc.h" #include "align8.h" #include "fft.h" -#include "QuickSpec.h" -#include "TStream.h" +#include "quickspec.h" +#include "tstream.h" #define ATOM_LOCALANCHOR 1 #define HS_CONSTF 1
--- a/hssf.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/hssf.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -2,10 +2,10 @@ #include <math.h> -#include <mem.h> +#include <string.h> #include "hssf.h" #include "arrayalloc.h" -#include "Matrix.h" +#include "matrix.h" #include "vibrato.h" //---------------------------------------------------------------------------
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/matrix.cpp Tue Oct 05 11:04:40 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 11:04:40 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
--- a/procedures.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/procedures.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -1,11 +1,11 @@ //--------------------------------------------------------------------------- #include <math.h> -#include <mem.h> +#include <string.h> #include "procedures.h" #include "matrix.h" #include "opt.h" -#include "SinEst.h" +#include "sinest.h" //--------------------------------------------------------------------------- //TGMM methods
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/quickspec.cpp Tue Oct 05 11:04:40 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 11:04:40 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 11:04:40 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 11:04:40 2010 +0100 @@ -0,0 +1,346 @@ +#ifndef SinEstH +#define SinEstH + +/* + SinEst.cpp - sinusoid estimation algorithms +*/ + + +#include <string.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 11:04:40 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 11:04:40 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 11:04:40 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
--- a/vibrato.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/vibrato.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -2,12 +2,12 @@ #include <math.h> -#include <mem.h> +#include <string.h> #include "vibrato.h" #include "arrayalloc.h" #include "fft.h" #include "hssf.h" -#include "Matrix.h" +#include "matrix.h" #include "splines.h" #include "xcomplex.h" //---------------------------------------------------------------------------
--- a/wavelet.cpp Tue Oct 05 10:45:57 2010 +0100 +++ b/wavelet.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -1,7 +1,7 @@ //--------------------------------------------------------------------------- #include <math.h> -#include <mem.h> +#include <string.h> #include "wavelet.h" #include "matrix.h"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/windowfunctions.cpp Tue Oct 05 11:04:40 2010 +0100 @@ -0,0 +1,377 @@ +//--------------------------------------------------------------------------- +#include <math.h> +#include <string.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 11:04:40 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