Removing original freq-domain pitch code
[opus.git] / libcelt / pitch.c
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    - Neither the name of the Xiph.org Foundation nor the names of its
22    contributors may be used to endorse or promote products derived from
23    this software without specific prior written permission.
24    
25    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
28    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
29    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
30    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
31    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
32    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
33    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
34    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
35    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 */
37
38
39 #ifdef HAVE_CONFIG_H
40 #include "config.h"
41 #endif
42
43 #include "pitch.h"
44 #include "os_support.h"
45 #include "modes.h"
46 #include "stack_alloc.h"
47 #include "mathops.h"
48
49 void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y, int yshift, int len, int max_pitch, int best_pitch[2])
50 {
51    int i, j;
52    celt_word32 Syy=1;
53    celt_word16 best_num[2];
54    celt_word32 best_den[2];
55 #ifdef FIXED_POINT
56    int xshift;
57
58    xshift = celt_ilog2(maxcorr)-14;
59 #endif
60
61    best_num[0] = -1;
62    best_num[1] = -1;
63    best_den[0] = 0;
64    best_den[1] = 0;
65    best_pitch[0] = 0;
66    best_pitch[1] = 1;
67    for (j=0;j<len;j++)
68       Syy = MAC16_16(Syy, y[j],y[j]);
69    for (i=0;i<max_pitch;i++)
70    {
71       float score;
72       if (xcorr[i]>0)
73       {
74          celt_word16 num;
75          celt_word32 xcorr16;
76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
77          num = MULT16_16_Q15(xcorr16,xcorr16);
78          score = num*1./Syy;
79          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
80          {
81             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
82             {
83                best_num[1] = best_num[0];
84                best_den[1] = best_den[0];
85                best_pitch[1] = best_pitch[0];
86                best_num[0] = num;
87                best_den[0] = Syy;
88                best_pitch[0] = i;
89             } else {
90                best_num[1] = num;
91                best_den[1] = Syy;
92                best_pitch[1] = i;
93             }
94          }
95       }
96       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
97       Syy = MAX32(1, Syy);
98    }
99 }
100
101 void find_temporal_pitch(const CELTMode *m, const celt_sig * restrict x, celt_word16 * restrict y, int len, int max_pitch, int *pitch, int _C, celt_sig *xmem)
102 {
103    int i, j;
104    const int C = CHANNELS(_C);
105    const int lag = MAX_PERIOD;
106    const int N = FRAMESIZE(m);
107    int best_pitch[2]={0};
108    celt_word16 x_lp[len>>1];
109    celt_word16 x_lp4[len>>2];
110    celt_word16 y_lp4[lag>>2];
111    celt_word32 xcorr[max_pitch>>1];
112    celt_word32 maxcorr=1;
113    int offset;
114    int shift=0;
115
116    /* Down-sample by two and downmix to mono */
117    for (i=1;i<len>>1;i++)
118       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
119    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
120    *xmem = x[N-C];
121    if (C==2)
122    {
123       for (i=1;i<len>>1;i++)
124       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C+1]+x[(2*i+1)*C+1])+x[2*i*C+1]), SIG_SHIFT);
125       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
126       *xmem += x[N-C+1];
127    }
128
129    /* Downsample by 2 again */
130    for (j=0;j<len>>2;j++)
131       x_lp4[j] = x_lp[2*j];
132    for (j=0;j<lag>>2;j++)
133       y_lp4[j] = y[2*j];
134
135 #ifdef FIXED_POINT
136    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
137    if (shift>0)
138    {
139       for (j=0;j<len>>2;j++)
140          x_lp4[j] = SHR16(x_lp4[j], shift);
141       for (j=0;j<lag>>2;j++)
142          y_lp4[j] = SHR16(y_lp4[j], shift);
143       /* Use double the shift for a MAC */
144       shift *= 2;
145    } else {
146       shift = 0;
147    }
148 #endif
149
150    /* Coarse search with 4x decimation */
151
152    for (i=0;i<max_pitch>>2;i++)
153    {
154       celt_word32 sum = 0;
155       for (j=0;j<len>>2;j++)
156          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
157       xcorr[i] = MAX32(-1, sum);
158       maxcorr = MAX32(maxcorr, sum);
159    }
160    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
161
162    /* Finer search with 2x decimation */
163    maxcorr=1;
164    for (i=0;i<max_pitch>>1;i++)
165    {
166       celt_word32 sum=0;
167       xcorr[i] = 0;
168       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
169          continue;
170       for (j=0;j<len>>1;j++)
171          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
172       xcorr[i] = MAX32(-1, sum);
173       maxcorr = MAX32(maxcorr, sum);
174    }
175    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
176
177    /* Refine by pseudo-interpolation */
178    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
179    {
180       celt_word32 a, b, c;
181       a = xcorr[best_pitch[0]-1];
182       b = xcorr[best_pitch[0]];
183       c = xcorr[best_pitch[0]+1];
184       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
185          offset = 1;
186       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
187          offset = -1;
188       else 
189          offset = 0;
190    } else {
191       offset = 0;
192    }
193    *pitch = 2*best_pitch[0]-offset;
194
195    CELT_COPY(y, y+(N>>1), (lag-N)>>1);
196    CELT_COPY(y+((lag-N)>>1), x_lp, N>>1);
197
198    /*printf ("%d\n", *pitch);*/
199 }