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