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