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