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