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