#include "config.h"
#endif
-#include <stdio.h>
-#include <math.h>
+/*#include "_kiss_fft_guts.h"
+#include "kiss_fftr.h"*/
+#include "kfft_single.h"
+
#include "pitch.h"
#include "psy.h"
+#include "os_support.h"
+#include "mathops.h"
+#include "stack_alloc.h"
+
+kiss_fftr_cfg pitch_state_alloc(int max_lag)
+{
+ return real16_fft_alloc(max_lag);
+}
+
+void pitch_state_free(kiss_fftr_cfg st)
+{
+ real16_fft_free(st);
+}
+
+#ifdef FIXED_POINT
+static void normalise16(celt_word16_t *x, int len, celt_word16_t val)
+{
+ int i;
+ celt_word16_t maxabs;
+ maxabs = celt_maxabs16(x,len);
+ if (maxabs > val)
+ {
+ int shift = 0;
+ while (maxabs > val)
+ {
+ maxabs >>= 1;
+ shift++;
+ }
+ if (shift==0)
+ return;
+ i=0;
+ do{
+ x[i] = SHR16(x[i], shift);
+ } while (++i<len);
+ } else {
+ int shift=0;
+ if (maxabs == 0)
+ return;
+ val >>= 1;
+ while (maxabs < val)
+ {
+ val >>= 1;
+ shift++;
+ }
+ if (shift==0)
+ return;
+ i=0;
+ do{
+ x[i] = SHL16(x[i], shift);
+ } while (++i<len);
+ }
+}
+#else
+#define normalise16(x,len,val)
+#endif
+
+#define INPUT_SHIFT 15
-void find_spectral_pitch(kiss_fftr_cfg fft, struct PsyDecay *decay, celt_sig_t *x, celt_sig_t *y, int lag, int len, int C, int *pitch)
+void find_spectral_pitch(kiss_fftr_cfg fft, const struct PsyDecay *decay, const celt_sig_t * restrict x, const celt_sig_t * restrict y, const celt_word16_t * restrict window, int overlap, int lag, int len, int C, int *pitch)
{
int c, i;
- float max_corr;
- VARDECL(celt_word32_t *xx);
- VARDECL(celt_word32_t *X);
- VARDECL(celt_word32_t *Y);
- VARDECL(celt_mask_t *curve);
+ VARDECL(celt_word16_t, _X);
+ VARDECL(celt_word16_t, _Y);
+ VARDECL(celt_mask_t, curve);
+ celt_word16_t * restrict X, * restrict Y;
+ int n2;
+ int L2;
SAVE_STACK;
- int n2 = lag/2;
- ALLOC(xx, lag*C, celt_word32_t);
- ALLOC(X, lag*C, celt_word32_t);
- ALLOC(curve, n2*C, celt_mask_t);
-
- for (i=0;i<C*lag;i++)
- xx[i] = 0;
+ n2 = lag>>1;
+ L2 = len>>1;
+ ALLOC(_X, lag, celt_word16_t);
+ X = _X;
+ ALLOC(curve, n2, celt_mask_t);
+
+ CELT_MEMSET(X,0,lag);
+ /* Sum all channels of the current frame and copy into X in bit-reverse order */
for (c=0;c<C;c++)
- for (i=0;i<len;i++)
- xx[c*lag+i] = x[C*i+c];
-
- kiss_fftr(fft, xx, X);
-
- compute_masking(decay, X, curve, lag*C);
+ {
+ for (i=0;i<L2;i++)
+ {
+ X[2*BITREV(fft,i)] += SHR32(x[C*(2*i)+c],INPUT_SHIFT);
+ X[2*BITREV(fft,i)+1] += SHR32(x[C*(2*i+1)+c],INPUT_SHIFT);
+ }
+ }
+ /* Applying the window in the bit-reverse domain. It's a bit weird, but it
+ can help save memory */
+ for (i=0;i<overlap>>1;i++)
+ {
+ X[2*BITREV(fft,i)] = MULT16_16_Q15(window[2*i], X[2*BITREV(fft,i)]);
+ X[2*BITREV(fft,i)+1] = MULT16_16_Q15(window[2*i+1], X[2*BITREV(fft,i)+1]);
+ X[2*BITREV(fft,L2-i-1)] = MULT16_16_Q15(window[2*i+1], X[2*BITREV(fft,L2-i-1)]);
+ X[2*BITREV(fft,L2-i-1)+1] = MULT16_16_Q15(window[2*i], X[2*BITREV(fft,L2-i-1)+1]);
+ }
+ normalise16(X, lag, 8192);
+ /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
+ /* Forward real FFT (in-place) */
+ real16_fft_inplace(fft, X, lag);
+
+ compute_masking(decay, X, curve, lag);
/* Deferred allocation to reduce peak stack usage */
- ALLOC(Y, lag*C, celt_word32_t);
+ ALLOC(_Y, lag, celt_word16_t);
+ Y = _Y;
+ CELT_MEMSET(Y,0,lag);
+ /* Sum all channels of the past audio and copy into Y in bit-reverse order */
for (c=0;c<C;c++)
- for (i=0;i<lag;i++)
- xx[c*lag+i] = y[C*i+c];
- kiss_fftr(fft, xx, Y);
-
-
- for (i=1;i<C*n2;i++)
{
- float n, tmp;
- /*n = 1.f/(1e1+sqrt(sqrt((X[2*i-1]*X[2*i-1] + X[2*i ]*X[2*i ])*(Y[2*i-1]*Y[2*i-1] + Y[2*i ]*Y[2*i ]))));*/
- /*n = 1;*/
- n = 1.f/sqrt(1+curve[i]);
- /*printf ("%f ", n);*/
- /*n = 1.f/(1+curve[i]);*/
- tmp = X[2*i];
- X[2*i] = (1.f*X[2*i ]*Y[2*i ] + 1.f*X[2*i+1]*Y[2*i+1])*n;
- X[2*i+1] = (- 1.f*X[2*i+1]*Y[2*i ] + 1.f*tmp*Y[2*i+1])*n;
+ for (i=0;i<n2;i++)
+ {
+ Y[2*BITREV(fft,i)] += SHR32(y[C*(2*i)+c],INPUT_SHIFT);
+ Y[2*BITREV(fft,i)+1] += SHR32(y[C*(2*i+1)+c],INPUT_SHIFT);
+ }
+ }
+ normalise16(Y, lag, 8192);
+ /* Forward real FFT (in-place) */
+ real16_fft_inplace(fft, Y, lag);
+
+ /* Compute cross-spectrum using the inverse masking curve as weighting */
+ for (i=1;i<n2;i++)
+ {
+ celt_word16_t Xr, Xi, n;
+ /* weight = 1/sqrt(curve) */
+ n = celt_rsqrt(EPSILON+curve[i]);
+ /*n = SHR32(32767,(celt_ilog2(EPSILON+curve[i])>>1));*/
+ /* Pre-multiply X by n, so we can keep everything in 16 bits */
+ Xr = EXTRACT16(SHR32(MULT16_16(n, X[2*i ]),3));
+ Xi = EXTRACT16(SHR32(MULT16_16(n, X[2*i+1]),3));
+ /* Cross-spectrum between X and conj(Y) */
+ X[2*i] = ADD16(MULT16_16_Q15(Xr, Y[2*i ]), MULT16_16_Q15(Xi,Y[2*i+1]));
+ X[2*i+1] = SUB16(MULT16_16_Q15(Xr, Y[2*i+1]), MULT16_16_Q15(Xi,Y[2*i ]));
}
/*printf ("\n");*/
X[0] = X[1] = 0;
- kiss_fftri(fft, X, xx);
- /*for (i=0;i<C*lag;i++)
- printf ("%d %d\n", X[i], xx[i]);*/
+ /*for (i=0;i<lag;i++) printf ("%d ", X[i]);printf ("\n");*/
+ normalise16(X, lag, 50);
+ /* Inverse half-complex to real FFT gives us the correlation */
+ real16_ifft(fft, X, Y, lag);
- max_corr=-1e10;
- *pitch = 0;
- for (i=0;i<lag-len;i++)
- {
- /*printf ("%f ", xx[i]);*/
- if (xx[i] > max_corr)
- {
- *pitch = i;
- max_corr = xx[i];
- }
- }
- /*printf ("%f\n", max_corr);*/
- /*printf ("\n");
- printf ("%d %f\n", *pitch, max_corr);
- printf ("%d\n", *pitch);*/
+ /* The peak in the correlation gives us the pitch */
+ *pitch = find_max16(Y, lag-len);
RESTORE_STACK;
}
xiph / opus.git / blobdiff