decoder excitation now in 16-bit precision (was 32), which saves quite a bit
[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 <math.h>
39 #ifndef M_PI
40 #define M_PI 3.14159265358979323846
41 #endif
42
43
44 #include "misc.h"
45
46 #ifdef FIXED_POINT
47
48 #define LSP_LINEAR(i) (SHL16(i+1,11))
49 #define LSP_LINEAR_HIGH(i) (ADD16(MULT16_16_16(i,2560),6144))
50 #define LSP_DIV_256(x) (SHL16((spx_word16_t)x, 5))
51 #define LSP_DIV_512(x) (SHL16((spx_word16_t)x, 4))
52 #define LSP_DIV_1024(x) (SHL16((spx_word16_t)x, 3))
53 #define LSP_PI 25736
54
55 #else
56
57 #define LSP_LINEAR(i) (.25*(i)+.25)
58 #define LSP_LINEAR_HIGH(i) (.3125*(i)+.75)
59 #define LSP_SCALE 256.
60 #define LSP_DIV_256(x) (0.0039062*(x))
61 #define LSP_DIV_512(x) (0.0019531*(x))
62 #define LSP_DIV_1024(x) (0.00097656*(x))
63 #define LSP_PI M_PI
64
65 #endif
66
67 static void compute_quant_weights(spx_lsp_t *qlsp, spx_word16_t *quant_weight, int order)
68 {
69    int i;
70    spx_word16_t tmp1, tmp2;
71    for (i=0;i<order;i++)
72    {
73       if (i==0)
74          tmp1 = qlsp[i];
75       else
76          tmp1 = qlsp[i]-qlsp[i-1];
77       if (i==order-1)
78          tmp2 = LSP_PI-qlsp[i];
79       else
80          tmp2 = qlsp[i+1]-qlsp[i];
81       if (tmp2<tmp1)
82          tmp1 = tmp2;
83 #ifdef FIXED_POINT
84       quant_weight[i] = DIV32_16(81920,ADD16(300,tmp1));
85 #else
86       quant_weight[i] = 10/(.04+tmp1);
87 #endif
88    }
89
90 }
91
92 /* Note: x is modified*/
93 static int lsp_quant(spx_word16_t *x, const signed char *cdbk, int nbVec, int nbDim)
94 {
95    int i,j;
96    spx_word32_t dist;
97    spx_word16_t tmp;
98    spx_word32_t best_dist=0;
99    int best_id=0;
100    const signed char *ptr=cdbk;
101    for (i=0;i<nbVec;i++)
102    {
103       dist=0;
104       for (j=0;j<nbDim;j++)
105       {
106          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
107          dist=MAC16_16(dist,tmp,tmp);
108       }
109       if (dist<best_dist || i==0)
110       {
111          best_dist=dist;
112          best_id=i;
113       }
114    }
115
116    for (j=0;j<nbDim;j++)
117       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
118     
119    return best_id;
120 }
121
122 /* Note: x is modified*/
123 static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, const signed char *cdbk, int nbVec, int nbDim)
124 {
125    int i,j;
126    spx_word32_t dist;
127    spx_word16_t tmp;
128    spx_word32_t best_dist=0;
129    int best_id=0;
130    const signed char *ptr=cdbk;
131    for (i=0;i<nbVec;i++)
132    {
133       dist=0;
134       for (j=0;j<nbDim;j++)
135       {
136          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
137          dist=MAC16_32_Q15(dist,weight[j],MULT16_16(tmp,tmp));
138       }
139       if (dist<best_dist || i==0)
140       {
141          best_dist=dist;
142          best_id=i;
143       }
144    }
145    
146    for (j=0;j<nbDim;j++)
147       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
148    return best_id;
149 }
150
151
152 void lsp_quant_nb(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
153 {
154    int i;
155    int id;
156    spx_word16_t quant_weight[10];
157    
158    for (i=0;i<order;i++)
159       qlsp[i]=lsp[i];
160
161    compute_quant_weights(qlsp, quant_weight, order);
162
163    for (i=0;i<order;i++)
164       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
165
166 #ifndef FIXED_POINT
167    for (i=0;i<order;i++)
168       qlsp[i] = LSP_SCALE*qlsp[i];
169 #endif
170    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
171    speex_bits_pack(bits, id, 6);
172
173    for (i=0;i<order;i++)
174       qlsp[i]*=2;
175  
176    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
177    speex_bits_pack(bits, id, 6);
178
179    for (i=0;i<5;i++)
180       qlsp[i]*=2;
181
182    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
183    speex_bits_pack(bits, id, 6);
184
185    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
186    speex_bits_pack(bits, id, 6);
187
188    for (i=5;i<10;i++)
189       qlsp[i]*=2;
190
191    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
192    speex_bits_pack(bits, id, 6);
193
194 #ifdef FIXED_POINT
195    for (i=0;i<order;i++)
196       qlsp[i]=PSHR16(qlsp[i],2);
197 #else
198    for (i=0;i<order;i++)
199       qlsp[i]=qlsp[i] * .00097656;
200 #endif
201
202    for (i=0;i<order;i++)
203       qlsp[i]=lsp[i]-qlsp[i];
204 }
205
206 void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
207 {
208    int i, id;
209    for (i=0;i<order;i++)
210       lsp[i]=LSP_LINEAR(i);
211
212
213    id=speex_bits_unpack_unsigned(bits, 6);
214    for (i=0;i<10;i++)
215       lsp[i] = ADD32(lsp[i], LSP_DIV_256(cdbk_nb[id*10+i]));
216
217    id=speex_bits_unpack_unsigned(bits, 6);
218    for (i=0;i<5;i++)
219       lsp[i] = ADD16(lsp[i], LSP_DIV_512(cdbk_nb_low1[id*5+i]));
220
221    id=speex_bits_unpack_unsigned(bits, 6);
222    for (i=0;i<5;i++)
223       lsp[i] = ADD32(lsp[i], LSP_DIV_1024(cdbk_nb_low2[id*5+i]));
224
225    id=speex_bits_unpack_unsigned(bits, 6);
226    for (i=0;i<5;i++)
227       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_512(cdbk_nb_high1[id*5+i]));
228    
229    id=speex_bits_unpack_unsigned(bits, 6);
230    for (i=0;i<5;i++)
231       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_1024(cdbk_nb_high2[id*5+i]));
232 }
233
234
235 void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
236 {
237    int i;
238    int id;
239    spx_word16_t quant_weight[10];
240
241    for (i=0;i<order;i++)
242       qlsp[i]=lsp[i];
243
244    compute_quant_weights(qlsp, quant_weight, order);
245
246    for (i=0;i<order;i++)
247       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
248 #ifndef FIXED_POINT
249    for (i=0;i<order;i++)
250       qlsp[i]=qlsp[i]*LSP_SCALE;
251 #endif
252    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
253    speex_bits_pack(bits, id, 6);
254    
255    for (i=0;i<order;i++)
256       qlsp[i]*=2;
257    
258    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
259    speex_bits_pack(bits, id, 6);
260
261    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
262    speex_bits_pack(bits, id, 6);
263
264 #ifdef FIXED_POINT
265    for (i=0;i<order;i++)
266       qlsp[i] = PSHR16(qlsp[i],1);
267 #else
268    for (i=0;i<order;i++)
269       qlsp[i] = qlsp[i]*0.0019531;
270 #endif
271
272    for (i=0;i<order;i++)
273       qlsp[i]=lsp[i]-qlsp[i];
274 }
275
276 void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
277 {
278    int i, id;
279    for (i=0;i<order;i++)
280       lsp[i]=LSP_LINEAR(i);
281
282
283    id=speex_bits_unpack_unsigned(bits, 6);
284    for (i=0;i<10;i++)
285       lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
286
287    id=speex_bits_unpack_unsigned(bits, 6);
288    for (i=0;i<5;i++)
289       lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
290
291    id=speex_bits_unpack_unsigned(bits, 6);
292    for (i=0;i<5;i++)
293       lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
294    
295 }
296
297
298 #ifdef DISABLE_WIDEBAND
299 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
300 {
301    speex_error("Wideband and Ultra-wideband are disabled");
302 }
303 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
304 {
305    speex_error("Wideband and Ultra-wideband are disabled");
306 }
307 #else
308 extern const signed char high_lsp_cdbk[];
309 extern const signed char high_lsp_cdbk2[];
310
311
312 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
313 {
314    int i;
315    int id;
316    spx_word16_t quant_weight[10];
317
318    for (i=0;i<order;i++)
319       qlsp[i]=lsp[i];
320
321    compute_quant_weights(qlsp, quant_weight, order);
322
323    /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
324    quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
325    for (i=1;i<order-1;i++)
326    {
327       tmp1 = 10/(qlsp[i]-qlsp[i-1]);
328       tmp2 = 10/(qlsp[i+1]-qlsp[i]);
329       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
330       }*/
331
332    for (i=0;i<order;i++)
333       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR_HIGH(i));
334 #ifndef FIXED_POINT
335    for (i=0;i<order;i++)
336       qlsp[i] = qlsp[i]*LSP_SCALE;
337 #endif
338    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
339    speex_bits_pack(bits, id, 6);
340
341    for (i=0;i<order;i++)
342       qlsp[i]*=2;
343
344    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
345    speex_bits_pack(bits, id, 6);
346
347 #ifdef FIXED_POINT
348    for (i=0;i<order;i++)
349       qlsp[i] = PSHR16(qlsp[i],1);
350 #else
351    for (i=0;i<order;i++)
352       qlsp[i] = qlsp[i]*0.0019531;
353 #endif
354
355    for (i=0;i<order;i++)
356       qlsp[i]=lsp[i]-qlsp[i];
357 }
358
359 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
360 {
361
362    int i, id;
363    for (i=0;i<order;i++)
364       lsp[i]=LSP_LINEAR_HIGH(i);
365
366
367    id=speex_bits_unpack_unsigned(bits, 6);
368    for (i=0;i<order;i++)
369       lsp[i] += LSP_DIV_256(high_lsp_cdbk[id*order+i]);
370
371
372    id=speex_bits_unpack_unsigned(bits, 6);
373    for (i=0;i<order;i++)
374       lsp[i] += LSP_DIV_512(high_lsp_cdbk2[id*order+i]);
375 }
376
377 #endif
378
379
380 #ifdef EPIC_48K
381
382 extern const signed char cdbk_lsp_vlbr[5120];
383 extern const signed char cdbk_lsp2_vlbr[160];
384
385 void lsp_quant_48k(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
386 {
387    int i;
388    int id;
389    spx_word16_t quant_weight[10];
390
391    for (i=0;i<order;i++)
392       qlsp[i]=lsp[i];
393
394    compute_quant_weights(qlsp, quant_weight, order);
395
396    for (i=0;i<order;i++)
397       qlsp[i]=SUB16(qlsp[i],LSP_SCALING*(.25*i+.3125));
398 #ifndef FIXED_POINT
399    for (i=0;i<order;i++)
400       qlsp[i] = qlsp[i]*LSP_SCALE;
401 #endif
402    
403    id = lsp_quant(qlsp, cdbk_lsp_vlbr, 512, order);
404    speex_bits_pack(bits, id, 9);
405
406    for (i=0;i<order;i++)
407       qlsp[i]*=4;
408    
409    id = lsp_weight_quant(qlsp, quant_weight, cdbk_lsp2_vlbr, 16, 10);
410    speex_bits_pack(bits, id, 4);
411
412 #ifdef FIXED_POINT
413    for (i=0;i<order;i++)
414       qlsp[i]=PSHR(qlsp[i],2);
415 #else
416    for (i=0;i<order;i++)
417       qlsp[i]=qlsp[i]*0.00097655;
418 #endif
419
420    for (i=0;i<order;i++)
421       qlsp[i]=lsp[i]-qlsp[i];
422 }
423
424 void lsp_unquant_48k(spx_lsp_t *lsp, int order, SpeexBits *bits)
425 {
426    int i, id;
427    for (i=0;i<order;i++)
428       lsp[i]=LSP_SCALING*(.25*i+.3125);
429
430
431    id=speex_bits_unpack_unsigned(bits, 9);
432    for (i=0;i<10;i++)
433       lsp[i] += LSP_SCALING*0.0039062*cdbk_lsp_vlbr[id*10+i];
434
435    id=speex_bits_unpack_unsigned(bits, 4);
436    for (i=0;i<10;i++)
437       lsp[i] += LSP_SCALING*0.00097655*cdbk_lsp2_vlbr[id*10+i];
438    
439 }
440
441 #endif