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