A bunch of const qualifyers and a few comments
[opus.git] / libcelt / bands.c
1 /* (C) 2007 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
42
43 void exp_rotation(celt_norm_t *X, int len, float theta, int dir, int stride, int iter)
44 {
45    int i, k;
46    celt_word16_t c, s;
47    c = Q15ONE*cos(theta);
48    s = dir*Q15ONE*sin(theta);
49    for (k=0;k<iter;k++)
50    {
51       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
52          but at this point, I really don't think it's necessary */
53       for (i=0;i<len-stride;i++)
54       {
55          celt_norm_t x1, x2;
56          x1 = X[i];
57          x2 = X[i+stride];
58          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
59          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
60       }
61       for (i=len-2*stride-1;i>=0;i--)
62       {
63          celt_norm_t x1, x2;
64          x1 = X[i];
65          x2 = X[i+stride];
66          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
67          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
68       }
69    }
70 }
71
72 /* Compute the amplitude (sqrt energy) in each of the bands */
73 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
74 {
75    int i, c, B, C;
76    const int *eBands = m->eBands;
77    B = m->nbMdctBlocks;
78    C = m->nbChannels;
79    for (c=0;c<C;c++)
80    {
81       for (i=0;i<m->nbEBands;i++)
82       {
83          int j;
84          float sum = 1e-10;
85          for (j=B*eBands[i];j<B*eBands[i+1];j++)
86             sum += SIG_SCALING_1*SIG_SCALING_1*X[j*C+c]*X[j*C+c];
87          bank[i*C+c] = ENER_SCALING*sqrt(sum);
88          /*printf ("%f ", bank[i*C+c]);*/
89       }
90    }
91    /*printf ("\n");*/
92 }
93
94 /* Normalise each band such that the energy is one. */
95 void normalise_bands(const CELTMode *m, const celt_sig_t *freq, celt_norm_t *X, const celt_ener_t *bank)
96 {
97    int i, c, B, C;
98    const int *eBands = m->eBands;
99    B = m->nbMdctBlocks;
100    C = m->nbChannels;
101    for (c=0;c<C;c++)
102    {
103       for (i=0;i<m->nbEBands;i++)
104       {
105          int j;
106          float g = 1.f/(1e-10+ENER_SCALING_1*bank[i*C+c]*sqrt(C));
107          for (j=B*eBands[i];j<B*eBands[i+1];j++)
108             X[j*C+c] = NORM_SCALING*SIG_SCALING_1*freq[j*C+c]*g;
109       }
110    }
111    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
112       X[i] = 0;
113 }
114
115 #ifdef FIXED_POINT
116 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
117 {
118    int i;
119    VARDECL(celt_ener_t *tmpE);
120    VARDECL(celt_sig_t *freq);
121    SAVE_STACK;
122    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
123    ALLOC(freq, m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
124    for (i=0;i<m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1];i++)
125       freq[i] = SHL32(EXTEND32(X[i]), 10);
126    compute_band_energies(m, freq, tmpE);
127    normalise_bands(m, freq, X, tmpE);
128    RESTORE_STACK;
129 }
130 #else
131 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
132 {
133    VARDECL(celt_ener_t *tmpE);
134    SAVE_STACK;
135    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
136    compute_band_energies(m, X, tmpE);
137    normalise_bands(m, X, X, tmpE);
138    RESTORE_STACK;
139 }
140 #endif
141
142 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
143 void denormalise_bands(const CELTMode *m, const celt_norm_t *X, celt_sig_t *freq, const celt_ener_t *bank)
144 {
145    int i, c, B, C;
146    const int *eBands = m->eBands;
147    B = m->nbMdctBlocks;
148    C = m->nbChannels;
149    for (c=0;c<C;c++)
150    {
151       for (i=0;i<m->nbEBands;i++)
152       {
153          int j;
154          float g = ENER_SCALING_1*sqrt(C)*bank[i*C+c];
155          for (j=B*eBands[i];j<B*eBands[i+1];j++)
156             freq[j*C+c] = NORM_SCALING_1*SIG_SCALING*X[j*C+c] * g;
157       }
158    }
159    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
160       freq[i] = 0;
161 }
162
163
164 /* Compute the best gain for each "pitch band" */
165 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
166 {
167    int i, B;
168    const int *pBands = m->pBands;
169    B = m->nbMdctBlocks*m->nbChannels;
170    
171    for (i=0;i<m->nbPBands;i++)
172    {
173       celt_word32_t Sxy=0, Sxx=0;
174       int j;
175       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
176       for (j=B*pBands[i];j<B*pBands[i+1];j++)
177       {
178          Sxy = MAC16_16(Sxy, X[j], P[j]);
179          Sxx = MAC16_16(Sxx, X[j], X[j]);
180       }
181       /* No negative gain allowed */
182       if (Sxy < 0)
183          Sxy = 0;
184       /* Not sure how that would happen, just making sure */
185       if (Sxy > Sxx)
186          Sxy = Sxx;
187       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
188          residual doesn't quantise well */
189       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
190       /* gain = Sxy/Sxx */
191       gains[i] = DIV32_16(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON));
192       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
193    }
194    /*if(rand()%10==0)
195    {
196       for (i=0;i<m->nbPBands;i++)
197          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
198       printf ("\n");
199    }*/
200 }
201
202 /* Apply the (quantised) gain to each "pitch band" */
203 void pitch_quant_bands(const CELTMode *m, celt_norm_t *P, const celt_pgain_t *gains)
204 {
205    int i, B;
206    const int *pBands = m->pBands;
207    B = m->nbMdctBlocks*m->nbChannels;
208    for (i=0;i<m->nbPBands;i++)
209    {
210       int j;
211       for (j=B*pBands[i];j<B*pBands[i+1];j++)
212          P[j] = MULT16_16_Q15(gains[i], P[j]);
213       /*printf ("%f ", gain);*/
214    }
215    for (i=B*pBands[m->nbPBands];i<B*pBands[m->nbPBands+1];i++)
216       P[i] = 0;
217 }
218
219
220 /* Quantisation of the residual */
221 void quant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, celt_mask_t *W, int total_bits, ec_enc *enc)
222 {
223    int i, j, B, bits;
224    const int *eBands = m->eBands;
225    celt_word16_t alpha;
226    VARDECL(celt_norm_t *norm);
227    VARDECL(int *pulses);
228    VARDECL(int *offsets);
229    SAVE_STACK;
230
231    B = m->nbMdctBlocks*m->nbChannels;
232    
233    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
234    ALLOC(pulses, m->nbEBands, int);
235    ALLOC(offsets, m->nbEBands, int);
236
237    for (i=0;i<m->nbEBands;i++)
238       offsets[i] = 0;
239    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
240    bits = total_bits - ec_enc_tell(enc, 0) - 1;
241    compute_allocation(m, offsets, bits, pulses);
242    
243    /*printf("bits left: %d\n", bits);
244    for (i=0;i<m->nbEBands;i++)
245       printf ("%d ", pulses[i]);
246    printf ("\n");*/
247    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
248    for (i=0;i<m->nbEBands;i++)
249    {
250       int q;
251       float theta, n;
252       q = pulses[i];
253       /*Scale factor of .0625f is just there to prevent overflows in fixed-point
254        (has no effect on float)*/
255       n = .0625f*sqrt(B*(eBands[i+1]-eBands[i]));
256       theta = .007*(B*(eBands[i+1]-eBands[i]))/(.1f+q);
257
258       /* If pitch isn't available, use intra-frame prediction */
259       if (eBands[i] >= m->pitchEnd || q<=0)
260       {
261          q -= 1;
262          alpha = 0;
263          if (q<0)
264             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
265          else
266             intra_prediction(X+B*eBands[i], W+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, norm, P+B*eBands[i], B, eBands[i], enc);
267       } else {
268          alpha = QCONST16(.7f,15);
269       }
270       
271       if (q > 0)
272       {
273          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
274          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
275          alg_quant(X+B*eBands[i], W+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, enc);
276          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, 1, B, 8);
277       }
278       for (j=B*eBands[i];j<B*eBands[i+1];j++)
279          norm[j] = X[j] * n;
280    }
281    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
282       X[i] = 0;
283    RESTORE_STACK;
284 }
285
286 /* Decoding of the residual */
287 void unquant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, int total_bits, ec_dec *dec)
288 {
289    int i, j, B, bits;
290    const int *eBands = m->eBands;
291    celt_word16_t alpha;
292    VARDECL(celt_norm_t *norm);
293    VARDECL(int *pulses);
294    VARDECL(int *offsets);
295    SAVE_STACK;
296
297    B = m->nbMdctBlocks*m->nbChannels;
298    
299    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
300    ALLOC(pulses, m->nbEBands, int);
301    ALLOC(offsets, m->nbEBands, int);
302
303    for (i=0;i<m->nbEBands;i++)
304       offsets[i] = 0;
305    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
306    bits = total_bits - ec_dec_tell(dec, 0) - 1;
307    compute_allocation(m, offsets, bits, pulses);
308
309    for (i=0;i<m->nbEBands;i++)
310    {
311       int q;
312       float theta, n;
313       q = pulses[i];
314       /*Scale factor of .0625f is just there to prevent overflows in fixed-point
315       (has no effect on float)*/
316       n = .0625f*sqrt(B*(eBands[i+1]-eBands[i]));
317       theta = .007*(B*(eBands[i+1]-eBands[i]))/(.1f+q);
318
319       /* If pitch isn't available, use intra-frame prediction */
320       if (eBands[i] >= m->pitchEnd || q<=0)
321       {
322          q -= 1;
323          alpha = 0;
324          if (q<0)
325             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
326          else
327             intra_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, norm, P+B*eBands[i], B, eBands[i], dec);
328       } else {
329          alpha = QCONST16(.7f,15);
330       }
331       
332       if (q > 0)
333       {
334          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
335          alg_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, dec);
336          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, 1, B, 8);
337       }
338       for (j=B*eBands[i];j<B*eBands[i+1];j++)
339          norm[j] = X[j] * n;
340    }
341    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
342       X[i] = 0;
343    RESTORE_STACK;
344 }
345
346 void stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int dir)
347 {
348    int i, B, C;
349    const int *eBands = m->eBands;
350    B = m->nbMdctBlocks;
351    C = m->nbChannels;
352    for (i=0;i<m->nbEBands;i++)
353    {
354       int j;
355       float left, right;
356       float a1, a2;
357       left = bank[i*C];
358       right = bank[i*C+1];
359       a1 = left/sqrt(.01+left*left+right*right);
360       a2 = dir*right/sqrt(.01+left*left+right*right);
361       for (j=B*eBands[i];j<B*eBands[i+1];j++)
362       {
363          float r, l;
364          l = X[j*C];
365          r = X[j*C+1];         
366          X[j*C] = a1*l + a2*r;
367          X[j*C+1] = a1*r - a2*l;
368       }
369    }
370    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
371       X[i] = 0;
372
373 }