Moved the application of the pitch gain to (un)quant_bands(). This doesn't
[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 #ifdef EXP_PSY
132 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, 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_word32_t sum = 1e-10;
143          for (j=eBands[i];j<eBands[i+1];j++)
144             sum += X[j*C+c]*X[j*C+c]*tonality[j];
145          bank[i*C+c] = sqrt(sum);
146          /*printf ("%f ", bank[i*C+c]);*/
147       }
148    }
149    /*printf ("\n");*/
150 }
151 #endif
152
153 /* Normalise each band such that the energy is one. */
154 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
155 {
156    int i, c;
157    const celt_int16_t *eBands = m->eBands;
158    const int C = CHANNELS(m);
159    for (c=0;c<C;c++)
160    {
161       for (i=0;i<m->nbEBands;i++)
162       {
163          int j;
164          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
165          for (j=eBands[i];j<eBands[i+1];j++)
166             X[j*C+c] = freq[j*C+c]*g;
167       }
168    }
169 }
170
171 #endif /* FIXED_POINT */
172
173 #ifndef DISABLE_STEREO
174 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
175 {
176    int i, c;
177    const celt_int16_t *eBands = m->eBands;
178    const int C = CHANNELS(m);
179    for (c=0;c<C;c++)
180    {
181       i=0; do {
182          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
183       } while (++i<m->nbEBands);
184    }
185 }
186 #endif /* DISABLE_STEREO */
187
188 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
189 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
190 {
191    int i, c;
192    const celt_int16_t *eBands = m->eBands;
193    const int C = CHANNELS(m);
194    if (C>2)
195       celt_fatal("denormalise_bands() not implemented for >2 channels");
196    for (c=0;c<C;c++)
197    {
198       for (i=0;i<m->nbEBands;i++)
199       {
200          int j;
201          celt_word32_t g = MULT16_32_Q15(sqrtC_1[C-1],bank[i*C+c]);
202          j=eBands[i]; do {
203             freq[j*C+c] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
204          } while (++j<eBands[i+1]);
205       }
206    }
207    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
208       freq[i] = 0;
209 }
210
211
212 /* Compute the best gain for each "pitch band" */
213 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
214 {
215    int i;
216    const celt_int16_t *pBands = m->pBands;
217    const int C = CHANNELS(m);
218
219    for (i=0;i<m->nbPBands;i++)
220    {
221       celt_word32_t Sxy=0, Sxx=0;
222       int j;
223       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
224       for (j=C*pBands[i];j<C*pBands[i+1];j++)
225       {
226          Sxy = MAC16_16(Sxy, X[j], P[j]);
227          Sxx = MAC16_16(Sxx, X[j], X[j]);
228       }
229       /* No negative gain allowed */
230       if (Sxy < 0)
231          Sxy = 0;
232       /* Not sure how that would happen, just making sure */
233       if (Sxy > Sxx)
234          Sxy = Sxx;
235       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
236          residual doesn't quantise well */
237       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
238       /* gain = Sxy/Sxx */
239       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
240       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
241    }
242    /*if(rand()%10==0)
243    {
244       for (i=0;i<m->nbPBands;i++)
245          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
246       printf ("\n");
247    }*/
248 }
249
250 static void intensity_band(celt_norm_t * restrict X, int len)
251 {
252    int j;
253    celt_word32_t E = 1e-15;
254    celt_word32_t E2 = 1e-15;
255    for (j=0;j<len;j++)
256    {
257       X[j] = X[2*j];
258       E = MAC16_16(E, X[j],X[j]);
259       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
260    }
261 #ifndef FIXED_POINT
262    E  = celt_sqrt(E+E2)/celt_sqrt(E);
263    for (j=0;j<len;j++)
264       X[j] *= E;
265 #endif
266    for (j=0;j<len;j++)
267       X[len+j] = 0;
268
269 }
270
271 static void dup_band(celt_norm_t * restrict X, int len)
272 {
273    int j;
274    for (j=len-1;j>=0;j--)
275    {
276       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
277       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
278    }
279 }
280
281 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)
282 {
283    int i = bandID;
284    const celt_int16_t *eBands = m->eBands;
285    const int C = CHANNELS(m);
286    {
287       int j;
288       if (stereo_mode[i] && dir <0)
289       {
290          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
291       } else {
292          celt_word16_t a1, a2;
293          if (stereo_mode[i]==0)
294          {
295             /* Do mid-side when not doing intensity stereo */
296             a1 = QCONST16(.70711f,14);
297             a2 = dir*QCONST16(.70711f,14);
298          } else {
299             celt_word16_t left, right;
300             celt_word16_t norm;
301 #ifdef FIXED_POINT
302             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
303 #endif
304             left = VSHR32(bank[i*C],shift);
305             right = VSHR32(bank[i*C+1],shift);
306             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
307             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
308             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
309          }
310          for (j=eBands[i];j<eBands[i+1];j++)
311          {
312             celt_norm_t r, l;
313             l = X[j*C];
314             r = X[j*C+1];
315             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
316             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
317          }
318       }
319       if (stereo_mode[i] && dir>0)
320       {
321          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
322       }
323    }
324 }
325
326 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
327 {
328    int i;
329    for (i=0;i<len-5;i++)
330       stereo_mode[i] = 0;
331    for (;i<len;i++)
332       stereo_mode[i] = 0;
333 }
334
335
336
337 /* Quantisation of the residual */
338 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
339 {
340    int i, j, remaining_bits, balance;
341    const celt_int16_t * restrict eBands = m->eBands;
342    celt_norm_t * restrict norm;
343    VARDECL(celt_norm_t, _norm);
344    const int C = CHANNELS(m);
345    const celt_int16_t *pBands = m->pBands;
346    int pband=-1;
347    int B;
348    SAVE_STACK;
349
350    B = shortBlocks ? m->nbShortMdcts : 1;
351    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
352    norm = _norm;
353
354    balance = 0;
355    /*printf("bits left: %d\n", bits);
356    for (i=0;i<m->nbEBands;i++)
357       printf ("(%d %d) ", pulses[i], ebits[i]);
358    printf ("\n");*/
359    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
360    for (i=0;i<m->nbEBands;i++)
361    {
362       int tell;
363       int q;
364       celt_word16_t n;
365       const celt_int16_t * const *BPbits;
366       
367       int curr_balance, curr_bits;
368       
369       if (C>1 && stereo_mode[i]==0)
370          BPbits = m->bits_stereo;
371       else
372          BPbits = m->bits;
373
374       tell = ec_enc_tell(enc, 4);
375       if (i != 0)
376          balance -= tell;
377       remaining_bits = (total_bits<<BITRES)-tell-1;
378       curr_balance = (m->nbEBands-i);
379       if (curr_balance > 3)
380          curr_balance = 3;
381       curr_balance = balance / curr_balance;
382       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
383       curr_bits = BPbits[i][q];
384       remaining_bits -= curr_bits;
385       if (remaining_bits < 0)
386       {
387          remaining_bits += curr_bits;
388          if (q>0) {
389            q--;
390            curr_bits = BPbits[i][q];
391            remaining_bits -= curr_bits;
392          }
393       }
394       balance += pulses[i] + tell;
395       
396       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
397
398       /* If pitch isn't available, use intra-frame prediction */
399       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
400       {
401          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
402       } else if (pitch_used && eBands[i] < m->pitchEnd)
403       {
404          if (eBands[i] == pBands[pband+1])
405             pband++;
406          for (j=C*eBands[i];j<C*eBands[i+1];j++)
407             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
408       } else {
409          for (j=C*eBands[i];j<C*eBands[i+1];j++)
410             P[j] = 0;
411       }
412       
413       if (q > 0)
414       {
415          int ch=C;
416          if (C==2 && stereo_mode[i]==1)
417             ch = 1;
418          if (C==2)
419          {
420             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
421             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
422          }
423          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
424          if (C==2)
425             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
426       } else {
427          for (j=C*eBands[i];j<C*eBands[i+1];j++)
428             X[j] = P[j];
429       }
430       for (j=C*eBands[i];j<C*eBands[i+1];j++)
431          norm[j] = MULT16_16_Q15(n,X[j]);
432    }
433    RESTORE_STACK;
434 }
435
436 /* Decoding of the residual */
437 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
438 {
439    int i, j, remaining_bits, balance;
440    const celt_int16_t * restrict eBands = m->eBands;
441    celt_norm_t * restrict norm;
442    VARDECL(celt_norm_t, _norm);
443    const int C = CHANNELS(m);
444    const celt_int16_t *pBands = m->pBands;
445    int pband=-1;
446    int B;
447    SAVE_STACK;
448
449    B = shortBlocks ? m->nbShortMdcts : 1;
450    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
451    norm = _norm;
452
453    balance = 0;
454    for (i=0;i<m->nbEBands;i++)
455    {
456       int tell;
457       int q;
458       celt_word16_t n;
459       const celt_int16_t * const *BPbits;
460       
461       int curr_balance, curr_bits;
462       
463       if (C>1 && stereo_mode[i]==0)
464          BPbits = m->bits_stereo;
465       else
466          BPbits = m->bits;
467
468       tell = ec_dec_tell(dec, 4);
469       if (i != 0)
470          balance -= tell;
471       remaining_bits = (total_bits<<BITRES)-tell-1;
472       curr_balance = (m->nbEBands-i);
473       if (curr_balance > 3)
474          curr_balance = 3;
475       curr_balance = balance / curr_balance;
476       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
477       curr_bits = BPbits[i][q];
478       remaining_bits -= curr_bits;
479       if (remaining_bits < 0)
480       {
481          remaining_bits += curr_bits;
482          if (q>0) {
483            q--;
484            curr_bits = BPbits[i][q];
485            remaining_bits -= curr_bits;
486          }
487       }
488       balance += pulses[i] + tell;
489
490       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
491
492       /* If pitch isn't available, use intra-frame prediction */
493       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
494       {
495          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
496       } else if (pitch_used && eBands[i] < m->pitchEnd)
497       {
498          if (eBands[i] == pBands[pband+1])
499             pband++;
500          for (j=C*eBands[i];j<C*eBands[i+1];j++)
501             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
502       } else {
503          for (j=C*eBands[i];j<C*eBands[i+1];j++)
504             P[j] = 0;
505       }
506       
507       if (q > 0)
508       {
509          int ch=C;
510          if (C==2 && stereo_mode[i]==1)
511             ch = 1;
512          if (C==2)
513             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
514          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
515          if (C==2)
516             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
517       } else {
518          for (j=C*eBands[i];j<C*eBands[i+1];j++)
519             X[j] = P[j];
520       }
521       for (j=C*eBands[i];j<C*eBands[i+1];j++)
522          norm[j] = MULT16_16_Q15(n,X[j]);
523    }
524    RESTORE_STACK;
525 }
526