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