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