better indexing in exp_rotation()
[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 "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44
45 void exp_rotation(celt_norm_t *X, int len, int dir, int stride, int iter)
46 {
47    int i, k;
48    celt_word16_t c, s;
49    celt_norm_t *Xptr;
50    /* Equivalent to cos(.3) and sin(.3) */
51    c = QCONST16(0.95534,15);
52    s = dir*QCONST16(0.29552,15);
53    for (k=0;k<iter;k++)
54    {
55       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
56          but at this point, I really don't think it's necessary */
57       Xptr = X;
58       for (i=0;i<len-stride;i++)
59       {
60          celt_norm_t x1, x2;
61          x1 = Xptr[0];
62          x2 = Xptr[stride];
63          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
64          *Xptr++      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
65       }
66       Xptr = &X[len-2*stride-1];
67       for (i=len-2*stride-1;i>=0;i--)
68       {
69          celt_norm_t x1, x2;
70          x1 = Xptr[0];
71          x2 = Xptr[stride];
72          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
73          *Xptr--      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
74       }
75    }
76 }
77
78
79
80 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
81
82 #ifdef FIXED_POINT
83 /* Compute the amplitude (sqrt energy) in each of the bands */
84 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
85 {
86    int i, c;
87    const celt_int16_t *eBands = m->eBands;
88    const int C = CHANNELS(m);
89    for (c=0;c<C;c++)
90    {
91       for (i=0;i<m->nbEBands;i++)
92       {
93          int j;
94          celt_word32_t maxval=0;
95          celt_word32_t sum = 0;
96          for (j=eBands[i];j<eBands[i+1];j++)
97             maxval = MAX32(maxval, ABS32(X[j*C+c]));
98          if (maxval > 0)
99          {
100             int shift = celt_ilog2(maxval)-10;
101             for (j=eBands[i];j<eBands[i+1];j++)
102                sum += MULT16_16(EXTRACT16(VSHR32(X[j*C+c],shift)),EXTRACT16(VSHR32(X[j*C+c],shift)));
103             /* We're adding one here to make damn sure we never end up with a pitch vector that's
104                larger than unity norm */
105             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
106          } else {
107             bank[i*C+c] = EPSILON;
108          }
109          /*printf ("%f ", bank[i*C+c]);*/
110       }
111    }
112    /*printf ("\n");*/
113 }
114
115 /* Normalise each band such that the energy is one. */
116 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
117 {
118    int i, c;
119    const celt_int16_t *eBands = m->eBands;
120    const int C = CHANNELS(m);
121    for (c=0;c<C;c++)
122    {
123       i=0; do {
124          celt_word16_t g;
125          int j,shift;
126          celt_word16_t E;
127          shift = celt_zlog2(bank[i*C+c])-13;
128          E = VSHR32(bank[i*C+c], shift);
129          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
130          for (j=eBands[i];j<eBands[i+1];j++)
131             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
132       } while (++i<m->nbEBands);
133    }
134    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
135       X[i] = 0;
136 }
137
138 #ifndef DISABLE_STEREO
139 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict 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->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
147    for (i=0;i<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 #endif /* DISABLE_STEREO */
154 #else /* FIXED_POINT */
155 /* Compute the amplitude (sqrt energy) in each of the bands */
156 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
157 {
158    int i, c;
159    const celt_int16_t *eBands = m->eBands;
160    const int C = CHANNELS(m);
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=eBands[i];j<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 * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
178 {
179    int i, c;
180    const celt_int16_t *eBands = m->eBands;
181    const int C = CHANNELS(m);
182    for (c=0;c<C;c++)
183    {
184       for (i=0;i<m->nbEBands;i++)
185       {
186          int j;
187          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
188          for (j=eBands[i];j<eBands[i+1];j++)
189             X[j*C+c] = freq[j*C+c]*g;
190       }
191    }
192    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
193       X[i] = 0;
194 }
195
196 #ifndef DISABLE_STEREO
197 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict 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 /* DISABLE_STEREO */
207 #endif /* FIXED_POINT */
208
209 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
210 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
211 {
212    int i, c;
213    const celt_int16_t *eBands = m->eBands;
214    const int C = CHANNELS(m);
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_Q13(sqrtC_1[C-1],bank[i*C+c]);
223          for (j=eBands[i];j<eBands[i+1];j++)
224             freq[j*C+c] = MULT16_32_Q15(X[j*C+c], g);
225       }
226    }
227    for (i=C*eBands[m->nbEBands];i<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;
236    const celt_int16_t *pBands = m->pBands;
237    const int C = CHANNELS(m);
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=C*pBands[i];j<C*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] = EXTRACT16(celt_div(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 * restrict P, const celt_pgain_t * restrict gains)
272 {
273    int i;
274    const celt_int16_t *pBands = m->pBands;
275    const int C = CHANNELS(m);
276    for (i=0;i<m->nbPBands;i++)
277    {
278       int j;
279       for (j=C*pBands[i];j<C*pBands[i+1];j++)
280          P[j] = MULT16_16_Q15(gains[i], P[j]);
281       /*printf ("%f ", gain);*/
282    }
283    for (i=C*pBands[m->nbPBands];i<C*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 * restrict X, celt_norm_t *P, celt_mask_t *W, int total_bits, ec_enc *enc)
290 {
291    int i, j, bits;
292    const celt_int16_t * restrict eBands = m->eBands;
293    celt_norm_t * restrict norm;
294    VARDECL(celt_norm_t, _norm);
295    VARDECL(int, pulses);
296    VARDECL(int, offsets);
297    const int C = CHANNELS(m);
298    SAVE_STACK;
299
300    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
301    ALLOC(pulses, m->nbEBands, int);
302    ALLOC(offsets, m->nbEBands, int);
303    norm = _norm;
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(C*(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          if (q<0)
328             intra_fold(X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], C, eBands[i], eBands[m->nbEBands+1]);
329          else
330             intra_prediction(X+C*eBands[i], W+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], C, eBands[i], enc);
331       }
332       
333       if (q > 0)
334       {
335          int nb_rotations = q <= 2*C ? 2*C/q : 0;
336          if (nb_rotations != 0)
337          {
338             exp_rotation(P+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);
339             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);
340          }
341          alg_quant(X+C*eBands[i], W+C*eBands[i], C*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
342          if (nb_rotations != 0)
343             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), 1, C, nb_rotations);
344       }
345       for (j=C*eBands[i];j<C*eBands[i+1];j++)
346          norm[j] = MULT16_16_Q15(n,X[j]);
347    }
348    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
349       X[i] = 0;
350    RESTORE_STACK;
351 }
352
353 /* Decoding of the residual */
354 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int total_bits, ec_dec *dec)
355 {
356    int i, j, bits;
357    const celt_int16_t * restrict eBands = m->eBands;
358    celt_norm_t * restrict norm;
359    VARDECL(celt_norm_t, _norm);
360    VARDECL(int, pulses);
361    VARDECL(int, offsets);
362    const int C = CHANNELS(m);
363    SAVE_STACK;
364
365    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
366    ALLOC(pulses, m->nbEBands, int);
367    ALLOC(offsets, m->nbEBands, int);
368    norm = _norm;
369
370    for (i=0;i<m->nbEBands;i++)
371       offsets[i] = 0;
372    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
373    bits = total_bits - ec_dec_tell(dec, 0) - 1;
374    compute_allocation(m, offsets, bits, pulses);
375
376    for (i=0;i<m->nbEBands;i++)
377    {
378       int q;
379       celt_word16_t n;
380       q = pulses[i];
381       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
382
383       /* If pitch isn't available, use intra-frame prediction */
384       if (eBands[i] >= m->pitchEnd || q<=0)
385       {
386          q -= 1;
387          if (q<0)
388             intra_fold(X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], C, eBands[i], eBands[m->nbEBands+1]);
389          else
390             intra_unquant(X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], C, eBands[i], dec);
391       }
392       
393       if (q > 0)
394       {
395          int nb_rotations = q <= 2*C ? 2*C/q : 0;
396          if (nb_rotations != 0)
397             exp_rotation(P+C*eBands[i], C*(eBands[i+1]-eBands[i]), -1, C, nb_rotations);
398          alg_unquant(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
399          if (nb_rotations != 0)
400             exp_rotation(X+C*eBands[i], C*(eBands[i+1]-eBands[i]), 1, C, nb_rotations);
401       }
402       for (j=C*eBands[i];j<C*eBands[i+1];j++)
403          norm[j] = MULT16_16_Q15(n,X[j]);
404    }
405    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
406       X[i] = 0;
407    RESTORE_STACK;
408 }
409
410 #ifndef DISABLE_STEREO
411 void stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int dir)
412 {
413    int i;
414    const celt_int16_t *eBands = m->eBands;
415    const int C = CHANNELS(m);
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_zlog2(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=eBands[i];j<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=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
440       X[i] = 0;
441 }
442 #endif