1 /* Copyright (c) 2007-2008 CSIRO 2 Copyright (c) 2007-2009 Xiph.Org Foundation 3 Written by Jean-Marc Valin */ 4 /** 5 @file pitch.c 6 @brief Pitch analysis 7 */ 8 9 /* 10 Redistribution and use in source and binary forms, with or without 11 modification, are permitted provided that the following conditions 12 are met: 13 14 - Redistributions of source code must retain the above copyright 15 notice, this list of conditions and the following disclaimer. 16 17 - Redistributions in binary form must reproduce the above copyright 18 notice, this list of conditions and the following disclaimer in the 19 documentation and/or other materials provided with the distribution. 20 21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 25 OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 26 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 27 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 28 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 29 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 30 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 */ 33 34 #ifdef HAVE_CONFIG_H 35 #include "config.h" 36 #endif 37 38 #include "pitch.h" 39 #include "os_support.h" 40 #include "modes.h" 41 #include "stack_alloc.h" 42 #include "mathops.h" 43 #include "celt_lpc.h" 44 45 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len, 46 int max_pitch, int *best_pitch 47 #ifdef FIXED_POINT 48 , int yshift, opus_val32 maxcorr 49 #endif 50 ) 51 { 52 int i, j; 53 opus_val32 Syy=1; 54 opus_val16 best_num[2]; 55 opus_val32 best_den[2]; 56 #ifdef FIXED_POINT 57 int xshift; 58 59 xshift = celt_ilog2(maxcorr)-14; 60 #endif 61 62 best_num[0] = -1; 63 best_num[1] = -1; 64 best_den[0] = 0; 65 best_den[1] = 0; 66 best_pitch[0] = 0; 67 best_pitch[1] = 1; 68 for (j=0;j<len;j++) 69 Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift)); 70 for (i=0;i<max_pitch;i++) 71 { 72 if (xcorr[i]>0) 73 { 74 opus_val16 num; 75 opus_val32 xcorr16; 76 xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift)); 77 #ifndef FIXED_POINT 78 /* Considering the range of xcorr16, this should avoid both underflows 79 and overflows (inf) when squaring xcorr16 */ 80 xcorr16 *= 1e-12f; 81 #endif 82 num = MULT16_16_Q15(xcorr16,xcorr16); 83 if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy)) 84 { 85 if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy)) 86 { 87 best_num[1] = best_num[0]; 88 best_den[1] = best_den[0]; 89 best_pitch[1] = best_pitch[0]; 90 best_num[0] = num; 91 best_den[0] = Syy; 92 best_pitch[0] = i; 93 } else { 94 best_num[1] = num; 95 best_den[1] = Syy; 96 best_pitch[1] = i; 97 } 98 } 99 } 100 Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift); 101 Syy = MAX32(1, Syy); 102 } 103 } 104 105 void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp, 106 int len, int C) 107 { 108 int i; 109 opus_val32 ac[5]; 110 opus_val16 tmp=Q15ONE; 111 opus_val16 lpc[4], mem[4]={0,0,0,0}; 112 #ifdef FIXED_POINT 113 int shift; 114 opus_val32 maxabs = celt_maxabs32(x[0], len); 115 if (C==2) 116 { 117 opus_val32 maxabs_1 = celt_maxabs32(x[1], len); 118 maxabs = MAX32(maxabs, maxabs_1); 119 } 120 if (maxabs<1) 121 maxabs=1; 122 shift = celt_ilog2(maxabs)-10; 123 if (shift<0) 124 shift=0; 125 if (C==2) 126 shift++; 127 #endif 128 for (i=1;i<len>>1;i++) 129 x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift); 130 x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift); 131 if (C==2) 132 { 133 for (i=1;i<len>>1;i++) 134 x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift); 135 x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift); 136 } 137 138 _celt_autocorr(x_lp, ac, NULL, 0, 139 4, len>>1); 140 141 /* Noise floor -40 dB */ 142 #ifdef FIXED_POINT 143 ac[0] += SHR32(ac[0],13); 144 #else 145 ac[0] *= 1.0001f; 146 #endif 147 /* Lag windowing */ 148 for (i=1;i<=4;i++) 149 { 150 /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/ 151 #ifdef FIXED_POINT 152 ac[i] -= MULT16_32_Q15(2*i*i, ac[i]); 153 #else 154 ac[i] -= ac[i]*(.008f*i)*(.008f*i); 155 #endif 156 } 157 158 _celt_lpc(lpc, ac, 4); 159 for (i=0;i<4;i++) 160 { 161 tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp); 162 lpc[i] = MULT16_16_Q15(lpc[i], tmp); 163 } 164 celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem); 165 166 mem[0]=0; 167 lpc[0]=QCONST16(.8f,12); 168 celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem); 169 170 } 171 172 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y, 173 int len, int max_pitch, int *pitch) 174 { 175 int i, j; 176 int lag; 177 int best_pitch[2]={0,0}; 178 VARDECL(opus_val16, x_lp4); 179 VARDECL(opus_val16, y_lp4); 180 VARDECL(opus_val32, xcorr); 181 #ifdef FIXED_POINT 182 opus_val32 maxcorr=1; 183 opus_val16 xmax, ymax; 184 int shift=0; 185 #endif 186 int offset; 187 188 SAVE_STACK; 189 190 celt_assert(len>0); 191 celt_assert(max_pitch>0); 192 lag = len+max_pitch; 193 194 ALLOC(x_lp4, len>>2, opus_val16); 195 ALLOC(y_lp4, lag>>2, opus_val16); 196 ALLOC(xcorr, max_pitch>>1, opus_val32); 197 198 /* Downsample by 2 again */ 199 for (j=0;j<len>>2;j++) 200 x_lp4[j] = x_lp[2*j]; 201 for (j=0;j<lag>>2;j++) 202 y_lp4[j] = y[2*j]; 203 204 #ifdef FIXED_POINT 205 xmax = celt_maxabs16(x_lp4, len>>2); 206 ymax = celt_maxabs16(y_lp4, lag>>2); 207 shift = celt_ilog2(MAX16(1, MAX16(xmax, ymax)))-11; 208 if (shift>0) 209 { 210 for (j=0;j<len>>2;j++) 211 x_lp4[j] = SHR16(x_lp4[j], shift); 212 for (j=0;j<lag>>2;j++) 213 y_lp4[j] = SHR16(y_lp4[j], shift); 214 /* Use double the shift for a MAC */ 215 shift *= 2; 216 } else { 217 shift = 0; 218 } 219 #endif 220 221 /* Coarse search with 4x decimation */ 222 223 for (i=0;i<max_pitch>>2;i++) 224 { 225 opus_val32 sum = 0; 226 for (j=0;j<len>>2;j++) 227 sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]); 228 xcorr[i] = MAX32(-1, sum); 229 #ifdef FIXED_POINT 230 maxcorr = MAX32(maxcorr, sum); 231 #endif 232 } 233 find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch 234 #ifdef FIXED_POINT 235 , 0, maxcorr 236 #endif 237 ); 238 239 /* Finer search with 2x decimation */ 240 #ifdef FIXED_POINT 241 maxcorr=1; 242 #endif 243 for (i=0;i<max_pitch>>1;i++) 244 { 245 opus_val32 sum=0; 246 xcorr[i] = 0; 247 if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2) 248 continue; 249 for (j=0;j<len>>1;j++) 250 sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift); 251 xcorr[i] = MAX32(-1, sum); 252 #ifdef FIXED_POINT 253 maxcorr = MAX32(maxcorr, sum); 254 #endif 255 } 256 find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch 257 #ifdef FIXED_POINT 258 , shift+1, maxcorr 259 #endif 260 ); 261 262 /* Refine by pseudo-interpolation */ 263 if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1) 264 { 265 opus_val32 a, b, c; 266 a = xcorr[best_pitch[0]-1]; 267 b = xcorr[best_pitch[0]]; 268 c = xcorr[best_pitch[0]+1]; 269 if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a)) 270 offset = 1; 271 else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c)) 272 offset = -1; 273 else 274 offset = 0; 275 } else { 276 offset = 0; 277 } 278 *pitch = 2*best_pitch[0]-offset; 279 280 RESTORE_STACK; 281 } 282 283 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2}; 284 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod, 285 int N, int *T0_, int prev_period, opus_val16 prev_gain) 286 { 287 int k, i, T, T0; 288 opus_val16 g, g0; 289 opus_val16 pg; 290 opus_val32 xy,xx,yy; 291 opus_val32 xcorr[3]; 292 opus_val32 best_xy, best_yy; 293 int offset; 294 int minperiod0; 295 296 minperiod0 = minperiod; 297 maxperiod /= 2; 298 minperiod /= 2; 299 *T0_ /= 2; 300 prev_period /= 2; 301 N /= 2; 302 x += maxperiod; 303 if (*T0_>=maxperiod) 304 *T0_=maxperiod-1; 305 306 T = T0 = *T0_; 307 xx=xy=yy=0; 308 for (i=0;i<N;i++) 309 { 310 xy = MAC16_16(xy, x[i], x[i-T0]); 311 xx = MAC16_16(xx, x[i], x[i]); 312 yy = MAC16_16(yy, x[i-T0],x[i-T0]); 313 } 314 best_xy = xy; 315 best_yy = yy; 316 #ifdef FIXED_POINT 317 { 318 opus_val32 x2y2; 319 int sh, t; 320 x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy)); 321 sh = celt_ilog2(x2y2)>>1; 322 t = VSHR32(x2y2, 2*(sh-7)); 323 g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); 324 } 325 #else 326 g = g0 = xy/celt_sqrt(1+xx*yy); 327 #endif 328 /* Look for any pitch at T/k */ 329 for (k=2;k<=15;k++) 330 { 331 int T1, T1b; 332 opus_val16 g1; 333 opus_val16 cont=0; 334 T1 = (2*T0+k)/(2*k); 335 if (T1 < minperiod) 336 break; 337 /* Look for another strong correlation at T1b */ 338 if (k==2) 339 { 340 if (T1+T0>maxperiod) 341 T1b = T0; 342 else 343 T1b = T0+T1; 344 } else 345 { 346 T1b = (2*second_check[k]*T0+k)/(2*k); 347 } 348 xy=yy=0; 349 for (i=0;i<N;i++) 350 { 351 xy = MAC16_16(xy, x[i], x[i-T1]); 352 yy = MAC16_16(yy, x[i-T1], x[i-T1]); 353 354 xy = MAC16_16(xy, x[i], x[i-T1b]); 355 yy = MAC16_16(yy, x[i-T1b], x[i-T1b]); 356 } 357 #ifdef FIXED_POINT 358 { 359 opus_val32 x2y2; 360 int sh, t; 361 x2y2 = 1+MULT32_32_Q31(xx,yy); 362 sh = celt_ilog2(x2y2)>>1; 363 t = VSHR32(x2y2, 2*(sh-7)); 364 g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1); 365 } 366 #else 367 g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy); 368 #endif 369 if (abs(T1-prev_period)<=1) 370 cont = prev_gain; 371 else if (abs(T1-prev_period)<=2 && 5*k*k < T0) 372 cont = HALF32(prev_gain); 373 else 374 cont = 0; 375 if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont) 376 { 377 best_xy = xy; 378 best_yy = yy; 379 T = T1; 380 g = g1; 381 } 382 } 383 best_xy = MAX32(0, best_xy); 384 if (best_yy <= best_xy) 385 pg = Q15ONE; 386 else 387 pg = SHR32(frac_div32(best_xy,best_yy+1),16); 388 389 for (k=0;k<3;k++) 390 { 391 int T1 = T+k-1; 392 xy = 0; 393 for (i=0;i<N;i++) 394 xy = MAC16_16(xy, x[i], x[i-T1]); 395 xcorr[k] = xy; 396 } 397 if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0])) 398 offset = 1; 399 else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2])) 400 offset = -1; 401 else 402 offset = 0; 403 if (pg > g) 404 pg = g; 405 *T0_ = 2*T+offset; 406 407 if (*T0_<minperiod0) 408 *T0_=minperiod0; 409 return pg; 410 } 411
