Speed improvements (got rid of a couple divide ops), cleanup...
[speexdsp.git] / libspeex / cb_search.c
1 /* Original copyright */
2 /*-----------------------------------------------------------------------*\
3
4     FILE........: GAINSHAPE.C
5     TYPE........: C Module
6     AUTHOR......: David Rowe
7     COMPANY.....: Voicetronix
8     DATE CREATED: 19/2/02
9
10     General gain-shape codebook search.
11
12 \*-----------------------------------------------------------------------*/
13
14
15 /* Modified by Jean-Marc Valin 2002
16
17    This library is free software; you can redistribute it and/or
18    modify it under the terms of the GNU Lesser General Public
19    License as published by the Free Software Foundation; either
20    version 2.1 of the License, or (at your option) any later version.
21    
22    This library is distributed in the hope that it will be useful,
23    but WITHOUT ANY WARRANTY; without even the implied warranty of
24    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
25    Lesser General Public License for more details.
26    
27    You should have received a copy of the GNU Lesser General Public
28    License along with this library; if not, write to the Free Software
29    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
30 */
31
32
33
34 #include <stdlib.h>
35 #include <cb_search.h>
36 #include "filters.h"
37 #include <math.h>
38 #include <stdio.h>
39 #include "stack_alloc.h"
40 #include "vq.h"
41 #include "matrix.h"
42
43 static float scal_gains4[16] = {
44    0.27713,
45    0.49282,
46    0.69570,
47    0.90786,
48    1.14235,
49    1.42798,
50    1.80756,
51    2.42801
52 };
53
54
55 /*---------------------------------------------------------------------------*\
56                                                                              
57  void overlap_cb_search()                                                             
58                                                                               
59  Searches a gain/shape codebook consisting of overlapping entries for the    
60  closest vector to the target.  Gives identical results to search() above   
61  buts uses fast end correction algorithm for the synthesis of response       
62  vectors.                                                                     
63                                                                              
64 \*---------------------------------------------------------------------------*/
65
66 float overlap_cb_search(
67 float target[],                 /* target vector */
68 float ak[],                     /* LPCs for this subframe */
69 float awk1[],                   /* Weighted LPCs for this subframe */
70 float awk2[],                   /* Weighted LPCs for this subframe */
71 float codebook[],               /* overlapping codebook */
72 int   entries,                  /* number of overlapping entries to search */
73 float *gain,                    /* gain of optimum entry */
74 int   *index,                   /* index of optimum entry */
75 int   p,                        /* number of LPC coeffs */
76 int   nsf                       /* number of samples in subframe */
77 )
78 {
79   float *resp;                  /* zero state response to current entry */
80   float *h;                     /* impulse response of synthesis filter */
81   float *impulse;               /* excitation vector containing one impulse */
82   float d,e,g,score;            /* codebook searching variables */
83   float bscore;                 /* score of "best" vector so far */
84   int i,k;                      /* loop variables */
85
86   /* Initialise */
87   
88   resp = (float*)malloc(sizeof(float)*nsf);
89   h = (float*)malloc(sizeof(float)*nsf);
90   impulse = (float*)malloc(sizeof(float)*nsf);
91
92   for(i=0; i<nsf; i++)
93     impulse[i] = 0.0;
94    
95   *gain = 0.0;
96   *index = 0;
97   bscore = 0.0;
98   impulse[0] = 1.0;
99
100   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
101   residue_zero(impulse, awk1, h, nsf, p);
102   syn_filt_zero(h, ak, h, nsf, p);
103   syn_filt_zero(h, awk2, h, nsf,p);
104   
105   /* Calculate codebook zero-response */
106   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
107   syn_filt_zero(resp,ak,resp,nsf,p);
108   syn_filt_zero(resp,awk2,resp,nsf,p);
109     
110   /* Search codebook backwards using end correction for synthesis */
111   
112   for(k=entries-1; k>=0; k--) {
113
114     d = 0.0; e = 0.0;
115     for(i=0; i<nsf; i++) {
116       d += target[i]*resp[i];
117       e += resp[i]*resp[i];
118     }
119     g = d/(e+.1);
120     score = g*d;
121     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
122     if (score >= bscore) {
123       bscore = score;
124       *gain = g;
125       *index = k;
126     }
127     
128     /* Synthesise next entry */
129     
130     if (k) {
131       for(i=nsf-1; i>=1; i--)
132         resp[i] = resp[i-1] + codebook[k-1]*h[i];
133       resp[0] = codebook[k-1]*h[0];
134     }
135   }
136
137   free(resp);
138   free(h);
139   free(impulse);
140   return bscore;
141 }
142
143
144
145 void split_cb_search(
146 float target[],                 /* target vector */
147 float ak[],                     /* LPCs for this subframe */
148 float awk1[],                   /* Weighted LPCs for this subframe */
149 float awk2[],                   /* Weighted LPCs for this subframe */
150 void *par,                      /* Codebook/search parameters*/
151 int   p,                        /* number of LPC coeffs */
152 int   nsf,                      /* number of samples in subframe */
153 float *exc,
154 FrameBits *bits,
155 float *stack
156 )
157 {
158    int i,j, id;
159    float *resp, *E, q;
160    float *t, *r, *e;
161    float *gains;
162    int *ind;
163    float *shape_cb;
164    int shape_cb_size, subvect_size, nb_subvect;
165    float exc_energy=0;
166    split_cb_params *params;
167
168    params = (split_cb_params *) par;
169    subvect_size = params->subvect_size;
170    nb_subvect = params->nb_subvect;
171    shape_cb_size = 1<<params->shape_bits;
172    shape_cb = params->shape_cb;
173    resp = PUSH(stack, shape_cb_size*subvect_size);
174    E = PUSH(stack, shape_cb_size);
175    t = PUSH(stack, nsf);
176    r = PUSH(stack, nsf);
177    e = PUSH(stack, nsf);
178    gains = PUSH(stack, nb_subvect);
179    ind = (int*)PUSH(stack, nb_subvect);
180
181    /* Compute energy of the "real excitation" */
182    syn_filt_zero(target, awk1, e, nsf, p);
183    residue_zero(e, ak, e, nsf, p);
184    residue_zero(e, awk2, e, nsf, p);
185    for (i=0;i<nsf;i++)
186       exc_energy += e[i]*e[i];
187    exc_energy=sqrt(exc_energy/nb_subvect);
188
189    /* Quantize global ("average") gain */
190    q=log(exc_energy+.1);
191    q=floor(.5+2*(q-2));
192    if (q<0)
193       q=0;
194    if (q>15)
195       q=15;
196    id = (int)q;
197    frame_bits_pack(bits, id, 4);
198    exc_energy=exp(.5*q+2);
199
200
201    for (i=0;i<nsf;i++)
202       t[i]=target[i];
203
204    e[0]=1;
205    for (i=1;i<nsf;i++)
206       e[i]=0;
207    residue_zero(e, awk1, r, nsf, p);
208    syn_filt_zero(r, ak, r, nsf, p);
209    syn_filt_zero(r, awk2, r, nsf,p);
210    
211    /* Pre-compute codewords response and energy */
212    for (i=0;i<shape_cb_size;i++)
213    {
214       float *res = resp+i*subvect_size;
215
216       /* Compute codeword response */
217       int k;
218       for(j=0;j<subvect_size;j++)
219          res[j]=0;
220       for(j=0;j<subvect_size;j++)
221       {
222          for (k=j;k<subvect_size;k++)
223             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
224       }
225       /* Compute energy of codeword response */
226       E[i]=0;
227       for(j=0;j<subvect_size;j++)
228          E[i]+=res[j]*res[j];
229       E[i]=1/(.001+E[i]);
230    }
231
232    for (i=0;i<nb_subvect;i++)
233    {
234       int best_index=0, k, m;
235       float g, corr, best_gain=0, score, best_score=-1;
236       /* Find best codeword for current sub-vector */
237       for (j=0;j<shape_cb_size;j++)
238       {
239          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
240          score=corr*corr*E[j];
241          g = corr*E[j];
242          if (score>best_score)
243          {
244             best_index=j;
245             best_score=score;
246             best_gain=g;
247          }
248       }
249       frame_bits_pack(bits,best_index,params->shape_bits);
250       
251       /* Quantize gain */
252       {
253          int s=0, best_id;
254          best_gain /= .01+exc_energy;
255          if (best_gain<0)
256          {
257             best_gain=-best_gain;
258             s=1;
259          }
260
261          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
262          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
263
264          best_gain=scal_gains4[best_id];
265          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
266          if (s)
267             best_gain=-best_gain;
268          best_gain *= exc_energy;
269          frame_bits_pack(bits,s,1);
270          frame_bits_pack(bits,best_id,3);
271       }
272       ind[i]=best_index;
273       gains[i]=best_gain;
274       /* Update target for next subvector */
275       for (j=0;j<subvect_size;j++)
276       {
277          g=best_gain*shape_cb[best_index*subvect_size+j];
278          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
279             t[k] -= g*r[m];
280       }
281    }
282    
283    /* Put everything back together */
284    for (i=0;i<nb_subvect;i++)
285       for (j=0;j<subvect_size;j++)
286          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
287
288    /* Update excitation */
289    for (j=0;j<nsf;j++)
290       exc[j]+=e[j];
291    
292    /* Update target */
293    residue_zero(e, awk1, r, nsf, p);
294    syn_filt_zero(r, ak, r, nsf, p);
295    syn_filt_zero(r, awk2, r, nsf,p);
296    for (j=0;j<nsf;j++)
297       target[j]-=r[j];
298
299    
300
301
302    POP(stack);
303    POP(stack);
304    POP(stack);
305    POP(stack);
306    POP(stack);
307    POP(stack);
308 }
309
310
311
312
313 void split_cb_unquant(
314 float *exc,
315 void *par,                      /* non-overlapping codebook */
316 int   nsf,                      /* number of samples in subframe */
317 FrameBits *bits,
318 float *stack
319 )
320 {
321    int i,j;
322    int *ind;
323    float *gains;
324    float *sign;
325    float *shape_cb, exc_energy;
326    int shape_cb_size, subvect_size, nb_subvect;
327    split_cb_params *params;
328
329    params = (split_cb_params *) par;
330    subvect_size = params->subvect_size;
331    nb_subvect = params->nb_subvect;
332    shape_cb_size = 1<<params->shape_bits;
333    shape_cb = params->shape_cb;
334    
335    ind = (int*)PUSH(stack, nb_subvect);
336    gains = PUSH(stack, nb_subvect);
337    sign = PUSH(stack, nb_subvect);
338
339    /* Decode global (average) gain */
340    {
341       int id;
342       id = frame_bits_unpack_unsigned(bits, 4);
343       exc_energy=exp(.5*id+2);
344    }
345
346    /* Decode codewords and gains */
347    for (i=0;i<nb_subvect;i++)
348    {
349       int gain_id;
350       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
351       if (frame_bits_unpack_unsigned(bits, 1))
352          sign[i]=-1;
353       else
354          sign[i]=1;
355       
356       gain_id = frame_bits_unpack_unsigned(bits, 3);
357       gains[i]=scal_gains4[gain_id];
358       gains[i] *= sign[i];
359       gains[i] *= exc_energy;
360
361
362    }
363
364    /* Compute decoded excitation */
365    for (i=0;i<nb_subvect;i++)
366       for (j=0;j<subvect_size;j++)
367          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
368
369    POP(stack);
370    POP(stack);
371    POP(stack);
372 }