src/third_party/webrtc/modules/audio_coding/codecs/isac/main/source/filter_functions.c

   1 /*
   2  *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
   3  *
   4  *  Use of this source code is governed by a BSD-style license
   5  *  that can be found in the LICENSE file in the root of the source
   6  *  tree. An additional intellectual property rights grant can be found
   7  *  in the file PATENTS.  All contributing project authors may
   8  *  be found in the AUTHORS file in the root of the source tree.
   9  */
  10
  11 #include <memory.h>
  12 #include <string.h>
  13 #ifdef WEBRTC_ANDROID
  14 #include <stdlib.h>
  15 #endif
  16 #include "pitch_estimator.h"
  17 #include "lpc_analysis.h"
  18 #include "codec.h"
  19
  20
  21
  22 void WebRtcIsac_AllPoleFilter(double *InOut, double *Coef, int lengthInOut, int orderCoef){
  23
  24   /* the state of filter is assumed to be in InOut[-1] to InOut[-orderCoef] */
  25   double scal;
  26   double sum;
  27   int n,k;
  28
  29   //if (fabs(Coef[0]-1.0)<0.001) {
  30   if ( (Coef[0] > 0.9999) && (Coef[0] < 1.0001) )
  31   {
  32     for(n = 0; n < lengthInOut; n++)
  33     {
  34       sum = Coef[1] * InOut[-1];
  35       for(k = 2; k <= orderCoef; k++){
  36         sum += Coef[k] * InOut[-k];
  37       }
  38       *InOut++ -= sum;
  39     }
  40   }
  41   else
  42   {
  43     scal = 1.0 / Coef[0];
  44     for(n=0;n<lengthInOut;n++)
  45     {
  46       *InOut *= scal;
  47       for(k=1;k<=orderCoef;k++){
  48         *InOut -= scal*Coef[k]*InOut[-k];
  49       }
  50       InOut++;
  51     }
  52   }
  53 }
  54
  55
  56 void WebRtcIsac_AllZeroFilter(double *In, double *Coef, int lengthInOut, int orderCoef, double *Out){
  57
  58   /* the state of filter is assumed to be in In[-1] to In[-orderCoef] */
  59
  60   int n, k;
  61   double tmp;
  62
  63   for(n = 0; n < lengthInOut; n++)
  64   {
  65     tmp = In[0] * Coef[0];
  66
  67     for(k = 1; k <= orderCoef; k++){
  68       tmp += Coef[k] * In[-k];
  69     }
  70
  71     *Out++ = tmp;
  72     In++;
  73   }
  74 }
  75
  76
  77
  78 void WebRtcIsac_ZeroPoleFilter(double *In, double *ZeroCoef, double *PoleCoef, int lengthInOut, int orderCoef, double *Out){
  79
  80   /* the state of the zero section is assumed to be in In[-1] to In[-orderCoef] */
  81   /* the state of the pole section is assumed to be in Out[-1] to Out[-orderCoef] */
  82
  83   WebRtcIsac_AllZeroFilter(In,ZeroCoef,lengthInOut,orderCoef,Out);
  84   WebRtcIsac_AllPoleFilter(Out,PoleCoef,lengthInOut,orderCoef);
  85 }
  86
  87
  88 void WebRtcIsac_AutoCorr(
  89     double *r,
  90     const double *x,
  91     int N,
  92     int order
  93                         )
  94 {
  95   int  lag, n;
  96   double sum, prod;
  97   const double *x_lag;
  98
  99   for (lag = 0; lag <= order; lag++)
 100   {
 101     sum = 0.0f;
 102     x_lag = &x[lag];
 103     prod = x[0] * x_lag[0];
 104     for (n = 1; n < N - lag; n++) {
 105       sum += prod;
 106       prod = x[n] * x_lag[n];
 107     }
 108     sum += prod;
 109     r[lag] = sum;
 110   }
 111
 112 }
 113
 114
 115 void WebRtcIsac_BwExpand(double *out, double *in, double coef, short length) {
 116   int i;
 117   double  chirp;
 118
 119   chirp = coef;
 120
 121   out[0] = in[0];
 122   for (i = 1; i < length; i++) {
 123     out[i] = chirp * in[i];
 124     chirp *= coef;
 125   }
 126 }
 127
 128 void WebRtcIsac_WeightingFilter(const double *in, double *weiout, double *whiout, WeightFiltstr *wfdata) {
 129
 130   double  tmpbuffer[PITCH_FRAME_LEN + PITCH_WLPCBUFLEN];
 131   double  corr[PITCH_WLPCORDER+1], rc[PITCH_WLPCORDER+1];
 132   double apol[PITCH_WLPCORDER+1], apolr[PITCH_WLPCORDER+1];
 133   double  rho=0.9, *inp, *dp, *dp2;
 134   double  whoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
 135   double  weoutbuf[PITCH_WLPCBUFLEN + PITCH_WLPCORDER];
 136   double  *weo, *who, opol[PITCH_WLPCORDER+1], ext[PITCH_WLPCWINLEN];
 137   int     k, n, endpos, start;
 138
 139   /* Set up buffer and states */
 140   memcpy(tmpbuffer, wfdata->buffer, sizeof(double) * PITCH_WLPCBUFLEN);
 141   memcpy(tmpbuffer+PITCH_WLPCBUFLEN, in, sizeof(double) * PITCH_FRAME_LEN);
 142   memcpy(wfdata->buffer, tmpbuffer+PITCH_FRAME_LEN, sizeof(double) * PITCH_WLPCBUFLEN);
 143
 144   dp=weoutbuf;
 145   dp2=whoutbuf;
 146   for (k=0;k<PITCH_WLPCORDER;k++) {
 147     *dp++ = wfdata->weostate[k];
 148     *dp2++ = wfdata->whostate[k];
 149     opol[k]=0.0;
 150   }
 151   opol[0]=1.0;
 152   opol[PITCH_WLPCORDER]=0.0;
 153   weo=dp;
 154   who=dp2;
 155
 156   endpos=PITCH_WLPCBUFLEN + PITCH_SUBFRAME_LEN;
 157   inp=tmpbuffer + PITCH_WLPCBUFLEN;
 158
 159   for (n=0; n<PITCH_SUBFRAMES; n++) {
 160     /* Windowing */
 161     start=endpos-PITCH_WLPCWINLEN;
 162     for (k=0; k<PITCH_WLPCWINLEN; k++) {
 163       ext[k]=wfdata->window[k]*tmpbuffer[start+k];
 164     }
 165
 166     /* Get LPC polynomial */
 167     WebRtcIsac_AutoCorr(corr, ext, PITCH_WLPCWINLEN, PITCH_WLPCORDER);
 168     corr[0]=1.01*corr[0]+1.0; /* White noise correction */
 169     WebRtcIsac_LevDurb(apol, rc, corr, PITCH_WLPCORDER);
 170     WebRtcIsac_BwExpand(apolr, apol, rho, PITCH_WLPCORDER+1);
 171
 172     /* Filtering */
 173     WebRtcIsac_ZeroPoleFilter(inp, apol, apolr, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, weo);
 174     WebRtcIsac_ZeroPoleFilter(inp, apolr, opol, PITCH_SUBFRAME_LEN, PITCH_WLPCORDER, who);
 175
 176     inp+=PITCH_SUBFRAME_LEN;
 177     endpos+=PITCH_SUBFRAME_LEN;
 178     weo+=PITCH_SUBFRAME_LEN;
 179     who+=PITCH_SUBFRAME_LEN;
 180   }
 181
 182   /* Export filter states */
 183   for (k=0;k<PITCH_WLPCORDER;k++) {
 184     wfdata->weostate[k]=weoutbuf[PITCH_FRAME_LEN+k];
 185     wfdata->whostate[k]=whoutbuf[PITCH_FRAME_LEN+k];
 186   }
 187
 188   /* Export output data */
 189   memcpy(weiout, weoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
 190   memcpy(whiout, whoutbuf+PITCH_WLPCORDER, sizeof(double) * PITCH_FRAME_LEN);
 191 }
 192
 193
 194 static const double APupper[ALLPASSSECTIONS] = {0.0347, 0.3826};
 195 static const double APlower[ALLPASSSECTIONS] = {0.1544, 0.744};
 196
 197
 198
 199 void WebRtcIsac_AllpassFilterForDec(double *InOut,
 200                                    const double *APSectionFactors,
 201                                    int lengthInOut,
 202                                    double *FilterState)
 203 {
 204   //This performs all-pass filtering--a series of first order all-pass sections are used
 205   //to filter the input in a cascade manner.
 206   int n,j;
 207   double temp;
 208   for (j=0; j<ALLPASSSECTIONS; j++){
 209     for (n=0;n<lengthInOut;n+=2){
 210       temp = InOut[n]; //store input
 211       InOut[n] = FilterState[j] + APSectionFactors[j]*temp;
 212       FilterState[j] = -APSectionFactors[j]*InOut[n] + temp;
 213     }
 214   }
 215 }
 216
 217 void WebRtcIsac_DecimateAllpass(const double *in,
 218                                 double *state_in,        /* array of size: 2*ALLPASSSECTIONS+1 */
 219                                 int N,                   /* number of input samples */
 220                                 double *out)             /* array of size N/2 */
 221 {
 222   int n;
 223   double data_vec[PITCH_FRAME_LEN];
 224
 225   /* copy input */
 226   memcpy(data_vec+1, in, sizeof(double) * (N-1));
 227
 228   data_vec[0] = state_in[2*ALLPASSSECTIONS];   //the z^(-1) state
 229   state_in[2*ALLPASSSECTIONS] = in[N-1];
 230
 231   WebRtcIsac_AllpassFilterForDec(data_vec+1, APupper, N, state_in);
 232   WebRtcIsac_AllpassFilterForDec(data_vec, APlower, N, state_in+ALLPASSSECTIONS);
 233
 234   for (n=0;n<N/2;n++)
 235     out[n] = data_vec[2*n] + data_vec[2*n+1];
 236
 237 }
 238
 239
 240
 241 /* create high-pass filter ocefficients
 242  * z = 0.998 * exp(j*2*pi*35/8000);
 243  * p = 0.94 * exp(j*2*pi*140/8000);
 244  * HP_b = [1, -2*real(z), abs(z)^2];
 245  * HP_a = [1, -2*real(p), abs(p)^2]; */
 246 static const double a_coef[2] = { 1.86864659625574, -0.88360000000000};
 247 static const double b_coef[2] = {-1.99524591718270,  0.99600400000000};
 248 static const float a_coef_float[2] = { 1.86864659625574f, -0.88360000000000f};
 249 static const float b_coef_float[2] = {-1.99524591718270f,  0.99600400000000f};
 250
 251 /* second order high-pass filter */
 252 void WebRtcIsac_Highpass(const double *in, double *out, double *state, int N)
 253 {
 254   int k;
 255
 256   for (k=0; k<N; k++) {
 257     *out = *in + state[1];
 258     state[1] = state[0] + b_coef[0] * *in + a_coef[0] * *out;
 259     state[0] = b_coef[1] * *in++ + a_coef[1] * *out++;
 260   }
 261 }
 262
 263 void WebRtcIsac_Highpass_float(const float *in, double *out, double *state, int N)
 264 {
 265   int k;
 266
 267   for (k=0; k<N; k++) {
 268     *out = (double)*in + state[1];
 269     state[1] = state[0] + b_coef_float[0] * *in + a_coef_float[0] * *out;
 270     state[0] = b_coef_float[1] * (double)*in++ + a_coef_float[1] * *out++;
 271   }
 272 }