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