fixed-point: LSPs are now stored quantized
[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    float dist;
103    spx_word16_t tmp;
104    float 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    spx_word16_t nlsp[10];
135    
136    for (i=0;i<order;i++)
137       qlsp[i]=lsp[i];
138
139    compute_quant_weights(qlsp, quant_weight, order);
140
141    for (i=0;i<order;i++)
142       qlsp[i]-=LSP_SCALING*(.25*i+.25);
143
144 #ifdef FIXED_POINT
145    for (i=0;i<order;i++)
146       nlsp[i]=floor(.5+qlsp[i]*LSP_SCALE);
147 #else
148    for (i=0;i<order;i++)
149       nlsp[i] = LSP_SCALE*qlsp[i];
150 #endif
151    id = lsp_quant(nlsp, cdbk_nb, NB_CDBK_SIZE, order);
152    speex_bits_pack(bits, id, 6);
153
154    for (i=0;i<order;i++)
155       nlsp[i]*=2;
156  
157    id = lsp_weight_quant(nlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
158    speex_bits_pack(bits, id, 6);
159
160    for (i=0;i<5;i++)
161       nlsp[i]*=2;
162
163    id = lsp_weight_quant(nlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
164    speex_bits_pack(bits, id, 6);
165
166    id = lsp_weight_quant(nlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
167    speex_bits_pack(bits, id, 6);
168
169    for (i=5;i<10;i++)
170       nlsp[i]*=2;
171
172    id = lsp_weight_quant(nlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
173    speex_bits_pack(bits, id, 6);
174
175    for (i=0;i<order;i++)
176       qlsp[i]=nlsp[i] * (.00097656/LSP_OVERSCALE);
177
178    for (i=0;i<order;i++)
179       qlsp[i]=lsp[i]-qlsp[i];
180 }
181
182 void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
183 {
184    int i, id;
185    for (i=0;i<order;i++)
186       lsp[i]=LSP_SCALING*(.25*i+.25);
187
188
189    id=speex_bits_unpack_unsigned(bits, 6);
190    for (i=0;i<10;i++)
191       lsp[i] += LSP_SCALING*(0.0039062*cdbk_nb[id*10+i]);
192
193    id=speex_bits_unpack_unsigned(bits, 6);
194    for (i=0;i<5;i++)
195       lsp[i] += LSP_SCALING*(0.0019531 * cdbk_nb_low1[id*5+i]);
196
197    id=speex_bits_unpack_unsigned(bits, 6);
198    for (i=0;i<5;i++)
199       lsp[i] += LSP_SCALING*(0.00097656 * cdbk_nb_low2[id*5+i]);
200
201    id=speex_bits_unpack_unsigned(bits, 6);
202    for (i=0;i<5;i++)
203       lsp[i+5] += LSP_SCALING*(0.0019531 * cdbk_nb_high1[id*5+i]);
204    
205    id=speex_bits_unpack_unsigned(bits, 6);
206    for (i=0;i<5;i++)
207       lsp[i+5] += LSP_SCALING*(0.00097656 * cdbk_nb_high2[id*5+i]);
208 }
209
210
211 void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
212 {
213    int i;
214    int id;
215    spx_word16_t quant_weight[10];
216    spx_word16_t nlsp[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 #ifdef FIXED_POINT
226    for (i=0;i<order;i++)
227       nlsp[i]=floor(.5+qlsp[i]*LSP_SCALE);
228 #else
229    for (i=0;i<order;i++)
230       nlsp[i]=qlsp[i]*LSP_SCALE;
231 #endif
232    id = lsp_quant(nlsp, cdbk_nb, NB_CDBK_SIZE, order);
233    speex_bits_pack(bits, id, 6);
234    
235    for (i=0;i<order;i++)
236       nlsp[i]*=2;
237    
238    id = lsp_weight_quant(nlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
239    speex_bits_pack(bits, id, 6);
240
241    id = lsp_weight_quant(nlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
242    speex_bits_pack(bits, id, 6);
243
244    for (i=0;i<order;i++)
245       qlsp[i] = nlsp[i]*(0.0019531/LSP_OVERSCALE);
246
247    for (i=0;i<order;i++)
248       qlsp[i]=lsp[i]-qlsp[i];
249 }
250
251 void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
252 {
253    int i, id;
254    for (i=0;i<order;i++)
255       lsp[i]=LSP_SCALING*(.25*i+.25);
256
257
258    id=speex_bits_unpack_unsigned(bits, 6);
259    for (i=0;i<10;i++)
260       lsp[i] += LSP_SCALING*0.0039062*cdbk_nb[id*10+i];
261
262    id=speex_bits_unpack_unsigned(bits, 6);
263    for (i=0;i<5;i++)
264       lsp[i] += LSP_SCALING*0.0019531*cdbk_nb_low1[id*5+i];
265
266    id=speex_bits_unpack_unsigned(bits, 6);
267    for (i=0;i<5;i++)
268       lsp[i+5] += LSP_SCALING*0.0019531*cdbk_nb_high1[id*5+i];
269    
270 }
271
272
273 extern signed char high_lsp_cdbk[];
274 extern signed char high_lsp_cdbk2[];
275
276
277 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
278 {
279    int i;
280    int id;
281    spx_word16_t quant_weight[10];
282    spx_word16_t nlsp[10];
283
284    for (i=0;i<order;i++)
285       qlsp[i]=lsp[i];
286
287    compute_quant_weights(qlsp, quant_weight, order);
288
289    /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
290    quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
291    for (i=1;i<order-1;i++)
292    {
293       tmp1 = 10/(qlsp[i]-qlsp[i-1]);
294       tmp2 = 10/(qlsp[i+1]-qlsp[i]);
295       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
296       }*/
297
298    for (i=0;i<order;i++)
299       qlsp[i]-=LSP_SCALING*(.3125*i+.75);
300 #ifdef FIXED_POINT
301    for (i=0;i<order;i++)
302       nlsp[i] = floor(.5+qlsp[i]*LSP_SCALE);
303 #else
304    for (i=0;i<order;i++)
305       nlsp[i] = qlsp[i]*LSP_SCALE;
306 #endif
307    id = lsp_quant(nlsp, high_lsp_cdbk, 64, order);
308    speex_bits_pack(bits, id, 6);
309
310    for (i=0;i<order;i++)
311       nlsp[i]*=2;
312
313    id = lsp_weight_quant(nlsp, quant_weight, high_lsp_cdbk2, 64, order);
314    speex_bits_pack(bits, id, 6);
315
316    for (i=0;i<order;i++)
317       qlsp[i] = nlsp[i]*(0.0019531/LSP_OVERSCALE);
318
319    for (i=0;i<order;i++)
320       qlsp[i]=lsp[i]-qlsp[i];
321 }
322
323 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
324 {
325
326    int i, id;
327    for (i=0;i<order;i++)
328       lsp[i]=LSP_SCALING*(.3125*i+.75);
329
330
331    id=speex_bits_unpack_unsigned(bits, 6);
332    for (i=0;i<order;i++)
333       lsp[i] += LSP_SCALING*0.0039062*high_lsp_cdbk[id*order+i];
334
335
336    id=speex_bits_unpack_unsigned(bits, 6);
337    for (i=0;i<order;i++)
338       lsp[i] += LSP_SCALING*0.0019531*high_lsp_cdbk2[id*order+i];
339 }
340
341
342 #ifdef EPIC_48K
343
344 extern signed char cdbk_lsp_vlbr[5120];
345 extern signed char cdbk_lsp2_vlbr[160];
346
347 void lsp_quant_48k(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
348 {
349    int i;
350    int id;
351    spx_word16_t quant_weight[10];
352    spx_word16_t nlsp[10];
353
354    for (i=0;i<order;i++)
355       qlsp[i]=lsp[i];
356
357    compute_quant_weights(qlsp, quant_weight, order);
358
359    for (i=0;i<order;i++)
360       qlsp[i]-=LSP_SCALING*(.25*i+.3125);
361    for (i=0;i<order;i++)
362       nlsp[i]=qlsp[i]*LSP_SCALE;
363    
364    id = lsp_quant(qlsp, cdbk_lsp_vlbr, 512, order);
365    speex_bits_pack(bits, id, 9);
366
367    for (i=0;i<order;i++)
368       qlsp[i]*=4;
369    
370    id = lsp_weight_quant(qlsp, quant_weight, cdbk_lsp2_vlbr, 16, 10);
371    speex_bits_pack(bits, id, 4);
372
373    for (i=0;i<order;i++)
374       qlsp[i]=nlsp[i]*(0.00097655/LSP_OVERSCALE);
375
376    for (i=0;i<order;i++)
377       qlsp[i]=lsp[i]-qlsp[i];
378 }
379
380 void lsp_unquant_48k(spx_lsp_t *lsp, int order, SpeexBits *bits)
381 {
382    int i, id;
383    for (i=0;i<order;i++)
384       lsp[i]=LSP_SCALING*(.25*i+.3125);
385
386
387    id=speex_bits_unpack_unsigned(bits, 9);
388    for (i=0;i<10;i++)
389       lsp[i] += LSP_SCALING*0.0039062*cdbk_lsp_vlbr[id*10+i];
390
391    id=speex_bits_unpack_unsigned(bits, 4);
392    for (i=0;i<10;i++)
393       lsp[i] += LSP_SCALING*0.00097655*cdbk_lsp2_vlbr[id*10+i];
394    
395 }
396
397 #endif