fixed-point: LSP quantization work, also LSP's are now in the angle domain
[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
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 /*---------------------------------------------------------------------------*\
60
61         FUNCTION....: cheb_poly_eva()
62
63         AUTHOR......: David Rowe
64         DATE CREATED: 24/2/93
65
66     This function evaluates a series of Chebyshev polynomials
67
68 \*---------------------------------------------------------------------------*/
69
70 #ifdef FIXED_POINT
71
72 static float cheb_poly_eva(spx_word32_t *coef,float x,int m,char *stack)
73 /*  float coef[]        coefficients of the polynomial to be evaluated  */
74 /*  float x             the point where polynomial is to be evaluated   */
75 /*  int m               order of the polynomial                         */
76 {
77     int i;
78     spx_word16_t *T;
79     spx_word32_t sum;
80     int m2=m>>1;
81     spx_word16_t xn;
82     spx_word16_t *coefn;
83
84     /* Allocate memory for Chebyshev series formulation */
85     T=PUSH(stack, m2+1, spx_word16_t);
86     coefn=PUSH(stack, m2+1, spx_word16_t);
87
88     xn = floor(.5+x*16384);
89     
90     for (i=0;i<m2+1;i++)
91     {
92        coefn[i] = coef[i];
93        /*printf ("%f ", coef[i]);*/
94     }
95     /*printf ("\n");*/
96
97     /* Initialise values */
98     T[0]=16384;
99     T[1]=xn;
100
101     /* Evaluate Chebyshev series formulation using iterative approach  */
102     /* Evaluate polynomial and return value also free memory space */
103     sum = coefn[m2] + MULT16_16_P14(coefn[m2-1],xn);
104     /*x *= 2;*/
105     for(i=2;i<=m2;i++)
106     {
107        T[i] = MULT16_16_Q13(xn,T[i-1]) - T[i-2];
108        sum += MULT16_16_P14(coefn[m2-i],T[i]);
109        /*printf ("%f ", sum);*/
110     }
111     
112     /*printf ("\n");*/
113     return sum;
114 }
115 #else
116 static float cheb_poly_eva(spx_word32_t *coef,float x,int m,char *stack)
117 /*  float coef[]        coefficients of the polynomial to be evaluated  */
118 /*  float x             the point where polynomial is to be evaluated   */
119 /*  int m               order of the polynomial                         */
120 {
121     int i;
122     float *T,sum;
123     int m2=m>>1;
124
125     /* Allocate memory for Chebyshev series formulation */
126     T=PUSH(stack, m2+1, float);
127
128     /* Initialise values */
129     T[0]=1;
130     T[1]=x;
131
132     /* Evaluate Chebyshev series formulation using iterative approach  */
133     /* Evaluate polynomial and return value also free memory space */
134     sum = coef[m2] + coef[m2-1]*x;
135     x *= 2;
136     for(i=2;i<=m2;i++)
137     {
138        T[i] = x*T[i-1] - T[i-2];
139        sum += coef[m2-i] * T[i];
140     }
141     
142     return sum;
143 }
144 #endif
145
146 /*---------------------------------------------------------------------------*\
147
148         FUNCTION....: lpc_to_lsp()
149
150         AUTHOR......: David Rowe
151         DATE CREATED: 24/2/93
152
153     This function converts LPC coefficients to LSP
154     coefficients.
155
156 \*---------------------------------------------------------------------------*/
157
158
159 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,float *freq,int nb,float delta, char *stack)
160 /*  float *a                    lpc coefficients                        */
161 /*  int lpcrdr                  order of LPC coefficients (10)          */
162 /*  float *freq                 LSP frequencies in the x domain         */
163 /*  int nb                      number of sub-intervals (4)             */
164 /*  float delta                 grid spacing interval (0.02)            */
165
166
167 {
168
169     float psuml,psumr,psumm,temp_xr,xl,xr,xm=0;
170     float temp_psumr/*,temp_qsumr*/;
171     int i,j,m,flag,k;
172     spx_word32_t *Q;                    /* ptrs for memory allocation           */
173     spx_word32_t *P;
174     spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */
175     spx_word32_t *qx;
176     spx_word32_t *p;
177     spx_word32_t *q;
178     spx_word32_t *pt;                   /* ptr used for cheb_poly_eval()
179                                 whether P' or Q'                        */
180     int roots=0;                /* DR 8/2/94: number of roots found     */
181     flag = 1;                   /*  program is searching for a root when,
182                                 1 else has found one                    */
183     m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynomials   */
184
185
186     /* Allocate memory space for polynomials */
187     Q = PUSH(stack, (m+1), spx_word32_t);
188     P = PUSH(stack, (m+1), spx_word32_t);
189
190     /* determine P'(z)'s and Q'(z)'s coefficients where
191       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
192
193     px = P;                      /* initialise ptrs                     */
194     qx = Q;
195     p = px;
196     q = qx;
197
198 #ifdef FIXED_POINT
199     *px++ = LPC_SCALING;
200     *qx++ = LPC_SCALING;
201     for(i=1;i<=m;i++){
202         *px++ = (a[i]+a[lpcrdr+1-i]) - *p++;
203         *qx++ = (a[i]-a[lpcrdr+1-i]) + *q++;
204     }
205     px = P;
206     qx = Q;
207     for(i=0;i<m;i++)
208     {
209        /*if (fabs(*px)>=32768)
210           speex_warning_int("px", *px);
211        if (fabs(*qx)>=32768)
212        speex_warning_int("qx", *qx);*/
213        *px = (2+*px)>>2;
214        *qx = (2+*qx)>>2;
215        px++;
216        qx++;
217     }
218     P[m] = (4+P[m])>>3;
219     Q[m] = (4+Q[m])>>3;
220 #else
221     *px++ = LPC_SCALING;
222     *qx++ = LPC_SCALING;
223     for(i=1;i<=m;i++){
224         *px++ = (a[i]+a[lpcrdr+1-i]) - *p++;
225         *qx++ = (a[i]-a[lpcrdr+1-i]) + *q++;
226     }
227     px = P;
228     qx = Q;
229     for(i=0;i<m;i++){
230         *px = 2**px;
231         *qx = 2**qx;
232          px++;
233          qx++;
234     }
235 #endif
236
237     px = P;                     /* re-initialise ptrs                   */
238     qx = Q;
239
240     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
241     Keep alternating between the two polynomials as each zero is found  */
242
243     xr = 0;                     /* initialise xr to zero                */
244     xl = 1.0;                   /* start at point xl = 1                */
245
246
247     for(j=0;j<lpcrdr;j++){
248         if(j%2)                 /* determines whether P' or Q' is eval. */
249             pt = qx;
250         else
251             pt = px;
252
253         psuml = cheb_poly_eva(pt,xl,lpcrdr,stack);      /* evals poly. at xl    */
254         flag = 1;
255         while(flag && (xr >= -1.0)){
256            float dd;
257            /* Modified by JMV to provide smaller steps around x=+-1 */
258            dd=(delta*(1-.9*xl*xl));
259            if (fabs(psuml)<.2)
260               dd *= .5;
261
262            xr = xl - dd;                                /* interval spacing     */
263             psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x)       */
264             temp_psumr = psumr;
265             temp_xr = xr;
266
267     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
268     sign change.
269     if a sign change has occurred the interval is bisected and then
270     checked again for a sign change which determines in which
271     interval the zero lies in.
272     If there is no sign change between poly(xm) and poly(xl) set interval
273     between xm and xr else set interval between xl and xr and repeat till
274     root is located within the specified limits                         */
275
276             if((psumr*psuml)<0.0){
277                 roots++;
278
279                 psumm=psuml;
280                 for(k=0;k<=nb;k++){
281                     xm = (xl+xr)/2;             /* bisect the interval  */
282                     psumm=cheb_poly_eva(pt,xm,lpcrdr,stack);
283                     if(psumm*psuml>0.){
284                         psuml=psumm;
285                         xl=xm;
286                     }
287                     else{
288                         psumr=psumm;
289                         xr=xm;
290                     }
291                 }
292
293                /* once zero is found, reset initial interval to xr      */
294                freq[j] = acos(xm);
295                xl = xm;
296                flag = 0;                /* reset flag for next search   */
297             }
298             else{
299                 psuml=temp_psumr;
300                 xl=temp_xr;
301             }
302         }
303     }
304     return(roots);
305 }
306
307
308 /*---------------------------------------------------------------------------*\
309
310         FUNCTION....: lsp_to_lpc()
311
312         AUTHOR......: David Rowe
313         DATE CREATED: 24/2/93
314
315     lsp_to_lpc: This function converts LSP coefficients to LPC
316     coefficients.
317
318 \*---------------------------------------------------------------------------*/
319
320 #ifdef FIXED_POINT
321
322 void lsp_to_lpc(float *freq,spx_coef_t *ak,int lpcrdr, char *stack)
323 /*  float *freq         array of LSP frequencies in the x domain        */
324 /*  float *ak           array of LPC coefficients                       */
325 /*  int lpcrdr          order of LPC coefficients                       */
326
327
328 {
329     int i,j;
330     spx_word32_t xout1,xout2,xin1,xin2;
331     spx_word32_t *Wp;
332     spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
333     spx_word16_t *freqn;
334     int m = lpcrdr/2;
335     
336     freqn = PUSH(stack, lpcrdr, spx_word16_t);
337     for (i=0;i<lpcrdr;i++)
338        freqn[i] = cos(freq[i])*32768.;
339
340     Wp = PUSH(stack, 4*m+2, spx_word32_t);
341     pw = Wp;
342
343
344     /* initialise contents of array */
345
346     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
347         *pw++ = 0.0;
348     }
349
350     /* Set pointers up */
351
352     pw = Wp;
353     xin1 = 1048576;
354     xin2 = 1048576;
355
356     /* reconstruct P(z) and Q(z) by  cascading second order
357       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
358       LSP coefficient */
359
360     for(j=0;j<=lpcrdr;j++){
361        int i2=0;
362         for(i=0;i<m;i++,i2+=2){
363             n1 = pw+(i*4);
364             n2 = n1 + 1;
365             n3 = n2 + 1;
366             n4 = n3 + 1;
367             xout1 = xin1 - MULT16_32_Q14(freqn[i2],*n1) + *n2;
368             xout2 = xin2 - MULT16_32_Q14(freqn[i2+1],*n3) + *n4;
369             *n2 = *n1;
370             *n4 = *n3;
371             *n1 = xin1;
372             *n3 = xin2;
373             xin1 = xout1;
374             xin2 = xout2;
375         }
376         xout1 = xin1 + *(n4+1);
377         xout2 = xin2 - *(n4+2);
378         /* FIXME: perhaps apply bandwidth expansion in case of overflow? */
379         if (xout1 + xout2>256*32766)
380            ak[j] = 32767;
381         else if (xout1 + xout2 < -256*32767)
382            ak[j] = -32768;
383         else
384            ak[j] = ((128+xout1 + xout2)>>8);
385         *(n4+1) = xin1;
386         *(n4+2) = xin2;
387
388         xin1 = 0.0;
389         xin2 = 0.0;
390     }
391 }
392 #else
393
394 void lsp_to_lpc(float *freq,spx_coef_t *ak,int lpcrdr, char *stack)
395 /*  float *freq         array of LSP frequencies in the x domain        */
396 /*  float *ak           array of LPC coefficients                       */
397 /*  int lpcrdr          order of LPC coefficients                       */
398
399
400 {
401     int i,j;
402     float xout1,xout2,xin1,xin2;
403     float *Wp;
404     float *pw,*n1,*n2,*n3,*n4=NULL;
405     int m = lpcrdr/2;
406
407     Wp = PUSH(stack, 4*m+2, float);
408     pw = Wp;
409
410     /* initialise contents of array */
411
412     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
413         *pw++ = 0.0;
414     }
415
416     /* Set pointers up */
417
418     pw = Wp;
419     xin1 = 1.0;
420     xin2 = 1.0;
421
422     /* reconstruct P(z) and Q(z) by  cascading second order
423       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
424       LSP coefficient */
425
426     for(j=0;j<=lpcrdr;j++){
427        int i2=0;
428         for(i=0;i<m;i++,i2+=2){
429             n1 = pw+(i*4);
430             n2 = n1 + 1;
431             n3 = n2 + 1;
432             n4 = n3 + 1;
433             xout1 = xin1 - 2*(cos(freq[i2])) * *n1 + *n2;
434             xout2 = xin2 - 2*(cos(freq[i2+1])) * *n3 + *n4;
435             *n2 = *n1;
436             *n4 = *n3;
437             *n1 = xin1;
438             *n3 = xin2;
439             xin1 = xout1;
440             xin2 = xout2;
441         }
442         xout1 = xin1 + *(n4+1);
443         xout2 = xin2 - *(n4+2);
444         ak[j] = (xout1 + xout2)*0.5;
445         *(n4+1) = xin1;
446         *(n4+2) = xin2;
447
448         xin1 = 0.0;
449         xin2 = 0.0;
450     }
451
452 }
453 #endif
454
455 /*Added by JMV
456   Makes sure the LSPs are stable*/
457 void lsp_enforce_margin(float *lsp, int len, float margin)
458 {
459    int i;
460    if (lsp[0]<margin)
461       lsp[0]=margin;
462    if (lsp[len-1]>M_PI-margin)
463       lsp[len-1]=M_PI-margin;
464    for (i=1;i<len-1;i++)
465    {
466       if (lsp[i]<lsp[i-1]+margin)
467          lsp[i]=lsp[i-1]+margin;
468
469       if (lsp[i]>lsp[i+1]-margin)
470          lsp[i]= .5* (lsp[i] + lsp[i+1]-margin);
471    }
472 }