Changing some constants to float
[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       if (xcorr[i]>0)
72       {
73          celt_word16 num;
74          celt_word32 xcorr16;
75          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
76          num = MULT16_16_Q15(xcorr16,xcorr16);
77          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
78          {
79             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
80             {
81                best_num[1] = best_num[0];
82                best_den[1] = best_den[0];
83                best_pitch[1] = best_pitch[0];
84                best_num[0] = num;
85                best_den[0] = Syy;
86                best_pitch[0] = i;
87             } else {
88                best_num[1] = num;
89                best_den[1] = Syy;
90                best_pitch[1] = i;
91             }
92          }
93       }
94       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
95       Syy = MAX32(1, Syy);
96    }
97 }
98
99 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)
100 {
101    int i;
102    const int C = CHANNELS(_C);
103    for (i=1;i<len>>1;i++)
104       x_lp[i] = SHR32(HALF32(HALF32(x[(2*i-1)*C]+x[(2*i+1)*C])+x[2*i*C]), SIG_SHIFT);
105    x_lp[0] = SHR32(HALF32(HALF32(*xmem+x[C])+x[0]), SIG_SHIFT);
106    *xmem = x[end-C];
107    if (C==2)
108    {
109       for (i=1;i<len>>1;i++)
110       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);
111       x_lp[0] += SHR32(HALF32(HALF32(x[C+1])+x[1]), SIG_SHIFT);
112       *xmem += x[end-C+1];
113    }
114 }
115
116 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)
117 {
118    int i, j;
119    const int lag = MAX_PERIOD;
120    const int N = M*m->shortMdctSize;
121    int best_pitch[2]={0};
122    VARDECL(celt_word16, x_lp4);
123    VARDECL(celt_word16, y_lp4);
124    VARDECL(celt_word32, xcorr);
125    celt_word32 maxcorr=1;
126    int offset;
127    int shift=0;
128
129    SAVE_STACK;
130
131    ALLOC(x_lp4, len>>2, celt_word16);
132    ALLOC(y_lp4, lag>>2, celt_word16);
133    ALLOC(xcorr, max_pitch>>1, celt_word32);
134
135    /* Downsample by 2 again */
136    for (j=0;j<len>>2;j++)
137       x_lp4[j] = x_lp[2*j];
138    for (j=0;j<lag>>2;j++)
139       y_lp4[j] = y[2*j];
140
141 #ifdef FIXED_POINT
142    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
143    if (shift>0)
144    {
145       for (j=0;j<len>>2;j++)
146          x_lp4[j] = SHR16(x_lp4[j], shift);
147       for (j=0;j<lag>>2;j++)
148          y_lp4[j] = SHR16(y_lp4[j], shift);
149       /* Use double the shift for a MAC */
150       shift *= 2;
151    } else {
152       shift = 0;
153    }
154 #endif
155
156    /* Coarse search with 4x decimation */
157
158    for (i=0;i<max_pitch>>2;i++)
159    {
160       celt_word32 sum = 0;
161       for (j=0;j<len>>2;j++)
162          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
163       xcorr[i] = MAX32(-1, sum);
164       maxcorr = MAX32(maxcorr, sum);
165    }
166    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
167
168    /* Finer search with 2x decimation */
169    maxcorr=1;
170    for (i=0;i<max_pitch>>1;i++)
171    {
172       celt_word32 sum=0;
173       xcorr[i] = 0;
174       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
175          continue;
176       for (j=0;j<len>>1;j++)
177          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
178       xcorr[i] = MAX32(-1, sum);
179       maxcorr = MAX32(maxcorr, sum);
180    }
181    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
182
183    /* Refine by pseudo-interpolation */
184    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
185    {
186       celt_word32 a, b, c;
187       a = xcorr[best_pitch[0]-1];
188       b = xcorr[best_pitch[0]];
189       c = xcorr[best_pitch[0]+1];
190       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
191          offset = 1;
192       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
193          offset = -1;
194       else 
195          offset = 0;
196    } else {
197       offset = 0;
198    }
199    *pitch = 2*best_pitch[0]-offset;
200
201    CELT_MOVE(y, y+(N>>1), (lag-N)>>1);
202    CELT_MOVE(y+((lag-N)>>1), x_lp, N>>1);
203
204    RESTORE_STACK;
205
206    /*printf ("%d\n", *pitch);*/
207 }