short-block MDCT
[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    return;
48    int d, n, k;
49    for (d=0;d<dim;d++)
50    {
51       float x[len];
52       for (n=0;n<len;n++)
53          x[n] = X[dim*n+d];
54       for (k=0;k<len;k++)
55       {
56          float sum = 0;
57          for (n=0;n<len;n++)
58             sum += x[n]*cos(M_PI/len*(n+.5)*(k+.5));
59          X[dim*k+d] = sqrt(2.f/len)*sum;
60       }
61    }
62 }
63 #if 0
64 void exp_rotation(celt_norm_t *X, int len, int dir, int stride, int iter)
65 {
66    int i, k;
67    celt_word16_t c, s;
68    celt_norm_t *Xptr;
69    /* Equivalent to cos(.3) and sin(.3) */
70    c = QCONST16(0.95534,15);
71    s = dir*QCONST16(0.29552,15);
72    for (k=0;k<iter;k++)
73    {
74       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
75          but at this point, I really don't think it's necessary */
76       Xptr = X;
77       for (i=0;i<len-stride;i++)
78       {
79          celt_norm_t x1, x2;
80          x1 = Xptr[0];
81          x2 = Xptr[stride];
82          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
83          *Xptr++      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
84       }
85       Xptr = &X[len-2*stride-1];
86       for (i=len-2*stride-1;i>=0;i--)
87       {
88          celt_norm_t x1, x2;
89          x1 = Xptr[0];
90          x2 = Xptr[stride];
91          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
92          *Xptr--      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
93       }
94    }
95 }
96 #endif
97
98
99 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
100
101 #ifdef FIXED_POINT
102 /* Compute the amplitude (sqrt energy) in each of the bands */
103 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
104 {
105    int i, c;
106    const celt_int16_t *eBands = m->eBands;
107    const int C = CHANNELS(m);
108    for (c=0;c<C;c++)
109    {
110       for (i=0;i<m->nbEBands;i++)
111       {
112          int j;
113          celt_word32_t maxval=0;
114          celt_word32_t sum = 0;
115          
116          j=eBands[i]; do {
117             maxval = MAX32(maxval, X[j*C+c]);
118             maxval = MAX32(maxval, -X[j*C+c]);
119          } while (++j<eBands[i+1]);
120          
121          if (maxval > 0)
122          {
123             int shift = celt_ilog2(maxval)-10;
124             j=eBands[i]; do {
125                sum += MULT16_16(EXTRACT16(VSHR32(X[j*C+c],shift)),
126                                 EXTRACT16(VSHR32(X[j*C+c],shift)));
127             } while (++j<eBands[i+1]);
128             /* We're adding one here to make damn sure we never end up with a pitch vector that's
129                larger than unity norm */
130             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
131          } else {
132             bank[i*C+c] = EPSILON;
133          }
134          /*printf ("%f ", bank[i*C+c]);*/
135       }
136    }
137    /*printf ("\n");*/
138 }
139
140 /* Normalise each band such that the energy is one. */
141 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
142 {
143    int i, c;
144    const celt_int16_t *eBands = m->eBands;
145    const int C = CHANNELS(m);
146    for (c=0;c<C;c++)
147    {
148       i=0; do {
149          celt_word16_t g;
150          int j,shift;
151          celt_word16_t E;
152          shift = celt_zlog2(bank[i*C+c])-13;
153          E = VSHR32(bank[i*C+c], shift);
154          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
155          j=eBands[i]; do {
156             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
157          } while (++j<eBands[i+1]);
158       } while (++i<m->nbEBands);
159    }
160 }
161
162 #ifndef DISABLE_STEREO
163 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
164 {
165    int i;
166    VARDECL(celt_ener_t, tmpE);
167    VARDECL(celt_sig_t, freq);
168    SAVE_STACK;
169    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
170    ALLOC(freq, m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
171    for (i=0;i<m->nbChannels*m->eBands[m->nbEBands+1];i++)
172       freq[i] = SHL32(EXTEND32(X[i]), 10);
173    compute_band_energies(m, freq, tmpE);
174    normalise_bands(m, freq, X, tmpE);
175    RESTORE_STACK;
176 }
177 #endif /* DISABLE_STEREO */
178 #else /* FIXED_POINT */
179 /* Compute the amplitude (sqrt energy) in each of the bands */
180 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
181 {
182    int i, c;
183    const celt_int16_t *eBands = m->eBands;
184    const int C = CHANNELS(m);
185    for (c=0;c<C;c++)
186    {
187       for (i=0;i<m->nbEBands;i++)
188       {
189          int j;
190          celt_word32_t sum = 1e-10;
191          for (j=eBands[i];j<eBands[i+1];j++)
192             sum += X[j*C+c]*X[j*C+c];
193          bank[i*C+c] = sqrt(sum);
194          /*printf ("%f ", bank[i*C+c]);*/
195       }
196    }
197    /*printf ("\n");*/
198 }
199
200 /* Normalise each band such that the energy is one. */
201 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
202 {
203    int i, c;
204    const celt_int16_t *eBands = m->eBands;
205    const int C = CHANNELS(m);
206    for (c=0;c<C;c++)
207    {
208       for (i=0;i<m->nbEBands;i++)
209       {
210          int j;
211          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
212          for (j=eBands[i];j<eBands[i+1];j++)
213             X[j*C+c] = freq[j*C+c]*g;
214       }
215    }
216 }
217
218 #ifndef DISABLE_STEREO
219 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
220 {
221    VARDECL(celt_ener_t, tmpE);
222    SAVE_STACK;
223    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
224    compute_band_energies(m, X, tmpE);
225    normalise_bands(m, X, X, tmpE);
226    RESTORE_STACK;
227 }
228 #endif /* DISABLE_STEREO */
229 #endif /* FIXED_POINT */
230
231 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
232 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
233 {
234    int i, c;
235    const celt_int16_t *eBands = m->eBands;
236    const int C = CHANNELS(m);
237    if (C>2)
238       celt_fatal("denormalise_bands() not implemented for >2 channels");
239    for (c=0;c<C;c++)
240    {
241       for (i=0;i<m->nbEBands;i++)
242       {
243          int j;
244          celt_word32_t g = MULT16_32_Q13(sqrtC_1[C-1],bank[i*C+c]);
245          j=eBands[i]; do {
246             freq[j*C+c] = MULT16_32_Q15(X[j*C+c], g);
247          } while (++j<eBands[i+1]);
248       }
249    }
250    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
251       freq[i] = 0;
252 }
253
254
255 /* Compute the best gain for each "pitch band" */
256 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
257 {
258    int i;
259    const celt_int16_t *pBands = m->pBands;
260    const int C = CHANNELS(m);
261
262    for (i=0;i<m->nbPBands;i++)
263    {
264       celt_word32_t Sxy=0, Sxx=0;
265       int j;
266       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
267       for (j=C*pBands[i];j<C*pBands[i+1];j++)
268       {
269          Sxy = MAC16_16(Sxy, X[j], P[j]);
270          Sxx = MAC16_16(Sxx, X[j], X[j]);
271       }
272       /* No negative gain allowed */
273       if (Sxy < 0)
274          Sxy = 0;
275       /* Not sure how that would happen, just making sure */
276       if (Sxy > Sxx)
277          Sxy = Sxx;
278       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
279          residual doesn't quantise well */
280       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
281       /* gain = Sxy/Sxx */
282       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
283       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
284    }
285    /*if(rand()%10==0)
286    {
287       for (i=0;i<m->nbPBands;i++)
288          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
289       printf ("\n");
290    }*/
291 }
292
293 /* Apply the (quantised) gain to each "pitch band" */
294 void pitch_quant_bands(const CELTMode *m, celt_norm_t * restrict P, const celt_pgain_t * restrict gains)
295 {
296    int i;
297    const celt_int16_t *pBands = m->pBands;
298    const int C = CHANNELS(m);
299    for (i=0;i<m->nbPBands;i++)
300    {
301       int j;
302       for (j=C*pBands[i];j<C*pBands[i+1];j++)
303          P[j] = MULT16_16_Q15(gains[i], P[j]);
304       /*printf ("%f ", gain);*/
305    }
306    for (i=C*pBands[m->nbPBands];i<C*pBands[m->nbPBands+1];i++)
307       P[i] = 0;
308 }
309
310 static void intensity_band(celt_norm_t * restrict X, int len)
311 {
312    int j;
313    celt_word32_t E = 1e-15;
314    celt_word32_t E2 = 1e-15;
315    for (j=0;j<len;j++)
316    {
317       X[j] = X[2*j];
318       E += MULT16_16(X[j],X[j]);
319       E2 += MULT16_16(X[2*j+1],X[2*j+1]);
320    }
321 #ifndef FIXED_POINT
322    E  = celt_sqrt(E+E2)/celt_sqrt(E);
323    for (j=0;j<len;j++)
324       X[j] *= E;
325 #endif
326    for (j=0;j<len;j++)
327       X[len+j] = 0;
328
329 }
330
331 static void dup_band(celt_norm_t * restrict X, int len)
332 {
333    int j;
334    for (j=len-1;j>=0;j--)
335    {
336       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
337       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
338    }
339 }
340
341 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)
342 {
343    int i = bandID;
344    const celt_int16_t *eBands = m->eBands;
345    const int C = CHANNELS(m);
346    {
347       int j;
348       if (stereo_mode[i] && dir <0)
349       {
350          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
351       } else {
352          celt_word16_t a1, a2;
353          if (stereo_mode[i]==0)
354          {
355             /* Do mid-side when not doing intensity stereo */
356             a1 = QCONST16(.70711f,14);
357             a2 = dir*QCONST16(.70711f,14);
358          } else {
359             celt_word16_t left, right;
360             celt_word16_t norm;
361 #ifdef FIXED_POINT
362             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
363 #endif
364             left = VSHR32(bank[i*C],shift);
365             right = VSHR32(bank[i*C+1],shift);
366             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
367             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
368             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
369          }
370          for (j=eBands[i];j<eBands[i+1];j++)
371          {
372             celt_norm_t r, l;
373             l = X[j*C];
374             r = X[j*C+1];
375             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
376             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
377          }
378       }
379       if (stereo_mode[i] && dir>0)
380       {
381          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
382       }
383    }
384 }
385
386 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
387 {
388    int i;
389    for (i=0;i<len-5;i++)
390       stereo_mode[i] = 0;
391    for (;i<len;i++)
392       stereo_mode[i] = 0;
393 }
394
395
396
397 /* Quantisation of the residual */
398 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)
399 {
400    int i, j, bits;
401    const celt_int16_t * restrict eBands = m->eBands;
402    celt_norm_t * restrict norm;
403    VARDECL(celt_norm_t, _norm);
404    VARDECL(int, pulses);
405    VARDECL(int, offsets);
406    const int C = CHANNELS(m);
407    SAVE_STACK;
408
409    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
410    ALLOC(pulses, m->nbEBands, int);
411    ALLOC(offsets, m->nbEBands, int);
412    norm = _norm;
413
414    for (i=0;i<m->nbEBands;i++)
415       offsets[i] = 0;
416    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
417    bits = total_bits - ec_enc_tell(enc, 0) - 1;
418    compute_allocation(m, offsets, stereo_mode, bits, pulses);
419    
420    /*printf("bits left: %d\n", bits);
421    for (i=0;i<m->nbEBands;i++)
422       printf ("%d ", pulses[i]);
423    printf ("\n");*/
424    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
425    for (i=0;i<m->nbEBands;i++)
426    {
427       int q;
428       celt_word16_t n;
429       q = pulses[i];
430       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
431
432       if (time_domain)
433          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
434       /* If pitch isn't available, use intra-frame prediction */
435       if (eBands[i] >= m->pitchEnd || q<=0)
436       {
437          q -= 1;
438          if (!time_domain)
439          {
440             if (q<0)
441                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
442             else
443                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);
444          }
445       }
446       
447       if (q > 0)
448       {
449          int ch=C;
450          if (C==2 && stereo_mode[i]==1)
451             ch = 1;
452          if (C==2)
453          {
454             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
455             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
456          }
457          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
458          if (C==2)
459             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
460       } else {
461          for (j=C*eBands[i];j<C*eBands[i+1];j++)
462             X[j] = P[j];
463       }
464       if (time_domain)
465          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
466       for (j=C*eBands[i];j<C*eBands[i+1];j++)
467          norm[j] = MULT16_16_Q15(n,X[j]);
468    }
469    RESTORE_STACK;
470 }
471
472 /* Decoding of the residual */
473 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)
474 {
475    int i, j, bits;
476    const celt_int16_t * restrict eBands = m->eBands;
477    celt_norm_t * restrict norm;
478    VARDECL(celt_norm_t, _norm);
479    VARDECL(int, pulses);
480    VARDECL(int, offsets);
481    const int C = CHANNELS(m);
482    SAVE_STACK;
483
484    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
485    ALLOC(pulses, m->nbEBands, int);
486    ALLOC(offsets, m->nbEBands, int);
487    norm = _norm;
488
489    for (i=0;i<m->nbEBands;i++)
490       offsets[i] = 0;
491    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
492    bits = total_bits - ec_dec_tell(dec, 0) - 1;
493    compute_allocation(m, offsets, stereo_mode, bits, pulses);
494
495    for (i=0;i<m->nbEBands;i++)
496    {
497       int q;
498       celt_word16_t n;
499       q = pulses[i];
500       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
501
502       /* If pitch isn't available, use intra-frame prediction */
503       if (eBands[i] >= m->pitchEnd || q<=0)
504       {
505          q -= 1;
506          if (!time_domain)
507          {
508             if (q<0)
509                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
510             else
511                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);
512          }
513       }
514       
515       if (q > 0)
516       {
517          int ch=C;
518          if (C==2 && stereo_mode[i]==1)
519             ch = 1;
520          if (C==2)
521             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
522          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
523          if (C==2)
524             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
525       } else {
526          for (j=C*eBands[i];j<C*eBands[i+1];j++)
527             X[j] = P[j];
528       }
529       if (time_domain)
530          dctIV(X+C*eBands[i], eBands[i+1]-eBands[i], C);
531       for (j=C*eBands[i];j<C*eBands[i+1];j++)
532          norm[j] = MULT16_16_Q15(n,X[j]);
533    }
534    RESTORE_STACK;
535 }
536