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