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