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