Mercurial > hg > x

comparison SinSyn.cpp @ 1:6422640a802f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first upload
author Wen X <xue.wen@elec.qmul.ac.uk>
date Tue, 05 Oct 2010 10:45:57 +0100
parents
children
comparison
equal deleted inserted replaced
0:9b9f21935f24 1:6422640a802f
1 //---------------------------------------------------------------------------
2
3
4 #include "align8.h"
5 #include "SinSyn.h"
6 #include "splines.h"
7
8 //---------------------------------------------------------------------------
9 /*
10 function Sinuoid: original McAuley-Quatieri synthesizer interpolation between two measurement points.
11
12 In: T: length from measurement point 1 to measurement point 2
13 a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
14 a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
15 ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
16 Out: data[T]: output buffer
17 a[T], f[T], p[T]: resynthesized amplitude, frequency and phase
18
19 No return value.
20 */
21 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)
22 {
23 int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
24 double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
25 double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
26 double la=a1, da=(a2-a1)/T;
27 if (ad)
28 for (int t=0; t<T; t++)
29 {
30 double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
31 data[t]+=la*cos(lp);
32 a[t]=la;
33 p[t]=lp;
34 f[t]=lf;
35 la=la+da;
36 }
37 else
38 for (int t=0; t<T; t++)
39 {
40 double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
41 data[t]=la*cos(lp);
42 a[t]=la;
43 p[t]=lp;
44 f[t]=lf;
45 la=la+da;
46 }
47 }//Sinusoid
48
49 /*
50 function Sinuoid: original McAuley-Quatieri synthesizer interpolation between two measurement points,
51 without returning interpolated sinusoid parameters.
52
53 In: T: length from measurement point 1 to measurement point 2
54 a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
55 a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
56 ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
57 Out: data[T]: output buffer
58
59 No return value.
60 */
61 void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, bool ad)
62 {
63 int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
64 double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
65 double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
66 double la=a1, da=(a2-a1)/T;
67 if (ad)
68 for (int t=0; t<T; t++)
69 {
70 data[t]+=la*cos(pd+t*(pc+t*(pa+t*pb)));
71 la=la+da;
72 }
73 else
74 for (int t=0; t<T; t++)
75 {
76 data[t]=la*cos(pd+t*(pc+t*(pa+t*pb)));
77 la=la+da;
78 }
79 }//Sinusoid
80
81 //---------------------------------------------------------------------------
82 /*
83 function Sinusoid_direct: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
84 amplitude and frequency, direct implementation.
85
86 In: CountSt, CountEn
87 aa, ab, ac, ad: trinomial coefficients of amplitude
88 fa, fb, fc, fd: trinomial coefficients of frequency
89 p1: initial phase angle at 0 (NOT at CountSt)
90 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
91 Out: data[CountSt:CountEn-1]: output buffer.
92 p1: phase angle at CountEn
93
94 No return value.
95 */
96 void Sinusoid_direct(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
97 double fa, double fb, double fc, double fd, double &p1, bool add)
98 {
99 int i; double a, ph;
100 for (i=CountSt; i<CountEn; i++)
101 {
102 a=ad+i*(ac+i*(ab+i*aa));
103 ph=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
104 if (add) data[i]+=a*cos(ph);
105 else data[i]=a*cos(ph);
106 }
107 p1=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
108 }//Sinusoid
109
110 /*
111 function Sinusoid: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
112 amplitude and frequency, recursive implementation.
113
114 In: CountSt, CountEn
115 a3, a2, a1, a0: trinomial coefficients of amplitude
116 f3, f2, f1, f0: trinomial coefficients of frequency
117 ph: initial phase angle at 0 (NOT at CountSt)
118 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
119 Out: data[CountSt:CountEn-1]: output buffer.
120 ph: phase angle at CountEn
121
122 No return value. This function requires 8-byte stack alignment for optimal speed.
123 */
124 void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
125 double f3, double f2, double f1, double f0, double &ph, bool add)
126 {
127 int i;
128 double a, da, dda, ddda, dph, ddph, dddph, ddddph,
129 sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
130 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;
131 if (CountSt==0)
132 {
133 a=a0; da=a1+a2+a3; dda=2*a2+6*a3; ddda=6*a3;
134 dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
135 }
136 else
137 {
138 a=a0+CountSt*(a1+CountSt*(a2+CountSt*a3));
139 da=a1+a2+a3+CountSt*(2*a2+3*a3+CountSt*3*a3);
140 dda=2*a2+6*a3+CountSt*6*a3; ddda=6*a3;
141 ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
142 dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
143 ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
144 dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
145 }
146 sph=sin(ph), cph=cos(ph);
147 sdph=sin(dph), cdph=cos(dph);
148 sddph=sin(ddph), cddph=cos(ddph);
149 sdddph=sin(dddph), cdddph=cos(dddph);
150 sddddph=sin(ddddph), cddddph=cos(ddddph);
151 if (add)
152 {
153 for (i=CountSt; i<CountEn; i++)
154 {
155 data[i]+=a*cph;
156 a=a+da; da=da+dda; dda=dda+ddda;
157 tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
158 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
159 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
160 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
161 }
162 }
163 else
164 {
165 for (i=CountSt; i<CountEn; i++)
166 {
167 data[i]=a*cph;
168 a=a+da; da=da+dda; dda=dda+ddda;
169 tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
170 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
171 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
172 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
173 }
174 }
175 ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
176 }
177
178 /*
179 function SinusoidExp: synthesizes complex sinusoid whose derivative log amplitude and frequency are
180 trinomials
181
182 In: CountSt, CountEn
183 a3, a2, a1, a0: trinomial coefficients for the derivative of log amplitude
184 omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
185 ea, ph: initial log amplitude and phase angle at 0
186 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
187 Out: data[CountSt:CountEn-1]: output buffer.
188 ea, ph: log amplitude and phase angle at CountEn.
189
190 No return value.
191 */
192 void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
193 double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
194 {
195 double e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
196 p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
197 if (add) for (int i=CountSt; i<CountEn; i++)
198 {
199 double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
200 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
201 data[i]+=exp(cdouble(lea, lph));
202 }
203 else for (int i=CountSt; i<CountEn; i++)
204 {
205 double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
206 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
207 data[i]=exp(cdouble(lea, lph));
208 }
209 ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
210 ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
211 }//SinusoidExp
212
213 /*
214 function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
215 This version also synthesizes its derivative.
216
217 In: h[M][T], dih[M][T]: basis functions and their difference-integrals
218 lamda[M]: coefficients of h[M]
219 tmpexp: inital logarithm at 0
220 Out: s[T], ds[T]: synthesized sinusoid and its derivative
221 tmpexp: logarithm at T
222
223 No return value.
224 */
225 void SinusoidExp(int T, cdouble* s, cdouble* ds, int M, cdouble* lamda, double** h, double** dih, cdouble& tmpexp)
226 {
227 for (int t=0; t<T; t++)
228 {
229 s[t]=exp(tmpexp);
230 cdouble dexp=0, dR=0;
231 for (int m=0; m<M; m++) dexp+=lamda[m]*dih[m][t], dR+=lamda[m]*h[m][t];
232 tmpexp+=dexp;
233 ds[t]=s[t]*dR;
234 }
235 }//SinusoidExp
236
237 /*
238 function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
239 This version does not synthesize its derivative.
240
241 In: dih[M][T]: difference of integrals of basis functions h[M]
242 lamda[M]: coefficients of h[M]
243 tmpexp: inital logarithm at 0
244 Out: s[T]: synthesized sinusoid
245 tmpexp: logarithm at T
246
247 No return value.
248 */
249 void SinusoidExp(int T, cdouble* s, int M, cdouble* lamda, double** dih, cdouble& tmpexp)
250 {
251 for (int t=0; t<T; t++)
252 {
253 s[t]=exp(tmpexp);
254 cdouble dexp=0;
255 for (int m=0; m<M; m++) dexp+=lamda[m]*dih[m][t];
256 tmpexp+=dexp;
257 }
258 }//SinusoidExp
259
260 /*
261 function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials
262
263 In: CountSt, CountEn
264 a3, a2, a1, a0: trinomial coefficients for log amplitude
265 omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
266 ph: initial phase angle at 0
267 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
268 Out: data[CountSt:CountEn-1]: output buffer.
269 ph: phase angle at CountEn.
270
271 No return value.
272 */
273 void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
274 double omg3, double omg2, double omg1, double omg0, double &ph, bool add)
275 {
276 double p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
277 if (add) for (int i=CountSt; i<CountEn; i++)
278 {
279 double lea=a0+i*(a1+i*(a2+i*a3));
280 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
281 data[i]+=exp(cdouble(lea, lph));
282 }
283 else for (int i=CountSt; i<CountEn; i++)
284 {
285 double lea=a0+i*(a1+i*(a2+i*a3));
286 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
287 data[i]=exp(cdouble(lea, lph));
288 }
289 ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
290 }//SinusoidExpA
291
292 /*
293 function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials
294 with phase angle specified at both ends.
295
296 In: CountSt, CountEn
297 a3, a2, a1, a0: trinomial coefficients for log amplitude
298 omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
299 ph0, ph2: phase angles at 0 and CountEn.
300 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
301 Out: data[CountSt:CountEn-1]: output buffer.
302
303 No return value.
304 */
305 void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
306 double omg3, double omg2, double omg1, double omg0, double ph0, double ph2, bool add)
307 {
308 double p0=ph0, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
309 double pend=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
310
311 int k=floor((pend-ph2)/2/M_PI+0.5);
312 double d=ph2-pend+2*M_PI*k;
313 double _p=-2*d/CountEn/CountEn/CountEn;
314 double _q=3*d/CountEn/CountEn;
315
316 if (add) for (int i=CountSt; i<CountEn; i++)
317 {
318 double lea=a0+i*(a1+i*(a2+i*a3));
319 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
320 data[i]+=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
321 }
322 else for (int i=CountSt; i<CountEn; i++)
323 {
324 double lea=a0+i*(a1+i*(a2+i*a3));
325 double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
326 data[i]=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
327 }
328 }//SinusoidExpA
329
330 /*
331 function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
332 frequency is h[M]'q[M]. This version also synthesizes its derivative.
333
334 In: h[M][T], dh[M][T], dih[M][T]: basis functions and their derivatives and difference-integrals
335 p[M], q[M]: real and imaginary parts of coefficients of h[M]
336 tmpph: inital phase angle at 0
337 Out: s[T], ds[T]: synthesized sinusoid and its derivative
338 tmpph: phase angle at T
339
340 No return value.
341 */
342 void SinusoidExpA(int T, cdouble* s, cdouble* ds, int M, double* p, double* q, double** h, double** dh, double** dih, double& tmpph)
343 {
344 for (int t=0; t<T; t++)
345 {
346 double e=0, dph=0, drr=0, dri=0;
347 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];
348 s[t]=exp(cdouble(e, tmpph));
349 ds[t]=s[t]*cdouble(drr, dri);
350 tmpph+=dph;
351 }
352 }//SinusoidExpA
353
354 /*
355 function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
356 frequency is h[M]'q[M]. This version does not synthesize its derivative.
357
358 In: h[M][T], dih[M][T]: basis functions and their difference-integrals
359 p[M], q[M]: real and imaginary parts of coefficients of h[M]
360 tmpph: inital phase angle at 0
361 Out: s[T]: synthesized sinusoid
362 tmpph: phase angle at T
363
364 No return value.
365 */
366 void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double& tmpph)
367 {
368 for (int t=0; t<T; t++)
369 {
370 double e=0, dph=0;
371 for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
372 s[t]=exp(cdouble(e, tmpph));
373 tmpph+=dph;
374 }
375 }//SinusoidExpA
376
377 /*
378 function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
379 frequency is h[M]'q[M] with phase angle specified at both ends. This version does not synthesize its
380 derivative.
381
382 In: h[M][T], dih[M][T]: basis functions and their difference-integrals
383 p[M], q[M]: real and imaginary parts of coefficients of h[M]
384 ph1, ph2: phase angles at 0 and T.
385 Out: s[T]: synthesized sinusoid
386
387 No return value.
388 */
389 void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double ph1, double ph2)
390 {
391 double pend=ph1;
392 for (int t=0; t<T; t++)
393 {
394 double dph=0;
395 for (int m=0; m<M; m++) dph+=q[m]*dih[m][t];
396 pend+=dph;
397 }
398
399 int k=floor((pend-ph2)/2/M_PI+0.5);
400 double d=ph2-pend+2*M_PI*k;
401 double _p=-2*d/T/T/T;
402 double _q=3*d/T/T;
403
404 double ph=ph1;
405 for (int t=0; t<T; t++)
406 {
407 double e=0, dph=0;
408 for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
409 if (e>300) e=300;
410 if (e<-300) e=-300;
411 s[t]=exp(cdouble(e, ph+(t*t*(_q+t*_p))));
412 ph+=dph;
413 }
414 }//SinusoidExpA
415
416 /*
417 //This is not used any longer as the recursion does not seem to help saving computation with all its overheads.
418 void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
419 double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
420 {
421 int i;
422 double dea, ddea, dddea, ddddea,
423 dph, ddph, dddph, ddddph,
424 sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
425 e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
426 p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4, tmp;
427 if (CountSt==0)
428 {
429 dea=e1+e2+e3+e4; ddea=2*e2+6*e3+14*e4; dddea=6*e3+36*e4; ddddea=24*e3;
430 dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
431 }
432 else
433 {
434 ea=e0+CountSt*(e1+CountSt*(e2+CountSt*(e3+CountSt*e4)));
435 dea=e1+e2+e3+e4+CountSt*(2*e2+3*e3+4*e4+CountSt*(3*e3+6*e4+CountSt*4*e4));
436 ddea=2*e2+6*e3+14*e4+CountSt*(6*e3+24*e4+CountSt*12*e4);
437 dddea=6*e3+36*e4+CountSt*24*e4; ddddea=24*e4;
438 ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
439 dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
440 ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
441 dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
442 }
443 sph=sin(ph), cph=cos(ph);
444 sdph=sin(dph), cdph=cos(dph);
445 sddph=sin(ddph), cddph=cos(ddph);
446 sdddph=sin(dddph), cdddph=cos(dddph);
447 sddddph=sin(ddddph), cddddph=cos(ddddph);
448 if (add)
449 {
450 for (i=CountSt; i<CountEn; i++)
451 {
452 data[i]+=exp(ea)*cdouble(cph, sph);
453 ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
454 tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
455 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
456 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
457 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
458 }
459 }
460 else
461 {
462 for (i=CountSt; i<CountEn; i++)
463 {
464 data[i]=exp(ea)*cdouble(cph, sph);
465 ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
466 tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
467 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
468 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
469 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
470 }
471 }
472 ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
473 ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
474 } //*/
475
476 /*
477 function Sinusoid: recursive cos-sin generator with trinomial frequency
478
479 In: CountSt, CountEn
480 f3, f2, f1, f0: trinomial coefficients of frequency
481 ph: initial phase angle at 0 (NOT at CountSt)
482 Out: datar[CountSt:CountEn-1], datai[CountSt:CountEn-1]: synthesized pair of cosine and sine functions
483 ph: phase angle at CountEn
484
485 No return value.
486 */
487 void Sinusoid(double* datar, double* datai, int CountSt, int CountEn, double f3, double f2, double f1, double f0, double &ph)
488 {
489 int i;
490 double dph, ddph, dddph, ddddph,
491 sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
492 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;
493 if (CountSt==0)
494 {
495 dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
496 }
497 else
498 {
499 ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
500 dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
501 ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
502 dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
503 }
504 sph=sin(ph), cph=cos(ph);
505 sdph=sin(dph), cdph=cos(dph);
506 sddph=sin(ddph), cddph=cos(ddph);
507 sdddph=sin(dddph), cdddph=cos(dddph);
508 sddddph=sin(ddddph), cddddph=cos(ddddph);
509
510 for (i=CountSt; i<CountEn; i++)
511 {
512 datar[i]=cph; datai[i]=sph;
513 tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
514 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
515 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
516 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
517 }
518 ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
519 }//Sinusoid*/
520
521 /*
522 function Sinusoids: recursive harmonic multi-sinusoid generator
523
524 In: st, en
525 M: number of partials
526 a3[M], a2[M], a1[M], a0[M]: trinomial coefficients for partial amplitudes
527 f3, f2, f1, f0: trinomial coefficients for fundamental frequency
528 ph[M]: partial phases at 0.
529 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
530 Out: data[st:en-1]: output buffer.
531 ph[M]: partial phases at en.
532
533 No return value.
534 */
535 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)
536 {
537 double dph, ddph, dddph, ddddph;
538 double sdph, cdph, cdph2, sddph, cddph, sdddph, cdddph, sddddph, cddddph, sdmph, cdmph, sdm_1ph, cdm_1ph;
539 double p0, p1, p2, p3, p4, tmp, tmp2;
540 double *a=(double*)malloc8(sizeof(double)*M*6), *da=&a[M], *dda=&a[M*2], *ddda=&a[M*3],
541 *sph=&a[M*4], *cph=&a[M*5];
542
543 for (int m=0; m<M; m++)
544 {
545 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;
546 if (st==0)
547 {
548 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];
549 }
550 else
551 {
552 a[m]=a0[m]+st*(a1[m]+st*(a2[m]+st*a3[m]));
553 da[m]=a1[m]+a2[m]+a3[m]+st*(2*a2[m]+3*a3[m]+st*3*a3[m]);
554 dda[m]=2*a2[m]+6*a3[m]+st*6*a3[m]; ddda[m]=6*a3[m];
555 ph[m]=p0+st*(p1+st*(p2+st*(p3+st*p4)));
556 }
557 sph[m]=sin(ph[m]), cph[m]=cos(ph[m]);
558 ph[m]=p0+(m+1)*en*(p1+en*(p2+en*(p3+en*p4)));
559 }
560
561 if (st==0)
562 {
563 dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
564 }
565 else
566 {
567 dph=p1+p2+p3+p4+st*(2*p2+3*p3+4*p4+st*(3*p3+6*p4+st*4*p4));
568 ddph=2*p2+6*p3+14*p4+st*(6*p3+24*p4+st*12*p4);
569 dddph=6*p3+36*p4+st*24*p4; ddddph=24*p4;
570 }
571 sdph=sin(dph), cdph=cos(dph);
572 sddph=sin(ddph), cddph=cos(ddph);
573 sdddph=sin(dddph), cdddph=cos(dddph);
574 sddddph=sin(ddddph), cddddph=cos(ddddph);
575
576 if (add)
577 {
578 for (int i=st; i<en; i++)
579 {
580 data[i]+=a[0]*cph[0]; a[0]+=da[0]; da[0]+=dda[0]; dda[0]+=ddda[0];
581 tmp=cph[0]*cdph-sph[0]*sdph; sph[0]=sph[0]*cdph+cph[0]*sdph; cph[0]=tmp;
582 cdm_1ph=1, sdm_1ph=0, cdmph=cdph, sdmph=sdph, cdph2=2*cdph;
583
584 for (int m=1; m<M; m++)
585 {
586 data[i]+=a[m]*cph[m]; a[m]+=da[m]; da[m]+=dda[m]; dda[m]+=ddda[m];
587 // 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]}
588 tmp=cdmph, tmp2=sdmph;
589 cdmph=cdmph*cdph2-cdm_1ph; sdmph=sdmph*cdph2-sdm_1ph;
590 cdm_1ph=tmp, sdm_1ph=tmp2;
591
592 tmp=cph[m]*cdmph-sph[m]*sdmph; sph[m]=sph[m]*cdmph+cph[m]*sdmph; cph[m]=tmp;
593 // 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]}
594 }
595
596 tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
597 tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
598 tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
599 }
600 }
601 else
602 {
603 }
604 free8(a);
605 }//Sinusoids*/
606
607 /*
608 function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients.
609
610 In: CountSt, CountEn
611 aa, ab, ac, ad: trinomial coefficients of amplitude.
612 fa, fb, fc, fd: trinomial coefficients of frequency.
613 ph0, ph2: phase angles at 0 and CountEn.
614 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
615 Out: data[CountSt:CountEn-1]: output buffer.
616
617 No return value.
618 */
619 void Sinusoid(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
620 double fa, double fb, double fc, double fd, double ph0, double ph2, bool add)
621 {
622 double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
623 int k=floor((pend-ph2)/2/M_PI+0.5);
624 double d=ph2-pend+2*M_PI*k;
625 double p=-2*d/CountEn/CountEn/CountEn;
626 double q=3*d/CountEn/CountEn, a, ph;
627 for (int i=CountSt; i<CountEn; i++)
628 {
629 a=ad+i*(ac+i*(ab+i*aa)); if (a<0) a=0;
630 ph=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
631 if (add) data[i]+=a*cos(ph);
632 else data[i]=a*cos(ph);
633 }
634 }//Sinusoid
635
636 /*
637 function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients,
638 returning sinusoid coefficients instead of waveform.
639
640 In: CountSt, CountEn
641 aa, ab, ac, ad: trinomial coefficients of amplitude (or log amplitude if LogA=true)
642 fa, fb, fc, fd: trinomial coefficients of frequency.
643 ph0, ph2: phase angles at 0 and CountEn.
644 LogA: specifies whether log amplitude or amplitude is a trinomial
645 Out: f[CountSt:CountEn-1], a[CountSt:CountEn-1], ph[CountSt:CountEn-1]: synthesized sinusoid parameters
646 da[CountSt:CountEn-1]: derivative of synthesized amplitude, optional
647
648 No return value.
649 */
650 void Sinusoid(double* f, double* a, double* ph, double* da, int CountSt, int CountEn, double aa, double ab,
651 double ac, double ad, double fa, double fb, double fc, double fd, double ph0, double ph2, bool LogA)
652 {
653 double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
654 int k=floor((pend-ph2)/2/M_PI+0.5);
655 double d=ph2-pend+2*M_PI*k;
656 double p=-2*d/CountEn/CountEn/CountEn;
657 double q=3*d/CountEn/CountEn;
658 if (LogA) for (int i=CountSt; i<CountEn; i++)
659 {
660 a[i]=exp(ad+i*(ac+i*(ab+i*aa)));
661 if (da) da[i]=a[i]*(ac+i*(2*ab+i*3*aa));
662 f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
663 ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
664 }
665 else for (int i=CountSt; i<CountEn; i++)
666 {
667 a[i]=ad+i*(ac+i*(ab+i*aa));
668 if (da) da[i]=ac+i*(2*ab+i*3*aa);
669 f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
670 ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
671 }
672 }//Sinusoid
673
674 /*
675 function Sinusoid: generates trinomial frequency and phase with phase correction.
676
677 In: CountSt, CountEn
678 fa, fb, fc, fd: trinomial coefficients of frequency.
679 ph0, ph2: phase angles at 0 and CountEn.
680 Out: f[CountSt:CountEn-1], ph[CountSt:CountEn-1]: output buffers holding frequency and phase.
681
682 No return value.
683 */
684 void Sinusoid(double* f, double* ph, int CountSt, int CountEn, double fa, double fb,
685 double fc, double fd, double ph0, double ph2)
686 {
687 double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
688 int k=floor((pend-ph2)/2/M_PI+0.5);
689 double d=ph2-pend+2*M_PI*k;
690 double p=-2*d/CountEn/CountEn/CountEn;
691 double q=3*d/CountEn/CountEn;
692 for (int i=CountSt; i<CountEn; i++)
693 {
694 f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
695 ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
696 }
697 }//Sinusoid
698
699 /*
700 function SynthesizeSinusoid: synthesizes a time-varying sinusoid from a sequence of frequencies and amplitudes
701
702 In: xs[Fr]: measurement points, should be integers although *xs has double type.
703 fs[Fr], as[Fr]: sequence of frequencies and amplitudes at xs[Fr]
704 phs[0]: initial phase angle at (int)xs[0].
705 dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
706 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
707 Out: xrec[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.
708 phs[Fr]: phase angles at xs[Fr]
709
710 Returns pointer to xrec.
711 */
712 double* SynthesizeSinusoid(double* xrec, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add, bool* terminatetag)
713 {
714 double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
715 *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
716 CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
717 CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
718 double ph=phs[0];
719 for (int fr=0; fr<Fr-1; fr++)
720 {
721 phs[fr]=ph;
722 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);)
723 if (terminatetag && *terminatetag) {delete[] f3; return 0;}
724 }
725 phs[Fr-1]=ph;
726 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);
727 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);)
728 delete[] f3;
729 return xrec;
730 }//SynthesizeSinusoid
731
732 /*
733 function ShiftTrinomial: shifts the origin of a trinomial from 0 to T
734
735 In: a3, a2, a1, a0.
736 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
737
738 No return value.
739 */
740 void ShiftTrinomial(double T, double& b3, double& b2, double& b1, double& b0, double a3, double a2, double a1, double a0)
741 {
742 b3=a3;
743 b2=a2+T*3*b3;
744 b1=a1+T*(2*b2-T*3*b3);
745 b0=a0+T*(b1-T*(b2-T*b3));
746 }//ShiftTrinomial
747
748 /*
749 function SynthesizeSinusoidP: synthesizes a time-varying sinusoid from a sequence of frequencies,
750 amplitudes and phase angles
751
752 In: xs[Fr]: measurement points, should be integers although *xs has double type.
753 fs[Fr], as[Fr], phs[Fr]: sequence of frequencies, amplitudes and phase angles at xs[Fr]
754 dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
755 add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output
756 buffer
757 Out: xrecm[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.
758
759 Returns pointer to xrecm.
760 */
761 double* SynthesizeSinusoidP(double* xrecm, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add)
762 {
763 double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
764 *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
765 CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
766 CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
767 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);
768 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);
769 //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].
770 tmpph=phs[Fr-1];
771 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]);
772 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]);
773 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);
774 delete[] f3;
775 return xrecm;
776 }//SynthesizeSinusoidP