applied symbian related config and casting diffs from Colin Ward
[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 #ifdef HAVE_CONFIG_H
47 #include "config.h"
48 #endif
49
50 #include <math.h>
51 #include "lsp.h"
52 #include "stack_alloc.h"
53 #include "math_approx.h"
54
55 #ifndef M_PI
56 #define M_PI           3.14159265358979323846  /* pi */
57 #endif
58
59 #ifndef NULL
60 #define NULL 0
61 #endif
62
63 #ifdef FIXED_POINT
64
65 #define C1 8192
66 #define C2 -4096
67 #define C3 340
68 #define C4 -10
69
70 static spx_word16_t spx_cos(spx_word16_t x)
71 {
72    spx_word16_t x2;
73
74    if (x<12868)
75    {
76       x2 = MULT16_16_P13(x,x);
77       return ADD32(C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
78    } else {
79       x = 25736-x;
80       x2 = MULT16_16_P13(x,x);
81       return SUB32(-C1, MULT16_16_P13(x2, ADD32(C2, MULT16_16_P13(x2, ADD32(C3, MULT16_16_P13(C4, x2))))));
82       /*return SUB32(-C1, MULT16_16_Q13(x2, ADD32(C2, MULT16_16_Q13(C3, x2))));*/
83    }
84 }
85
86
87 #define FREQ_SCALE 16384
88
89 /*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
90 #define ANGLE2X(a) (SHL(spx_cos(a),2))
91
92 /*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
93 #define X2ANGLE(x) (spx_acos(x))
94
95 #else
96
97 /*#define C1 0.99940307
98 #define C2 -0.49558072
99 #define C3 0.03679168*/
100
101 #define C1 0.9999932946
102 #define C2 -0.4999124376
103 #define C3 0.0414877472
104 #define C4 -0.0012712095
105
106
107 #define SPX_PI_2 1.5707963268
108 static inline spx_word16_t spx_cos(spx_word16_t x)
109 {
110    if (x<SPX_PI_2)
111    {
112       x *= x;
113       return C1 + x*(C2+x*(C3+C4*x));
114    } else {
115       x = M_PI-x;
116       x *= x;
117       return -(C1 + x*(C2+x*(C3+C4*x)));
118    }
119 }
120 #define FREQ_SCALE 1.
121 #define ANGLE2X(a) (spx_cos(a))
122 #define X2ANGLE(x) (acos(x))
123
124 #endif
125
126
127 /*---------------------------------------------------------------------------*\
128
129         FUNCTION....: cheb_poly_eva()
130
131         AUTHOR......: David Rowe
132         DATE CREATED: 24/2/93
133
134     This function evaluates a series of Chebyshev polynomials
135
136 \*---------------------------------------------------------------------------*/
137
138 #ifdef FIXED_POINT
139
140 static spx_word32_t cheb_poly_eva(spx_word32_t *coef,spx_word16_t x,int m,char *stack)
141 /*  float coef[]        coefficients of the polynomial to be evaluated  */
142 /*  float x             the point where polynomial is to be evaluated   */
143 /*  int m               order of the polynomial                         */
144 {
145     int i;
146     spx_word16_t *T;
147     spx_word32_t sum;
148     int m2=m>>1;
149     spx_word16_t *coefn;
150
151     /*Prevents overflows*/
152     if (x>16383)
153        x = 16383;
154     if (x<-16383)
155        x = -16383;
156
157     /* Allocate memory for Chebyshev series formulation */
158     T=PUSH(stack, m2+1, spx_word16_t);
159     coefn=PUSH(stack, m2+1, spx_word16_t);
160
161     for (i=0;i<m2+1;i++)
162     {
163        coefn[i] = coef[i];
164        /*printf ("%f ", coef[i]);*/
165     }
166     /*printf ("\n");*/
167
168     /* Initialise values */
169     T[0]=16384;
170     T[1]=x;
171
172     /* Evaluate Chebyshev series formulation using iterative approach  */
173     /* Evaluate polynomial and return value also free memory space */
174     sum = ADD32(coefn[m2], MULT16_16_P14(coefn[m2-1],x));
175     /*x *= 2;*/
176     for(i=2;i<=m2;i++)
177     {
178        T[i] = MULT16_16_Q13(x,T[i-1]) - T[i-2];
179        sum = ADD32(sum, MULT16_16_P14(coefn[m2-i],T[i]));
180        /*printf ("%f ", sum);*/
181     }
182     
183     /*printf ("\n");*/
184     return sum;
185 }
186 #else
187 static float cheb_poly_eva(spx_word32_t *coef,float x,int m,char *stack)
188 /*  float coef[]        coefficients of the polynomial to be evaluated  */
189 /*  float x             the point where polynomial is to be evaluated   */
190 /*  int m               order of the polynomial                         */
191 {
192     int i;
193     float *T,sum;
194     int m2=m>>1;
195
196     /* Allocate memory for Chebyshev series formulation */
197     T=PUSH(stack, m2+1, float);
198
199     /* Initialise values */
200     T[0]=1;
201     T[1]=x;
202
203     /* Evaluate Chebyshev series formulation using iterative approach  */
204     /* Evaluate polynomial and return value also free memory space */
205     sum = coef[m2] + coef[m2-1]*x;
206     x *= 2;
207     for(i=2;i<=m2;i++)
208     {
209        T[i] = x*T[i-1] - T[i-2];
210        sum += coef[m2-i] * T[i];
211     }
212     
213     return sum;
214 }
215 #endif
216
217 /*---------------------------------------------------------------------------*\
218
219         FUNCTION....: lpc_to_lsp()
220
221         AUTHOR......: David Rowe
222         DATE CREATED: 24/2/93
223
224     This function converts LPC coefficients to LSP
225     coefficients.
226
227 \*---------------------------------------------------------------------------*/
228
229 #ifdef FIXED_POINT
230 #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
231 #else
232 #define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
233 #endif
234
235
236 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
237 /*  float *a                    lpc coefficients                        */
238 /*  int lpcrdr                  order of LPC coefficients (10)          */
239 /*  float *freq                 LSP frequencies in the x domain         */
240 /*  int nb                      number of sub-intervals (4)             */
241 /*  float delta                 grid spacing interval (0.02)            */
242
243
244 {
245     spx_word16_t temp_xr,xl,xr,xm=0;
246     spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
247     int i,j,m,flag,k;
248     spx_word32_t *Q;                    /* ptrs for memory allocation           */
249     spx_word32_t *P;
250     spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */
251     spx_word32_t *qx;
252     spx_word32_t *p;
253     spx_word32_t *q;
254     spx_word32_t *pt;                   /* ptr used for cheb_poly_eval()
255                                 whether P' or Q'                        */
256     int roots=0;                /* DR 8/2/94: number of roots found     */
257     flag = 1;                   /*  program is searching for a root when,
258                                 1 else has found one                    */
259     m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynomials   */
260
261     /* Allocate memory space for polynomials */
262     Q = PUSH(stack, (m+1), spx_word32_t);
263     P = PUSH(stack, (m+1), spx_word32_t);
264
265     /* determine P'(z)'s and Q'(z)'s coefficients where
266       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
267
268     px = P;                      /* initialise ptrs                     */
269     qx = Q;
270     p = px;
271     q = qx;
272
273 #ifdef FIXED_POINT
274     *px++ = LPC_SCALING;
275     *qx++ = LPC_SCALING;
276     for(i=1;i<=m;i++){
277         *px++ = (a[i]+a[lpcrdr+1-i]) - *p++;
278         *qx++ = (a[i]-a[lpcrdr+1-i]) + *q++;
279     }
280     px = P;
281     qx = Q;
282     for(i=0;i<m;i++)
283     {
284        /*if (fabs(*px)>=32768)
285           speex_warning_int("px", *px);
286        if (fabs(*qx)>=32768)
287        speex_warning_int("qx", *qx);*/
288        *px = (2+*px)>>2;
289        *qx = (2+*qx)>>2;
290        px++;
291        qx++;
292     }
293     P[m] = PSHR(P[m],3);
294     Q[m] = PSHR(Q[m],3);
295 #else
296     *px++ = LPC_SCALING;
297     *qx++ = LPC_SCALING;
298     for(i=1;i<=m;i++){
299         *px++ = (a[i]+a[lpcrdr+1-i]) - *p++;
300         *qx++ = (a[i]-a[lpcrdr+1-i]) + *q++;
301     }
302     px = P;
303     qx = Q;
304     for(i=0;i<m;i++){
305         *px = 2**px;
306         *qx = 2**qx;
307          px++;
308          qx++;
309     }
310 #endif
311
312     px = P;                     /* re-initialise ptrs                   */
313     qx = Q;
314
315     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
316     Keep alternating between the two polynomials as each zero is found  */
317
318     xr = 0;                     /* initialise xr to zero                */
319     xl = FREQ_SCALE;                    /* start at point xl = 1                */
320
321
322     for(j=0;j<lpcrdr;j++){
323         if(j&1)                 /* determines whether P' or Q' is eval. */
324             pt = qx;
325         else
326             pt = px;
327
328         psuml = cheb_poly_eva(pt,xl,lpcrdr,stack);      /* evals poly. at xl    */
329         flag = 1;
330         while(flag && (xr >= -FREQ_SCALE)){
331            spx_word16_t dd;
332            /* Modified by JMV to provide smaller steps around x=+-1 */
333 #ifdef FIXED_POINT
334            dd = MULT16_16_Q15(delta,(FREQ_SCALE - MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
335            if (psuml<512 && psuml>-512)
336               dd = PSHR(dd,1);
337 #else
338            dd=delta*(1-.9*xl*xl);
339            if (fabs(psuml)<.2)
340               dd *= .5;
341 #endif
342            xr = xl - dd;                                /* interval spacing     */
343             psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x)       */
344             temp_psumr = psumr;
345             temp_xr = xr;
346
347     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
348     sign change.
349     if a sign change has occurred the interval is bisected and then
350     checked again for a sign change which determines in which
351     interval the zero lies in.
352     If there is no sign change between poly(xm) and poly(xl) set interval
353     between xm and xr else set interval between xl and xr and repeat till
354     root is located within the specified limits                         */
355
356             if(SIGN_CHANGE(psumr,psuml))
357             {
358                 roots++;
359
360                 psumm=psuml;
361                 for(k=0;k<=nb;k++){
362 #ifdef FIXED_POINT
363                     xm = ADD16(PSHR(xl,1),PSHR(xr,1));          /* bisect the interval  */
364 #else
365                     xm = .5*(xl+xr);            /* bisect the interval  */
366 #endif
367                     psumm=cheb_poly_eva(pt,xm,lpcrdr,stack);
368                     /*if(psumm*psuml>0.)*/
369                     if(!SIGN_CHANGE(psumm,psuml))
370                     {
371                         psuml=psumm;
372                         xl=xm;
373                     } else {
374                         psumr=psumm;
375                         xr=xm;
376                     }
377                 }
378
379                /* once zero is found, reset initial interval to xr      */
380                freq[j] = X2ANGLE(xm);
381                xl = xm;
382                flag = 0;                /* reset flag for next search   */
383             }
384             else{
385                 psuml=temp_psumr;
386                 xl=temp_xr;
387             }
388         }
389     }
390     return(roots);
391 }
392
393
394 /*---------------------------------------------------------------------------*\
395
396         FUNCTION....: lsp_to_lpc()
397
398         AUTHOR......: David Rowe
399         DATE CREATED: 24/2/93
400
401     lsp_to_lpc: This function converts LSP coefficients to LPC
402     coefficients.
403
404 \*---------------------------------------------------------------------------*/
405
406 #ifdef FIXED_POINT
407
408 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
409 /*  float *freq         array of LSP frequencies in the x domain        */
410 /*  float *ak           array of LPC coefficients                       */
411 /*  int lpcrdr          order of LPC coefficients                       */
412
413
414 {
415     int i,j;
416     spx_word32_t xout1,xout2,xin1,xin2;
417     spx_word32_t *Wp;
418     spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
419     spx_word16_t *freqn;
420     int m = lpcrdr>>1;
421     
422     freqn = PUSH(stack, lpcrdr, spx_word16_t);
423     for (i=0;i<lpcrdr;i++)
424        freqn[i] = ANGLE2X(freq[i]);
425
426     Wp = PUSH(stack, 4*m+2, spx_word32_t);
427     pw = Wp;
428
429
430     /* initialise contents of array */
431
432     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
433         *pw++ = 0.0;
434     }
435
436     /* Set pointers up */
437
438     pw = Wp;
439     xin1 = 1048576;
440     xin2 = 1048576;
441
442     /* reconstruct P(z) and Q(z) by  cascading second order
443       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
444       LSP coefficient */
445
446     for(j=0;j<=lpcrdr;j++){
447        int i2=0;
448         for(i=0;i<m;i++,i2+=2){
449             n1 = pw+(i*4);
450             n2 = n1 + 1;
451             n3 = n2 + 1;
452             n4 = n3 + 1;
453             xout1 = ADD32(SUB32(xin1, MULT16_32_Q14(freqn[i2],*n1)), *n2);
454             xout2 = ADD32(SUB32(xin2, MULT16_32_Q14(freqn[i2+1],*n3)), *n4);
455             *n2 = *n1;
456             *n4 = *n3;
457             *n1 = xin1;
458             *n3 = xin2;
459             xin1 = xout1;
460             xin2 = xout2;
461         }
462         xout1 = xin1 + *(n4+1);
463         xout2 = xin2 - *(n4+2);
464         /* FIXME: perhaps apply bandwidth expansion in case of overflow? */
465         if (xout1 + xout2>256*32766)
466            ak[j] = 32767;
467         else if (xout1 + xout2 < -256*32767)
468            ak[j] = -32768;
469         else
470            ak[j] = PSHR(ADD32(xout1,xout2),8);
471         *(n4+1) = xin1;
472         *(n4+2) = xin2;
473
474         xin1 = 0.0;
475         xin2 = 0.0;
476     }
477 }
478 #else
479
480 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
481 /*  float *freq         array of LSP frequencies in the x domain        */
482 /*  float *ak           array of LPC coefficients                       */
483 /*  int lpcrdr          order of LPC coefficients                       */
484
485
486 {
487     int i,j;
488     float xout1,xout2,xin1,xin2;
489     float *Wp;
490     float *pw,*n1,*n2,*n3,*n4=NULL;
491     float *x_freq;
492     int m = lpcrdr>>1;
493
494     Wp = PUSH(stack, 4*m+2, float);
495     pw = Wp;
496
497     /* initialise contents of array */
498
499     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
500         *pw++ = 0.0;
501     }
502
503     /* Set pointers up */
504
505     pw = Wp;
506     xin1 = 1.0;
507     xin2 = 1.0;
508
509     x_freq=PUSH(stack, lpcrdr, float);
510     for (i=0;i<lpcrdr;i++)
511        x_freq[i] = ANGLE2X(freq[i]);
512
513     /* reconstruct P(z) and Q(z) by  cascading second order
514       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
515       LSP coefficient */
516
517     for(j=0;j<=lpcrdr;j++){
518        int i2=0;
519         for(i=0;i<m;i++,i2+=2){
520             n1 = pw+(i*4);
521             n2 = n1 + 1;
522             n3 = n2 + 1;
523             n4 = n3 + 1;
524             xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
525             xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
526             *n2 = *n1;
527             *n4 = *n3;
528             *n1 = xin1;
529             *n3 = xin2;
530             xin1 = xout1;
531             xin2 = xout2;
532         }
533         xout1 = xin1 + *(n4+1);
534         xout2 = xin2 - *(n4+2);
535         ak[j] = (xout1 + xout2)*0.5f;
536         *(n4+1) = xin1;
537         *(n4+2) = xin2;
538
539         xin1 = 0.0;
540         xin2 = 0.0;
541     }
542
543 }
544 #endif
545
546
547 #ifdef FIXED_POINT
548
549 /*Makes sure the LSPs are stable*/
550 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
551 {
552    int i;
553    spx_word16_t m = margin;
554    spx_word16_t m2 = 25736-margin;
555   
556    if (lsp[0]<m)
557       lsp[0]=m;
558    if (lsp[len-1]>m2)
559       lsp[len-1]=m2;
560    for (i=1;i<len-1;i++)
561    {
562       if (lsp[i]<lsp[i-1]+m)
563          lsp[i]=lsp[i-1]+m;
564
565       if (lsp[i]>lsp[i+1]-m)
566          lsp[i]= SHR(lsp[i],1) + SHR(lsp[i+1]-m,1);
567    }
568 }
569
570
571 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)
572 {
573    int i;
574    spx_word16_t tmp = DIV32_16(SHL(1 + subframe,14),nb_subframes);
575    spx_word16_t tmp2 = 16384-tmp;
576    for (i=0;i<len;i++)
577    {
578       interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
579    }
580 }
581
582 #else
583
584 /*Makes sure the LSPs are stable*/
585 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
586 {
587    int i;
588    if (lsp[0]<LSP_SCALING*margin)
589       lsp[0]=LSP_SCALING*margin;
590    if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
591       lsp[len-1]=LSP_SCALING*(M_PI-margin);
592    for (i=1;i<len-1;i++)
593    {
594       if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
595          lsp[i]=lsp[i-1]+LSP_SCALING*margin;
596
597       if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
598          lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
599    }
600 }
601
602
603 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)
604 {
605    int i;
606    float tmp = (1.0f + subframe)/nb_subframes;
607    for (i=0;i<len;i++)
608    {
609       interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
610    }
611 }
612
613 #endif