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