Fix some symbol and Makefile issues
[asterisk/asterisk.git] / codecs / ilbc / helpfun.c
1  
2 /****************************************************************** 
3  
4     iLBC Speech Coder ANSI-C Source Code 
5  
6     helpfun.c  
7  
8     Copyright (c) 2001, 
9     Global IP Sound AB. 
10     All rights reserved. 
11  
12 ******************************************************************/ 
13  
14 #include <math.h> 
15  
16 #include "iLBC_define.h" 
17 #include "constants.h" 
18  
19 /*----------------------------------------------------------------* 
20  *  calculation of auto correlation  
21  *---------------------------------------------------------------*/ 
22  
23 void autocorr(  
24     float *r,       /* (o) autocorrelation vector */ 
25     const float *x, /* (i) data vector */ 
26     int N,          /* (i) length of data vector */ 
27     int order       /* largest lag for calculated autocorrelations */ 
28 ){ 
29     int     lag, n; 
30     float   sum; 
31      
32     for (lag = 0; lag <= order; lag++) { 
33         sum = 0; 
34         for (n = 0; n < N - lag; n++) { 
35             sum += x[n] * x[n+lag]; 
36         } 
37         r[lag] = sum; 
38     } 
39
40  
41 /*----------------------------------------------------------------* 
42  *  window multiplication   
43  *---------------------------------------------------------------*/ 
44  
45 void lbc_window(  
46     float *z,       /* (o) the windowed data */ 
47     const float *x, /* (i) the original data vector */ 
48     const float *y, /* (i) the window */ 
49     int N           /* (i) length of all vectors */ 
50 ){ 
51     int     i; 
52      
53     for (i = 0; i < N; i++) { 
54         z[i] = x[i] * y[i]; 
55     } 
56
57  
58 /*----------------------------------------------------------------* 
59  *  levinson-durbin solution for lpc coefficients                          
60  *---------------------------------------------------------------*/ 
61  
62 void levdurb(  
63     float *a,       /* (o) lpc coefficient vector starting with 1.0 
64 */ 
65     float *k,       /* (o) reflection coefficients */ 
66     float *r,       /* (i) autocorrelation vector */ 
67     int order       /* (i) order of lpc filter */ 
68 ){ 
69     float  sum, alpha; 
70     int     m, m_h, i; 
71  
72     a[0] = 1.0; 
73      
74     if (r[0] < EPS) { /* if r[0] <= 0, set LPC coeff. to zero */ 
75         for (i = 0; i < order; i++) { 
76             k[i] = 0; 
77             a[i+1] = 0; 
78         }    
79     } else { 
80         a[1] = k[0] = -r[1]/r[0]; 
81         alpha = r[0] + r[1] * k[0]; 
82         for (m = 1; m < order; m++){ 
83             sum = r[m + 1]; 
84             for (i = 0; i < m; i++){ 
85                 sum += a[i+1] * r[m - i]; 
86             } 
87             k[m] = -sum / alpha; 
88             alpha += k[m] * sum; 
89             m_h = (m + 1) >> 1; 
90             for (i = 0; i < m_h; i++){ 
91                 sum = a[i+1] + k[m] * a[m - i]; 
92                 a[m - i] += k[m] * a[i+1]; 
93                 a[i+1] = sum; 
94             } 
95             a[m+1] = k[m]; 
96         } 
97     } 
98
99  
100 /*----------------------------------------------------------------* 
101  *  interpolation between vectors  
102  *---------------------------------------------------------------*/ 
103  
104 void interpolate(  
105     float *out,     /* (o) the interpolated vector */ 
106     float *in1,     /* (i) the first vector for the interpolation */ 
107     float *in2,     /* (i) the second vector for the interpolation */ 
108     float coef,     /* (i) interpolation weights */ 
109     int length      /* (i) length of all vectors */ 
110 ){ 
111     int i; 
112     float invcoef; 
113  
114     invcoef = (float)1.0 - coef; 
115     for (i = 0; i < length; i++) { 
116         out[i] = coef * in1[i] + invcoef * in2[i]; 
117     } 
118
119  
120 /*----------------------------------------------------------------* 
121  *  lpc bandwidth expansion                    
122  *---------------------------------------------------------------*/ 
123  
124 void bwexpand(  
125     float *out,     /* (o) the bandwidth expanded lpc coefficients */ 
126     float *in,      /* (i) the lpc coefficients before bandwidth  
127                            expansion */ 
128     float coef,     /* (i) the bandwidth expansion factor */ 
129     int length      /* (i) the length of lpc coefficient vectors */ 
130 ){ 
131     int i; 
132     float  chirp; 
133      
134     chirp = coef; 
135      
136     out[0] = in[0]; 
137     for (i = 1; i < length; i++) { 
138         out[i] = chirp * in[i]; 
139         chirp *= coef; 
140     } 
141
142  
143 /*----------------------------------------------------------------* 
144  *  vector quantization  
145  *---------------------------------------------------------------*/ 
146  
147 void vq(  
148     float *Xq,      /* (o) the quantized vector */ 
149     int *index,     /* (o) the quantization index */ 
150     const float *CB,/* (i) the vector quantization codebook */ 
151     float *X,       /* (i) the vector to quantize */ 
152     int n_cb,       /* (i) the number of vectors in the codebook */ 
153     int dim         /* (i) the dimension of all vectors */ 
154 ){ 
155     int     i, j; 
156     int     pos, minindex; 
157     float   dist, tmp, mindist; 
158  
159     pos = 0; 
160     mindist = FLOAT_MAX; 
161     minindex = 0; 
162     for (j = 0; j < n_cb; j++) { 
163         dist = X[0] - CB[pos]; 
164         dist *= dist; 
165         for (i = 1; i < dim; i++) { 
166             tmp = X[i] - CB[pos + i]; 
167             dist += tmp*tmp; 
168         } 
169          
170         if (dist < mindist) { 
171             mindist = dist; 
172             minindex = j; 
173         } 
174         pos += dim; 
175     } 
176     for (i = 0; i < dim; i++) { 
177         Xq[i] = CB[minindex*dim + i]; 
178     } 
179     *index = minindex; 
180
181  
182 /*----------------------------------------------------------------* 
183  *  split vector quantization  
184  *---------------------------------------------------------------*/ 
185  
186 void SplitVQ(  
187     float *qX,      /* (o) the quantized vector */ 
188     int *index,     /* (o) a vector of indexes for all vector 
189                            codebooks in the split */ 
190     float *X,       /* (i) the vector to quantize */ 
191     const float *CB,/* (i) the quantizer codebook */ 
192     int nsplit,     /* the number of vector splits */ 
193     const int *dim, /* the dimension of X and qX */ 
194     const int *cbsize /* the number of vectors in the codebook */ 
195 ){ 
196     int    cb_pos, X_pos, i; 
197      
198     cb_pos = 0; 
199     X_pos= 0; 
200     for (i = 0; i < nsplit; i++) { 
201         vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos,  
202             cbsize[i], dim[i]); 
203         X_pos += dim[i]; 
204         cb_pos += dim[i] * cbsize[i]; 
205     } 
206
207  
208 /*----------------------------------------------------------------* 
209  *  scalar quantization  
210  *---------------------------------------------------------------*/ 
211  
212 void sort_sq(  
213     float *xq,      /* (o) the quantized value */ 
214     int *index,     /* (o) the quantization index */ 
215     float x,    /* (i) the value to quantize */ 
216     const float *cb,/* (i) the quantization codebook */ 
217     int cb_size     /* (i) the size of the quantization codebook */ 
218 ){ 
219     int i; 
220      
221     if (x <= cb[0]) { 
222         *index = 0; 
223         *xq = cb[0]; 
224     } else { 
225         i = 0; 
226         while ((x > cb[i]) && i < cb_size - 1) { 
227             i++; 
228         } 
229          
230         if (x > ((cb[i] + cb[i - 1])/2)) { 
231             *index = i; 
232             *xq = cb[i]; 
233         } else { 
234             *index = i - 1; 
235             *xq = cb[i - 1]; 
236         } 
237     } 
238
239  
240 /*----------------------------------------------------------------* 
241  *  check for stability of lsf coefficients 
242  *---------------------------------------------------------------*/ 
243  
244 int LSF_check(      /* (o) 1 for stable lsf vectors and 0 for  
245                            nonstable ones */ 
246     float *lsf,     /* (i) a table of lsf vectors */ 
247     int dim,    /* (i) the dimension of each lsf vector */ 
248     int NoAn    /* (i) the number of lsf vectors in the table */ 
249 ){ 
250     int k,n,m, Nit=2, change=0,pos; 
251     float tmp; 
252     static float eps=(float)0.039; /* 50 Hz */ 
253     static float eps2=(float)0.0195; 
254     static float maxlsf=(float)3.14; /* 4000 Hz */ 
255     static float minlsf=(float)0.01; /* 0 Hz */ 
256      
257     /* LSF separation check*/ 
258  
259     for (n=0;n<Nit;n++) { /* Run through a couple of times */ 
260         for (m=0;m<NoAn;m++) { /* Number of analyses per frame */ 
261             for (k=0;k<(dim-1);k++) { 
262                 pos=m*dim+k; 
263                  
264                 if ((lsf[pos+1]-lsf[pos])<eps) { 
265                      
266                     if (lsf[pos+1]<lsf[pos]) { 
267                         tmp=lsf[pos+1]; 
268                         lsf[pos+1]= lsf[pos]+eps2; 
269                         lsf[pos]= lsf[pos+1]-eps2; 
270                     } else { 
271                         lsf[pos]-=eps2; 
272                         lsf[pos+1]+=eps2; 
273                     } 
274                     change=1; 
275                 } 
276                  
277                 if (lsf[pos]<minlsf) {  
278                     lsf[pos]=minlsf; 
279                     change=1; 
280                 } 
281                  
282                 if (lsf[pos]>maxlsf) {  
283                     lsf[pos]=maxlsf; 
284                     change=1; 
285                 }                
286             } 
287         } 
288     } 
289      
290     return change;   
291
292  
293