In silk_dec_API.c a memcpy was blindly copying data of differing types (opus_int16...
[opus.git] / libcelt / vq.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /*
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15
16    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
20    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
21    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
22    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 #ifdef HAVE_CONFIG_H
30 #include "config.h"
31 #endif
32
33 #include "mathops.h"
34 #include "cwrs.h"
35 #include "vq.h"
36 #include "arch.h"
37 #include "os_support.h"
38 #include "bands.h"
39 #include "rate.h"
40
41 static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
42 {
43    int i;
44    celt_norm *Xptr;
45    Xptr = X;
46    for (i=0;i<len-stride;i++)
47    {
48       celt_norm x1, x2;
49       x1 = Xptr[0];
50       x2 = Xptr[stride];
51       Xptr[stride] = EXTRACT16(SHR32(MULT16_16(c,x2) + MULT16_16(s,x1), 15));
52       *Xptr++      = EXTRACT16(SHR32(MULT16_16(c,x1) - MULT16_16(s,x2), 15));
53    }
54    Xptr = &X[len-2*stride-1];
55    for (i=len-2*stride-1;i>=0;i--)
56    {
57       celt_norm x1, x2;
58       x1 = Xptr[0];
59       x2 = Xptr[stride];
60       Xptr[stride] = EXTRACT16(SHR32(MULT16_16(c,x2) + MULT16_16(s,x1), 15));
61       *Xptr--      = EXTRACT16(SHR32(MULT16_16(c,x1) - MULT16_16(s,x2), 15));
62    }
63 }
64
65 static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
66 {
67    static const int SPREAD_FACTOR[3]={15,10,5};
68    int i;
69    opus_val16 c, s;
70    opus_val16 gain, theta;
71    int stride2=0;
72    int factor;
73    /*int i;
74    if (len>=30)
75    {
76       for (i=0;i<len;i++)
77          X[i] = 0;
78       X[14] = 1;
79       K=5;
80    }*/
81    if (2*K>=len || spread==SPREAD_NONE)
82       return;
83    factor = SPREAD_FACTOR[spread-1];
84
85    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
86    theta = HALF16(MULT16_16_Q15(gain,gain));
87
88    c = celt_cos_norm(EXTEND32(theta));
89    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
90
91    if (len>=8*stride)
92    {
93       stride2 = 1;
94       /* This is just a simple way of computing sqrt(len/stride) with rounding.
95          It's basically incrementing long as (stride2+0.5)^2 < len/stride.
96          I _think_ it is bit-exact */
97       while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
98          stride2++;
99    }
100    /*TODO: We should be passing around log2(B), not B, for both this and for
101       extract_collapse_mask().*/
102    len /= stride;
103    for (i=0;i<stride;i++)
104    {
105       if (dir < 0)
106       {
107          if (stride2)
108             exp_rotation1(X+i*len, len, stride2, s, c);
109          exp_rotation1(X+i*len, len, 1, c, s);
110       } else {
111          exp_rotation1(X+i*len, len, 1, c, -s);
112          if (stride2)
113             exp_rotation1(X+i*len, len, stride2, s, -c);
114       }
115    }
116    /*if (len>=30)
117    {
118       for (i=0;i<len;i++)
119          printf ("%f ", X[i]);
120       printf ("\n");
121       exit(0);
122    }*/
123 }
124
125 /** Takes the pitch vector and the decoded residual vector, computes the gain
126     that will give ||p+g*y||=1 and mixes the residual with the pitch. */
127 static void normalise_residual(int * restrict iy, celt_norm * restrict X,
128       int N, opus_val32 Ryy, opus_val16 gain)
129 {
130    int i;
131 #ifdef FIXED_POINT
132    int k;
133 #endif
134    opus_val32 t;
135    opus_val16 g;
136
137 #ifdef FIXED_POINT
138    k = celt_ilog2(Ryy)>>1;
139 #endif
140    t = VSHR32(Ryy, (k-7)<<1);
141    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
142
143    i=0;
144    do
145       X[i] = EXTRACT16(PSHR32(MULT16_16(g, iy[i]), k+1));
146    while (++i < N);
147 }
148
149 static unsigned extract_collapse_mask(int *iy, int N, int B)
150 {
151    unsigned collapse_mask;
152    int N0;
153    int i;
154    if (B<=1)
155       return 1;
156    /*TODO: We should be passing around log2(B), not B, for both this and for
157       exp_rotation().*/
158    N0 = N/B;
159    collapse_mask = 0;
160    i=0; do {
161       int j;
162       j=0; do {
163          collapse_mask |= (iy[i*N0+j]!=0)<<i;
164       } while (++j<N0);
165    } while (++i<B);
166    return collapse_mask;
167 }
168
169 unsigned alg_quant(celt_norm *X, int N, int K, int spread, int B,
170       ec_enc *enc, opus_val16 gain)
171 {
172    VARDECL(celt_norm, y);
173    VARDECL(int, iy);
174    VARDECL(opus_val16, signx);
175    int i, j;
176    opus_val16 s;
177    int pulsesLeft;
178    opus_val32 sum;
179    opus_val32 xy;
180    opus_val16 yy;
181    unsigned collapse_mask;
182    SAVE_STACK;
183
184    celt_assert2(K>0, "alg_quant() needs at least one pulse");
185    celt_assert2(N>1, "alg_quant() needs at least two dimensions");
186
187    ALLOC(y, N, celt_norm);
188    ALLOC(iy, N, int);
189    ALLOC(signx, N, opus_val16);
190
191    exp_rotation(X, N, 1, B, K, spread);
192
193    /* Get rid of the sign */
194    sum = 0;
195    j=0; do {
196       if (X[j]>0)
197          signx[j]=1;
198       else {
199          signx[j]=-1;
200          X[j]=-X[j];
201       }
202       iy[j] = 0;
203       y[j] = 0;
204    } while (++j<N);
205
206    xy = yy = 0;
207
208    pulsesLeft = K;
209
210    /* Do a pre-search by projecting on the pyramid */
211    if (K > (N>>1))
212    {
213       opus_val16 rcp;
214       j=0; do {
215          sum += X[j];
216       }  while (++j<N);
217
218       /* If X is too small, just replace it with a pulse at 0 */
219 #ifdef FIXED_POINT
220       if (sum <= K)
221 #else
222       /* Prevents infinities and NaNs from causing too many pulses
223          to be allocated. 64 is an approximation of infinity here. */
224       if (!(sum > EPSILON && sum < 64))
225 #endif
226       {
227          X[0] = QCONST16(1.f,14);
228          j=1; do
229             X[j]=0;
230          while (++j<N);
231          sum = QCONST16(1.f,14);
232       }
233       /* Do we have sufficient accuracy here? */
234       rcp = EXTRACT16(MULT16_32_Q16(K-1, celt_rcp(sum)));
235       j=0; do {
236 #ifdef FIXED_POINT
237          /* It's really important to round *towards zero* here */
238          iy[j] = MULT16_16_Q15(X[j],rcp);
239 #else
240          iy[j] = (int)floor(rcp*X[j]);
241 #endif
242          y[j] = iy[j];
243          yy = MAC16_16(yy, y[j],y[j]);
244          xy = MAC16_16(xy, X[j],y[j]);
245          y[j] *= 2;
246          pulsesLeft -= iy[j];
247       }  while (++j<N);
248    }
249    celt_assert2(pulsesLeft>=1, "Allocated too many pulses in the quick pass");
250
251    /* This should never happen, but just in case it does (e.g. on silence)
252       we fill the first bin with pulses. */
253 #ifdef FIXED_POINT_DEBUG
254    celt_assert2(pulsesLeft<=N+3, "Not enough pulses in the quick pass");
255 #endif
256    if (pulsesLeft > N+3)
257    {
258       opus_val16 tmp = pulsesLeft;
259       yy = MAC16_16(yy, tmp, tmp);
260       yy = MAC16_16(yy, tmp, y[0]);
261       iy[0] += pulsesLeft;
262       pulsesLeft=0;
263    }
264
265    s = 1;
266    for (i=0;i<pulsesLeft;i++)
267    {
268       int best_id;
269       opus_val32 best_num = -VERY_LARGE16;
270       opus_val16 best_den = 0;
271 #ifdef FIXED_POINT
272       int rshift;
273 #endif
274 #ifdef FIXED_POINT
275       rshift = 1+celt_ilog2(K-pulsesLeft+i+1);
276 #endif
277       best_id = 0;
278       /* The squared magnitude term gets added anyway, so we might as well
279          add it outside the loop */
280       yy = ADD32(yy, 1);
281       j=0;
282       do {
283          opus_val16 Rxy, Ryy;
284          /* Temporary sums of the new pulse(s) */
285          Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[j])),rshift));
286          /* We're multiplying y[j] by two so we don't have to do it here */
287          Ryy = ADD16(yy, y[j]);
288
289          /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
290             Rxy is positive because the sign is pre-computed) */
291          Rxy = MULT16_16_Q15(Rxy,Rxy);
292          /* The idea is to check for num/den >= best_num/best_den, but that way
293             we can do it without any division */
294          /* OPT: Make sure to use conditional moves here */
295          if (MULT16_16(best_den, Rxy) > MULT16_16(Ryy, best_num))
296          {
297             best_den = Ryy;
298             best_num = Rxy;
299             best_id = j;
300          }
301       } while (++j<N);
302
303       /* Updating the sums of the new pulse(s) */
304       xy = ADD32(xy, EXTEND32(X[best_id]));
305       /* We're multiplying y[j] by two so we don't have to do it here */
306       yy = ADD16(yy, y[best_id]);
307
308       /* Only now that we've made the final choice, update y/iy */
309       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
310       y[best_id] += 2*s;
311       iy[best_id]++;
312    }
313
314    /* Put the original sign back */
315    j=0;
316    do {
317       X[j] = MULT16_16(signx[j],X[j]);
318       if (signx[j] < 0)
319          iy[j] = -iy[j];
320    } while (++j<N);
321    encode_pulses(iy, N, K, enc);
322
323 #ifdef RESYNTH
324    normalise_residual(iy, X, N, yy, gain);
325    exp_rotation(X, N, -1, B, K, spread);
326 #endif
327
328    collapse_mask = extract_collapse_mask(iy, N, B);
329    RESTORE_STACK;
330    return collapse_mask;
331 }
332
333 /** Decode pulse vector and combine the result with the pitch vector to produce
334     the final normalised signal in the current band. */
335 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
336       ec_dec *dec, opus_val16 gain)
337 {
338    int i;
339    opus_val32 Ryy;
340    unsigned collapse_mask;
341    VARDECL(int, iy);
342    SAVE_STACK;
343
344    celt_assert2(K>0, "alg_unquant() needs at least one pulse");
345    celt_assert2(N>1, "alg_unquant() needs at least two dimensions");
346    ALLOC(iy, N, int);
347    decode_pulses(iy, N, K, dec);
348    Ryy = 0;
349    i=0;
350    do {
351       Ryy = MAC16_16(Ryy, iy[i], iy[i]);
352    } while (++i < N);
353    normalise_residual(iy, X, N, Ryy, gain);
354    exp_rotation(X, N, -1, B, K, spread);
355    collapse_mask = extract_collapse_mask(iy, N, B);
356    RESTORE_STACK;
357    return collapse_mask;
358 }
359
360 void renormalise_vector(celt_norm *X, int N, opus_val16 gain)
361 {
362    int i;
363 #ifdef FIXED_POINT
364    int k;
365 #endif
366    opus_val32 E = EPSILON;
367    opus_val16 g;
368    opus_val32 t;
369    celt_norm *xptr = X;
370    for (i=0;i<N;i++)
371    {
372       E = MAC16_16(E, *xptr, *xptr);
373       xptr++;
374    }
375 #ifdef FIXED_POINT
376    k = celt_ilog2(E)>>1;
377 #endif
378    t = VSHR32(E, (k-7)<<1);
379    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
380
381    xptr = X;
382    for (i=0;i<N;i++)
383    {
384       *xptr = EXTRACT16(PSHR32(MULT16_16(g, *xptr), k+1));
385       xptr++;
386    }
387    /*return celt_sqrt(E);*/
388 }
389
390 int stereo_itheta(celt_norm *X, celt_norm *Y, int stereo, int N)
391 {
392    int i;
393    int itheta;
394    opus_val16 mid, side;
395    opus_val32 Emid, Eside;
396
397    Emid = Eside = EPSILON;
398    if (stereo)
399    {
400       for (i=0;i<N;i++)
401       {
402          celt_norm m, s;
403          m = ADD16(SHR16(X[i],1),SHR16(Y[i],1));
404          s = SUB16(SHR16(X[i],1),SHR16(Y[i],1));
405          Emid = MAC16_16(Emid, m, m);
406          Eside = MAC16_16(Eside, s, s);
407       }
408    } else {
409       for (i=0;i<N;i++)
410       {
411          celt_norm m, s;
412          m = X[i];
413          s = Y[i];
414          Emid = MAC16_16(Emid, m, m);
415          Eside = MAC16_16(Eside, s, s);
416       }
417    }
418    mid = celt_sqrt(Emid);
419    side = celt_sqrt(Eside);
420 #ifdef FIXED_POINT
421    /* 0.63662 = 2/pi */
422    itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
423 #else
424    itheta = (int)floor(.5f+16384*0.63662f*atan2(side,mid));
425 #endif
426
427    return itheta;
428 }