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