Added both LSP and prediction gain quantization... the code is a bit ugly
[speexdsp.git] / libspeex / quant_lsp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: quant_lsp.c
3    LSP vector quantization
4 */
5
6 #include "quant_lsp.h"
7 #include <math.h>
8 #include <stdio.h>
9
10 static float quant_weight[MAX_LSP_SIZE];
11
12 int lsp_quant(float *x, float *cdbk, int nbVec, int nbDim)
13 {
14    int i,j;
15    float dist, tmp;
16    float best_dist=0;
17    int best_id=0;
18    float *ptr=cdbk;
19    for (i=0;i<nbVec;i++)
20    {
21       dist=0;
22       for (j=0;j<nbDim;j++)
23       {
24          tmp=(x[j]-*ptr++);
25          dist+=tmp*tmp;
26       }
27       if (dist<best_dist || i==0)
28       {
29          best_dist=dist;
30          best_id=i;
31       }
32    }
33
34    for (j=0;j<nbDim;j++)
35       x[j] -= cdbk[best_id*nbDim+j];
36     
37    return best_id;
38 }
39
40 int lsp_weight_quant(float *x, float *weight, 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+=weight[j]*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    return best_id;
65 }
66
67
68 unsigned int lsp_quant_nb(float *lsp, int order)
69 {
70    int i;
71    float tmp1, tmp2;
72    unsigned int id;
73    quant_weight[0] = 1/(lsp[1]-lsp[0]);
74    quant_weight[order-1] = 1/(lsp[order-1]-lsp[order-2]);
75    for (i=1;i<order-1;i++)
76    {
77       tmp1 = 1/(lsp[i]-lsp[i-1]);
78       tmp2 = 1/(lsp[i+1]-lsp[i]);
79       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
80    }
81    id = lsp_quant(lsp, cdbk_nb, NB_CDBK_SIZE, order);
82    
83    id *= NB_CDBK_SIZE_LOW1;
84    id += lsp_weight_quant(lsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
85    
86    id *= NB_CDBK_SIZE_LOW2;
87    id += lsp_weight_quant(lsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
88    
89    id *= NB_CDBK_SIZE_HIGH1;
90    id += lsp_weight_quant(lsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
91    
92    id *= NB_CDBK_SIZE_HIGH2;
93    id += lsp_weight_quant(lsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
94    
95    return id;
96 }
97
98 void lsp_unquant_nb(float *lsp, int order, unsigned int id)
99 {
100    int i, tmp;
101    for (i=0;i<order;i++)
102       lsp[i]=0;
103
104
105    tmp = id % NB_CDBK_SIZE_HIGH2;
106    for (i=0;i<5;i++)
107       lsp[i+5] += cdbk_nb_high2[tmp*5+i];
108    id /= NB_CDBK_SIZE_HIGH2;
109    
110    tmp = id % NB_CDBK_SIZE_HIGH1;
111    for (i=0;i<5;i++)
112       lsp[i+5] += cdbk_nb_high1[tmp*5+i];
113    id /= NB_CDBK_SIZE_HIGH1;
114    
115    tmp = id % NB_CDBK_SIZE_LOW2;
116    for (i=0;i<5;i++)
117       lsp[i] += cdbk_nb_low2[tmp*5+i];
118    id /= NB_CDBK_SIZE_LOW2;
119
120    tmp = id % NB_CDBK_SIZE_LOW1;
121    for (i=0;i<5;i++)
122       lsp[i] += cdbk_nb_low1[tmp*5+i];
123    id /= NB_CDBK_SIZE_LOW1;
124
125    tmp=id;
126    for (i=0;i<10;i++)
127       lsp[i] += cdbk_nb[tmp*10+i];
128    
129 }