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 "common.h"
40 //#include "modes.h"
41 //#include "stack_alloc.h"
42 //#include "mathops.h"
43 #include "celt_lpc.h"
44 #include "math.h"
45
find_best_pitch(opus_val32 * xcorr,opus_val16 * y,int len,int max_pitch,int * best_pitch,int yshift,opus_val32 maxcorr)46 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
47 int max_pitch, int *best_pitch
48 #ifdef FIXED_POINT
49 , int yshift, opus_val32 maxcorr
50 #endif
51 )
52 {
53 int i, j;
54 opus_val32 Syy=1;
55 opus_val16 best_num[2];
56 opus_val32 best_den[2];
57 #ifdef FIXED_POINT
58 int xshift;
59
60 xshift = celt_ilog2(maxcorr)-14;
61 #endif
62
63 best_num[0] = -1;
64 best_num[1] = -1;
65 best_den[0] = 0;
66 best_den[1] = 0;
67 best_pitch[0] = 0;
68 best_pitch[1] = 1;
69 for (j=0;j<len;j++)
70 Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
71 for (i=0;i<max_pitch;i++)
72 {
73 if (xcorr[i]>0)
74 {
75 opus_val16 num;
76 opus_val32 xcorr16;
77 xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
78 #ifndef FIXED_POINT
79 /* Considering the range of xcorr16, this should avoid both underflows
80 and overflows (inf) when squaring xcorr16 */
81 xcorr16 *= 1e-12f;
82 #endif
83 num = MULT16_16_Q15(xcorr16,xcorr16);
84 if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
85 {
86 if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
87 {
88 best_num[1] = best_num[0];
89 best_den[1] = best_den[0];
90 best_pitch[1] = best_pitch[0];
91 best_num[0] = num;
92 best_den[0] = Syy;
93 best_pitch[0] = i;
94 } else {
95 best_num[1] = num;
96 best_den[1] = Syy;
97 best_pitch[1] = i;
98 }
99 }
100 }
101 Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
102 Syy = MAX32(1, Syy);
103 }
104 }
105
celt_fir5(const opus_val16 * x,const opus_val16 * num,opus_val16 * y,int N,opus_val16 * mem)106 static void celt_fir5(const opus_val16 *x,
107 const opus_val16 *num,
108 opus_val16 *y,
109 int N,
110 opus_val16 *mem)
111 {
112 int i;
113 opus_val16 num0, num1, num2, num3, num4;
114 opus_val32 mem0, mem1, mem2, mem3, mem4;
115 num0=num[0];
116 num1=num[1];
117 num2=num[2];
118 num3=num[3];
119 num4=num[4];
120 mem0=mem[0];
121 mem1=mem[1];
122 mem2=mem[2];
123 mem3=mem[3];
124 mem4=mem[4];
125 for (i=0;i<N;i++)
126 {
127 opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
128 sum = MAC16_16(sum,num0,mem0);
129 sum = MAC16_16(sum,num1,mem1);
130 sum = MAC16_16(sum,num2,mem2);
131 sum = MAC16_16(sum,num3,mem3);
132 sum = MAC16_16(sum,num4,mem4);
133 mem4 = mem3;
134 mem3 = mem2;
135 mem2 = mem1;
136 mem1 = mem0;
137 mem0 = x[i];
138 y[i] = ROUND16(sum, SIG_SHIFT);
139 }
140 mem[0]=mem0;
141 mem[1]=mem1;
142 mem[2]=mem2;
143 mem[3]=mem3;
144 mem[4]=mem4;
145 }
146
147
pitch_downsample(celt_sig * x[],opus_val16 * x_lp,int len,int C)148 void pitch_downsample(celt_sig *x[], opus_val16 *x_lp,
149 int len, int C)
150 {
151 int i;
152 opus_val32 ac[5];
153 opus_val16 tmp=Q15ONE;
154 opus_val16 lpc[4], mem[5]={0,0,0,0,0};
155 opus_val16 lpc2[5];
156 opus_val16 c1 = QCONST16(.8f,15);
157 #ifdef FIXED_POINT
158 int shift;
159 opus_val32 maxabs = celt_maxabs32(x[0], len);
160 if (C==2)
161 {
162 opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
163 maxabs = MAX32(maxabs, maxabs_1);
164 }
165 if (maxabs<1)
166 maxabs=1;
167 shift = celt_ilog2(maxabs)-10;
168 if (shift<0)
169 shift=0;
170 if (C==2)
171 shift++;
172 #endif
173 for (i=1;i<len>>1;i++)
174 x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
175 x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
176 if (C==2)
177 {
178 for (i=1;i<len>>1;i++)
179 x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
180 x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
181 }
182
183 _celt_autocorr(x_lp, ac, NULL, 0,
184 4, len>>1);
185
186 /* Noise floor -40 dB */
187 #ifdef FIXED_POINT
188 ac[0] += SHR32(ac[0],13);
189 #else
190 ac[0] *= 1.0001f;
191 #endif
192 /* Lag windowing */
193 for (i=1;i<=4;i++)
194 {
195 /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
196 #ifdef FIXED_POINT
197 ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
198 #else
199 ac[i] -= ac[i]*(.008f*i)*(.008f*i);
200 #endif
201 }
202
203 _celt_lpc(lpc, ac, 4);
204 for (i=0;i<4;i++)
205 {
206 tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
207 lpc[i] = MULT16_16_Q15(lpc[i], tmp);
208 }
209 /* Add a zero */
210 lpc2[0] = lpc[0] + QCONST16(.8f,SIG_SHIFT);
211 lpc2[1] = lpc[1] + MULT16_16_Q15(c1,lpc[0]);
212 lpc2[2] = lpc[2] + MULT16_16_Q15(c1,lpc[1]);
213 lpc2[3] = lpc[3] + MULT16_16_Q15(c1,lpc[2]);
214 lpc2[4] = MULT16_16_Q15(c1,lpc[3]);
215 celt_fir5(x_lp, lpc2, x_lp, len>>1, mem);
216 }
217
celt_pitch_xcorr(const opus_val16 * _x,const opus_val16 * _y,opus_val32 * xcorr,int len,int max_pitch)218 void celt_pitch_xcorr(const opus_val16 *_x, const opus_val16 *_y,
219 opus_val32 *xcorr, int len, int max_pitch)
220 {
221
222 #if 0 /* This is a simple version of the pitch correlation that should work
223 well on DSPs like Blackfin and TI C5x/C6x */
224 int i, j;
225 #ifdef FIXED_POINT
226 opus_val32 maxcorr=1;
227 #endif
228 for (i=0;i<max_pitch;i++)
229 {
230 opus_val32 sum = 0;
231 for (j=0;j<len;j++)
232 sum = MAC16_16(sum, _x[j], _y[i+j]);
233 xcorr[i] = sum;
234 #ifdef FIXED_POINT
235 maxcorr = MAX32(maxcorr, sum);
236 #endif
237 }
238 #ifdef FIXED_POINT
239 return maxcorr;
240 #endif
241
242 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */
243 int i;
244 /*The EDSP version requires that max_pitch is at least 1, and that _x is
245 32-bit aligned.
246 Since it's hard to put asserts in assembly, put them here.*/
247 #ifdef FIXED_POINT
248 opus_val32 maxcorr=1;
249 #endif
250 celt_assert(max_pitch>0);
251 celt_assert((((unsigned char *)_x-(unsigned char *)NULL)&3)==0);
252 for (i=0;i<max_pitch-3;i+=4)
253 {
254 opus_val32 sum[4]={0,0,0,0};
255 xcorr_kernel(_x, _y+i, sum, len);
256 xcorr[i]=sum[0];
257 xcorr[i+1]=sum[1];
258 xcorr[i+2]=sum[2];
259 xcorr[i+3]=sum[3];
260 #ifdef FIXED_POINT
261 sum[0] = MAX32(sum[0], sum[1]);
262 sum[2] = MAX32(sum[2], sum[3]);
263 sum[0] = MAX32(sum[0], sum[2]);
264 maxcorr = MAX32(maxcorr, sum[0]);
265 #endif
266 }
267 /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */
268 for (;i<max_pitch;i++)
269 {
270 opus_val32 sum;
271 sum = celt_inner_prod(_x, _y+i, len);
272 xcorr[i] = sum;
273 #ifdef FIXED_POINT
274 maxcorr = MAX32(maxcorr, sum);
275 #endif
276 }
277 #ifdef FIXED_POINT
278 return maxcorr;
279 #endif
280 #endif
281 }
282
pitch_search(const opus_val16 * x_lp,opus_val16 * y,int len,int max_pitch,int * pitch)283 void pitch_search(const opus_val16 *x_lp, opus_val16 *y,
284 int len, int max_pitch, int *pitch)
285 {
286 int i, j;
287 int lag;
288 int best_pitch[2]={0,0};
289 #ifdef FIXED_POINT
290 opus_val32 maxcorr;
291 opus_val32 xmax, ymax;
292 int shift=0;
293 #endif
294 int offset;
295
296 celt_assert(len>0);
297 celt_assert(max_pitch>0);
298 lag = len+max_pitch;
299
300 opus_val16 x_lp4[len>>2];
301 opus_val16 y_lp4[lag>>2];
302 opus_val32 xcorr[max_pitch>>1];
303
304 /* Downsample by 2 again */
305 for (j=0;j<len>>2;j++)
306 x_lp4[j] = x_lp[2*j];
307 for (j=0;j<lag>>2;j++)
308 y_lp4[j] = y[2*j];
309
310 #ifdef FIXED_POINT
311 xmax = celt_maxabs16(x_lp4, len>>2);
312 ymax = celt_maxabs16(y_lp4, lag>>2);
313 shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax)))-11;
314 if (shift>0)
315 {
316 for (j=0;j<len>>2;j++)
317 x_lp4[j] = SHR16(x_lp4[j], shift);
318 for (j=0;j<lag>>2;j++)
319 y_lp4[j] = SHR16(y_lp4[j], shift);
320 /* Use double the shift for a MAC */
321 shift *= 2;
322 } else {
323 shift = 0;
324 }
325 #endif
326
327 /* Coarse search with 4x decimation */
328
329 #ifdef FIXED_POINT
330 maxcorr =
331 #endif
332 celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2);
333
334 find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
335 #ifdef FIXED_POINT
336 , 0, maxcorr
337 #endif
338 );
339
340 /* Finer search with 2x decimation */
341 #ifdef FIXED_POINT
342 maxcorr=1;
343 #endif
344 for (i=0;i<max_pitch>>1;i++)
345 {
346 opus_val32 sum;
347 xcorr[i] = 0;
348 if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
349 continue;
350 #ifdef FIXED_POINT
351 sum = 0;
352 for (j=0;j<len>>1;j++)
353 sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
354 #else
355 sum = celt_inner_prod(x_lp, y+i, len>>1);
356 #endif
357 xcorr[i] = MAX32(-1, sum);
358 #ifdef FIXED_POINT
359 maxcorr = MAX32(maxcorr, sum);
360 #endif
361 }
362 find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
363 #ifdef FIXED_POINT
364 , shift+1, maxcorr
365 #endif
366 );
367
368 /* Refine by pseudo-interpolation */
369 if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
370 {
371 opus_val32 a, b, c;
372 a = xcorr[best_pitch[0]-1];
373 b = xcorr[best_pitch[0]];
374 c = xcorr[best_pitch[0]+1];
375 if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
376 offset = 1;
377 else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
378 offset = -1;
379 else
380 offset = 0;
381 } else {
382 offset = 0;
383 }
384 *pitch = 2*best_pitch[0]-offset;
385 }
386
387 #ifdef FIXED_POINT
compute_pitch_gain(opus_val32 xy,opus_val32 xx,opus_val32 yy)388 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy)
389 {
390 opus_val32 x2y2;
391 int sx, sy, shift;
392 opus_val32 g;
393 opus_val16 den;
394 if (xy == 0 || xx == 0 || yy == 0)
395 return 0;
396 sx = celt_ilog2(xx)-14;
397 sy = celt_ilog2(yy)-14;
398 shift = sx + sy;
399 x2y2 = SHR32(MULT16_16(VSHR32(xx, sx), VSHR32(yy, sy)), 14);
400 if (shift & 1) {
401 if (x2y2 < 32768)
402 {
403 x2y2 <<= 1;
404 shift--;
405 } else {
406 x2y2 >>= 1;
407 shift++;
408 }
409 }
410 den = celt_rsqrt_norm(x2y2);
411 g = MULT16_32_Q15(den, xy);
412 g = VSHR32(g, (shift>>1)-1);
413 return EXTRACT16(MIN32(g, Q15ONE));
414 }
415 #else
compute_pitch_gain(opus_val32 xy,opus_val32 xx,opus_val32 yy)416 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy)
417 {
418 return xy/sqrt(1+xx*yy);
419 }
420 #endif
421
422 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
remove_doubling(opus_val16 * x,int maxperiod,int minperiod,int N,int * T0_,int prev_period,opus_val16 prev_gain)423 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
424 int N, int *T0_, int prev_period, opus_val16 prev_gain)
425 {
426 int k, i, T, T0;
427 opus_val16 g, g0;
428 opus_val16 pg;
429 opus_val32 xy,xx,yy,xy2;
430 opus_val32 xcorr[3];
431 opus_val32 best_xy, best_yy;
432 int offset;
433 int minperiod0;
434
435 minperiod0 = minperiod;
436 maxperiod /= 2;
437 minperiod /= 2;
438 *T0_ /= 2;
439 prev_period /= 2;
440 N /= 2;
441 x += maxperiod;
442 if (*T0_>=maxperiod)
443 *T0_=maxperiod-1;
444
445 T = T0 = *T0_;
446 opus_val32 yy_lookup[maxperiod+1];
447 dual_inner_prod(x, x, x-T0, N, &xx, &xy);
448 yy_lookup[0] = xx;
449 yy=xx;
450 for (i=1;i<=maxperiod;i++)
451 {
452 yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]);
453 yy_lookup[i] = MAX32(0, yy);
454 }
455 yy = yy_lookup[T0];
456 best_xy = xy;
457 best_yy = yy;
458 g = g0 = compute_pitch_gain(xy, xx, yy);
459 /* Look for any pitch at T/k */
460 for (k=2;k<=15;k++)
461 {
462 int T1, T1b;
463 opus_val16 g1;
464 opus_val16 cont=0;
465 opus_val16 thresh;
466 T1 = (2*T0+k)/(2*k);
467 if (T1 < minperiod)
468 break;
469 /* Look for another strong correlation at T1b */
470 if (k==2)
471 {
472 if (T1+T0>maxperiod)
473 T1b = T0;
474 else
475 T1b = T0+T1;
476 } else
477 {
478 T1b = (2*second_check[k]*T0+k)/(2*k);
479 }
480 dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2);
481 xy = HALF32(xy + xy2);
482 yy = HALF32(yy_lookup[T1] + yy_lookup[T1b]);
483 g1 = compute_pitch_gain(xy, xx, yy);
484 if (abs(T1-prev_period)<=1)
485 cont = prev_gain;
486 else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
487 cont = HALF16(prev_gain);
488 else
489 cont = 0;
490 thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont);
491 /* Bias against very high pitch (very short period) to avoid false-positives
492 due to short-term correlation */
493 if (T1<3*minperiod)
494 thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont);
495 else if (T1<2*minperiod)
496 thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont);
497 if (g1 > thresh)
498 {
499 best_xy = xy;
500 best_yy = yy;
501 T = T1;
502 g = g1;
503 }
504 }
505 best_xy = MAX32(0, best_xy);
506 if (best_yy <= best_xy)
507 pg = Q15ONE;
508 else
509 pg = best_xy/(best_yy+1);
510
511 for (k=0;k<3;k++)
512 xcorr[k] = celt_inner_prod(x, x-(T+k-1), N);
513 if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
514 offset = 1;
515 else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
516 offset = -1;
517 else
518 offset = 0;
519 if (pg > g)
520 pg = g;
521 *T0_ = 2*T+offset;
522
523 if (*T0_<minperiod0)
524 *T0_=minperiod0;
525 return pg;
526 }
527