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