Speeding up PVQ using unlikely() and moving first position out of the loop
[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 COPYRIGHT OWNER
20    OR 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 #include "pitch.h"
41
42 #ifndef OVERRIDE_vq_exp_rotation1
43 static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
44 {
45    int i;
46    opus_val16 ms;
47    celt_norm *Xptr;
48    Xptr = X;
49    ms = NEG16(s);
50    for (i=0;i<len-stride;i++)
51    {
52       celt_norm x1, x2;
53       x1 = Xptr[0];
54       x2 = Xptr[stride];
55       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
56       *Xptr++      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
57    }
58    Xptr = &X[len-2*stride-1];
59    for (i=len-2*stride-1;i>=0;i--)
60    {
61       celt_norm x1, x2;
62       x1 = Xptr[0];
63       x2 = Xptr[stride];
64       Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
65       *Xptr--      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
66    }
67 }
68 #endif /* OVERRIDE_vq_exp_rotation1 */
69
70 static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
71 {
72    static const int SPREAD_FACTOR[3]={15,10,5};
73    int i;
74    opus_val16 c, s;
75    opus_val16 gain, theta;
76    int stride2=0;
77    int factor;
78
79    if (2*K>=len || spread==SPREAD_NONE)
80       return;
81    factor = SPREAD_FACTOR[spread-1];
82
83    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
84    theta = HALF16(MULT16_16_Q15(gain,gain));
85
86    c = celt_cos_norm(EXTEND32(theta));
87    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
88
89    if (len>=8*stride)
90    {
91       stride2 = 1;
92       /* This is just a simple (equivalent) way of computing sqrt(len/stride) with rounding.
93          It's basically incrementing long as (stride2+0.5)^2 < len/stride. */
94       while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
95          stride2++;
96    }
97    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
98       extract_collapse_mask().*/
99    len = celt_udiv(len, stride);
100    for (i=0;i<stride;i++)
101    {
102       if (dir < 0)
103       {
104          if (stride2)
105             exp_rotation1(X+i*len, len, stride2, s, c);
106          exp_rotation1(X+i*len, len, 1, c, s);
107       } else {
108          exp_rotation1(X+i*len, len, 1, c, -s);
109          if (stride2)
110             exp_rotation1(X+i*len, len, stride2, s, -c);
111       }
112    }
113 }
114
115 /** Takes the pitch vector and the decoded residual vector, computes the gain
116     that will give ||p+g*y||=1 and mixes the residual with the pitch. */
117 static void normalise_residual(int * OPUS_RESTRICT iy, celt_norm * OPUS_RESTRICT X,
118       int N, opus_val32 Ryy, opus_val16 gain)
119 {
120    int i;
121 #ifdef FIXED_POINT
122    int k;
123 #endif
124    opus_val32 t;
125    opus_val16 g;
126
127 #ifdef FIXED_POINT
128    k = celt_ilog2(Ryy)>>1;
129 #endif
130    t = VSHR32(Ryy, 2*(k-7));
131    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
132
133    i=0;
134    do
135       X[i] = EXTRACT16(PSHR32(MULT16_16(g, iy[i]), k+1));
136    while (++i < N);
137 }
138
139 static unsigned extract_collapse_mask(int *iy, int N, int B)
140 {
141    unsigned collapse_mask;
142    int N0;
143    int i;
144    if (B<=1)
145       return 1;
146    /*NOTE: As a minor optimization, we could be passing around log2(B), not B, for both this and for
147       exp_rotation().*/
148    N0 = celt_udiv(N, B);
149    collapse_mask = 0;
150    i=0; do {
151       int j;
152       unsigned tmp=0;
153       j=0; do {
154          tmp |= iy[i*N0+j];
155       } while (++j<N0);
156       collapse_mask |= (tmp!=0)<<i;
157    } while (++i<B);
158    return collapse_mask;
159 }
160
161 unsigned alg_quant(celt_norm *X, int N, int K, int spread, int B, ec_enc *enc,
162       opus_val16 gain, int resynth)
163 {
164    VARDECL(celt_norm, y);
165    VARDECL(int, iy);
166    VARDECL(opus_val16, signx);
167    int i, j;
168    int pulsesLeft;
169    opus_val32 sum;
170    opus_val32 xy;
171    opus_val16 yy;
172    unsigned collapse_mask;
173    SAVE_STACK;
174
175    celt_assert2(K>0, "alg_quant() needs at least one pulse");
176    celt_assert2(N>1, "alg_quant() needs at least two dimensions");
177
178    ALLOC(y, N, celt_norm);
179    ALLOC(iy, N, int);
180    ALLOC(signx, N, opus_val16);
181
182    exp_rotation(X, N, 1, B, K, spread);
183
184    /* Get rid of the sign */
185    sum = 0;
186    j=0; do {
187       /* OPT: Make sure the following two lines result in conditional moves
188          rather than branches. */
189       signx[j] = X[j]>0 ? 1 : -1;
190       X[j] = ABS16(X[j]);
191       iy[j] = 0;
192       y[j] = 0;
193    } while (++j<N);
194
195    xy = yy = 0;
196
197    pulsesLeft = K;
198
199    /* Do a pre-search by projecting on the pyramid */
200    if (K > (N>>1))
201    {
202       opus_val16 rcp;
203       j=0; do {
204          sum += X[j];
205       }  while (++j<N);
206
207       /* If X is too small, just replace it with a pulse at 0 */
208 #ifdef FIXED_POINT
209       if (sum <= K)
210 #else
211       /* Prevents infinities and NaNs from causing too many pulses
212          to be allocated. 64 is an approximation of infinity here. */
213       if (!(sum > EPSILON && sum < 64))
214 #endif
215       {
216          X[0] = QCONST16(1.f,14);
217          j=1; do
218             X[j]=0;
219          while (++j<N);
220          sum = QCONST16(1.f,14);
221       }
222       rcp = EXTRACT16(MULT16_32_Q16(K-1, celt_rcp(sum)));
223       j=0; do {
224 #ifdef FIXED_POINT
225          /* It's really important to round *towards zero* here */
226          iy[j] = MULT16_16_Q15(X[j],rcp);
227 #else
228          iy[j] = (int)floor(rcp*X[j]);
229 #endif
230          y[j] = (celt_norm)iy[j];
231          yy = MAC16_16(yy, y[j],y[j]);
232          xy = MAC16_16(xy, X[j],y[j]);
233          y[j] *= 2;
234          pulsesLeft -= iy[j];
235       }  while (++j<N);
236    }
237    celt_assert2(pulsesLeft>=1, "Allocated too many pulses in the quick pass");
238
239    /* This should never happen, but just in case it does (e.g. on silence)
240       we fill the first bin with pulses. */
241 #ifdef FIXED_POINT_DEBUG
242    celt_assert2(pulsesLeft<=N+3, "Not enough pulses in the quick pass");
243 #endif
244    if (pulsesLeft > N+3)
245    {
246       opus_val16 tmp = (opus_val16)pulsesLeft;
247       yy = MAC16_16(yy, tmp, tmp);
248       yy = MAC16_16(yy, tmp, y[0]);
249       iy[0] += pulsesLeft;
250       pulsesLeft=0;
251    }
252
253    for (i=0;i<pulsesLeft;i++)
254    {
255       opus_val16 Rxy, Ryy;
256       int best_id;
257       opus_val32 best_num;
258       opus_val16 best_den;
259 #ifdef FIXED_POINT
260       int rshift;
261 #endif
262 #ifdef FIXED_POINT
263       rshift = 1+celt_ilog2(K-pulsesLeft+i+1);
264 #endif
265       best_id = 0;
266       /* The squared magnitude term gets added anyway, so we might as well
267          add it outside the loop */
268       yy = ADD16(yy, 1);
269
270       /* Calculations for position 0 are out of the loop, in part to reduce
271          mispredicted branches (since the if condition is usually false)
272          in the loop. */
273       /* Temporary sums of the new pulse(s) */
274       Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[0])),rshift));
275       /* We're multiplying y[j] by two so we don't have to do it here */
276       Ryy = ADD16(yy, y[0]);
277
278       /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
279          Rxy is positive because the sign is pre-computed) */
280       Rxy = MULT16_16_Q15(Rxy,Rxy);
281       best_den = Ryy;
282       best_num = Rxy;
283       j=1;
284       do {
285          /* Temporary sums of the new pulse(s) */
286          Rxy = EXTRACT16(SHR32(ADD32(xy, EXTEND32(X[j])),rshift));
287          /* We're multiplying y[j] by two so we don't have to do it here */
288          Ryy = ADD16(yy, y[j]);
289
290          /* Approximate score: we maximise Rxy/sqrt(Ryy) (we're guaranteed that
291             Rxy is positive because the sign is pre-computed) */
292          Rxy = MULT16_16_Q15(Rxy,Rxy);
293          /* The idea is to check for num/den >= best_num/best_den, but that way
294             we can do it without any division */
295          /* OPT: It's not clear whether a cmov is faster than a branch here
296             since the condition is more often false than true and using
297             a cmov introduces data dependencies across iterations. The optimal
298             choice may be architecture-dependent. */
299          if (opus_unlikely(MULT16_16(best_den, Rxy) > MULT16_16(Ryy, best_num)))
300          {
301             best_den = Ryy;
302             best_num = Rxy;
303             best_id = j;
304          }
305       } while (++j<N);
306
307       /* Updating the sums of the new pulse(s) */
308       xy = ADD32(xy, EXTEND32(X[best_id]));
309       /* We're multiplying y[j] by two so we don't have to do it here */
310       yy = ADD16(yy, y[best_id]);
311
312       /* Only now that we've made the final choice, update y/iy */
313       /* Multiplying y[j] by 2 so we don't have to do it everywhere else */
314       y[best_id] += 2;
315       iy[best_id]++;
316    }
317
318    /* Put the original sign back */
319    j=0;
320    do {
321       X[j] = MULT16_16(signx[j],X[j]);
322       /* OPT: Make sure your compiler uses a conditional move here rather than
323          a branch. */
324       iy[j] = signx[j] < 0 ? -iy[j] : iy[j];
325    } while (++j<N);
326    encode_pulses(iy, N, K, enc);
327
328    if (resynth)
329    {
330       normalise_residual(iy, X, N, yy, gain);
331       exp_rotation(X, N, -1, B, K, spread);
332    }
333
334    collapse_mask = extract_collapse_mask(iy, N, B);
335    RESTORE_STACK;
336    return collapse_mask;
337 }
338
339 /** Decode pulse vector and combine the result with the pitch vector to produce
340     the final normalised signal in the current band. */
341 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
342       ec_dec *dec, opus_val16 gain)
343 {
344    opus_val32 Ryy;
345    unsigned collapse_mask;
346    VARDECL(int, iy);
347    SAVE_STACK;
348
349    celt_assert2(K>0, "alg_unquant() needs at least one pulse");
350    celt_assert2(N>1, "alg_unquant() needs at least two dimensions");
351    ALLOC(iy, N, int);
352    Ryy = decode_pulses(iy, N, K, dec);
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 #ifndef OVERRIDE_renormalise_vector
361 void renormalise_vector(celt_norm *X, int N, opus_val16 gain, int arch)
362 {
363    int i;
364 #ifdef FIXED_POINT
365    int k;
366 #endif
367    opus_val32 E;
368    opus_val16 g;
369    opus_val32 t;
370    celt_norm *xptr;
371    E = EPSILON + celt_inner_prod(X, X, N, arch);
372 #ifdef FIXED_POINT
373    k = celt_ilog2(E)>>1;
374 #endif
375    t = VSHR32(E, 2*(k-7));
376    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
377
378    xptr = X;
379    for (i=0;i<N;i++)
380    {
381       *xptr = EXTRACT16(PSHR32(MULT16_16(g, *xptr), k+1));
382       xptr++;
383    }
384    /*return celt_sqrt(E);*/
385 }
386 #endif /* OVERRIDE_renormalise_vector */
387
388 int stereo_itheta(const celt_norm *X, const celt_norm *Y, int stereo, int N, int arch)
389 {
390    int i;
391    int itheta;
392    opus_val16 mid, side;
393    opus_val32 Emid, Eside;
394
395    Emid = Eside = EPSILON;
396    if (stereo)
397    {
398       for (i=0;i<N;i++)
399       {
400          celt_norm m, s;
401          m = ADD16(SHR16(X[i],1),SHR16(Y[i],1));
402          s = SUB16(SHR16(X[i],1),SHR16(Y[i],1));
403          Emid = MAC16_16(Emid, m, m);
404          Eside = MAC16_16(Eside, s, s);
405       }
406    } else {
407       Emid += celt_inner_prod(X, X, N, arch);
408       Eside += celt_inner_prod(Y, Y, N, arch);
409    }
410    mid = celt_sqrt(Emid);
411    side = celt_sqrt(Eside);
412 #ifdef FIXED_POINT
413    /* 0.63662 = 2/pi */
414    itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
415 #else
416    itheta = (int)floor(.5f+16384*0.63662f*atan2(side,mid));
417 #endif
418
419    return itheta;
420 }