Just removing old code that was commented out anyway
[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 static 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 pitch_downsample(const celt_sig * restrict x, celt_word16 * restrict x_lp, int len, int end, int _C, celt_sig * restrict xmem, celt_word16 * restrict filt_mem)
102 {
103    int i;
104    const int C = CHANNELS(_C);
105    for (i=1;i<len>>1;i++)
106       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
107    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
108    *xmem = x[end-C];
109    if (C==2)
110    {
111       for (i=1;i<len>>1;i++)
112       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);
113       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
114       *xmem += x[end-C+1];
115    }
116 }
117
118 void pitch_search(const CELTMode *m, const celt_word16 * restrict x_lp, celt_word16 * restrict y, int len, int max_pitch, int *pitch, celt_sig *xmem, int M)
119 {
120    int i, j;
121    const int lag = MAX_PERIOD;
122    const int N = M*m->shortMdctSize;
123    int best_pitch[2]={0};
124    VARDECL(celt_word16, x_lp4);
125    VARDECL(celt_word16, y_lp4);
126    VARDECL(celt_word32, xcorr);
127    celt_word32 maxcorr=1;
128    int offset;
129    int shift=0;
130
131    SAVE_STACK;
132
133    ALLOC(x_lp4, len>>2, celt_word16);
134    ALLOC(y_lp4, lag>>2, celt_word16);
135    ALLOC(xcorr, max_pitch>>1, celt_word32);
136
137    /* Downsample by 2 again */
138    for (j=0;j<len>>2;j++)
139       x_lp4[j] = x_lp[2*j];
140    for (j=0;j<lag>>2;j++)
141       y_lp4[j] = y[2*j];
142
143 #ifdef FIXED_POINT
144    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
145    if (shift>0)
146    {
147       for (j=0;j<len>>2;j++)
148          x_lp4[j] = SHR16(x_lp4[j], shift);
149       for (j=0;j<lag>>2;j++)
150          y_lp4[j] = SHR16(y_lp4[j], shift);
151       /* Use double the shift for a MAC */
152       shift *= 2;
153    } else {
154       shift = 0;
155    }
156 #endif
157
158    /* Coarse search with 4x decimation */
159
160    for (i=0;i<max_pitch>>2;i++)
161    {
162       celt_word32 sum = 0;
163       for (j=0;j<len>>2;j++)
164          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
165       xcorr[i] = MAX32(-1, sum);
166       maxcorr = MAX32(maxcorr, sum);
167    }
168    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
169
170    /* Finer search with 2x decimation */
171    maxcorr=1;
172    for (i=0;i<max_pitch>>1;i++)
173    {
174       celt_word32 sum=0;
175       xcorr[i] = 0;
176       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
177          continue;
178       for (j=0;j<len>>1;j++)
179          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
180       xcorr[i] = MAX32(-1, sum);
181       maxcorr = MAX32(maxcorr, sum);
182    }
183    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
184
185    /* Refine by pseudo-interpolation */
186    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
187    {
188       celt_word32 a, b, c;
189       a = xcorr[best_pitch[0]-1];
190       b = xcorr[best_pitch[0]];
191       c = xcorr[best_pitch[0]+1];
192       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
193          offset = 1;
194       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
195          offset = -1;
196       else 
197          offset = 0;
198    } else {
199       offset = 0;
200    }
201    *pitch = 2*best_pitch[0]-offset;
202
203    CELT_MOVE(y, y+(N>>1), (lag-N)>>1);
204    CELT_MOVE(y+((lag-N)>>1), x_lp, N>>1);
205
206    RESTORE_STACK;
207
208    /*printf ("%d\n", *pitch);*/
209 }