1 /*---------------------------------------------------------------------------*\
2 Original copyright
3 FILE........: AKSLSPD.C
4 TYPE........: Turbo C
5 COMPANY.....: Voicetronix
6 AUTHOR......: David Rowe
7 DATE CREATED: 24/2/93
8
9 Heavily modified by Jean-Marc Valin (fixed-point, optimizations,
10 additional functions, ...)
11
12 This file contains functions for converting Linear Prediction
13 Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the
14 LSP coefficients are not in radians format but in the x domain of the
15 unit circle.
16
17 Speex License:
18
19 Redistribution and use in source and binary forms, with or without
20 modification, are permitted provided that the following conditions
21 are met:
22
23 - Redistributions of source code must retain the above copyright
24 notice, this list of conditions and the following disclaimer.
25
26 - Redistributions in binary form must reproduce the above copyright
27 notice, this list of conditions and the following disclaimer in the
28 documentation and/or other materials provided with the distribution.
29
30 - Neither the name of the Xiph.org Foundation nor the names of its
31 contributors may be used to endorse or promote products derived from
32 this software without specific prior written permission.
33
34 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35 ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
37 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR
38 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
39 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
40 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
41 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
42 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
43 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
44 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
45 */
46
47 #ifdef HAVE_CONFIG_H
48 #include "config.h"
49 #endif
50
51 #include <math.h>
52 #include "lsp.h"
53 #include "stack_alloc.h"
54 #include "math_approx.h"
55
56 #ifndef M_PI
57 #define M_PI 3.14159265358979323846 /* pi */
58 #endif
59
60 #ifndef NULL
61 #define NULL 0
62 #endif
63
64 #ifdef FIXED_POINT
65
66
67
68 #define FREQ_SCALE 16384
69
70 /*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
71 #define ANGLE2X(a) (SHL16(spx_cos(a),2))
72
73 /*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
74 #define X2ANGLE(x) (spx_acos(x))
75
76 #else
77
78 /*#define C1 0.99940307
79 #define C2 -0.49558072
80 #define C3 0.03679168*/
81
82 #define FREQ_SCALE 1.
83 #define ANGLE2X(a) (spx_cos(a))
84 #define X2ANGLE(x) (acos(x))
85
86 #endif
87
88
89 /*---------------------------------------------------------------------------*\
90
91 FUNCTION....: cheb_poly_eva()
92
93 AUTHOR......: David Rowe
94 DATE CREATED: 24/2/93
95
96 This function evaluates a series of Chebyshev polynomials
97
98 \*---------------------------------------------------------------------------*/
99
100 #ifdef FIXED_POINT
101
cheb_poly_eva(spx_word32_t * coef,spx_word16_t x,int m,char * stack)102 static inline spx_word32_t cheb_poly_eva(spx_word32_t *coef,spx_word16_t x,int m,char *stack)
103 /* float coef[] coefficients of the polynomial to be evaluated */
104 /* float x the point where polynomial is to be evaluated */
105 /* int m order of the polynomial */
106 {
107 int i;
108 VARDECL(spx_word16_t *T);
109 spx_word32_t sum;
110 int m2=m>>1;
111 VARDECL(spx_word16_t *coefn);
112
113 /*Prevents overflows*/
114 if (x>16383)
115 x = 16383;
116 if (x<-16383)
117 x = -16383;
118
119 /* Allocate memory for Chebyshev series formulation */
120 ALLOC(T, m2+1, spx_word16_t);
121 ALLOC(coefn, m2+1, spx_word16_t);
122
123 for (i=0;i<m2+1;i++)
124 {
125 coefn[i] = coef[i];
126 /*printf ("%f ", coef[i]);*/
127 }
128 /*printf ("\n");*/
129
130 /* Initialise values */
131 T[0]=16384;
132 T[1]=x;
133
134 /* Evaluate Chebyshev series formulation using iterative approach */
135 /* Evaluate polynomial and return value also free memory space */
136 sum = ADD32(EXTEND32(coefn[m2]), EXTEND32(MULT16_16_P14(coefn[m2-1],x)));
137 /*x *= 2;*/
138 for(i=2;i<=m2;i++)
139 {
140 T[i] = SUB16(MULT16_16_Q13(x,T[i-1]), T[i-2]);
141 sum = ADD32(sum, EXTEND32(MULT16_16_P14(coefn[m2-i],T[i])));
142 /*printf ("%f ", sum);*/
143 }
144
145 /*printf ("\n");*/
146 return sum;
147 }
148 #else
cheb_poly_eva(spx_word32_t * coef,float x,int m,char * stack)149 static float cheb_poly_eva(spx_word32_t *coef,float x,int m,char *stack)
150 /* float coef[] coefficients of the polynomial to be evaluated */
151 /* float x the point where polynomial is to be evaluated */
152 /* int m order of the polynomial */
153 {
154 int i;
155 VARDECL(float *T);
156 float sum;
157 int m2=m>>1;
158
159 /* Allocate memory for Chebyshev series formulation */
160 ALLOC(T, m2+1, float);
161
162 /* Initialise values */
163 T[0]=1;
164 T[1]=x;
165
166 /* Evaluate Chebyshev series formulation using iterative approach */
167 /* Evaluate polynomial and return value also free memory space */
168 sum = coef[m2] + coef[m2-1]*x;
169 x *= 2;
170 for(i=2;i<=m2;i++)
171 {
172 T[i] = x*T[i-1] - T[i-2];
173 sum += coef[m2-i] * T[i];
174 }
175
176 return sum;
177 }
178 #endif
179
180 /*---------------------------------------------------------------------------*\
181
182 FUNCTION....: lpc_to_lsp()
183
184 AUTHOR......: David Rowe
185 DATE CREATED: 24/2/93
186
187 This function converts LPC coefficients to LSP
188 coefficients.
189
190 \*---------------------------------------------------------------------------*/
191
192 #ifdef FIXED_POINT
193 #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
194 #else
195 #define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
196 #endif
197
198
lpc_to_lsp(spx_coef_t * a,int lpcrdr,spx_lsp_t * freq,int nb,spx_word16_t delta,char * stack)199 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
200 /* float *a lpc coefficients */
201 /* int lpcrdr order of LPC coefficients (10) */
202 /* float *freq LSP frequencies in the x domain */
203 /* int nb number of sub-intervals (4) */
204 /* float delta grid spacing interval (0.02) */
205
206
207 {
208 spx_word16_t temp_xr,xl,xr,xm=0;
209 spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
210 int i,j,m,flag,k;
211 VARDECL(spx_word32_t *Q); /* ptrs for memory allocation */
212 VARDECL(spx_word32_t *P);
213 spx_word32_t *px; /* ptrs of respective P'(z) & Q'(z) */
214 spx_word32_t *qx;
215 spx_word32_t *p;
216 spx_word32_t *q;
217 spx_word32_t *pt; /* ptr used for cheb_poly_eval()
218 whether P' or Q' */
219 int roots=0; /* DR 8/2/94: number of roots found */
220 flag = 1; /* program is searching for a root when,
221 1 else has found one */
222 m = lpcrdr/2; /* order of P'(z) & Q'(z) polynomials */
223
224 /* Allocate memory space for polynomials */
225 ALLOC(Q, (m+1), spx_word32_t);
226 ALLOC(P, (m+1), spx_word32_t);
227
228 /* determine P'(z)'s and Q'(z)'s coefficients where
229 P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
230
231 px = P; /* initialise ptrs */
232 qx = Q;
233 p = px;
234 q = qx;
235
236 #ifdef FIXED_POINT
237 *px++ = LPC_SCALING;
238 *qx++ = LPC_SCALING;
239 for(i=0;i<m;i++){
240 *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
241 *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
242 }
243 px = P;
244 qx = Q;
245 for(i=0;i<m;i++)
246 {
247 /*if (fabs(*px)>=32768)
248 speex_warning_int("px", *px);
249 if (fabs(*qx)>=32768)
250 speex_warning_int("qx", *qx);*/
251 *px = PSHR32(*px,2);
252 *qx = PSHR32(*qx,2);
253 px++;
254 qx++;
255 }
256 /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
257 P[m] = PSHR32(P[m],3);
258 Q[m] = PSHR32(Q[m],3);
259 #else
260 *px++ = LPC_SCALING;
261 *qx++ = LPC_SCALING;
262 for(i=0;i<m;i++){
263 *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
264 *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
265 }
266 px = P;
267 qx = Q;
268 for(i=0;i<m;i++){
269 *px = 2**px;
270 *qx = 2**qx;
271 px++;
272 qx++;
273 }
274 #endif
275
276 px = P; /* re-initialise ptrs */
277 qx = Q;
278
279 /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
280 Keep alternating between the two polynomials as each zero is found */
281
282 xr = 0; /* initialise xr to zero */
283 xl = FREQ_SCALE; /* start at point xl = 1 */
284
285
286 for(j=0;j<lpcrdr;j++){
287 if(j&1) /* determines whether P' or Q' is eval. */
288 pt = qx;
289 else
290 pt = px;
291
292 psuml = cheb_poly_eva(pt,xl,lpcrdr,stack); /* evals poly. at xl */
293 flag = 1;
294 while(flag && (xr >= -FREQ_SCALE)){
295 spx_word16_t dd;
296 /* Modified by JMV to provide smaller steps around x=+-1 */
297 #ifdef FIXED_POINT
298 dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
299 if (psuml<512 && psuml>-512)
300 dd = PSHR16(dd,1);
301 #else
302 dd=delta*(1-.9*xl*xl);
303 if (fabs(psuml)<.2)
304 dd *= .5;
305 #endif
306 xr = SUB16(xl, dd); /* interval spacing */
307 psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x) */
308 temp_psumr = psumr;
309 temp_xr = xr;
310
311 /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
312 sign change.
313 if a sign change has occurred the interval is bisected and then
314 checked again for a sign change which determines in which
315 interval the zero lies in.
316 If there is no sign change between poly(xm) and poly(xl) set interval
317 between xm and xr else set interval between xl and xr and repeat till
318 root is located within the specified limits */
319
320 if(SIGN_CHANGE(psumr,psuml))
321 {
322 roots++;
323
324 psumm=psuml;
325 for(k=0;k<=nb;k++){
326 #ifdef FIXED_POINT
327 xm = ADD16(PSHR16(xl,1),PSHR16(xr,1)); /* bisect the interval */
328 #else
329 xm = .5*(xl+xr); /* bisect the interval */
330 #endif
331 psumm=cheb_poly_eva(pt,xm,lpcrdr,stack);
332 /*if(psumm*psuml>0.)*/
333 if(!SIGN_CHANGE(psumm,psuml))
334 {
335 psuml=psumm;
336 xl=xm;
337 } else {
338 psumr=psumm;
339 xr=xm;
340 }
341 }
342
343 /* once zero is found, reset initial interval to xr */
344 freq[j] = X2ANGLE(xm);
345 xl = xm;
346 flag = 0; /* reset flag for next search */
347 }
348 else{
349 psuml=temp_psumr;
350 xl=temp_xr;
351 }
352 }
353 }
354 return(roots);
355 }
356
357
358 /*---------------------------------------------------------------------------*\
359
360 FUNCTION....: lsp_to_lpc()
361
362 AUTHOR......: David Rowe
363 DATE CREATED: 24/2/93
364
365 lsp_to_lpc: This function converts LSP coefficients to LPC
366 coefficients.
367
368 \*---------------------------------------------------------------------------*/
369
370 #ifdef FIXED_POINT
371
lsp_to_lpc(spx_lsp_t * freq,spx_coef_t * ak,int lpcrdr,char * stack)372 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
373 /* float *freq array of LSP frequencies in the x domain */
374 /* float *ak array of LPC coefficients */
375 /* int lpcrdr order of LPC coefficients */
376
377
378 {
379 int i,j;
380 spx_word32_t xout1,xout2,xin1,xin2;
381 VARDECL(spx_word32_t *Wp);
382 spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
383 VARDECL(spx_word16_t *freqn);
384 int m = lpcrdr>>1;
385
386 ALLOC(freqn, lpcrdr, spx_word16_t);
387 for (i=0;i<lpcrdr;i++)
388 freqn[i] = ANGLE2X(freq[i]);
389
390 ALLOC(Wp, 4*m+2, spx_word32_t);
391 pw = Wp;
392
393
394 /* initialise contents of array */
395
396 for(i=0;i<=4*m+1;i++){ /* set contents of buffer to 0 */
397 *pw++ = 0;
398 }
399
400 /* Set pointers up */
401
402 pw = Wp;
403 xin1 = 1048576;
404 xin2 = 1048576;
405
406 /* reconstruct P(z) and Q(z) by cascading second order
407 polynomials in form 1 - 2xz(-1) +z(-2), where x is the
408 LSP coefficient */
409
410 for(j=0;j<=lpcrdr;j++){
411 spx_word16_t *fr=freqn;
412 for(i=0;i<m;i++){
413 n1 = pw+(i<<2);
414 n2 = n1 + 1;
415 n3 = n2 + 1;
416 n4 = n3 + 1;
417 xout1 = ADD32(SUB32(xin1, MULT16_32_Q14(*fr,*n1)), *n2);
418 fr++;
419 xout2 = ADD32(SUB32(xin2, MULT16_32_Q14(*fr,*n3)), *n4);
420 fr++;
421 *n2 = *n1;
422 *n4 = *n3;
423 *n1 = xin1;
424 *n3 = xin2;
425 xin1 = xout1;
426 xin2 = xout2;
427 }
428 xout1 = xin1 + *(n4+1);
429 xout2 = xin2 - *(n4+2);
430 /* FIXME: perhaps apply bandwidth expansion in case of overflow? */
431 if (j>0)
432 {
433 if (xout1 + xout2>SHL32(EXTEND32(32766),8))
434 ak[j-1] = 32767;
435 else if (xout1 + xout2 < -SHL32(EXTEND32(32766),8))
436 ak[j-1] = -32767;
437 else
438 ak[j-1] = EXTRACT16(PSHR32(ADD32(xout1,xout2),8));
439 } else {/*speex_warning_int("ak[0] = ", EXTRACT16(PSHR32(ADD32(xout1,xout2),8)));*/}
440 *(n4+1) = xin1;
441 *(n4+2) = xin2;
442
443 xin1 = 0;
444 xin2 = 0;
445 }
446 }
447 #else
448
lsp_to_lpc(spx_lsp_t * freq,spx_coef_t * ak,int lpcrdr,char * stack)449 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
450 /* float *freq array of LSP frequencies in the x domain */
451 /* float *ak array of LPC coefficients */
452 /* int lpcrdr order of LPC coefficients */
453
454
455 {
456 int i,j;
457 float xout1,xout2,xin1,xin2;
458 VARDECL(float *Wp);
459 float *pw,*n1,*n2,*n3,*n4=NULL;
460 VARDECL(float *x_freq);
461 int m = lpcrdr>>1;
462
463 ALLOC(Wp, 4*m+2, float);
464 pw = Wp;
465
466 /* initialise contents of array */
467
468 for(i=0;i<=4*m+1;i++){ /* set contents of buffer to 0 */
469 *pw++ = 0.0;
470 }
471
472 /* Set pointers up */
473
474 pw = Wp;
475 xin1 = 1.0;
476 xin2 = 1.0;
477
478 ALLOC(x_freq, lpcrdr, float);
479 for (i=0;i<lpcrdr;i++)
480 x_freq[i] = ANGLE2X(freq[i]);
481
482 /* reconstruct P(z) and Q(z) by cascading second order
483 polynomials in form 1 - 2xz(-1) +z(-2), where x is the
484 LSP coefficient */
485
486 for(j=0;j<=lpcrdr;j++){
487 int i2=0;
488 for(i=0;i<m;i++,i2+=2){
489 n1 = pw+(i*4);
490 n2 = n1 + 1;
491 n3 = n2 + 1;
492 n4 = n3 + 1;
493 xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
494 xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
495 *n2 = *n1;
496 *n4 = *n3;
497 *n1 = xin1;
498 *n3 = xin2;
499 xin1 = xout1;
500 xin2 = xout2;
501 }
502 xout1 = xin1 + *(n4+1);
503 xout2 = xin2 - *(n4+2);
504 if (j>0)
505 ak[j-1] = (xout1 + xout2)*0.5f;
506 *(n4+1) = xin1;
507 *(n4+2) = xin2;
508
509 xin1 = 0.0;
510 xin2 = 0.0;
511 }
512
513 }
514 #endif
515
516
517 #ifdef FIXED_POINT
518
519 /*Makes sure the LSPs are stable*/
lsp_enforce_margin(spx_lsp_t * lsp,int len,spx_word16_t margin)520 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
521 {
522 int i;
523 spx_word16_t m = margin;
524 spx_word16_t m2 = 25736-margin;
525
526 if (lsp[0]<m)
527 lsp[0]=m;
528 if (lsp[len-1]>m2)
529 lsp[len-1]=m2;
530 for (i=1;i<len-1;i++)
531 {
532 if (lsp[i]<lsp[i-1]+m)
533 lsp[i]=lsp[i-1]+m;
534
535 if (lsp[i]>lsp[i+1]-m)
536 lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
537 }
538 }
539
540
lsp_interpolate(spx_lsp_t * old_lsp,spx_lsp_t * new_lsp,spx_lsp_t * interp_lsp,int len,int subframe,int nb_subframes)541 void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
542 {
543 int i;
544 spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
545 spx_word16_t tmp2 = 16384-tmp;
546 for (i=0;i<len;i++)
547 {
548 interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
549 }
550 }
551
552 #else
553
554 /*Makes sure the LSPs are stable*/
lsp_enforce_margin(spx_lsp_t * lsp,int len,spx_word16_t margin)555 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
556 {
557 int i;
558 if (lsp[0]<LSP_SCALING*margin)
559 lsp[0]=LSP_SCALING*margin;
560 if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
561 lsp[len-1]=LSP_SCALING*(M_PI-margin);
562 for (i=1;i<len-1;i++)
563 {
564 if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
565 lsp[i]=lsp[i-1]+LSP_SCALING*margin;
566
567 if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
568 lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
569 }
570 }
571
572
lsp_interpolate(spx_lsp_t * old_lsp,spx_lsp_t * new_lsp,spx_lsp_t * interp_lsp,int len,int subframe,int nb_subframes)573 void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
574 {
575 int i;
576 float tmp = (1.0f + subframe)/nb_subframes;
577 for (i=0;i<len;i++)
578 {
579 interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
580 }
581 }
582
583 #endif
584