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