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