3785a726044315d451940a6eb9980aa9b26e8fec
[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(float *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] = floor(.5+1024*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(float *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     float *Q;                   /* ptrs for memory allocation           */
173     float *P;
174     float *px;                  /* ptrs of respective P'(z) & Q'(z)     */
175     float *qx;
176     float *p;
177     float *q;
178     float *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), float);
188     P = PUSH(stack, (m+1), float);
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     *px++ = 1.0;
198     *qx++ = 1.0;
199     for(i=1;i<=m;i++){
200         *px++ = (a[i]+a[lpcrdr+1-i])/8192.-*p++;
201         *qx++ = (a[i]-a[lpcrdr+1-i])/8192.+*q++;
202     }
203     px = P;
204     qx = Q;
205     for(i=0;i<m;i++){
206         *px = 2**px;
207         *qx = 2**qx;
208          px++;
209          qx++;
210     }
211     px = P;                     /* re-initialise ptrs                   */
212     qx = Q;
213
214     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
215     Keep alternating between the two polynomials as each zero is found  */
216
217     xr = 0;                     /* initialise xr to zero                */
218     xl = 1.0;                   /* start at point xl = 1                */
219
220
221     for(j=0;j<lpcrdr;j++){
222         if(j%2)                 /* determines whether P' or Q' is eval. */
223             pt = qx;
224         else
225             pt = px;
226
227         psuml = cheb_poly_eva(pt,xl,lpcrdr,stack);      /* evals poly. at xl    */
228         flag = 1;
229         while(flag && (xr >= -1.0)){
230            float dd;
231            /* Modified by JMV to provide smaller steps around x=+-1 */
232            dd=(delta*(1-.9*xl*xl));
233            if (fabs(psuml)<.2)
234               dd *= .5;
235
236            xr = xl - dd;                                /* interval spacing     */
237             psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x)       */
238             temp_psumr = psumr;
239             temp_xr = xr;
240
241     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
242     sign change.
243     if a sign change has occurred the interval is bisected and then
244     checked again for a sign change which determines in which
245     interval the zero lies in.
246     If there is no sign change between poly(xm) and poly(xl) set interval
247     between xm and xr else set interval between xl and xr and repeat till
248     root is located within the specified limits                         */
249
250             if((psumr*psuml)<0.0){
251                 roots++;
252
253                 psumm=psuml;
254                 for(k=0;k<=nb;k++){
255                     xm = (xl+xr)/2;             /* bisect the interval  */
256                     psumm=cheb_poly_eva(pt,xm,lpcrdr,stack);
257                     if(psumm*psuml>0.){
258                         psuml=psumm;
259                         xl=xm;
260                     }
261                     else{
262                         psumr=psumm;
263                         xr=xm;
264                     }
265                 }
266
267                /* once zero is found, reset initial interval to xr      */
268                freq[j] = (xm);
269                xl = xm;
270                flag = 0;                /* reset flag for next search   */
271             }
272             else{
273                 psuml=temp_psumr;
274                 xl=temp_xr;
275             }
276         }
277     }
278     return(roots);
279 }
280
281
282 /*---------------------------------------------------------------------------*\
283
284         FUNCTION....: lsp_to_lpc()
285
286         AUTHOR......: David Rowe
287         DATE CREATED: 24/2/93
288
289     lsp_to_lpc: This function converts LSP coefficients to LPC
290     coefficients.
291
292 \*---------------------------------------------------------------------------*/
293
294 #ifdef FIXED_POINT
295
296 void lsp_to_lpc(float *freq,spx_coef_t *ak,int lpcrdr, char *stack)
297 /*  float *freq         array of LSP frequencies in the x domain        */
298 /*  float *ak           array of LPC coefficients                       */
299 /*  int lpcrdr          order of LPC coefficients                       */
300
301
302 {
303     int i,j;
304     spx_word32_t xout1,xout2,xin1,xin2;
305     spx_word32_t *Wp;
306     spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL;
307     spx_word16_t *freqn;
308     int m = lpcrdr/2;
309     
310     freqn = PUSH(stack, lpcrdr, spx_word16_t);
311     for (i=0;i<lpcrdr;i++)
312        freqn[i] = freq[i]*32768.;
313
314     Wp = PUSH(stack, 4*m+2, spx_word32_t);
315     pw = Wp;
316
317
318     /* initialise contents of array */
319
320     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
321         *pw++ = 0.0;
322     }
323
324     /* Set pointers up */
325
326     pw = Wp;
327     xin1 = 1048576;
328     xin2 = 1048576;
329
330     /* reconstruct P(z) and Q(z) by  cascading second order
331       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
332       LSP coefficient */
333
334     for(j=0;j<=lpcrdr;j++){
335        int i2=0;
336         for(i=0;i<m;i++,i2+=2){
337             n1 = pw+(i*4);
338             n2 = n1 + 1;
339             n3 = n2 + 1;
340             n4 = n3 + 1;
341             xout1 = xin1 - MULT16_32_Q14(freqn[i2],*n1) + *n2;
342             xout2 = xin2 - MULT16_32_Q14(freqn[i2+1],*n3) + *n4;
343             *n2 = *n1;
344             *n4 = *n3;
345             *n1 = xin1;
346             *n3 = xin2;
347             xin1 = xout1;
348             xin2 = xout2;
349         }
350         xout1 = xin1 + *(n4+1);
351         xout2 = xin2 - *(n4+2);
352         ak[j] = ((128+xout1 + xout2)>>8);
353         *(n4+1) = xin1;
354         *(n4+2) = xin2;
355
356         xin1 = 0.0;
357         xin2 = 0.0;
358     }
359 }
360 #else
361
362 void lsp_to_lpc(float *freq,spx_coef_t *ak,int lpcrdr, char *stack)
363 /*  float *freq         array of LSP frequencies in the x domain        */
364 /*  float *ak           array of LPC coefficients                       */
365 /*  int lpcrdr          order of LPC coefficients                       */
366
367
368 {
369     int i,j;
370     float xout1,xout2,xin1,xin2;
371     float *Wp;
372     float *pw,*n1,*n2,*n3,*n4=NULL;
373     int m = lpcrdr/2;
374
375     Wp = PUSH(stack, 4*m+2, float);
376     pw = Wp;
377
378     /* initialise contents of array */
379
380     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
381         *pw++ = 0.0;
382     }
383
384     /* Set pointers up */
385
386     pw = Wp;
387     xin1 = 1.0;
388     xin2 = 1.0;
389
390     /* reconstruct P(z) and Q(z) by  cascading second order
391       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
392       LSP coefficient */
393
394     for(j=0;j<=lpcrdr;j++){
395        int i2=0;
396         for(i=0;i<m;i++,i2+=2){
397             n1 = pw+(i*4);
398             n2 = n1 + 1;
399             n3 = n2 + 1;
400             n4 = n3 + 1;
401             xout1 = xin1 - 2*(freq[i2]) * *n1 + *n2;
402             xout2 = xin2 - 2*(freq[i2+1]) * *n3 + *n4;
403             *n2 = *n1;
404             *n4 = *n3;
405             *n1 = xin1;
406             *n3 = xin2;
407             xin1 = xout1;
408             xin2 = xout2;
409         }
410         xout1 = xin1 + *(n4+1);
411         xout2 = xin2 - *(n4+2);
412         ak[j] = (xout1 + xout2)*0.5;
413         *(n4+1) = xin1;
414         *(n4+2) = xin2;
415
416         xin1 = 0.0;
417         xin2 = 0.0;
418     }
419
420 }
421 #endif
422
423 /*Added by JMV
424   Makes sure the LSPs are stable*/
425 void lsp_enforce_margin(float *lsp, int len, float margin)
426 {
427    int i;
428    if (lsp[0]<margin)
429       lsp[0]=margin;
430    if (lsp[len-1]>M_PI-margin)
431       lsp[len-1]=M_PI-margin;
432    for (i=1;i<len-1;i++)
433    {
434       if (lsp[i]<lsp[i-1]+margin)
435          lsp[i]=lsp[i-1]+margin;
436
437       if (lsp[i]>lsp[i+1]-margin)
438          lsp[i]= .5* (lsp[i] + lsp[i+1]-margin);
439    }
440 }