FrameBits renamed to SpeexBits and a "lost" argument was added to the
[speexdsp.git] / libspeex / quant_lsp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: quant_lsp.c
3    LSP vector quantization
4
5    This library is free software; you can redistribute it and/or
6    modify it under the terms of the GNU Lesser General Public
7    License as published by the Free Software Foundation; either
8    version 2.1 of the License, or (at your option) any later version.
9    
10    This library is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    Lesser General Public License for more details.
14    
15    You should have received a copy of the GNU Lesser General Public
16    License along with this library; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20 #include "quant_lsp.h"
21 #include <math.h>
22 #include <stdio.h>
23
24 extern int lsp_nb_vqid[64];
25 static float quant_weight[MAX_LSP_SIZE];
26
27 /* Note: x is modified*/
28 static int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
29 {
30    int i,j;
31    float dist, tmp;
32    float best_dist=0;
33    int best_id=0;
34    float *ptr=cdbk;
35    for (i=0;i<nbVec;i++)
36    {
37       dist=0;
38       for (j=0;j<nbDim;j++)
39       {
40          tmp=(x[j]-*ptr++);
41          dist+=tmp*tmp;
42       }
43       if (dist<best_dist || i==0)
44       {
45          best_dist=dist;
46          best_id=i;
47       }
48    }
49
50    for (j=0;j<nbDim;j++)
51       x[j] -= cdbk[best_id*nbDim+j];
52     
53    return best_id;
54 }
55
56 /* Note: x is modified*/
57 static int lsp_weight_quant(float *x, float *weight, float *cdbk, int nbVec, int nbDim)
58 {
59    int i,j;
60    float dist, tmp;
61    float best_dist=0;
62    int best_id=0;
63    float *ptr=cdbk;
64    for (i=0;i<nbVec;i++)
65    {
66       dist=0;
67       for (j=0;j<nbDim;j++)
68       {
69          tmp=(x[j]-*ptr++);
70          dist+=weight[j]*tmp*tmp;
71       }
72       if (dist<best_dist || i==0)
73       {
74          best_dist=dist;
75          best_id=i;
76       }
77    }
78    
79    for (j=0;j<nbDim;j++)
80       x[j] -= cdbk[best_id*nbDim+j];
81    return best_id;
82 }
83
84
85 void lsp_quant_nb(float *lsp, float *qlsp, int order, SpeexBits *bits)
86 {
87    int i;
88    float tmp1, tmp2;
89    int id;
90
91    for (i=0;i<order;i++)
92       qlsp[i]=lsp[i];
93
94    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
95    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
96    for (i=1;i<order-1;i++)
97    {
98       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
99       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
100       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
101    }
102    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
103    speex_bits_pack(bits, id, 6);
104
105    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
106    speex_bits_pack(bits, id, 6);
107
108    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
109    speex_bits_pack(bits, id, 6);
110
111    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
112    speex_bits_pack(bits, id, 6);
113
114    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
115    speex_bits_pack(bits, id, 6);
116
117    for (i=0;i<order;i++)
118       qlsp[i]=lsp[i]-qlsp[i];
119 }
120
121 void lsp_unquant_nb(float *lsp, int order, SpeexBits *bits)
122 {
123    int i, id;
124    for (i=0;i<order;i++)
125       lsp[i]=0;
126
127
128    id=speex_bits_unpack_unsigned(bits, 6);
129    for (i=0;i<10;i++)
130       lsp[i] += cdbk_nb[id*10+i];
131
132    id=speex_bits_unpack_unsigned(bits, 6);
133    for (i=0;i<5;i++)
134       lsp[i] += cdbk_nb_low1[id*5+i];
135
136    id=speex_bits_unpack_unsigned(bits, 6);
137    for (i=0;i<5;i++)
138       lsp[i] += cdbk_nb_low2[id*5+i];
139
140    id=speex_bits_unpack_unsigned(bits, 6);
141    for (i=0;i<5;i++)
142       lsp[i+5] += cdbk_nb_high1[id*5+i];
143    
144    id=speex_bits_unpack_unsigned(bits, 6);
145    for (i=0;i<5;i++)
146       lsp[i+5] += cdbk_nb_high2[id*5+i];
147 }
148
149
150 extern float lsp_cdbk_wb[];
151 extern float lsp_cdbk_wb11[];
152 extern float lsp_cdbk_wb12[];
153 extern float lsp_cdbk_wb21[];
154 extern float lsp_cdbk_wb22[];
155 extern float lsp_cdbk_wb31[];
156 extern float lsp_cdbk_wb32[];
157 extern float lsp_cdbk_wb41[];
158 extern float lsp_cdbk_wb42[];
159
160 void lsp_quant_wb(float *lsp, float *qlsp, int order, SpeexBits *bits)
161 {
162    int i;
163    float tmp1, tmp2;
164    int id;
165    for (i=0;i<order;i++)
166       qlsp[i]=lsp[i];
167
168    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
169    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
170    for (i=1;i<order-1;i++)
171    {
172       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
173       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
174       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
175    }
176    id = lsp_quant(qlsp, lsp_cdbk_wb, 64, order);
177    speex_bits_pack(bits, id, 6);
178
179    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb11, 64, 4);
180    speex_bits_pack(bits, id, 6);
181
182    id = lsp_weight_quant(qlsp, quant_weight, lsp_cdbk_wb12, 64, 4);
183    speex_bits_pack(bits, id, 6);
184
185    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb21, 64, 4);
186    speex_bits_pack(bits, id, 6);
187
188    id = lsp_weight_quant(qlsp+4, quant_weight, lsp_cdbk_wb22, 64, 4);
189    speex_bits_pack(bits, id, 6);
190
191    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb31, 64, 4);
192    speex_bits_pack(bits, id, 6);
193
194    id = lsp_weight_quant(qlsp+8, quant_weight, lsp_cdbk_wb32, 16, 4);
195    speex_bits_pack(bits, id, 4);
196
197    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb41, 64, 4);
198    speex_bits_pack(bits, id, 6);
199
200    id = lsp_weight_quant(qlsp+12, quant_weight, lsp_cdbk_wb42, 16, 4);
201    speex_bits_pack(bits, id, 4);
202
203    for (i=0;i<order;i++)
204       qlsp[i]=lsp[i]-qlsp[i];
205
206 }
207
208
209 void lsp_unquant_wb(float *lsp, int order, SpeexBits *bits)
210 {
211
212    int i, id;
213    for (i=0;i<order;i++)
214       lsp[i]=0;
215
216
217    id=speex_bits_unpack_unsigned(bits, 6);
218    for (i=0;i<16;i++)
219       lsp[i] += lsp_cdbk_wb[id*16+i];
220
221
222    id=speex_bits_unpack_unsigned(bits, 6);
223    for (i=0;i<4;i++)
224       lsp[i] += lsp_cdbk_wb11[id*4+i];
225
226    id=speex_bits_unpack_unsigned(bits, 6);
227    for (i=0;i<4;i++)
228       lsp[i] += lsp_cdbk_wb12[id*4+i];
229
230
231    id=speex_bits_unpack_unsigned(bits, 6);
232    for (i=0;i<4;i++)
233       lsp[i+4] += lsp_cdbk_wb21[id*4+i];
234
235    id=speex_bits_unpack_unsigned(bits, 6);
236    for (i=0;i<4;i++)
237       lsp[i+4] += lsp_cdbk_wb22[id*4+i];
238
239
240    id=speex_bits_unpack_unsigned(bits, 6);
241    for (i=0;i<4;i++)
242       lsp[i+8] += lsp_cdbk_wb31[id*4+i];
243
244    id=speex_bits_unpack_unsigned(bits, 4);
245    for (i=0;i<4;i++)
246       lsp[i+8] += lsp_cdbk_wb32[id*4+i];
247
248
249    id=speex_bits_unpack_unsigned(bits, 6);
250    for (i=0;i<4;i++)
251       lsp[i+12] += lsp_cdbk_wb41[id*4+i];
252
253    id=speex_bits_unpack_unsigned(bits, 4);
254    for (i=0;i<4;i++)
255       lsp[i+12] += lsp_cdbk_wb42[id*4+i];
256
257 }
258
259 extern float high_lsp_cdbk[];
260 extern float high_lsp_cdbk2[];
261
262
263 void lsp_quant_high(float *lsp, float *qlsp, int order, SpeexBits *bits)
264 {
265    int i;
266    float tmp1, tmp2;
267    int id;
268    for (i=0;i<order;i++)
269       qlsp[i]=lsp[i];
270
271    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
272    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
273    for (i=1;i<order-1;i++)
274    {
275       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
276       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
277       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
278    }
279    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
280    speex_bits_pack(bits, id, 6);
281
282    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
283    speex_bits_pack(bits, id, 6);
284
285    for (i=0;i<order;i++)
286       qlsp[i]=lsp[i]-qlsp[i];
287 }
288
289 void lsp_unquant_high(float *lsp, int order, SpeexBits *bits)
290 {
291
292    int i, id;
293    for (i=0;i<order;i++)
294       lsp[i]=0;
295
296
297    id=speex_bits_unpack_unsigned(bits, 6);
298    for (i=0;i<order;i++)
299       lsp[i] += high_lsp_cdbk[id*order+i];
300
301
302    id=speex_bits_unpack_unsigned(bits, 6);
303    for (i=0;i<order;i++)
304       lsp[i] += high_lsp_cdbk2[id*order+i];
305 }