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