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