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