Implemented two pre-echo avoidance techniques: time-domain pre-emphasis and
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2 */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44
45 static void dctIV(float *X, int len, int dim)
46 {
47    int d, n, k;
48    for (d=0;d<dim;d++)
49    {
50       float x[len];
51       for (n=0;n<len;n++)
52          x[n] = X[dim*n+d];
53       for (k=0;k<len;k++)
54       {
55          float sum = 0;
56          for (n=0;n<len;n++)
57             sum += x[n]*cos(M_PI/len*(n+.5)*(k+.5));
58          X[dim*k+d] = sqrt(2.f/len)*sum;
59       }
60    }
61 }
62 #if 0
63 void exp_rotation(celt_norm_t *X, int len, int dir, int stride, int iter)
64 {
65    int i, k;
66    celt_word16_t c, s;
67    celt_norm_t *Xptr;
68    /* Equivalent to cos(.3) and sin(.3) */
69    c = QCONST16(0.95534,15);
70    s = dir*QCONST16(0.29552,15);
71    for (k=0;k<iter;k++)
72    {
73       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
74          but at this point, I really don't think it's necessary */
75       Xptr = X;
76       for (i=0;i<len-stride;i++)
77       {
78          celt_norm_t x1, x2;
79          x1 = Xptr[0];
80          x2 = Xptr[stride];
81          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
82          *Xptr++      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
83       }
84       Xptr = &X[len-2*stride-1];
85       for (i=len-2*stride-1;i>=0;i--)
86       {
87          celt_norm_t x1, x2;
88          x1 = Xptr[0];
89          x2 = Xptr[stride];
90          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
91          *Xptr--      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
92       }
93    }
94 }
95 #endif
96
97
98 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
99
100 #ifdef FIXED_POINT
101 /* Compute the amplitude (sqrt energy) in each of the bands */
102 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
103 {
104    int i, c;
105    const celt_int16_t *eBands = m->eBands;
106    const int C = CHANNELS(m);
107    for (c=0;c<C;c++)
108    {
109       for (i=0;i<m->nbEBands;i++)
110       {
111          int j;
112          celt_word32_t maxval=0;
113          celt_word32_t sum = 0;
114          
115          j=eBands[i]; do {
116             maxval = MAX32(maxval, X[j*C+c]);
117             maxval = MAX32(maxval, -X[j*C+c]);
118          } while (++j<eBands[i+1]);
119          
120          if (maxval > 0)
121          {
122             int shift = celt_ilog2(maxval)-10;
123             j=eBands[i]; do {
124                sum += MULT16_16(EXTRACT16(VSHR32(X[j*C+c],shift)),
125                                 EXTRACT16(VSHR32(X[j*C+c],shift)));
126             } while (++j<eBands[i+1]);
127             /* We're adding one here to make damn sure we never end up with a pitch vector that's
128                larger than unity norm */
129             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
130          } else {
131             bank[i*C+c] = EPSILON;
132          }
133          /*printf ("%f ", bank[i*C+c]);*/
134       }
135    }
136    /*printf ("\n");*/
137 }
138
139 /* Normalise each band such that the energy is one. */
140 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
141 {
142    int i, c;
143    const celt_int16_t *eBands = m->eBands;
144    const int C = CHANNELS(m);
145    for (c=0;c<C;c++)
146    {
147       i=0; do {
148          celt_word16_t g;
149          int j,shift;
150          celt_word16_t E;
151          shift = celt_zlog2(bank[i*C+c])-13;
152          E = VSHR32(bank[i*C+c], shift);
153          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
154          j=eBands[i]; do {
155             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
156          } while (++j<eBands[i+1]);
157       } while (++i<m->nbEBands);
158    }
159 }
160
161 #ifndef DISABLE_STEREO
162 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
163 {
164    int i;
165    VARDECL(celt_ener_t, tmpE);
166    VARDECL(celt_sig_t, freq);
167    SAVE_STACK;
168    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
169    ALLOC(freq, m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
170    for (i=0;i<m->nbChannels*m->eBands[m->nbEBands+1];i++)
171       freq[i] = SHL32(EXTEND32(X[i]), 10);
172    compute_band_energies(m, freq, tmpE);
173    normalise_bands(m, freq, X, tmpE);
174    RESTORE_STACK;
175 }
176 #endif /* DISABLE_STEREO */
177 #else /* FIXED_POINT */
178 /* Compute the amplitude (sqrt energy) in each of the bands */
179 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
180 {
181    int i, c;
182    const celt_int16_t *eBands = m->eBands;
183    const int C = CHANNELS(m);
184    for (c=0;c<C;c++)
185    {
186       for (i=0;i<m->nbEBands;i++)
187       {
188          int j;
189          celt_word32_t sum = 1e-10;
190          for (j=eBands[i];j<eBands[i+1];j++)
191             sum += X[j*C+c]*X[j*C+c];
192          bank[i*C+c] = sqrt(sum);
193          /*printf ("%f ", bank[i*C+c]);*/
194       }
195    }
196    /*printf ("\n");*/
197 }
198
199 /* Normalise each band such that the energy is one. */
200 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
201 {
202    int i, c;
203    const celt_int16_t *eBands = m->eBands;
204    const int C = CHANNELS(m);
205    for (c=0;c<C;c++)
206    {
207       for (i=0;i<m->nbEBands;i++)
208       {
209          int j;
210          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
211          for (j=eBands[i];j<eBands[i+1];j++)
212             X[j*C+c] = freq[j*C+c]*g;
213       }
214    }
215 }
216
217 #ifndef DISABLE_STEREO
218 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
219 {
220    VARDECL(celt_ener_t, tmpE);
221    SAVE_STACK;
222    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
223    compute_band_energies(m, X, tmpE);
224    normalise_bands(m, X, X, tmpE);
225    RESTORE_STACK;
226 }
227 #endif /* DISABLE_STEREO */
228 #endif /* FIXED_POINT */
229
230 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
231 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
232 {
233    int i, c;
234    const celt_int16_t *eBands = m->eBands;
235    const int C = CHANNELS(m);
236    if (C>2)
237       celt_fatal("denormalise_bands() not implemented for >2 channels");
238    for (c=0;c<C;c++)
239    {
240       for (i=0;i<m->nbEBands;i++)
241       {
242          int j;
243          celt_word32_t g = MULT16_32_Q13(sqrtC_1[C-1],bank[i*C+c]);
244          j=eBands[i]; do {
245             freq[j*C+c] = MULT16_32_Q15(X[j*C+c], g);
246          } while (++j<eBands[i+1]);
247       }
248    }
249    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
250       freq[i] = 0;
251 }
252
253
254 /* Compute the best gain for each "pitch band" */
255 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
256 {
257    int i;
258    const celt_int16_t *pBands = m->pBands;
259    const int C = CHANNELS(m);
260
261    for (i=0;i<m->nbPBands;i++)
262    {
263       celt_word32_t Sxy=0, Sxx=0;
264       int j;
265       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
266       for (j=C*pBands[i];j<C*pBands[i+1];j++)
267       {
268          Sxy = MAC16_16(Sxy, X[j], P[j]);
269          Sxx = MAC16_16(Sxx, X[j], X[j]);
270       }
271       /* No negative gain allowed */
272       if (Sxy < 0)
273          Sxy = 0;
274       /* Not sure how that would happen, just making sure */
275       if (Sxy > Sxx)
276          Sxy = Sxx;
277       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
278          residual doesn't quantise well */
279       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
280       /* gain = Sxy/Sxx */
281       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
282       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
283    }
284    /*if(rand()%10==0)
285    {
286       for (i=0;i<m->nbPBands;i++)
287          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
288       printf ("\n");
289    }*/
290 }
291
292 /* Apply the (quantised) gain to each "pitch band" */
293 void pitch_quant_bands(const CELTMode *m, celt_norm_t * restrict P, const celt_pgain_t * restrict gains)
294 {
295    int i;
296    const celt_int16_t *pBands = m->pBands;
297    const int C = CHANNELS(m);
298    for (i=0;i<m->nbPBands;i++)
299    {
300       int j;
301       for (j=C*pBands[i];j<C*pBands[i+1];j++)
302          P[j] = MULT16_16_Q15(gains[i], P[j]);
303       /*printf ("%f ", gain);*/
304    }
305    for (i=C*pBands[m->nbPBands];i<C*pBands[m->nbPBands+1];i++)
306       P[i] = 0;
307 }
308
309 static void intensity_band(celt_norm_t * restrict X, int len)
310 {
311    int j;
312    celt_word32_t E = 1e-15;
313    celt_word32_t E2 = 1e-15;
314    for (j=0;j<len;j++)
315    {
316       X[j] = X[2*j];
317       E += MULT16_16(X[j],X[j]);
318       E2 += MULT16_16(X[2*j+1],X[2*j+1]);
319    }
320 #ifndef FIXED_POINT
321    E  = celt_sqrt(E+E2)/celt_sqrt(E);
322    for (j=0;j<len;j++)
323       X[j] *= E;
324 #endif
325    for (j=0;j<len;j++)
326       X[len+j] = 0;
327
328 }
329
330 static void dup_band(celt_norm_t * restrict X, int len)
331 {
332    int j;
333    for (j=len-1;j>=0;j--)
334    {
335       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
336       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
337    }
338 }
339
340 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, const int *stereo_mode, int bandID, int dir)
341 {
342    int i = bandID;
343    const celt_int16_t *eBands = m->eBands;
344    const int C = CHANNELS(m);
345    {
346       int j;
347       if (stereo_mode[i] && dir <0)
348       {
349          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
350       } else {
351          celt_word16_t a1, a2;
352          if (stereo_mode[i]==0)
353          {
354             /* Do mid-side when not doing intensity stereo */
355             a1 = QCONST16(.70711f,14);
356             a2 = dir*QCONST16(.70711f,14);
357          } else {
358             celt_word16_t left, right;
359             celt_word16_t norm;
360 #ifdef FIXED_POINT
361             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
362 #endif
363             left = VSHR32(bank[i*C],shift);
364             right = VSHR32(bank[i*C+1],shift);
365             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
366             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
367             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
368          }
369          for (j=eBands[i];j<eBands[i+1];j++)
370          {
371             celt_norm_t r, l;
372             l = X[j*C];
373             r = X[j*C+1];
374             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
375             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
376          }
377       }
378       if (stereo_mode[i] && dir>0)
379       {
380          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
381       }
382    }
383 }
384
385 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
386 {
387    int i;
388    for (i=0;i<len-5;i++)
389       stereo_mode[i] = 0;
390    for (;i<len;i++)
391       stereo_mode[i] = 1;
392 }
393
394
395
396 /* Quantisation of the residual */
397 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, const celt_ener_t *bandE, const int *stereo_mode, int total_bits, int time_domain, ec_enc *enc)
398 {
399    int i, j, bits;
400    const celt_int16_t * restrict eBands = m->eBands;
401    celt_norm_t * restrict norm;
402    VARDECL(celt_norm_t, _norm);
403    VARDECL(int, pulses);
404    VARDECL(int, offsets);
405    const int C = CHANNELS(m);
406    SAVE_STACK;
407
408    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
409    ALLOC(pulses, m->nbEBands, int);
410    ALLOC(offsets, m->nbEBands, int);
411    norm = _norm;
412
413    for (i=0;i<m->nbEBands;i++)
414       offsets[i] = 0;
415    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
416    bits = total_bits - ec_enc_tell(enc, 0) - 1;
417    compute_allocation(m, offsets, stereo_mode, bits, pulses);
418    
419    /*printf("bits left: %d\n", bits);
420    for (i=0;i<m->nbEBands;i++)
421       printf ("%d ", pulses[i]);
422    printf ("\n");*/
423    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
424    for (i=0;i<m->nbEBands;i++)
425    {
426       int q;
427       celt_word16_t n;
428       q = pulses[i];
429       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
430
431       if (time_domain)
432          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
433       /* If pitch isn't available, use intra-frame prediction */
434       if (eBands[i] >= m->pitchEnd || q<=0)
435       {
436          q -= 1;
437          if (!time_domain)
438          {
439             if (q<0)
440                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
441             else
442                intra_prediction(m, X+C*eBands[i], W+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1], enc);
443          }
444       }
445       
446       if (q > 0)
447       {
448          int ch=C;
449          if (C==2 && stereo_mode[i]==1)
450             ch = 1;
451          if (C==2)
452          {
453             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
454             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
455          }
456          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
457          if (C==2)
458             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
459       } else {
460          for (j=C*eBands[i];j<C*eBands[i+1];j++)
461             X[j] = P[j];
462       }
463       if (time_domain)
464          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
465       for (j=C*eBands[i];j<C*eBands[i+1];j++)
466          norm[j] = MULT16_16_Q15(n,X[j]);
467    }
468    RESTORE_STACK;
469 }
470
471 /* Decoding of the residual */
472 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, const int *stereo_mode, int total_bits, int time_domain, ec_dec *dec)
473 {
474    int i, j, bits;
475    const celt_int16_t * restrict eBands = m->eBands;
476    celt_norm_t * restrict norm;
477    VARDECL(celt_norm_t, _norm);
478    VARDECL(int, pulses);
479    VARDECL(int, offsets);
480    const int C = CHANNELS(m);
481    SAVE_STACK;
482
483    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
484    ALLOC(pulses, m->nbEBands, int);
485    ALLOC(offsets, m->nbEBands, int);
486    norm = _norm;
487
488    for (i=0;i<m->nbEBands;i++)
489       offsets[i] = 0;
490    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
491    bits = total_bits - ec_dec_tell(dec, 0) - 1;
492    compute_allocation(m, offsets, stereo_mode, bits, pulses);
493
494    for (i=0;i<m->nbEBands;i++)
495    {
496       int q;
497       celt_word16_t n;
498       q = pulses[i];
499       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
500
501       /* If pitch isn't available, use intra-frame prediction */
502       if (eBands[i] >= m->pitchEnd || q<=0)
503       {
504          q -= 1;
505          if (!time_domain)
506          {
507             if (q<0)
508                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
509             else
510                intra_unquant(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1], dec);
511          }
512       }
513       
514       if (q > 0)
515       {
516          int ch=C;
517          if (C==2 && stereo_mode[i]==1)
518             ch = 1;
519          if (C==2)
520             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
521          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
522          if (C==2)
523             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
524       } else {
525          for (j=C*eBands[i];j<C*eBands[i+1];j++)
526             X[j] = P[j];
527       }
528       if (time_domain)
529          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
530       for (j=C*eBands[i];j<C*eBands[i+1];j++)
531          norm[j] = MULT16_16_Q15(n,X[j]);
532    }
533    RESTORE_STACK;
534 }
535