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