Remove redundant mid-only flag when side VAD flag is set.
[opus.git] / celt / 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    /*NOTE: As a minor optimization, we could 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, 2*(k-7));
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    /*NOTE: As a minor optimization, we could 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, ec_enc *enc
170 #ifdef RESYNTH
171    , opus_val16 gain
172 #endif
173    )
174 {
175    VARDECL(celt_norm, y);
176    VARDECL(int, iy);
177    VARDECL(opus_val16, signx);
178    int i, j;
179    opus_val16 s;
180    int pulsesLeft;
181    opus_val32 sum;
182    opus_val32 xy;
183    opus_val16 yy;
184    unsigned collapse_mask;
185    SAVE_STACK;
186
187    celt_assert2(K>0, "alg_quant() needs at least one pulse");
188    celt_assert2(N>1, "alg_quant() needs at least two dimensions");
189
190    ALLOC(y, N, celt_norm);
191    ALLOC(iy, N, int);
192    ALLOC(signx, N, opus_val16);
193
194    exp_rotation(X, N, 1, B, K, spread);
195
196    /* Get rid of the sign */
197    sum = 0;
198    j=0; do {
199       if (X[j]>0)
200          signx[j]=1;
201       else {
202          signx[j]=-1;
203          X[j]=-X[j];
204       }
205       iy[j] = 0;
206       y[j] = 0;
207    } while (++j<N);
208
209    xy = yy = 0;
210
211    pulsesLeft = K;
212
213    /* Do a pre-search by projecting on the pyramid */
214    if (K > (N>>1))
215    {
216       opus_val16 rcp;
217       j=0; do {
218          sum += X[j];
219       }  while (++j<N);
220
221       /* If X is too small, just replace it with a pulse at 0 */
222 #ifdef FIXED_POINT
223       if (sum <= K)
224 #else
225       /* Prevents infinities and NaNs from causing too many pulses
226          to be allocated. 64 is an approximation of infinity here. */
227       if (!(sum > EPSILON && sum < 64))
228 #endif
229       {
230          X[0] = QCONST16(1.f,14);
231          j=1; do
232             X[j]=0;
233          while (++j<N);
234          sum = QCONST16(1.f,14);
235       }
236       /* Do we have sufficient accuracy here? */
237       rcp = EXTRACT16(MULT16_32_Q16(K-1, celt_rcp(sum)));
238       j=0; do {
239 #ifdef FIXED_POINT
240          /* It's really important to round *towards zero* here */
241          iy[j] = MULT16_16_Q15(X[j],rcp);
242 #else
243          iy[j] = (int)floor(rcp*X[j]);
244 #endif
245          y[j] = (celt_norm)iy[j];
246          yy = MAC16_16(yy, y[j],y[j]);
247          xy = MAC16_16(xy, X[j],y[j]);
248          y[j] *= 2;
249          pulsesLeft -= iy[j];
250       }  while (++j<N);
251    }
252    celt_assert2(pulsesLeft>=1, "Allocated too many pulses in the quick pass");
253
254    /* This should never happen, but just in case it does (e.g. on silence)
255       we fill the first bin with pulses. */
256 #ifdef FIXED_POINT_DEBUG
257    celt_assert2(pulsesLeft<=N+3, "Not enough pulses in the quick pass");
258 #endif
259    if (pulsesLeft > N+3)
260    {
261       opus_val16 tmp = pulsesLeft;
262       yy = MAC16_16(yy, tmp, tmp);
263       yy = MAC16_16(yy, tmp, y[0]);
264       iy[0] += pulsesLeft;
265       pulsesLeft=0;
266    }
267
268    s = 1;
269    for (i=0;i<pulsesLeft;i++)
270    {
271       int best_id;
272       opus_val32 best_num = -VERY_LARGE16;
273       opus_val16 best_den = 0;
274 #ifdef FIXED_POINT
275       int rshift;
276 #endif
277 #ifdef FIXED_POINT
278       rshift = 1+celt_ilog2(K-pulsesLeft+i+1);
279 #endif
280       best_id = 0;
281       /* The squared magnitude term gets added anyway, so we might as well
282          add it outside the loop */
283       yy = ADD32(yy, 1);
284       j=0;
285       do {
286          opus_val16 Rxy, Ryy;
287          /* Temporary sums of the new pulse(s) */
288          Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[j])),rshift));
289          /* We're multiplying y[j] by two so we don't have to do it here */
290          Ryy = ADD16(yy, y[j]);
291
292          /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
293             Rxy is positive because the sign is pre-computed) */
294          Rxy = MULT16_16_Q15(Rxy,Rxy);
295          /* The idea is to check for num/den >= best_num/best_den, but that way
296             we can do it without any division */
297          /* OPT: Make sure to use conditional moves here */
298          if (MULT16_16(best_den, Rxy) > MULT16_16(Ryy, best_num))
299          {
300             best_den = Ryy;
301             best_num = Rxy;
302             best_id = j;
303          }
304       } while (++j<N);
305
306       /* Updating the sums of the new pulse(s) */
307       xy = ADD32(xy, EXTEND32(X[best_id]));
308       /* We're multiplying y[j] by two so we don't have to do it here */
309       yy = ADD16(yy, y[best_id]);
310
311       /* Only now that we've made the final choice, update y/iy */
312       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
313       y[best_id] += 2*s;
314       iy[best_id]++;
315    }
316
317    /* Put the original sign back */
318    j=0;
319    do {
320       X[j] = MULT16_16(signx[j],X[j]);
321       if (signx[j] < 0)
322          iy[j] = -iy[j];
323    } while (++j<N);
324    encode_pulses(iy, N, K, enc);
325
326 #ifdef RESYNTH
327    normalise_residual(iy, X, N, yy, gain);
328    exp_rotation(X, N, -1, B, K, spread);
329 #endif
330
331    collapse_mask = extract_collapse_mask(iy, N, B);
332    RESTORE_STACK;
333    return collapse_mask;
334 }
335
336 /** Decode pulse vector and combine the result with the pitch vector to produce
337     the final normalised signal in the current band. */
338 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
339       ec_dec *dec, opus_val16 gain)
340 {
341    int i;
342    opus_val32 Ryy;
343    unsigned collapse_mask;
344    VARDECL(int, iy);
345    SAVE_STACK;
346
347    celt_assert2(K>0, "alg_unquant() needs at least one pulse");
348    celt_assert2(N>1, "alg_unquant() needs at least two dimensions");
349    ALLOC(iy, N, int);
350    decode_pulses(iy, N, K, dec);
351    Ryy = 0;
352    i=0;
353    do {
354       Ryy = MAC16_16(Ryy, iy[i], iy[i]);
355    } while (++i < N);
356    normalise_residual(iy, X, N, Ryy, gain);
357    exp_rotation(X, N, -1, B, K, spread);
358    collapse_mask = extract_collapse_mask(iy, N, B);
359    RESTORE_STACK;
360    return collapse_mask;
361 }
362
363 void renormalise_vector(celt_norm *X, int N, opus_val16 gain)
364 {
365    int i;
366 #ifdef FIXED_POINT
367    int k;
368 #endif
369    opus_val32 E = EPSILON;
370    opus_val16 g;
371    opus_val32 t;
372    celt_norm *xptr = X;
373    for (i=0;i<N;i++)
374    {
375       E = MAC16_16(E, *xptr, *xptr);
376       xptr++;
377    }
378 #ifdef FIXED_POINT
379    k = celt_ilog2(E)>>1;
380 #endif
381    t = VSHR32(E, 2*(k-7));
382    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
383
384    xptr = X;
385    for (i=0;i<N;i++)
386    {
387       *xptr = EXTRACT16(PSHR32(MULT16_16(g, *xptr), k+1));
388       xptr++;
389    }
390    /*return celt_sqrt(E);*/
391 }
392
393 int stereo_itheta(celt_norm *X, celt_norm *Y, int stereo, int N)
394 {
395    int i;
396    int itheta;
397    opus_val16 mid, side;
398    opus_val32 Emid, Eside;
399
400    Emid = Eside = EPSILON;
401    if (stereo)
402    {
403       for (i=0;i<N;i++)
404       {
405          celt_norm m, s;
406          m = ADD16(SHR16(X[i],1),SHR16(Y[i],1));
407          s = SUB16(SHR16(X[i],1),SHR16(Y[i],1));
408          Emid = MAC16_16(Emid, m, m);
409          Eside = MAC16_16(Eside, s, s);
410       }
411    } else {
412       for (i=0;i<N;i++)
413       {
414          celt_norm m, s;
415          m = X[i];
416          s = Y[i];
417          Emid = MAC16_16(Emid, m, m);
418          Eside = MAC16_16(Eside, s, s);
419       }
420    }
421    mid = celt_sqrt(Emid);
422    side = celt_sqrt(Eside);
423 #ifdef FIXED_POINT
424    /* 0.63662 = 2/pi */
425    itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
426 #else
427    itheta = (int)floor(.5f+16384*0.63662f*atan2(side,mid));
428 #endif
429
430    return itheta;
431 }