Better use of the arithmetic operators
[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 #include "rate.h"
45
46 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
47
48 #ifdef FIXED_POINT
49 /* Compute the amplitude (sqrt energy) in each of the bands */
50 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
51 {
52    int i, c;
53    const celt_int16_t *eBands = m->eBands;
54    const int C = CHANNELS(m);
55    for (c=0;c<C;c++)
56    {
57       for (i=0;i<m->nbEBands;i++)
58       {
59          int j;
60          celt_word32_t maxval=0;
61          celt_word32_t sum = 0;
62          
63          j=eBands[i]; do {
64             maxval = MAX32(maxval, X[j*C+c]);
65             maxval = MAX32(maxval, -X[j*C+c]);
66          } while (++j<eBands[i+1]);
67          
68          if (maxval > 0)
69          {
70             int shift = celt_ilog2(maxval)-10;
71             j=eBands[i]; do {
72                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j*C+c],shift)),
73                                    EXTRACT16(VSHR32(X[j*C+c],shift)));
74             } while (++j<eBands[i+1]);
75             /* We're adding one here to make damn sure we never end up with a pitch vector that's
76                larger than unity norm */
77             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
78          } else {
79             bank[i*C+c] = EPSILON;
80          }
81          /*printf ("%f ", bank[i*C+c]);*/
82       }
83    }
84    /*printf ("\n");*/
85 }
86
87 /* Normalise each band such that the energy is one. */
88 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
89 {
90    int i, c;
91    const celt_int16_t *eBands = m->eBands;
92    const int C = CHANNELS(m);
93    for (c=0;c<C;c++)
94    {
95       i=0; do {
96          celt_word16_t g;
97          int j,shift;
98          celt_word16_t E;
99          shift = celt_zlog2(bank[i*C+c])-13;
100          E = VSHR32(bank[i*C+c], shift);
101          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
102          j=eBands[i]; do {
103             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
104          } while (++j<eBands[i+1]);
105       } while (++i<m->nbEBands);
106    }
107 }
108
109 #else /* FIXED_POINT */
110 /* Compute the amplitude (sqrt energy) in each of the bands */
111 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
112 {
113    int i, c;
114    const celt_int16_t *eBands = m->eBands;
115    const int C = CHANNELS(m);
116    for (c=0;c<C;c++)
117    {
118       for (i=0;i<m->nbEBands;i++)
119       {
120          int j;
121          celt_word32_t sum = 1e-10;
122          for (j=eBands[i];j<eBands[i+1];j++)
123             sum += X[j*C+c]*X[j*C+c];
124          bank[i*C+c] = sqrt(sum);
125          /*printf ("%f ", bank[i*C+c]);*/
126       }
127    }
128    /*printf ("\n");*/
129 }
130
131 /* Normalise each band such that the energy is one. */
132 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
133 {
134    int i, c;
135    const celt_int16_t *eBands = m->eBands;
136    const int C = CHANNELS(m);
137    for (c=0;c<C;c++)
138    {
139       for (i=0;i<m->nbEBands;i++)
140       {
141          int j;
142          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
143          for (j=eBands[i];j<eBands[i+1];j++)
144             X[j*C+c] = freq[j*C+c]*g;
145       }
146    }
147 }
148
149 #endif /* FIXED_POINT */
150
151 #ifndef DISABLE_STEREO
152 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
153 {
154    int i, c;
155    const celt_int16_t *eBands = m->eBands;
156    const int C = CHANNELS(m);
157    for (c=0;c<C;c++)
158    {
159       i=0; do {
160          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
161       } while (++i<m->nbEBands);
162    }
163 }
164 #endif /* DISABLE_STEREO */
165
166 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
167 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
168 {
169    int i, c;
170    const celt_int16_t *eBands = m->eBands;
171    const int C = CHANNELS(m);
172    if (C>2)
173       celt_fatal("denormalise_bands() not implemented for >2 channels");
174    for (c=0;c<C;c++)
175    {
176       for (i=0;i<m->nbEBands;i++)
177       {
178          int j;
179          celt_word32_t g = MULT16_32_Q15(sqrtC_1[C-1],bank[i*C+c]);
180          j=eBands[i]; do {
181             freq[j*C+c] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
182          } while (++j<eBands[i+1]);
183       }
184    }
185    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
186       freq[i] = 0;
187 }
188
189
190 /* Compute the best gain for each "pitch band" */
191 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
192 {
193    int i;
194    const celt_int16_t *pBands = m->pBands;
195    const int C = CHANNELS(m);
196
197    for (i=0;i<m->nbPBands;i++)
198    {
199       celt_word32_t Sxy=0, Sxx=0;
200       int j;
201       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
202       for (j=C*pBands[i];j<C*pBands[i+1];j++)
203       {
204          Sxy = MAC16_16(Sxy, X[j], P[j]);
205          Sxx = MAC16_16(Sxx, X[j], X[j]);
206       }
207       /* No negative gain allowed */
208       if (Sxy < 0)
209          Sxy = 0;
210       /* Not sure how that would happen, just making sure */
211       if (Sxy > Sxx)
212          Sxy = Sxx;
213       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
214          residual doesn't quantise well */
215       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
216       /* gain = Sxy/Sxx */
217       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
218       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
219    }
220    /*if(rand()%10==0)
221    {
222       for (i=0;i<m->nbPBands;i++)
223          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
224       printf ("\n");
225    }*/
226 }
227
228 /* Apply the (quantised) gain to each "pitch band" */
229 void pitch_quant_bands(const CELTMode *m, celt_norm_t * restrict P, const celt_pgain_t * restrict gains)
230 {
231    int i;
232    const celt_int16_t *pBands = m->pBands;
233    const int C = CHANNELS(m);
234    for (i=0;i<m->nbPBands;i++)
235    {
236       int j;
237       for (j=C*pBands[i];j<C*pBands[i+1];j++)
238          P[j] = MULT16_16_Q15(gains[i], P[j]);
239       /*printf ("%f ", gain);*/
240    }
241    for (i=C*pBands[m->nbPBands];i<C*pBands[m->nbPBands+1];i++)
242       P[i] = 0;
243 }
244
245 static void intensity_band(celt_norm_t * restrict X, int len)
246 {
247    int j;
248    celt_word32_t E = 1e-15;
249    celt_word32_t E2 = 1e-15;
250    for (j=0;j<len;j++)
251    {
252       X[j] = X[2*j];
253       E = MAC16_16(E, X[j],X[j]);
254       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
255    }
256 #ifndef FIXED_POINT
257    E  = celt_sqrt(E+E2)/celt_sqrt(E);
258    for (j=0;j<len;j++)
259       X[j] *= E;
260 #endif
261    for (j=0;j<len;j++)
262       X[len+j] = 0;
263
264 }
265
266 static void dup_band(celt_norm_t * restrict X, int len)
267 {
268    int j;
269    for (j=len-1;j>=0;j--)
270    {
271       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
272       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
273    }
274 }
275
276 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, const int *stereo_mode, int bandID, int dir)
277 {
278    int i = bandID;
279    const celt_int16_t *eBands = m->eBands;
280    const int C = CHANNELS(m);
281    {
282       int j;
283       if (stereo_mode[i] && dir <0)
284       {
285          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
286       } else {
287          celt_word16_t a1, a2;
288          if (stereo_mode[i]==0)
289          {
290             /* Do mid-side when not doing intensity stereo */
291             a1 = QCONST16(.70711f,14);
292             a2 = dir*QCONST16(.70711f,14);
293          } else {
294             celt_word16_t left, right;
295             celt_word16_t norm;
296 #ifdef FIXED_POINT
297             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
298 #endif
299             left = VSHR32(bank[i*C],shift);
300             right = VSHR32(bank[i*C+1],shift);
301             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
302             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
303             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
304          }
305          for (j=eBands[i];j<eBands[i+1];j++)
306          {
307             celt_norm_t r, l;
308             l = X[j*C];
309             r = X[j*C+1];
310             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
311             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
312          }
313       }
314       if (stereo_mode[i] && dir>0)
315       {
316          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
317       }
318    }
319 }
320
321 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
322 {
323    int i;
324    for (i=0;i<len-5;i++)
325       stereo_mode[i] = 0;
326    for (;i<len;i++)
327       stereo_mode[i] = 0;
328 }
329
330
331
332 /* Quantisation of the residual */
333 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int total_bits, ec_enc *enc)
334 {
335    int i, j, remaining_bits, balance;
336    const celt_int16_t * restrict eBands = m->eBands;
337    celt_norm_t * restrict norm;
338    VARDECL(celt_norm_t, _norm);
339    const int C = CHANNELS(m);
340    int B;
341    SAVE_STACK;
342
343    B = shortBlocks ? m->nbShortMdcts : 1;
344    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
345    norm = _norm;
346
347    balance = 0;
348    /*printf("bits left: %d\n", bits);
349    for (i=0;i<m->nbEBands;i++)
350       printf ("(%d %d) ", pulses[i], ebits[i]);
351    printf ("\n");*/
352    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
353    for (i=0;i<m->nbEBands;i++)
354    {
355       int tell;
356       int q;
357       celt_word16_t n;
358       
359       int curr_balance, curr_bits;
360       
361       tell = ec_enc_tell(enc, 4);
362       if (i != 0)
363          balance -= tell;
364       remaining_bits = (total_bits<<BITRES)-tell-1;
365       curr_balance = (m->nbEBands-i);
366       if (curr_balance > 3)
367          curr_balance = 3;
368       curr_balance = balance / curr_balance;
369       q = bits2pulses(m, m->bits[i], pulses[i]+curr_balance);
370       curr_bits = m->bits[i][q];
371       remaining_bits -= curr_bits;
372       if (remaining_bits < 0)
373       {
374          q--;
375          remaining_bits += curr_bits;
376          curr_bits = m->bits[i][q];
377          remaining_bits -= curr_bits;
378       }
379       balance += pulses[i] + tell;
380       
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(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
389          else
390             intra_prediction(m, X+C*eBands[i], W+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B, enc);
391       }
392       
393       if (q > 0)
394       {
395          int ch=C;
396          if (C==2 && stereo_mode[i]==1)
397             ch = 1;
398          if (C==2)
399          {
400             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
401             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
402          }
403          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
404          if (C==2)
405             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
406       } else {
407          for (j=C*eBands[i];j<C*eBands[i+1];j++)
408             X[j] = P[j];
409       }
410       for (j=C*eBands[i];j<C*eBands[i+1];j++)
411          norm[j] = MULT16_16_Q15(n,X[j]);
412    }
413    RESTORE_STACK;
414 }
415
416 /* Decoding of the residual */
417 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int total_bits, ec_dec *dec)
418 {
419    int i, j, remaining_bits, balance;
420    const celt_int16_t * restrict eBands = m->eBands;
421    celt_norm_t * restrict norm;
422    VARDECL(celt_norm_t, _norm);
423    const int C = CHANNELS(m);
424    int B;
425    SAVE_STACK;
426
427    B = shortBlocks ? m->nbShortMdcts : 1;
428    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
429    norm = _norm;
430
431    balance = 0;
432    for (i=0;i<m->nbEBands;i++)
433    {
434       int tell;
435       int q;
436       celt_word16_t n;
437       
438       int curr_balance, curr_bits;
439       
440       tell = ec_dec_tell(dec, 4);
441       if (i != 0)
442          balance -= tell;
443       remaining_bits = (total_bits<<BITRES)-tell-1;
444       curr_balance = (m->nbEBands-i);
445       if (curr_balance > 3)
446          curr_balance = 3;
447       curr_balance = balance / curr_balance;
448       q = bits2pulses(m, m->bits[i], pulses[i]+curr_balance);
449       curr_bits = m->bits[i][q];
450       remaining_bits -= curr_bits;
451       if (remaining_bits < 0)
452       {
453          q--;
454          remaining_bits += curr_bits;
455          curr_bits = m->bits[i][q];
456          remaining_bits -= curr_bits;
457       }
458       balance += pulses[i] + tell;
459
460       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
461
462       /* If pitch isn't available, use intra-frame prediction */
463       if (eBands[i] >= m->pitchEnd || q<=0)
464       {
465          q -= 1;
466          if (q<0)
467             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
468          else
469             intra_unquant(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B, dec);
470       }
471       
472       if (q > 0)
473       {
474          int ch=C;
475          if (C==2 && stereo_mode[i]==1)
476             ch = 1;
477          if (C==2)
478             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
479          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
480          if (C==2)
481             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
482       } else {
483          for (j=C*eBands[i];j<C*eBands[i+1];j++)
484             X[j] = P[j];
485       }
486       for (j=C*eBands[i];j<C*eBands[i+1];j++)
487          norm[j] = MULT16_16_Q15(n,X[j]);
488    }
489    RESTORE_STACK;
490 }
491