Removed the _t from all the celt*_t types to avoid clashing with POSIX
[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 "_kiss_fft_guts.h"
44 #include "kiss_fftr.h"*/
45 #include "kfft_single.h"
46
47 #include "pitch.h"
48 #include "psy.h"
49 #include "os_support.h"
50 #include "mathops.h"
51 #include "modes.h"
52 #include "stack_alloc.h"
53
54 kiss_fftr_cfg pitch_state_alloc(int max_lag)
55 {
56    return real16_fft_alloc(max_lag);
57 }
58
59 void pitch_state_free(kiss_fftr_cfg st)
60 {
61    real16_fft_free(st);
62 }
63
64 #ifdef FIXED_POINT
65 static void normalise16(celt_word16 *x, int len, celt_word16 val)
66 {
67    int i;
68    celt_word16 maxabs;
69    maxabs = celt_maxabs16(x,len);
70    if (maxabs > val)
71    {
72       int shift = 0;
73       while (maxabs > val)
74       {
75          maxabs >>= 1;
76          shift++;
77       }
78       if (shift==0)
79          return;
80       i=0;
81       do{
82          x[i] = SHR16(x[i], shift);
83       } while (++i<len);
84    } else {
85       int shift=0;
86       if (maxabs == 0)
87          return;
88       val >>= 1;
89       while (maxabs < val)
90       {
91          val >>= 1;
92          shift++;
93       }
94       if (shift==0)
95          return;
96       i=0;
97       do{
98          x[i] = SHL16(x[i], shift);
99       } while (++i<len);
100    }
101 }
102 #else
103 #define normalise16(x,len,val)
104 #endif
105
106 #define INPUT_SHIFT 15
107
108 void find_spectral_pitch(const CELTMode *m, kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig * restrict x, const celt_sig * restrict y, const celt_word16 * restrict window, celt_word16 * restrict spectrum, int len, int max_pitch, int *pitch, int _C)
109 {
110    int c, i;
111    VARDECL(celt_word16, _X);
112    VARDECL(celt_word16, _Y);
113    const celt_word16 * restrict wptr;
114 #ifndef SHORTCUTS
115    VARDECL(celt_mask, curve);
116 #endif
117    celt_word16 * restrict X, * restrict Y;
118    celt_word16 * restrict Xptr, * restrict Yptr;
119    const celt_sig * restrict yptr;
120    int n2;
121    int L2;
122    const int C = CHANNELS(_C);
123    const int overlap = OVERLAP(m);
124    const int lag = MAX_PERIOD;
125    SAVE_STACK;
126    n2 = lag>>1;
127    L2 = len>>1;
128    ALLOC(_X, lag, celt_word16);
129    X = _X;
130 #ifndef SHORTCUTS
131    ALLOC(curve, n2, celt_mask);
132 #endif
133    CELT_MEMSET(X,0,lag);
134    /* Sum all channels of the current frame and copy into X in bit-reverse order */
135    for (c=0;c<C;c++)
136    {
137       const celt_sig * restrict xptr = &x[c];
138       for (i=0;i<L2;i++)
139       {
140          X[2*BITREV(fft,i)] += SHR32(*xptr,INPUT_SHIFT);
141          xptr += C;
142          X[2*BITREV(fft,i)+1] += SHR32(*xptr,INPUT_SHIFT);
143          xptr += C;
144       }
145    }
146    /* Applying the window in the bit-reverse domain. It's a bit weird, but it
147       can help save memory */
148    wptr = window;
149    for (i=0;i<overlap>>1;i++)
150    {
151       X[2*BITREV(fft,i)]        = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,i)]);
152       X[2*BITREV(fft,i)+1]      = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,i)+1]);
153       X[2*BITREV(fft,L2-i-1)]   = MULT16_16_Q15(wptr[1], X[2*BITREV(fft,L2-i-1)]);
154       X[2*BITREV(fft,L2-i-1)+1] = MULT16_16_Q15(wptr[0], X[2*BITREV(fft,L2-i-1)+1]);
155       wptr += 2;
156    }
157    normalise16(X, lag, 8192);
158    /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
159    /* Forward real FFT (in-place) */
160    real16_fft_inplace(fft, X, lag);
161
162    if (spectrum)
163    {
164       for (i=0;i<lag/4;i++)
165       {
166          spectrum[2*i] = X[4*i];
167          spectrum[2*i+1] = X[4*i+1];
168       }
169    }
170 #ifndef SHORTCUTS
171    compute_masking(decay, X, curve, lag);
172 #endif
173    
174    /* Deferred allocation to reduce peak stack usage */
175    ALLOC(_Y, lag, celt_word16);
176    Y = _Y;
177    yptr = &y[0];
178    /* Copy first channel of the past audio into Y in bit-reverse order */
179    for (i=0;i<n2;i++)
180    {
181       Y[2*BITREV(fft,i)] = SHR32(*yptr,INPUT_SHIFT);
182       yptr += C;
183       Y[2*BITREV(fft,i)+1] = SHR32(*yptr,INPUT_SHIFT);
184       yptr += C;
185    }
186    /* Add remaining channels into Y in bit-reverse order */
187    for (c=1;c<C;c++)
188    {
189       yptr = &y[c];
190       for (i=0;i<n2;i++)
191       {
192          Y[2*BITREV(fft,i)] += SHR32(*yptr,INPUT_SHIFT);
193          yptr += C;
194          Y[2*BITREV(fft,i)+1] += SHR32(*yptr,INPUT_SHIFT);
195          yptr += C;
196       }
197    }
198    normalise16(Y, lag, 8192);
199    /* Forward real FFT (in-place) */
200    real16_fft_inplace(fft, Y, lag);
201
202    /* Compute cross-spectrum using the inverse masking curve as weighting */
203    Xptr = &X[2];
204    Yptr = &Y[2];
205    for (i=1;i<n2;i++)
206    {
207       celt_word16 Xr, Xi, n;
208       /* weight = 1/sqrt(curve) */
209       Xr = Xptr[0];
210       Xi = Xptr[1];
211 #ifdef SHORTCUTS
212       /*n = SHR32(32767,(celt_ilog2(EPSILON+curve[i])>>1));*/
213       n = 1+(8192>>(celt_ilog2(1+MULT16_16(Xr,Xr)+MULT16_16(Xi,Xi))>>1));
214       /* Pre-multiply X by n, so we can keep everything in 16 bits */
215       Xr = MULT16_16_16(n, Xr);
216       Xi = MULT16_16_16(n, Xi);
217 #else
218       n = celt_rsqrt(EPSILON+curve[i]);
219       /* Pre-multiply X by n, so we can keep everything in 16 bits */
220       Xr = EXTRACT16(SHR32(MULT16_16(n, Xr),3));
221       Xi = EXTRACT16(SHR32(MULT16_16(n, Xi),3));
222 #endif
223       /* Cross-spectrum between X and conj(Y) */
224       *Xptr++ = ADD16(MULT16_16_Q15(Xr, Yptr[0]), MULT16_16_Q15(Xi,Yptr[1]));
225       *Xptr++ = SUB16(MULT16_16_Q15(Xr, Yptr[1]), MULT16_16_Q15(Xi,Yptr[0]));
226       Yptr += 2;
227    }
228    /*printf ("\n");*/
229    X[0] = X[1] = 0;
230    /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
231    normalise16(X, lag, 50);
232    /* Inverse half-complex to real FFT gives us the correlation */
233    real16_ifft(fft, X, Y, lag);
234    
235    /* The peak in the correlation gives us the pitch */
236    *pitch = find_max16(Y, max_pitch);
237    RESTORE_STACK;
238 }