VAD should now work on wideband too.
[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 <stdio.h>
48 #include <stdlib.h>
49 #include "lsp.h"
50 #include "stack_alloc.h"
51
52
53 #ifndef M_PI
54 #define M_PI           3.14159265358979323846  /* pi */
55 #endif
56
57
58 /*---------------------------------------------------------------------------*\
59
60         FUNCTION....: cheb_poly_eva()
61
62         AUTHOR......: David Rowe
63         DATE CREATED: 24/2/93
64
65     This function evalutes a series of chebyshev polynomials
66
67 \*---------------------------------------------------------------------------*/
68
69
70
71 static float cheb_poly_eva(float *coef,float x,int m,void *stack)
72 /*  float coef[]        coefficients of the polynomial to be evaluated  */
73 /*  float x             the point where polynomial is to be evaluated   */
74 /*  int m               order of the polynomial                         */
75 {
76     int i;
77     float *T,sum;
78     int m2=m>>1;
79
80     /* Allocate memory for chebyshev series formulation */
81     T=PUSH(stack, m2+1, float);
82
83     /* Initialise values */
84     T[0]=1;
85     T[1]=x;
86
87     /* Evaluate chebyshev series formulation using iterative approach   */
88     /* Evaluate polynomial and return value also free memory space */
89     sum = coef[m2] + coef[m2-1]*x;
90     x *= 2;
91     for(i=2;i<=m2;i++)
92     {
93        T[i] = x*T[i-1] - T[i-2];
94        sum += coef[m2-i] * T[i];
95     }
96     
97     return sum;
98 }
99
100
101 /*---------------------------------------------------------------------------*\
102
103         FUNCTION....: lpc_to_lsp()
104
105         AUTHOR......: David Rowe
106         DATE CREATED: 24/2/93
107
108     This function converts LPC coefficients to LSP
109     coefficients.
110
111 \*---------------------------------------------------------------------------*/
112
113
114 int lpc_to_lsp (float *a,int lpcrdr,float *freq,int nb,float delta, void *stack)
115 /*  float *a                    lpc coefficients                        */
116 /*  int lpcrdr                  order of LPC coefficients (10)          */
117 /*  float *freq                 LSP frequencies in the x domain         */
118 /*  int nb                      number of sub-intervals (4)             */
119 /*  float delta                 grid spacing interval (0.02)            */
120
121
122 {
123
124     float psuml,psumr,psumm,temp_xr,xl,xr,xm=0;
125     float temp_psumr/*,temp_qsumr*/;
126     int i,j,m,flag,k;
127     float *Q;                   /* ptrs for memory allocation           */
128     float *P;
129     float *px;                  /* ptrs of respective P'(z) & Q'(z)     */
130     float *qx;
131     float *p;
132     float *q;
133     float *pt;                  /* ptr used for cheb_poly_eval()
134                                 whether P' or Q'                        */
135     int roots=0;                /* DR 8/2/94: number of roots found     */
136     flag = 1;                   /*  program is searching for a root when,
137                                 1 else has found one                    */
138     m = lpcrdr/2;               /* order of P'(z) & Q'(z) polynimials   */
139
140
141     /* Allocate memory space for polynomials */
142     Q = PUSH(stack, (m+1), float);
143     P = PUSH(stack, (m+1), float);
144
145     /* determine P'(z)'s and Q'(z)'s coefficients where
146       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
147
148     px = P;                      /* initilaise ptrs                     */
149     qx = Q;
150     p = px;
151     q = qx;
152     *px++ = 1.0;
153     *qx++ = 1.0;
154     for(i=1;i<=m;i++){
155         *px++ = a[i]+a[lpcrdr+1-i]-*p++;
156         *qx++ = a[i]-a[lpcrdr+1-i]+*q++;
157     }
158     px = P;
159     qx = Q;
160     for(i=0;i<m;i++){
161         *px = 2**px;
162         *qx = 2**qx;
163          px++;
164          qx++;
165     }
166     px = P;                     /* re-initialise ptrs                   */
167     qx = Q;
168
169     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
170     Keep alternating between the two polynomials as each zero is found  */
171
172     xr = 0;                     /* initialise xr to zero                */
173     xl = 1.0;                   /* start at point xl = 1                */
174
175
176     for(j=0;j<lpcrdr;j++){
177         if(j%2)                 /* determines whether P' or Q' is eval. */
178             pt = qx;
179         else
180             pt = px;
181
182         psuml = cheb_poly_eva(pt,xl,lpcrdr,stack);      /* evals poly. at xl    */
183         flag = 1;
184         while(flag && (xr >= -1.0)){
185            float dd;
186            /* Modified by JMV to provide smaller steps around x=+-1 */
187            dd=(delta*(1-.9*xl*xl));
188            if (fabs(psuml)<.2)
189               dd *= .5;
190
191            xr = xl - dd;                                /* interval spacing     */
192             psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x)       */
193             temp_psumr = psumr;
194             temp_xr = xr;
195
196     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
197     sign change.
198     if a sign change has occurred the interval is bisected and then
199     checked again for a sign change which determines in which
200     interval the zero lies in.
201     If there is no sign change between poly(xm) and poly(xl) set interval
202     between xm and xr else set interval between xl and xr and repeat till
203     root is located within the specified limits                         */
204
205             if((psumr*psuml)<0.0){
206                 roots++;
207
208                 psumm=psuml;
209                 for(k=0;k<=nb;k++){
210                     xm = (xl+xr)/2;             /* bisect the interval  */
211                     psumm=cheb_poly_eva(pt,xm,lpcrdr,stack);
212                     if(psumm*psuml>0.){
213                         psuml=psumm;
214                         xl=xm;
215                     }
216                     else{
217                         psumr=psumm;
218                         xr=xm;
219                     }
220                 }
221
222                /* once zero is found, reset initial interval to xr      */
223                freq[j] = (xm);
224                xl = xm;
225                flag = 0;                /* reset flag for next search   */
226             }
227             else{
228                 psuml=temp_psumr;
229                 xl=temp_xr;
230             }
231         }
232     }
233     return(roots);
234 }
235
236
237 /*---------------------------------------------------------------------------*\
238
239         FUNCTION....: lsp_to_lpc()
240
241         AUTHOR......: David Rowe
242         DATE CREATED: 24/2/93
243
244     lsp_to_lpc: This function converts LSP coefficients to LPC
245     coefficients.
246
247 \*---------------------------------------------------------------------------*/
248
249
250 void lsp_to_lpc(float *freq,float *ak,int lpcrdr, void *stack)
251 /*  float *freq         array of LSP frequencies in the x domain        */
252 /*  float *ak           array of LPC coefficients                       */
253 /*  int lpcrdr          order of LPC coefficients                       */
254
255
256 {
257     int i,j;
258     float xout1,xout2,xin1,xin2;
259     float *Wp;
260     float *pw,*n1,*n2,*n3,*n4=NULL;
261     int m = lpcrdr/2;
262
263     Wp = PUSH(stack, 4*m+2, float);
264     pw = Wp;
265
266     /* initialise contents of array */
267
268     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */
269         *pw++ = 0.0;
270     }
271
272     /* Set pointers up */
273
274     pw = Wp;
275     xin1 = 1.0;
276     xin2 = 1.0;
277
278     /* reconstruct P(z) and Q(z) by  cascading second order
279       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
280       LSP coefficient */
281
282     for(j=0;j<=lpcrdr;j++){
283        int i2=0;
284         for(i=0;i<m;i++,i2+=2){
285             n1 = pw+(i*4);
286             n2 = n1 + 1;
287             n3 = n2 + 1;
288             n4 = n3 + 1;
289             xout1 = xin1 - 2*(freq[i2]) * *n1 + *n2;
290             xout2 = xin2 - 2*(freq[i2+1]) * *n3 + *n4;
291             *n2 = *n1;
292             *n4 = *n3;
293             *n1 = xin1;
294             *n3 = xin2;
295             xin1 = xout1;
296             xin2 = xout2;
297         }
298         xout1 = xin1 + *(n4+1);
299         xout2 = xin2 - *(n4+2);
300         ak[j] = (xout1 + xout2)*0.5;
301         *(n4+1) = xin1;
302         *(n4+2) = xin2;
303
304         xin1 = 0.0;
305         xin2 = 0.0;
306     }
307
308 }
309
310 /*Added by JMV
311   Makes sure the LSPs are stable*/
312 void lsp_enforce_margin(float *lsp, int len, float margin)
313 {
314    int i;
315    if (lsp[0]<margin)
316       lsp[0]=margin;
317    if (lsp[len-1]>M_PI-margin)
318       lsp[len]=margin;
319    for (i=1;i<len-1;i++)
320    {
321       if (lsp[i]<lsp[i-1]+margin)
322          lsp[i]=lsp[i-1]+margin;
323
324       if (lsp[i]>lsp[i+1]-margin)
325          lsp[i]= .5* (lsp[i] + lsp[i+1]-margin);
326    }
327 }