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