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