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