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