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