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 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 #if 0
 118    {
 119       int j;
 120       float ac[3]={0,0,0};
 121       float ak[2];
 122       float det;
 123       celt_word16 mem[2];
 124       for (i=0;i<3;i++)
 125       {
 126          for (j=0;j<(len>>1)-i;j++)
 127          {
 128             ac[i] += x_lp[j]*x_lp[j+i];
 129          }
 130       }
 131       det = 1./(.1+ac[0]*ac[0]-ac[1]*ac[1]);
 132       ak[0] = .9*det*(ac[0]*ac[1] - ac[1]*ac[2]);
 133       ak[1] = .81*det*(-ac[1]*ac[1] + ac[0]*ac[2]);
 134       /*printf ("%f %f %f\n", 1., -ak[0], -ak[1]);*/
 135       mem[0]=filt_mem[0];
 136       mem[1]=filt_mem[1];
 137       filt_mem[0]=x_lp[(end>>1)-1];
 138       filt_mem[1]=x_lp[(end>>1)-2];
 139       for (j=0;j<len>>1;j++)
 140       {
 141          float tmp = x_lp[j];
 142          x_lp[j] = x_lp[j] - ak[0]*mem[0] - ak[1]*mem[1];
 143          mem[1]=mem[0];
 144          mem[0]=tmp;
 145       }
 146    }
 147 #endif
 148
 149 }
 150
 151 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)
 152 {
 153    int i, j;
 154    const int lag = MAX_PERIOD;
 155    const int N = M*m->eBands[m->nbEBands+1];
 156    int best_pitch[2]={0};
 157    VARDECL(celt_word16, x_lp4);
 158    VARDECL(celt_word16, y_lp4);
 159    VARDECL(celt_word32, xcorr);
 160    celt_word32 maxcorr=1;
 161    int offset;
 162    int shift=0;
 163
 164    SAVE_STACK;
 165
 166    ALLOC(x_lp4, len>>2, celt_word16);
 167    ALLOC(y_lp4, lag>>2, celt_word16);
 168    ALLOC(xcorr, max_pitch>>1, celt_word32);
 169
 170    /* Downsample by 2 again */
 171    for (j=0;j<len>>2;j++)
 172       x_lp4[j] = x_lp[2*j];
 173    for (j=0;j<lag>>2;j++)
 174       y_lp4[j] = y[2*j];
 175
 176 #ifdef FIXED_POINT
 177    shift = celt_ilog2(MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2)))-11;
 178    if (shift>0)
 179    {
 180       for (j=0;j<len>>2;j++)
 181          x_lp4[j] = SHR16(x_lp4[j], shift);
 182       for (j=0;j<lag>>2;j++)
 183          y_lp4[j] = SHR16(y_lp4[j], shift);
 184       /* Use double the shift for a MAC */
 185       shift *= 2;
 186    } else {
 187       shift = 0;
 188    }
 189 #endif
 190
 191    /* Coarse search with 4x decimation */
 192
 193    for (i=0;i<max_pitch>>2;i++)
 194    {
 195       celt_word32 sum = 0;
 196       for (j=0;j<len>>2;j++)
 197          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
 198       xcorr[i] = MAX32(-1, sum);
 199       maxcorr = MAX32(maxcorr, sum);
 200    }
 201    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
 202
 203    /* Finer search with 2x decimation */
 204    maxcorr=1;
 205    for (i=0;i<max_pitch>>1;i++)
 206    {
 207       celt_word32 sum=0;
 208       xcorr[i] = 0;
 209       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
 210          continue;
 211       for (j=0;j<len>>1;j++)
 212          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
 213       xcorr[i] = MAX32(-1, sum);
 214       maxcorr = MAX32(maxcorr, sum);
 215    }
 216    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
 217
 218    /* Refine by pseudo-interpolation */
 219    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
 220    {
 221       celt_word32 a, b, c;
 222       a = xcorr[best_pitch[0]-1];
 223       b = xcorr[best_pitch[0]];
 224       c = xcorr[best_pitch[0]+1];
 225       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
 226          offset = 1;
 227       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
 228          offset = -1;
 229       else
 230          offset = 0;
 231    } else {
 232       offset = 0;
 233    }
 234    *pitch = 2*best_pitch[0]-offset;
 235
 236    CELT_MOVE(y, y+(N>>1), (lag-N)>>1);
 237    CELT_MOVE(y+((lag-N)>>1), x_lp, N>>1);
 238
 239    RESTORE_STACK;
 240
 241    /*printf ("%d\n", *pitch);*/
 242 }