infrastructure changes for upcoming stereo improvements
[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 /* Quantisation of the residual */
293 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)
294 {
295    int i, j, bits;
296    const celt_int16_t * restrict eBands = m->eBands;
297    celt_norm_t * restrict norm;
298    VARDECL(celt_norm_t, _norm);
299    VARDECL(int, pulses);
300    VARDECL(int, offsets);
301    const int C = CHANNELS(m);
302    SAVE_STACK;
303
304    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
305    ALLOC(pulses, m->nbEBands, int);
306    ALLOC(offsets, m->nbEBands, int);
307    norm = _norm;
308
309    for (i=0;i<m->nbEBands;i++)
310       offsets[i] = 0;
311    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
312    bits = total_bits - ec_enc_tell(enc, 0) - 1;
313    compute_allocation(m, offsets, bits, pulses);
314    
315    /*printf("bits left: %d\n", bits);
316    for (i=0;i<m->nbEBands;i++)
317       printf ("%d ", pulses[i]);
318    printf ("\n");*/
319    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
320    for (i=0;i<m->nbEBands;i++)
321    {
322       int q;
323       celt_word16_t n;
324       q = pulses[i];
325       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
326
327       /* If pitch isn't available, use intra-frame prediction */
328       if (eBands[i] >= m->pitchEnd || q<=0)
329       {
330          q -= 1;
331          if (q<0)
332             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
333          else
334             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);
335       }
336       
337       if (q > 0)
338       {
339          /*int nb_rotations = q <= 2*C ? 2*C/q : 0;
340          if (nb_rotations != 0)
341          {
342             exp_rotation(P+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);
343             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);
344          }*/
345          alg_quant(X+C*eBands[i], W+C*eBands[i], C*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
346          /*if (nb_rotations != 0)
347             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), 1, C, nb_rotations);*/
348       }
349       for (j=C*eBands[i];j<C*eBands[i+1];j++)
350          norm[j] = MULT16_16_Q15(n,X[j]);
351    }
352    RESTORE_STACK;
353 }
354
355 /* Decoding of the residual */
356 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int total_bits, ec_dec *dec)
357 {
358    int i, j, bits;
359    const celt_int16_t * restrict eBands = m->eBands;
360    celt_norm_t * restrict norm;
361    VARDECL(celt_norm_t, _norm);
362    VARDECL(int, pulses);
363    VARDECL(int, offsets);
364    const int C = CHANNELS(m);
365    SAVE_STACK;
366
367    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
368    ALLOC(pulses, m->nbEBands, int);
369    ALLOC(offsets, m->nbEBands, int);
370    norm = _norm;
371
372    for (i=0;i<m->nbEBands;i++)
373       offsets[i] = 0;
374    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
375    bits = total_bits - ec_dec_tell(dec, 0) - 1;
376    compute_allocation(m, offsets, bits, pulses);
377
378    for (i=0;i<m->nbEBands;i++)
379    {
380       int q;
381       celt_word16_t n;
382       q = pulses[i];
383       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
384
385       /* If pitch isn't available, use intra-frame prediction */
386       if (eBands[i] >= m->pitchEnd || q<=0)
387       {
388          q -= 1;
389          if (q<0)
390             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
391          else
392             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);
393       }
394       
395       if (q > 0)
396       {
397          /*int nb_rotations = q <= 2*C ? 2*C/q : 0;
398          if (nb_rotations != 0)
399             exp_rotation(P+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);*/
400          alg_unquant(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
401          /*if (nb_rotations != 0)
402             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), 1, C, nb_rotations);*/
403       }
404       for (j=C*eBands[i];j<C*eBands[i+1];j++)
405          norm[j] = MULT16_16_Q15(n,X[j]);
406    }
407    RESTORE_STACK;
408 }
409
410 #ifndef DISABLE_STEREO
411 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
412 {
413    int i;
414    for (i=0;i<len-5;i++)
415       stereo_mode[i] = 0;
416    for (;i<len;i++)
417       stereo_mode[i] = 1;
418 }
419
420
421 void stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int dir)
422 {
423    int i;
424    const celt_int16_t *eBands = m->eBands;
425    const int C = CHANNELS(m);
426    for (i=0;i<m->nbEBands;i++)
427    {
428       int j;
429       celt_word16_t left, right;
430       celt_word16_t a1, a2;
431       celt_word16_t norm;
432 #ifdef FIXED_POINT
433       int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
434 #endif
435       left = VSHR32(bank[i*C],shift);
436       right = VSHR32(bank[i*C+1],shift);
437       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
438       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
439       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
440       for (j=eBands[i];j<eBands[i+1];j++)
441       {
442          celt_norm_t r, l;
443          l = X[j*C];
444          r = X[j*C+1];
445          X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
446          X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
447       }
448    }
449 }
450 #endif