resampler: passing NULL as input should help drain the resampler.
[speexdsp.git] / libspeex / quant_lsp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: quant_lsp.c
3    LSP vector quantization
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include "quant_lsp.h"
38 #include "os_support.h"
39 #include <math.h>
40 #ifndef M_PI
41 #define M_PI 3.14159265358979323846
42 #endif
43
44 #include "arch.h"
45
46 #ifdef BFIN_ASM
47 #include "quant_lsp_bfin.h"
48 #endif
49
50 #ifdef FIXED_POINT
51
52 #define LSP_LINEAR(i) (SHL16(i+1,11))
53 #define LSP_LINEAR_HIGH(i) (ADD16(MULT16_16_16(i,2560),6144))
54 #define LSP_DIV_256(x) (SHL16((spx_word16_t)x, 5))
55 #define LSP_DIV_512(x) (SHL16((spx_word16_t)x, 4))
56 #define LSP_DIV_1024(x) (SHL16((spx_word16_t)x, 3))
57 #define LSP_PI 25736
58
59 #else
60
61 #define LSP_LINEAR(i) (.25*(i)+.25)
62 #define LSP_LINEAR_HIGH(i) (.3125*(i)+.75)
63 #define LSP_SCALE 256.
64 #define LSP_DIV_256(x) (0.0039062*(x))
65 #define LSP_DIV_512(x) (0.0019531*(x))
66 #define LSP_DIV_1024(x) (0.00097656*(x))
67 #define LSP_PI M_PI
68
69 #endif
70
71 static void compute_quant_weights(spx_lsp_t *qlsp, spx_word16_t *quant_weight, int order)
72 {
73    int i;
74    spx_word16_t tmp1, tmp2;
75    for (i=0;i<order;i++)
76    {
77       if (i==0)
78          tmp1 = qlsp[i];
79       else
80          tmp1 = qlsp[i]-qlsp[i-1];
81       if (i==order-1)
82          tmp2 = LSP_PI-qlsp[i];
83       else
84          tmp2 = qlsp[i+1]-qlsp[i];
85       if (tmp2<tmp1)
86          tmp1 = tmp2;
87 #ifdef FIXED_POINT
88       quant_weight[i] = DIV32_16(81920,ADD16(300,tmp1));
89 #else
90       quant_weight[i] = 10/(.04+tmp1);
91 #endif
92    }
93
94 }
95
96 /* Note: x is modified*/
97 #ifndef OVERRIDE_LSP_QUANT
98 static int lsp_quant(spx_word16_t *x, const signed char *cdbk, int nbVec, int nbDim)
99 {
100    int i,j;
101    spx_word32_t dist;
102    spx_word16_t tmp;
103    spx_word32_t best_dist=VERY_LARGE32;
104    int best_id=0;
105    const signed char *ptr=cdbk;
106    for (i=0;i<nbVec;i++)
107    {
108       dist=0;
109       for (j=0;j<nbDim;j++)
110       {
111          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
112          dist=MAC16_16(dist,tmp,tmp);
113       } 
114       if (dist<best_dist)
115       {
116          best_dist=dist;
117          best_id=i;
118       }
119    }
120
121    for (j=0;j<nbDim;j++)
122       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
123     
124    return best_id;
125 }
126 #endif
127
128 /* Note: x is modified*/
129 #ifndef OVERRIDE_LSP_WEIGHT_QUANT
130 static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, const signed char *cdbk, int nbVec, int nbDim)
131 {
132    int i,j;
133    spx_word32_t dist;
134    spx_word16_t tmp;
135    spx_word32_t best_dist=VERY_LARGE32;
136    int best_id=0;
137    const signed char *ptr=cdbk;
138    for (i=0;i<nbVec;i++)
139    {
140       dist=0;
141       for (j=0;j<nbDim;j++)
142       {
143          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
144          dist=MAC16_32_Q15(dist,weight[j],MULT16_16(tmp,tmp));
145       }
146       if (dist<best_dist)
147       {
148          best_dist=dist;
149          best_id=i;
150       }
151    }
152    
153    for (j=0;j<nbDim;j++)
154       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
155    return best_id;
156 }
157 #endif
158
159 void lsp_quant_nb(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
160 {
161    int i;
162    int id;
163    spx_word16_t quant_weight[10];
164    
165    for (i=0;i<order;i++)
166       qlsp[i]=lsp[i];
167
168    compute_quant_weights(qlsp, quant_weight, order);
169
170    for (i=0;i<order;i++)
171       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
172
173 #ifndef FIXED_POINT
174    for (i=0;i<order;i++)
175       qlsp[i] = LSP_SCALE*qlsp[i];
176 #endif
177    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
178    speex_bits_pack(bits, id, 6);
179
180    for (i=0;i<order;i++)
181       qlsp[i]*=2;
182  
183    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
184    speex_bits_pack(bits, id, 6);
185
186    for (i=0;i<5;i++)
187       qlsp[i]*=2;
188
189    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
190    speex_bits_pack(bits, id, 6);
191
192    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
193    speex_bits_pack(bits, id, 6);
194
195    for (i=5;i<10;i++)
196       qlsp[i]*=2;
197
198    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
199    speex_bits_pack(bits, id, 6);
200
201 #ifdef FIXED_POINT
202    for (i=0;i<order;i++)
203       qlsp[i]=PSHR16(qlsp[i],2);
204 #else
205    for (i=0;i<order;i++)
206       qlsp[i]=qlsp[i] * .00097656;
207 #endif
208
209    for (i=0;i<order;i++)
210       qlsp[i]=lsp[i]-qlsp[i];
211 }
212
213 void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
214 {
215    int i, id;
216    for (i=0;i<order;i++)
217       lsp[i]=LSP_LINEAR(i);
218
219
220    id=speex_bits_unpack_unsigned(bits, 6);
221    for (i=0;i<10;i++)
222       lsp[i] = ADD32(lsp[i], LSP_DIV_256(cdbk_nb[id*10+i]));
223
224    id=speex_bits_unpack_unsigned(bits, 6);
225    for (i=0;i<5;i++)
226       lsp[i] = ADD16(lsp[i], LSP_DIV_512(cdbk_nb_low1[id*5+i]));
227
228    id=speex_bits_unpack_unsigned(bits, 6);
229    for (i=0;i<5;i++)
230       lsp[i] = ADD32(lsp[i], LSP_DIV_1024(cdbk_nb_low2[id*5+i]));
231
232    id=speex_bits_unpack_unsigned(bits, 6);
233    for (i=0;i<5;i++)
234       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_512(cdbk_nb_high1[id*5+i]));
235    
236    id=speex_bits_unpack_unsigned(bits, 6);
237    for (i=0;i<5;i++)
238       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_1024(cdbk_nb_high2[id*5+i]));
239 }
240
241
242 void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
243 {
244    int i;
245    int id;
246    spx_word16_t quant_weight[10];
247
248    for (i=0;i<order;i++)
249       qlsp[i]=lsp[i];
250
251    compute_quant_weights(qlsp, quant_weight, order);
252
253    for (i=0;i<order;i++)
254       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
255 #ifndef FIXED_POINT
256    for (i=0;i<order;i++)
257       qlsp[i]=qlsp[i]*LSP_SCALE;
258 #endif
259    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
260    speex_bits_pack(bits, id, 6);
261    
262    for (i=0;i<order;i++)
263       qlsp[i]*=2;
264    
265    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
266    speex_bits_pack(bits, id, 6);
267
268    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
269    speex_bits_pack(bits, id, 6);
270
271 #ifdef FIXED_POINT
272    for (i=0;i<order;i++)
273       qlsp[i] = PSHR16(qlsp[i],1);
274 #else
275    for (i=0;i<order;i++)
276       qlsp[i] = qlsp[i]*0.0019531;
277 #endif
278
279    for (i=0;i<order;i++)
280       qlsp[i]=lsp[i]-qlsp[i];
281 }
282
283 void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
284 {
285    int i, id;
286    for (i=0;i<order;i++)
287       lsp[i]=LSP_LINEAR(i);
288
289
290    id=speex_bits_unpack_unsigned(bits, 6);
291    for (i=0;i<10;i++)
292       lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
293
294    id=speex_bits_unpack_unsigned(bits, 6);
295    for (i=0;i<5;i++)
296       lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
297
298    id=speex_bits_unpack_unsigned(bits, 6);
299    for (i=0;i<5;i++)
300       lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
301    
302 }
303
304
305 #ifdef DISABLE_WIDEBAND
306 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
307 {
308    speex_fatal("Wideband and Ultra-wideband are disabled");
309 }
310 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
311 {
312    speex_fatal("Wideband and Ultra-wideband are disabled");
313 }
314 #else
315 extern const signed char high_lsp_cdbk[];
316 extern const signed char high_lsp_cdbk2[];
317
318
319 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
320 {
321    int i;
322    int id;
323    spx_word16_t quant_weight[10];
324
325    for (i=0;i<order;i++)
326       qlsp[i]=lsp[i];
327
328    compute_quant_weights(qlsp, quant_weight, order);
329
330    /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
331    quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
332    for (i=1;i<order-1;i++)
333    {
334       tmp1 = 10/(qlsp[i]-qlsp[i-1]);
335       tmp2 = 10/(qlsp[i+1]-qlsp[i]);
336       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
337       }*/
338
339    for (i=0;i<order;i++)
340       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR_HIGH(i));
341 #ifndef FIXED_POINT
342    for (i=0;i<order;i++)
343       qlsp[i] = qlsp[i]*LSP_SCALE;
344 #endif
345    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
346    speex_bits_pack(bits, id, 6);
347
348    for (i=0;i<order;i++)
349       qlsp[i]*=2;
350
351    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
352    speex_bits_pack(bits, id, 6);
353
354 #ifdef FIXED_POINT
355    for (i=0;i<order;i++)
356       qlsp[i] = PSHR16(qlsp[i],1);
357 #else
358    for (i=0;i<order;i++)
359       qlsp[i] = qlsp[i]*0.0019531;
360 #endif
361
362    for (i=0;i<order;i++)
363       qlsp[i]=lsp[i]-qlsp[i];
364 }
365
366 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
367 {
368
369    int i, id;
370    for (i=0;i<order;i++)
371       lsp[i]=LSP_LINEAR_HIGH(i);
372
373
374    id=speex_bits_unpack_unsigned(bits, 6);
375    for (i=0;i<order;i++)
376       lsp[i] += LSP_DIV_256(high_lsp_cdbk[id*order+i]);
377
378
379    id=speex_bits_unpack_unsigned(bits, 6);
380    for (i=0;i<order;i++)
381       lsp[i] += LSP_DIV_512(high_lsp_cdbk2[id*order+i]);
382 }
383
384 #endif
385