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