diff SinSyn.cpp @ 1:6422640a802f

first upload

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SinSyn.cpp	Tue Oct 05 10:45:57 2010 +0100
@@ -0,0 +1,776 @@
+//---------------------------------------------------------------------------
+
+
+#include "align8.h"
+#include "SinSyn.h"
+#include "splines.h"
+
+//---------------------------------------------------------------------------
+/*
+  function Sinuoid:	original McAuley-Quatieri synthesizer interpolation between two measurement points.
+
+  In: T: length from measurement point 1 to measurement point 2
+      a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
+      a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
+      ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
+  Out: data[T]: output buffer
+       a[T], f[T], p[T]: resynthesized amplitude, frequency and phase
+
+  No return value.
+*/
+void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, double* a, double* f, double* p, bool ad)
+{
+  int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
+  double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
+  double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
+  double la=a1, da=(a2-a1)/T;
+  if (ad)
+    for (int t=0; t<T; t++)
+    {
+      double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
+      data[t]+=la*cos(lp);
+      a[t]=la;
+      p[t]=lp;
+      f[t]=lf;
+      la=la+da;
+    }
+  else
+    for (int t=0; t<T; t++)
+    {
+      double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
+      data[t]=la*cos(lp);
+      a[t]=la;
+      p[t]=lp;
+      f[t]=lf;
+      la=la+da;
+    }
+}//Sinusoid
+
+/*
+  function Sinuoid:	original McAuley-Quatieri synthesizer interpolation between two measurement points,
+  without returning interpolated sinusoid parameters.
+
+  In: T: length from measurement point 1 to measurement point 2
+      a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
+      a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
+      ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
+  Out: data[T]: output buffer
+
+  No return value.
+*/
+void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, bool ad)
+{
+  int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
+  double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
+  double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
+  double la=a1, da=(a2-a1)/T;
+  if (ad)
+    for (int t=0; t<T; t++)
+    {
+      data[t]+=la*cos(pd+t*(pc+t*(pa+t*pb)));
+      la=la+da;
+    }
+  else
+    for (int t=0; t<T; t++)
+    {
+      data[t]=la*cos(pd+t*(pc+t*(pa+t*pb)));
+      la=la+da;
+    }
+}//Sinusoid
+
+//---------------------------------------------------------------------------
+/*
+  function Sinusoid_direct: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
+  amplitude and frequency, direct implementation.
+
+  In: CountSt, CountEn
+      aa, ab, ac, ad: trinomial coefficients of amplitude
+      fa, fb, fc, fd: trinomial coefficients of frequency
+      p1: initial phase angle at 0 (NOT at CountSt)
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+       p1: phase angle at CountEn
+
+  No return value.
+*/
+void Sinusoid_direct(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
+  double fa, double fb, double fc, double fd, double &p1, bool add)
+{
+  int i; double a, ph;
+  for (i=CountSt; i<CountEn; i++)
+  {
+     a=ad+i*(ac+i*(ab+i*aa));
+     ph=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
+    if (add) data[i]+=a*cos(ph);
+    else data[i]=a*cos(ph);
+  }
+  p1=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
+}//Sinusoid
+
+/*
+  function Sinusoid: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
+  amplitude and frequency, recursive implementation.
+
+  In: CountSt, CountEn
+      a3, a2, a1, a0: trinomial coefficients of amplitude
+      f3, f2, f1, f0: trinomial coefficients of frequency
+      ph: initial phase angle at 0 (NOT at CountSt)
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+       ph: phase angle at CountEn
+
+  No return value. This function requires 8-byte stack alignment for optimal speed.
+*/
+void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
+  double f3, double f2, double f1, double f0, double &ph, bool add)
+{
+  int i;
+  double a, da, dda, ddda, dph, ddph, dddph, ddddph,
+         sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
+         p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp;
+  if (CountSt==0)
+  {
+    a=a0; da=a1+a2+a3; dda=2*a2+6*a3; ddda=6*a3;
+    dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
+  }
+  else
+  {
+    a=a0+CountSt*(a1+CountSt*(a2+CountSt*a3));
+    da=a1+a2+a3+CountSt*(2*a2+3*a3+CountSt*3*a3);
+    dda=2*a2+6*a3+CountSt*6*a3; ddda=6*a3;
+    ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
+    dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
+    ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
+    dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
+  }
+  sph=sin(ph), cph=cos(ph);
+  sdph=sin(dph), cdph=cos(dph);
+  sddph=sin(ddph), cddph=cos(ddph);
+  sdddph=sin(dddph), cdddph=cos(dddph);
+  sddddph=sin(ddddph), cddddph=cos(ddddph);
+  if (add)
+  {
+    for (i=CountSt; i<CountEn; i++)
+    {
+      data[i]+=a*cph;
+      a=a+da; da=da+dda; dda=dda+ddda;
+      tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
+      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+    }
+  }
+  else
+  {
+    for (i=CountSt; i<CountEn; i++)
+    {
+      data[i]=a*cph;
+      a=a+da; da=da+dda; dda=dda+ddda;
+      tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
+      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+    }
+  }
+  ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+}
+
+/*
+  function SinusoidExp: synthesizes complex sinusoid whose derivative log amplitude and frequency are
+  trinomials
+
+  In: CountSt, CountEn
+      a3, a2, a1, a0: trinomial coefficients for the derivative of log amplitude
+      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
+      ea, ph: initial log amplitude and phase angle at 0
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+       ea, ph: log amplitude and phase angle at CountEn.
+
+  No return value.
+*/
+void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
+	double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
+{
+	double e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
+         p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
+	if (add) for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+		data[i]+=exp(cdouble(lea, lph));
+	}
+	else for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+		data[i]=exp(cdouble(lea, lph));
+	}
+	ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
+	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+}//SinusoidExp
+
+/*
+  function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
+  This version also synthesizes its derivative.
+
+  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
+      lamda[M]: coefficients of h[M]
+      tmpexp: inital logarithm at 0
+  Out: s[T], ds[T]: synthesized sinusoid and its derivative
+       tmpexp: logarithm at T
+
+  No return value.
+*/
+void SinusoidExp(int T, cdouble* s, cdouble* ds, int M, cdouble* lamda, double** h, double** dih, cdouble& tmpexp)
+{
+	for (int t=0; t<T; t++)
+	{
+		s[t]=exp(tmpexp);
+		cdouble dexp=0, dR=0;
+		for (int m=0; m<M; m++)	dexp+=lamda[m]*dih[m][t], dR+=lamda[m]*h[m][t];
+		tmpexp+=dexp;
+		ds[t]=s[t]*dR;
+	}
+}//SinusoidExp
+
+/*
+  function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
+  This version does not synthesize its derivative.
+
+  In: dih[M][T]: difference of integrals of basis functions h[M]
+      lamda[M]: coefficients of h[M]
+      tmpexp: inital logarithm at 0
+  Out: s[T]: synthesized sinusoid
+       tmpexp: logarithm at T
+
+  No return value.
+*/
+void SinusoidExp(int T, cdouble* s, int M, cdouble* lamda, double** dih, cdouble& tmpexp)
+{
+	for (int t=0; t<T; t++)
+	{
+		s[t]=exp(tmpexp);
+		cdouble dexp=0;
+		for (int m=0; m<M; m++)	dexp+=lamda[m]*dih[m][t];
+		tmpexp+=dexp;
+	}
+}//SinusoidExp
+
+/*
+  function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials
+
+  In: CountSt, CountEn
+      a3, a2, a1, a0: trinomial coefficients for log amplitude
+      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
+      ph: initial phase angle at 0
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+       ph: phase angle at CountEn.
+
+  No return value.
+*/
+void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
+	double omg3, double omg2, double omg1, double omg0, double &ph, bool add)
+{
+  double p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
+	if (add) for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=a0+i*(a1+i*(a2+i*a3));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+		data[i]+=exp(cdouble(lea, lph));
+	}
+	else for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=a0+i*(a1+i*(a2+i*a3));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+		data[i]=exp(cdouble(lea, lph));
+	}
+	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+}//SinusoidExpA
+
+/*
+  function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials
+  with phase angle specified at both ends.
+
+  In: CountSt, CountEn
+      a3, a2, a1, a0: trinomial coefficients for log amplitude
+      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
+      ph0, ph2: phase angles at 0 and CountEn.
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+
+  No return value.
+*/
+void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
+	double omg3, double omg2, double omg1, double omg0, double ph0, double ph2, bool add)
+{
+  double p0=ph0, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
+	double pend=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+
+	int k=floor((pend-ph2)/2/M_PI+0.5);
+	double d=ph2-pend+2*M_PI*k;
+	double _p=-2*d/CountEn/CountEn/CountEn;
+	double _q=3*d/CountEn/CountEn;
+
+	if (add) for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=a0+i*(a1+i*(a2+i*a3));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+    data[i]+=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
+	}
+	else for (int i=CountSt; i<CountEn; i++)
+	{
+		double lea=a0+i*(a1+i*(a2+i*a3));
+		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
+		data[i]=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
+	}
+}//SinusoidExpA
+
+/*
+  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
+  frequency is h[M]'q[M]. This version also synthesizes its derivative.
+
+  In: h[M][T], dh[M][T], dih[M][T]: basis functions and their derivatives and difference-integrals
+      p[M], q[M]: real and imaginary parts of coefficients of h[M]
+      tmpph: inital phase angle at 0
+  Out: s[T], ds[T]: synthesized sinusoid and its derivative
+       tmpph: phase angle at T
+
+  No return value.
+*/
+void SinusoidExpA(int T, cdouble* s, cdouble* ds, int M, double* p, double* q, double** h, double** dh, double** dih, double& tmpph)
+{
+	for (int t=0; t<T; t++)
+	{
+		double e=0, dph=0, drr=0, dri=0;
+		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t], drr+=p[m]*dh[m][t], dri+=q[m]*h[m][t];
+		s[t]=exp(cdouble(e, tmpph));
+		ds[t]=s[t]*cdouble(drr, dri);
+		tmpph+=dph;
+	}
+}//SinusoidExpA
+
+/*
+  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
+  frequency is h[M]'q[M]. This version does not synthesize its derivative.
+
+  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
+      p[M], q[M]: real and imaginary parts of coefficients of h[M]
+      tmpph: inital phase angle at 0
+  Out: s[T]: synthesized sinusoid
+       tmpph: phase angle at T
+
+  No return value.
+*/
+void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double& tmpph)
+{
+	for (int t=0; t<T; t++)
+	{
+		double e=0, dph=0;
+		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
+		s[t]=exp(cdouble(e, tmpph));
+		tmpph+=dph;
+	}
+}//SinusoidExpA
+
+/*
+  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
+  frequency is h[M]'q[M] with phase angle specified at both ends. This version does not synthesize its
+  derivative.
+
+  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
+      p[M], q[M]: real and imaginary parts of coefficients of h[M]
+      ph1, ph2: phase angles at 0 and T.
+  Out: s[T]: synthesized sinusoid
+
+  No return value.
+*/
+void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double ph1, double ph2)
+{
+	double pend=ph1;
+	for (int t=0; t<T; t++)
+	{
+		double dph=0;
+		for (int m=0; m<M; m++) dph+=q[m]*dih[m][t];
+		pend+=dph;
+	}
+
+	int k=floor((pend-ph2)/2/M_PI+0.5);
+	double d=ph2-pend+2*M_PI*k;
+	double _p=-2*d/T/T/T;
+	double _q=3*d/T/T;
+
+	double ph=ph1;
+	for (int t=0; t<T; t++)
+	{
+		double e=0, dph=0;
+		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
+		if (e>300) e=300;
+		if (e<-300) e=-300;
+		s[t]=exp(cdouble(e, ph+(t*t*(_q+t*_p))));
+		ph+=dph;
+	}
+}//SinusoidExpA
+
+/*
+//This is not used any longer as the recursion does not seem to help saving computation with all its overheads.
+void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
+	double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
+{
+  int i;
+	double dea, ddea, dddea, ddddea,
+				 dph, ddph, dddph, ddddph,
+				 sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
+				 e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
+				 p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4, tmp;
+	if (CountSt==0)
+  {
+		dea=e1+e2+e3+e4; ddea=2*e2+6*e3+14*e4; dddea=6*e3+36*e4; ddddea=24*e3;
+		dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
+  }
+  else
+  {
+		ea=e0+CountSt*(e1+CountSt*(e2+CountSt*(e3+CountSt*e4)));
+		dea=e1+e2+e3+e4+CountSt*(2*e2+3*e3+4*e4+CountSt*(3*e3+6*e4+CountSt*4*e4));
+		ddea=2*e2+6*e3+14*e4+CountSt*(6*e3+24*e4+CountSt*12*e4);
+		dddea=6*e3+36*e4+CountSt*24*e4; ddddea=24*e4;
+		ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
+		dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
+		ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
+		dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
+	}
+  sph=sin(ph), cph=cos(ph);
+  sdph=sin(dph), cdph=cos(dph);
+  sddph=sin(ddph), cddph=cos(ddph);
+  sdddph=sin(dddph), cdddph=cos(dddph);
+	sddddph=sin(ddddph), cddddph=cos(ddddph);
+  if (add)
+  {
+    for (i=CountSt; i<CountEn; i++)
+    {
+			data[i]+=exp(ea)*cdouble(cph, sph);
+			ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
+			tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
+			tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+			tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+			tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+		}
+	}
+	else
+	{
+		for (i=CountSt; i<CountEn; i++)
+		{
+			data[i]=exp(ea)*cdouble(cph, sph);
+			ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
+			tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
+			tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+			tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+			tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+		}
+	}
+	ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
+	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+}   //*/
+
+/*
+  function Sinusoid: recursive cos-sin generator with trinomial frequency
+
+  In: CountSt, CountEn
+      f3, f2, f1, f0: trinomial coefficients of frequency
+      ph: initial phase angle at 0 (NOT at CountSt)
+  Out: datar[CountSt:CountEn-1], datai[CountSt:CountEn-1]: synthesized pair of cosine and sine functions
+       ph: phase angle at CountEn
+
+  No return value.
+*/
+void Sinusoid(double* datar, double* datai, int CountSt, int CountEn, double f3, double f2, double f1, double f0, double &ph)
+{
+  int i;
+  double dph, ddph, dddph, ddddph,
+         sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
+         p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp;
+  if (CountSt==0)
+  {
+    dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
+  }
+  else
+  {
+    ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
+    dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
+    ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
+    dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
+  }
+  sph=sin(ph), cph=cos(ph);
+  sdph=sin(dph), cdph=cos(dph);
+  sddph=sin(ddph), cddph=cos(ddph);
+  sdddph=sin(dddph), cdddph=cos(dddph);
+  sddddph=sin(ddddph), cddddph=cos(ddddph);
+
+  for (i=CountSt; i<CountEn; i++)
+  {
+    datar[i]=cph; datai[i]=sph;
+    tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
+    tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+    tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+    tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+  }
+  ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
+}//Sinusoid*/
+
+/*
+  function Sinusoids: recursive harmonic multi-sinusoid generator
+
+  In: st, en
+      M: number of partials
+      a3[M], a2[M], a1[M], a0[M]: trinomial coefficients for partial amplitudes
+      f3, f2, f1, f0: trinomial coefficients for fundamental frequency
+      ph[M]: partial phases at 0.
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[st:en-1]: output buffer.
+       ph[M]: partial phases at en.
+
+  No return value.
+*/
+void Sinusoids(int M, double* data, int st, int en, double* a3, double* a2, double* a1, double* a0, double f3, double f2, double f1, double f0, double* ph, bool add)
+{
+  double dph, ddph, dddph, ddddph;
+  double sdph, cdph, cdph2, sddph, cddph, sdddph, cdddph, sddddph, cddddph, sdmph, cdmph, sdm_1ph, cdm_1ph;
+  double p0, p1, p2, p3, p4, tmp, tmp2;
+  double *a=(double*)malloc8(sizeof(double)*M*6), *da=&a[M], *dda=&a[M*2], *ddda=&a[M*3],
+         *sph=&a[M*4], *cph=&a[M*5];
+
+  for (int m=0; m<M; m++)
+  {
+    p0=ph[m], p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4;
+    if (st==0)
+    {
+      a[m]=a0[m]; da[m]=a1[m]+a2[m]+a3[m]; dda[m]=2*a2[m]+6*a3[m]; ddda[m]=6*a3[m];
+    }
+    else
+    {
+      a[m]=a0[m]+st*(a1[m]+st*(a2[m]+st*a3[m]));
+      da[m]=a1[m]+a2[m]+a3[m]+st*(2*a2[m]+3*a3[m]+st*3*a3[m]);
+      dda[m]=2*a2[m]+6*a3[m]+st*6*a3[m]; ddda[m]=6*a3[m];
+      ph[m]=p0+st*(p1+st*(p2+st*(p3+st*p4)));
+    }
+    sph[m]=sin(ph[m]), cph[m]=cos(ph[m]);
+    ph[m]=p0+(m+1)*en*(p1+en*(p2+en*(p3+en*p4)));
+  }
+
+    if (st==0)
+    {
+      dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
+    }
+    else
+    {
+      dph=p1+p2+p3+p4+st*(2*p2+3*p3+4*p4+st*(3*p3+6*p4+st*4*p4));
+      ddph=2*p2+6*p3+14*p4+st*(6*p3+24*p4+st*12*p4);
+      dddph=6*p3+36*p4+st*24*p4; ddddph=24*p4;
+    }
+    sdph=sin(dph), cdph=cos(dph);
+    sddph=sin(ddph), cddph=cos(ddph);
+    sdddph=sin(dddph), cdddph=cos(dddph);
+    sddddph=sin(ddddph), cddddph=cos(ddddph);
+
+  if (add)
+  {
+    for (int i=st; i<en; i++)
+    {
+      data[i]+=a[0]*cph[0]; a[0]+=da[0]; da[0]+=dda[0]; dda[0]+=ddda[0];
+      tmp=cph[0]*cdph-sph[0]*sdph; sph[0]=sph[0]*cdph+cph[0]*sdph; cph[0]=tmp;
+      cdm_1ph=1, sdm_1ph=0, cdmph=cdph, sdmph=sdph, cdph2=2*cdph;
+
+      for (int m=1; m<M; m++)
+      {
+        data[i]+=a[m]*cph[m]; a[m]+=da[m]; da[m]+=dda[m]; dda[m]+=ddda[m];
+//        asm{mov ecx,m} asm{mov eax,a} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,cph} asm{fld qword ptr [edx+ecx*8]} asm{fmul st(0),st(1)} asm{mov edx,data} asm{mov ebx,i} asm{fadd qword ptr [edx+ebx*8]} asm{fstp qword ptr [edx+ebx*8]} asm{mov edx,da} asm{fld qword ptr [edx+ecx*8]} asm{fadd st(1),st(0)} asm{mov ebx,dda} asm{fld qword ptr [ebx+ecx*8]} asm{fadd st(1),st(0)} asm{mov edi,ddda} asm{fadd qword ptr [edi+ecx*8]} asm{fstp qword ptr [ebx+ecx*8]} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]}
+        tmp=cdmph, tmp2=sdmph;
+        cdmph=cdmph*cdph2-cdm_1ph; sdmph=sdmph*cdph2-sdm_1ph;
+        cdm_1ph=tmp, sdm_1ph=tmp2;
+
+        tmp=cph[m]*cdmph-sph[m]*sdmph; sph[m]=sph[m]*cdmph+cph[m]*sdmph; cph[m]=tmp;
+//        asm{mov ecx,m} asm{mov eax,cph} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,sph} asm{fld qword ptr [edx+ecx*8]} asm{fld st(1)} asm{fmul sdmph} asm{fld st(1)} asm{fmul sdmph} asm{fld cdmph} asm{fmul st(4),st(0)} asm{fmulp st(3),st(0)} asm{fsubp st(3),st(0)} asm{faddp} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]}
+      }
+
+      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
+      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
+      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
+    }
+  }
+  else
+  {
+  }
+  free8(a);
+}//Sinusoids*/
+
+/*
+  function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients.
+
+  In: CountSt, CountEn
+      aa, ab, ac, ad: trinomial coefficients of amplitude.
+      fa, fb, fc, fd: trinomial coefficients of frequency.
+      ph0, ph2: phase angles at 0 and CountEn.
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: data[CountSt:CountEn-1]: output buffer.
+
+  No return value.
+*/
+void Sinusoid(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
+  double fa, double fb, double fc, double fd, double ph0, double ph2, bool add)
+{
+  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
+  int k=floor((pend-ph2)/2/M_PI+0.5);
+  double d=ph2-pend+2*M_PI*k;
+  double p=-2*d/CountEn/CountEn/CountEn;
+  double q=3*d/CountEn/CountEn, a, ph;
+  for (int i=CountSt; i<CountEn; i++)
+  {
+    a=ad+i*(ac+i*(ab+i*aa)); if (a<0) a=0;
+    ph=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
+    if (add) data[i]+=a*cos(ph);
+    else data[i]=a*cos(ph);
+  }
+}//Sinusoid
+
+/*
+  function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients,
+  returning sinusoid coefficients instead of waveform.
+
+  In: CountSt, CountEn
+      aa, ab, ac, ad: trinomial coefficients of amplitude (or log amplitude if LogA=true)
+      fa, fb, fc, fd: trinomial coefficients of frequency.
+      ph0, ph2: phase angles at 0 and CountEn.
+      LogA: specifies whether log amplitude or amplitude is a trinomial
+  Out: f[CountSt:CountEn-1], a[CountSt:CountEn-1], ph[CountSt:CountEn-1]: synthesized sinusoid parameters
+       da[CountSt:CountEn-1]: derivative of synthesized amplitude, optional
+
+  No return value.
+*/
+void Sinusoid(double* f, double* a, double* ph, double* da,	int CountSt, int CountEn, double aa, double ab,
+  double ac, double ad,	double fa, double fb, double fc, double fd, double ph0, double ph2, bool LogA)
+{
+  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
+  int k=floor((pend-ph2)/2/M_PI+0.5);
+  double d=ph2-pend+2*M_PI*k;
+  double p=-2*d/CountEn/CountEn/CountEn;
+  double q=3*d/CountEn/CountEn;
+	if (LogA) for (int i=CountSt; i<CountEn; i++)
+	{
+		a[i]=exp(ad+i*(ac+i*(ab+i*aa)));
+		if (da) da[i]=a[i]*(ac+i*(2*ab+i*3*aa));
+		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
+    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
+	}
+	else for (int i=CountSt; i<CountEn; i++)
+	{
+		a[i]=ad+i*(ac+i*(ab+i*aa));
+		if (da) da[i]=ac+i*(2*ab+i*3*aa);
+		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
+    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
+	}
+}//Sinusoid
+
+/*
+  function Sinusoid: generates trinomial frequency and phase with phase correction.
+
+  In: CountSt, CountEn
+      fa, fb, fc, fd: trinomial coefficients of frequency.
+      ph0, ph2: phase angles at 0 and CountEn.
+  Out: f[CountSt:CountEn-1], ph[CountSt:CountEn-1]: output buffers holding frequency and phase.
+
+  No return value.
+*/
+void Sinusoid(double* f, double* ph, int CountSt, int CountEn, double fa, double fb,
+  double fc, double fd, double ph0, double ph2)
+{
+  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
+  int k=floor((pend-ph2)/2/M_PI+0.5);
+  double d=ph2-pend+2*M_PI*k;
+  double p=-2*d/CountEn/CountEn/CountEn;
+  double q=3*d/CountEn/CountEn;
+	for (int i=CountSt; i<CountEn; i++)
+	{
+		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
+    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
+	}
+}//Sinusoid
+
+/*
+  function SynthesizeSinusoid: synthesizes a time-varying sinusoid from a sequence of frequencies and amplitudes
+
+  In: xs[Fr]: measurement points, should be integers although *xs has double type.
+      fs[Fr], as[Fr]: sequence of frequencies and amplitudes at xs[Fr]
+      phs[0]: initial phase angle at (int)xs[0].
+      dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
+  Out: xrec[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.
+       phs[Fr]: phase angles at xs[Fr]
+
+  Returns pointer to xrec.
+*/
+double* SynthesizeSinusoid(double* xrec, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add, bool* terminatetag)
+{
+  double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
+         *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
+  CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
+  CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
+  double ph=phs[0];
+  for (int fr=0; fr<Fr-1; fr++)
+  {
+    phs[fr]=ph;
+    ALIGN8(Sinusoid(&xrec[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, add);)
+    if (terminatetag && *terminatetag) {delete[] f3; return 0;}
+  }
+  phs[Fr-1]=ph;
+  ALIGN8(Sinusoid(&xrec[(int)xs[Fr-2]-dst], xs[Fr-1]-xs[Fr-2], den-xs[Fr-2], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2], ph, add);
+         Sinusoid(&xrec[(int)xs[0]-dst], dst-xs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], phs[0], add);)
+  delete[] f3;
+  return xrec;
+}//SynthesizeSinusoid
+
+/*
+  function ShiftTrinomial: shifts the origin of a trinomial from 0 to T
+
+  In: a3, a2, a1, a0.
+  Out: b3, b2, b1, b0, so that a3*x^3+a2*x^2+a1*x+a0=b3(x-T)^3+b2(x-T)^2+b1(x-T)+b0
+
+  No return value.
+*/
+void ShiftTrinomial(double T, double& b3, double& b2, double& b1, double& b0, double a3, double a2, double a1, double a0)
+{
+  b3=a3;
+  b2=a2+T*3*b3;
+  b1=a1+T*(2*b2-T*3*b3);
+  b0=a0+T*(b1-T*(b2-T*b3));
+}//ShiftTrinomial
+
+/*
+  function SynthesizeSinusoidP: synthesizes a time-varying sinusoid from a sequence of frequencies,
+  amplitudes and phase angles
+
+  In: xs[Fr]: measurement points, should be integers although *xs has double type.
+      fs[Fr], as[Fr], phs[Fr]: sequence of frequencies, amplitudes and phase angles at xs[Fr]
+      dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
+      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output
+        buffer
+  Out: xrecm[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.
+
+  Returns pointer to xrecm.
+*/
+double* SynthesizeSinusoidP(double* xrecm, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add)
+{
+  double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
+         *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
+  CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
+  CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
+  for (int fr=0; fr<Fr-1; fr++) Sinusoid(&xrecm[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], phs[fr], phs[fr+1], add);
+  double tmpph=phs[0]; Sinusoid(&xrecm[(int)xs[0]-dst], dst-xs[0], 0, 0, 0, 0, a0[0], f3[0], f2[0], f1[0], f0[0], tmpph, add);
+  //extend the trinomials on [xs[Fr-2], xs[Fr-1]) based at xs[Fr-2] to beyond xs[Fr-1] based at xs[Fr-1].
+  tmpph=phs[Fr-1];
+  ShiftTrinomial(xs[Fr-1]-xs[Fr-2], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2]);
+  ShiftTrinomial(xs[Fr-1]-xs[Fr-2], a3[Fr-1], a2[Fr-1], a1[Fr-1], a0[Fr-1], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2]);
+  Sinusoid(&xrecm[(int)xs[Fr-1]-dst], 0, den-xs[Fr-1], 0, 0, 0, a0[Fr-1], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], tmpph, add);
+  delete[] f3;
+  return xrecm;
+}//SynthesizeSinusoidP