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