6b53048b1f49c54358e6376476cdadeef64fd8d3
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Long-Term Prediction functions
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 <math.h>
34 #include "ltp.h"
35 #include "stack_alloc.h"
36 #include "filters.h"
37 #include "speex_bits.h"
38
39 #include <stdio.h>
40
41 #ifdef _USE_SSE
42 #include "ltp_sse.h"
43 #else
44 static float inner_prod(spx_sig_t *x, spx_sig_t *y, int len)
45 {
46    int i;
47    float sum1=0,sum2=0,sum3=0,sum4=0;
48    for (i=0;i<len;)
49    {
50       sum1 += x[i]*y[i];
51       sum2 += x[i+1]*y[i+1];
52       sum3 += x[i+2]*y[i+2];
53       sum4 += x[i+3]*y[i+3];
54       i+=4;
55    }
56    return sum1+sum2+sum3+sum4;
57 }
58 #endif
59
60 /*Original, non-optimized version*/
61 /*static float inner_prod(float *x, float *y, int len)
62 {
63    int i;
64    float sum=0;
65    for (i=0;i<len;i++)
66       sum += x[i]*y[i];
67    return sum;
68 }
69 */
70
71
72 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, float *gain, int N, char *stack)
73 {
74    int i,j,k;
75    /*float corr=0;*/
76    /*float energy;*/
77    float *best_score;
78    float e0;
79    float *corr, *energy, *score;
80
81    best_score = PUSH(stack,N, float);
82    corr = PUSH(stack,end-start+1, float);
83    energy = PUSH(stack,end-start+2, float);
84    score = PUSH(stack,end-start+1, float);
85    for (i=0;i<N;i++)
86    {
87         best_score[i]=-1;
88         gain[i]=0;
89    }
90    energy[0]=inner_prod(sw-start, sw-start, len);
91    e0=inner_prod(sw, sw, len);
92    for (i=start;i<=end;i++)
93    {
94       /* Update energy for next pitch*/
95       energy[i-start+1] = energy[i-start] + sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
96    }
97    for (i=start;i<=end;i++)
98    {
99       corr[i-start]=0;
100       score[i-start]=0;
101    }
102
103    for (i=start;i<=end;i++)
104    {
105       /* Compute correlation*/
106       corr[i-start]=inner_prod(sw, sw-i, len);
107       score[i-start]=corr[i-start]*corr[i-start]/(energy[i-start]+1);
108    }
109    for (i=start;i<=end;i++)
110    {
111       if (score[i-start]>best_score[N-1])
112       {
113          float g1, g;
114          g1 = corr[i-start]/(energy[i-start]+10);
115          g = sqrt(g1*corr[i-start]/(e0+10));
116          if (g>g1)
117             g=g1;
118          if (g<0)
119             g=0;
120          for (j=0;j<N;j++)
121          {
122             if (score[i-start] > best_score[j])
123             {
124                for (k=N-1;k>j;k--)
125                {
126                   best_score[k]=best_score[k-1];
127                   pitch[k]=pitch[k-1];
128                   gain[k] = gain[k-1];
129                }
130                best_score[j]=score[i-start];
131                pitch[j]=i;
132                gain[j]=g;
133                break;
134             }
135          }
136       }
137    }
138
139 }
140
141
142
143
144 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
145 static float pitch_gain_search_3tap(
146 spx_sig_t target[],                 /* Target vector */
147 spx_coef_t ak[],                     /* LPCs for this subframe */
148 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
149 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
150 spx_sig_t exc[],                    /* Excitation */
151 void *par,
152 int   pitch,                    /* Pitch value */
153 int   p,                        /* Number of LPC coeffs */
154 int   nsf,                      /* Number of samples in subframe */
155 SpeexBits *bits,
156 char *stack,
157 spx_sig_t *exc2,
158 spx_sig_t *r,
159 int  *cdbk_index,
160 int cdbk_offset
161 )
162 {
163    int i,j;
164    spx_sig_t *tmp, *tmp2;
165    spx_sig_t *x[3];
166    spx_sig_t *e[3];
167    float corr[3];
168    float A[3][3];
169    float gain[3];
170    int   gain_cdbk_size;
171    signed char *gain_cdbk;
172    float err1,err2;
173
174    ltp_params *params;
175    params = (ltp_params*) par;
176    gain_cdbk_size = 1<<params->gain_bits;
177    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
178    tmp = PUSH(stack, 3*nsf, spx_sig_t);
179    tmp2 = PUSH(stack, 3*nsf, spx_sig_t);
180
181    x[0]=tmp;
182    x[1]=tmp+nsf;
183    x[2]=tmp+2*nsf;
184
185    e[0]=tmp2;
186    e[1]=tmp2+nsf;
187    e[2]=tmp2+2*nsf;
188    
189    for (i=2;i>=0;i--)
190    {
191       int pp=pitch+1-i;
192       for (j=0;j<nsf;j++)
193       {
194          if (j-pp<0)
195             e[i][j]=exc2[j-pp];
196          else if (j-pp-pitch<0)
197             e[i][j]=exc2[j-pp-pitch];
198          else
199             e[i][j]=0;
200       }
201
202       if (i==2)
203          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
204       else {
205          for (j=0;j<nsf-1;j++)
206             x[i][j+1]=x[i+1][j];
207          x[i][0]=0;
208          for (j=0;j<nsf;j++)
209             x[i][j]+=e[i][0]*r[j]/SIG_SCALING;
210       }
211    }
212
213    for (i=0;i<3;i++)
214       corr[i]=inner_prod(x[i],target,nsf);
215    
216    for (i=0;i<3;i++)
217       for (j=0;j<=i;j++)
218          A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
219    
220    {
221       float C[9];
222       signed char *ptr=gain_cdbk;
223       int best_cdbk=0;
224       float best_sum=0;
225       C[0]=corr[2];
226       C[1]=corr[1];
227       C[2]=corr[0];
228       C[3]=A[1][2];
229       C[4]=A[0][1];
230       C[5]=A[0][2];
231       C[6]=A[2][2];
232       C[7]=A[1][1];
233       C[8]=A[0][0];
234       
235       for (i=0;i<gain_cdbk_size;i++)
236       {
237          float sum=0;
238          float g0,g1,g2;
239          ptr = gain_cdbk+3*i;
240          g0=0.015625*ptr[0]+.5;
241          g1=0.015625*ptr[1]+.5;
242          g2=0.015625*ptr[2]+.5;
243
244          sum += C[0]*g0;
245          sum += C[1]*g1;
246          sum += C[2]*g2;
247          sum -= C[3]*g0*g1;
248          sum -= C[4]*g2*g1;
249          sum -= C[5]*g2*g0;
250          sum -= .5*C[6]*g0*g0;
251          sum -= .5*C[7]*g1*g1;
252          sum -= .5*C[8]*g2*g2;
253
254          /* If 1, force "safe" pitch values to handle packet loss better */
255          if (0) {
256             float tot = fabs(ptr[1]);
257             if (ptr[0]>0)
258                tot+=ptr[0];
259             if (ptr[2]>0)
260                tot+=ptr[2];
261             if (tot>1)
262                continue;
263          }
264
265          if (sum>best_sum || i==0)
266          {
267             best_sum=sum;
268             best_cdbk=i;
269          }
270       }
271       gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5;
272       gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5;
273       gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5;
274
275       *cdbk_index=best_cdbk;
276    }
277    
278    for (i=0;i<nsf;i++)
279       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
280    
281    err1=0;
282    err2=0;
283    for (i=0;i<nsf;i++)
284       err1+=target[i]*target[i];
285    for (i=0;i<nsf;i++)
286       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
287       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
288
289    return err2;
290 }
291
292
293 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
294 int pitch_search_3tap(
295 spx_sig_t target[],                 /* Target vector */
296 spx_sig_t *sw,
297 spx_coef_t ak[],                     /* LPCs for this subframe */
298 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
299 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
300 spx_sig_t exc[],                    /* Excitation */
301 void *par,
302 int   start,                    /* Smallest pitch value allowed */
303 int   end,                      /* Largest pitch value allowed */
304 float pitch_coef,               /* Voicing (pitch) coefficient */
305 int   p,                        /* Number of LPC coeffs */
306 int   nsf,                      /* Number of samples in subframe */
307 SpeexBits *bits,
308 char *stack,
309 spx_sig_t *exc2,
310 spx_sig_t *r,
311 int complexity,
312 int cdbk_offset
313 )
314 {
315    int i,j;
316    int cdbk_index, pitch=0, best_gain_index=0;
317    float *best_exc;
318    int best_pitch=0;
319    float err, best_err=-1;
320    int N;
321    ltp_params *params;
322    int *nbest;
323    float *gains;
324
325    N=complexity;
326    if (N>10)
327       N=10;
328
329    nbest=PUSH(stack, N, int);
330    gains = PUSH(stack, N, float);
331    params = (ltp_params*) par;
332
333    if (N==0 || end<start)
334    {
335       speex_bits_pack(bits, 0, params->pitch_bits);
336       speex_bits_pack(bits, 0, params->gain_bits);
337       for (i=0;i<nsf;i++)
338          exc[i]=0;
339       return start;
340    }
341    
342    best_exc=PUSH(stack,nsf, float);
343    
344    if (N>end-start+1)
345       N=end-start+1;
346    open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
347    for (i=0;i<N;i++)
348    {
349       pitch=nbest[i];
350       for (j=0;j<nsf;j++)
351          exc[j]=0;
352       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
353                                  bits, stack, exc2, r, &cdbk_index, cdbk_offset);
354       if (err<best_err || best_err<0)
355       {
356          for (j=0;j<nsf;j++)
357             best_exc[j]=exc[j];
358          best_err=err;
359          best_pitch=pitch;
360          best_gain_index=cdbk_index;
361       }
362    }
363    
364    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
365    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
366    speex_bits_pack(bits, best_gain_index, params->gain_bits);
367    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
368    for (i=0;i<nsf;i++)
369       exc[i]=best_exc[i];
370
371    return pitch;
372 }
373
374 void pitch_unquant_3tap(
375 spx_sig_t exc[],                    /* Excitation */
376 int   start,                    /* Smallest pitch value allowed */
377 int   end,                      /* Largest pitch value allowed */
378 float pitch_coef,               /* Voicing (pitch) coefficient */
379 void *par,
380 int   nsf,                      /* Number of samples in subframe */
381 int *pitch_val,
382 float *gain_val,
383 SpeexBits *bits,
384 char *stack,
385 int count_lost,
386 int subframe_offset,
387 float last_pitch_gain,
388 int cdbk_offset
389 )
390 {
391    int i;
392    int pitch;
393    int gain_index;
394    float gain[3];
395    signed char *gain_cdbk;
396    int gain_cdbk_size;
397    ltp_params *params;
398    params = (ltp_params*) par;
399    gain_cdbk_size = 1<<params->gain_bits;
400    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset;
401
402    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
403    pitch += start;
404    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
405    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
406    gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5;
407    gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5;
408    gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5;
409
410    if (count_lost && pitch > subframe_offset)
411    {
412       float gain_sum;
413       if (1) {
414          float tmp = count_lost < 4 ? last_pitch_gain : 0.4 * last_pitch_gain;
415          if (tmp>.95)
416             tmp=.95;
417          gain_sum = fabs(gain[1]);
418          if (gain[0]>0)
419             gain_sum += gain[0];
420          else
421             gain_sum -= .5*gain[0];
422          if (gain[2]>0)
423             gain_sum += gain[2];
424          else
425             gain_sum -= .5*gain[2];
426          if (gain_sum > tmp) {
427             float fact = tmp/gain_sum;
428             for (i=0;i<3;i++)
429                gain[i]*=fact;
430
431          }
432
433       }
434
435       if (0) {
436       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
437          if (gain_sum>.95) {
438          float fact = .95/gain_sum;
439          for (i=0;i<3;i++)
440             gain[i]*=fact;
441       }
442    }
443    }
444
445    *pitch_val = pitch;
446    /**gain_val = gain[0]+gain[1]+gain[2];*/
447    gain_val[0]=gain[0];
448    gain_val[1]=gain[1];
449    gain_val[2]=gain[2];
450
451    {
452       spx_sig_t *e[3];
453       spx_sig_t *tmp2;
454       tmp2=PUSH(stack, 3*nsf, spx_sig_t);
455       e[0]=tmp2;
456       e[1]=tmp2+nsf;
457       e[2]=tmp2+2*nsf;
458       
459       for (i=0;i<3;i++)
460       {
461          int j;
462          int pp=pitch+1-i;
463 #if 0
464          for (j=0;j<nsf;j++)
465          {
466             if (j-pp<0)
467                e[i][j]=exc[j-pp];
468             else if (j-pp-pitch<0)
469                e[i][j]=exc[j-pp-pitch];
470             else
471                e[i][j]=0;
472          }
473 #else
474          {
475             int tmp1, tmp3;
476             tmp1=nsf;
477             if (tmp1>pp)
478                tmp1=pp;
479             for (j=0;j<tmp1;j++)
480                e[i][j]=exc[j-pp];
481             tmp3=nsf;
482             if (tmp3>pp+pitch)
483                tmp3=pp+pitch;
484             for (j=tmp1;j<tmp3;j++)
485                e[i][j]=exc[j-pp-pitch];
486             for (j=tmp3;j<nsf;j++)
487                e[i][j]=0;
488          }
489 #endif
490       }
491       for (i=0;i<nsf;i++)
492            exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
493    }
494 }
495
496
497 /** Forced pitch delay and gain */
498 int forced_pitch_quant(
499 spx_sig_t target[],                 /* Target vector */
500 spx_sig_t *sw,
501 spx_coef_t ak[],                     /* LPCs for this subframe */
502 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
503 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
504 spx_sig_t exc[],                    /* Excitation */
505 void *par,
506 int   start,                    /* Smallest pitch value allowed */
507 int   end,                      /* Largest pitch value allowed */
508 float pitch_coef,               /* Voicing (pitch) coefficient */
509 int   p,                        /* Number of LPC coeffs */
510 int   nsf,                      /* Number of samples in subframe */
511 SpeexBits *bits,
512 char *stack,
513 spx_sig_t *exc2,
514 spx_sig_t *r,
515 int complexity,
516 int cdbk_offset
517 )
518 {
519    int i;
520    if (pitch_coef>.99)
521       pitch_coef=.99;
522    for (i=0;i<nsf;i++)
523    {
524       exc[i]=exc[i-start]*pitch_coef;
525    }
526    return start;
527 }
528
529 /** Unquantize forced pitch delay and gain */
530 void forced_pitch_unquant(
531 spx_sig_t exc[],                    /* Excitation */
532 int   start,                    /* Smallest pitch value allowed */
533 int   end,                      /* Largest pitch value allowed */
534 float pitch_coef,               /* Voicing (pitch) coefficient */
535 void *par,
536 int   nsf,                      /* Number of samples in subframe */
537 int *pitch_val,
538 float *gain_val,
539 SpeexBits *bits,
540 char *stack,
541 int count_lost,
542 int subframe_offset,
543 float last_pitch_gain,
544 int cdbk_offset
545 )
546 {
547    int i;
548    /*pitch_coef=.9;*/
549    if (pitch_coef>.99)
550       pitch_coef=.99;
551    for (i=0;i<nsf;i++)
552    {
553       exc[i]=exc[i-start]*pitch_coef;
554    }
555    *pitch_val = start;
556    gain_val[0]=gain_val[2]=0;
557    gain_val[1] = pitch_coef;
558 }