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