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