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