Renamed celt_word* to opus_val*
[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 #ifndef M_PI
42 #define M_PI 3.141592653
43 #endif
44
45 static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
46 {
47    int i;
48    celt_norm *Xptr;
49    Xptr = X;
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(SHR32(MULT16_16(c,x2) + MULT16_16(s,x1), 15));
56       *Xptr++      = EXTRACT16(SHR32(MULT16_16(c,x1) - MULT16_16(s,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(SHR32(MULT16_16(c,x2) + MULT16_16(s,x1), 15));
65       *Xptr--      = EXTRACT16(SHR32(MULT16_16(c,x1) - MULT16_16(s,x2), 15));
66    }
67 }
68
69 static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
70 {
71    static const int SPREAD_FACTOR[3]={15,10,5};
72    int i;
73    opus_val16 c, s;
74    opus_val16 gain, theta;
75    int stride2=0;
76    int factor;
77    /*int i;
78    if (len>=30)
79    {
80       for (i=0;i<len;i++)
81          X[i] = 0;
82       X[14] = 1;
83       K=5;
84    }*/
85    if (2*K>=len || spread==SPREAD_NONE)
86       return;
87    factor = SPREAD_FACTOR[spread-1];
88
89    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
90    theta = HALF16(MULT16_16_Q15(gain,gain));
91
92    c = celt_cos_norm(EXTEND32(theta));
93    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
94
95    if (len>=8*stride)
96    {
97       stride2 = 1;
98       /* This is just a simple way of computing sqrt(len/stride) with rounding.
99          It's basically incrementing long as (stride2+0.5)^2 < len/stride.
100          I _think_ it is bit-exact */
101       while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
102          stride2++;
103    }
104    /*TODO: We should be passing around log2(B), not B, for both this and for
105       extract_collapse_mask().*/
106    len /= stride;
107    for (i=0;i<stride;i++)
108    {
109       if (dir < 0)
110       {
111          if (stride2)
112             exp_rotation1(X+i*len, len, stride2, s, c);
113          exp_rotation1(X+i*len, len, 1, c, s);
114       } else {
115          exp_rotation1(X+i*len, len, 1, c, -s);
116          if (stride2)
117             exp_rotation1(X+i*len, len, stride2, s, -c);
118       }
119    }
120    /*if (len>=30)
121    {
122       for (i=0;i<len;i++)
123          printf ("%f ", X[i]);
124       printf ("\n");
125       exit(0);
126    }*/
127 }
128
129 /** Takes the pitch vector and the decoded residual vector, computes the gain
130     that will give ||p+g*y||=1 and mixes the residual with the pitch. */
131 static void normalise_residual(int * restrict iy, celt_norm * restrict X,
132       int N, opus_val32 Ryy, opus_val16 gain)
133 {
134    int i;
135 #ifdef FIXED_POINT
136    int k;
137 #endif
138    opus_val32 t;
139    opus_val16 g;
140
141 #ifdef FIXED_POINT
142    k = celt_ilog2(Ryy)>>1;
143 #endif
144    t = VSHR32(Ryy, (k-7)<<1);
145    g = MULT16_16_P15(celt_rsqrt_norm(t),gain);
146
147    i=0;
148    do
149       X[i] = EXTRACT16(PSHR32(MULT16_16(g, iy[i]), k+1));
150    while (++i < N);
151 }
152
153 static unsigned extract_collapse_mask(int *iy, int N, int B)
154 {
155    unsigned collapse_mask;
156    int N0;
157    int i;
158    if (B<=1)
159       return 1;
160    /*TODO: We should be passing around log2(B), not B, for both this and for
161       exp_rotation().*/
162    N0 = N/B;
163    collapse_mask = 0;
164    i=0; do {
165       int j;
166       j=0; do {
167          collapse_mask |= (iy[i*N0+j]!=0)<<i;
168       } while (++j<N0);
169    } while (++i<B);
170    return collapse_mask;
171 }
172
173 unsigned alg_quant(celt_norm *X, int N, int K, int spread, int B,
174       int resynth, ec_enc *enc, opus_val16 gain)
175 {
176    VARDECL(celt_norm, y);
177    VARDECL(int, iy);
178    VARDECL(opus_val16, signx);
179    int i, j;
180    opus_val16 s;
181    int pulsesLeft;
182    opus_val32 sum;
183    opus_val32 xy;
184    opus_val16 yy;
185    unsigned collapse_mask;
186    SAVE_STACK;
187
188    celt_assert2(K!=0, "alg_quant() needs at least one pulse");
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] = 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    if (resynth)
327    {
328       normalise_residual(iy, X, N, yy, gain);
329       exp_rotation(X, N, -1, B, K, spread);
330    }
331    collapse_mask = extract_collapse_mask(iy, N, B);
332    RESTORE_STACK;
333    return collapse_mask;
334 }
335
336
337 /** Decode pulse vector and combine the result with the pitch vector to produce
338     the final normalised signal in the current band. */
339 unsigned alg_unquant(celt_norm *X, int N, int K, int spread, int B,
340       ec_dec *dec, opus_val16 gain)
341 {
342    int i;
343    opus_val32 Ryy;
344    unsigned collapse_mask;
345    VARDECL(int, iy);
346    SAVE_STACK;
347
348    celt_assert2(K!=0, "alg_unquant() needs at least one pulse");
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, (k-7)<<1);
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 }