cleanup: all use of libc has been moved to misc.c to make porting easier.
[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 extern int lsp_nb_vqid[64];
37 static float quant_weight[MAX_LSP_SIZE];
38
39 /* Note: x is modified*/
40 static int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
41 {
42    int i,j;
43    float dist, tmp;
44    float best_dist=0;
45    int best_id=0;
46    float *ptr=cdbk;
47    for (i=0;i<nbVec;i++)
48    {
49       dist=0;
50       for (j=0;j<nbDim;j++)
51       {
52          tmp=(x[j]-*ptr++);
53          dist+=tmp*tmp;
54       }
55       if (dist<best_dist || i==0)
56       {
57          best_dist=dist;
58          best_id=i;
59       }
60    }
61
62    for (j=0;j<nbDim;j++)
63       x[j] -= cdbk[best_id*nbDim+j];
64     
65    return best_id;
66 }
67
68 /* Note: x is modified*/
69 static int lsp_weight_quant(float *x, float *weight, float *cdbk, int nbVec, int nbDim)
70 {
71    int i,j;
72    float dist, tmp;
73    float best_dist=0;
74    int best_id=0;
75    float *ptr=cdbk;
76    for (i=0;i<nbVec;i++)
77    {
78       dist=0;
79       for (j=0;j<nbDim;j++)
80       {
81          tmp=(x[j]-*ptr++);
82          dist+=weight[j]*tmp*tmp;
83       }
84       if (dist<best_dist || i==0)
85       {
86          best_dist=dist;
87          best_id=i;
88       }
89    }
90    
91    for (j=0;j<nbDim;j++)
92       x[j] -= cdbk[best_id*nbDim+j];
93    return best_id;
94 }
95
96
97 void lsp_quant_nb(float *lsp, float *qlsp, int order, SpeexBits *bits)
98 {
99    int i;
100    float tmp1, tmp2;
101    int id;
102
103    for (i=0;i<order;i++)
104       qlsp[i]=lsp[i];
105
106    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
107    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
108    for (i=1;i<order-1;i++)
109    {
110 #if 1
111       tmp1 = 1/((.15+qlsp[i]-qlsp[i-1])*(.15+qlsp[i]-qlsp[i-1]));
112       tmp2 = 1/((.15+qlsp[i+1]-qlsp[i])*(.15+qlsp[i+1]-qlsp[i]));
113 #else
114       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
115       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
116 #endif
117       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
118    }
119    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
120    speex_bits_pack(bits, id, 6);
121
122    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
123    speex_bits_pack(bits, id, 6);
124
125    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
126    speex_bits_pack(bits, id, 6);
127
128    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
129    speex_bits_pack(bits, id, 6);
130
131    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
132    speex_bits_pack(bits, id, 6);
133
134    for (i=0;i<order;i++)
135       qlsp[i]=lsp[i]-qlsp[i];
136 }
137
138 void lsp_unquant_nb(float *lsp, int order, SpeexBits *bits)
139 {
140    int i, id;
141    for (i=0;i<order;i++)
142       lsp[i]=0;
143
144
145    id=speex_bits_unpack_unsigned(bits, 6);
146    for (i=0;i<10;i++)
147       lsp[i] += cdbk_nb[id*10+i];
148
149    id=speex_bits_unpack_unsigned(bits, 6);
150    for (i=0;i<5;i++)
151       lsp[i] += cdbk_nb_low1[id*5+i];
152
153    id=speex_bits_unpack_unsigned(bits, 6);
154    for (i=0;i<5;i++)
155       lsp[i] += cdbk_nb_low2[id*5+i];
156
157    id=speex_bits_unpack_unsigned(bits, 6);
158    for (i=0;i<5;i++)
159       lsp[i+5] += cdbk_nb_high1[id*5+i];
160    
161    id=speex_bits_unpack_unsigned(bits, 6);
162    for (i=0;i<5;i++)
163       lsp[i+5] += cdbk_nb_high2[id*5+i];
164 }
165
166
167 void lsp_quant_lbr(float *lsp, float *qlsp, int order, SpeexBits *bits)
168 {
169    int i;
170    float tmp1, tmp2;
171    int id;
172
173    for (i=0;i<order;i++)
174       qlsp[i]=lsp[i];
175
176    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
177    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
178    for (i=1;i<order-1;i++)
179    {
180 #if 1
181       tmp1 = 1/((.15+qlsp[i]-qlsp[i-1])*(.15+qlsp[i]-qlsp[i-1]));
182       tmp2 = 1/((.15+qlsp[i+1]-qlsp[i])*(.15+qlsp[i+1]-qlsp[i]));
183 #else
184       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
185       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
186 #endif
187       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
188    }
189    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
190    speex_bits_pack(bits, id, 6);
191
192    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
193    speex_bits_pack(bits, id, 6);
194
195    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
196    speex_bits_pack(bits, id, 6);
197
198    for (i=0;i<order;i++)
199       qlsp[i]=lsp[i]-qlsp[i];
200 }
201
202 void lsp_unquant_lbr(float *lsp, int order, SpeexBits *bits)
203 {
204    int i, id;
205    for (i=0;i<order;i++)
206       lsp[i]=0;
207
208
209    id=speex_bits_unpack_unsigned(bits, 6);
210    for (i=0;i<10;i++)
211       lsp[i] += cdbk_nb[id*10+i];
212
213    id=speex_bits_unpack_unsigned(bits, 6);
214    for (i=0;i<5;i++)
215       lsp[i] += 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+5] += cdbk_nb_high1[id*5+i];
220    
221 }
222
223
224 extern float high_lsp_cdbk[];
225 extern float high_lsp_cdbk2[];
226
227
228 void lsp_quant_high(float *lsp, float *qlsp, int order, SpeexBits *bits)
229 {
230    int i;
231    float tmp1, tmp2;
232    int id;
233    for (i=0;i<order;i++)
234       qlsp[i]=lsp[i];
235
236    quant_weight[0] = 1/(qlsp[1]-qlsp[0]);
237    quant_weight[order-1] = 1/(qlsp[order-1]-qlsp[order-2]);
238    for (i=1;i<order-1;i++)
239    {
240       tmp1 = 1/(qlsp[i]-qlsp[i-1]);
241       tmp2 = 1/(qlsp[i+1]-qlsp[i]);
242       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
243    }
244    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
245    speex_bits_pack(bits, id, 6);
246
247    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
248    speex_bits_pack(bits, id, 6);
249
250    for (i=0;i<order;i++)
251       qlsp[i]=lsp[i]-qlsp[i];
252 }
253
254 void lsp_unquant_high(float *lsp, int order, SpeexBits *bits)
255 {
256
257    int i, id;
258    for (i=0;i<order;i++)
259       lsp[i]=0;
260
261
262    id=speex_bits_unpack_unsigned(bits, 6);
263    for (i=0;i<order;i++)
264       lsp[i] += high_lsp_cdbk[id*order+i];
265
266
267    id=speex_bits_unpack_unsigned(bits, 6);
268    for (i=0;i<order;i++)
269       lsp[i] += high_lsp_cdbk2[id*order+i];
270 }