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