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
WebRtcIsac_AllPoleFilter(double * InOut,double * Coef,int lengthInOut,int orderCoef)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
WebRtcIsac_AllZeroFilter(double * In,double * Coef,int lengthInOut,int orderCoef,double * Out)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
WebRtcIsac_ZeroPoleFilter(double * In,double * ZeroCoef,double * PoleCoef,int lengthInOut,int orderCoef,double * Out)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
WebRtcIsac_AutoCorr(double * r,const double * x,int N,int order)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
WebRtcIsac_BwExpand(double * out,double * in,double coef,short length)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
WebRtcIsac_WeightingFilter(const double * in,double * weiout,double * whiout,WeightFiltstr * wfdata)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
WebRtcIsac_AllpassFilterForDec(double * InOut,const double * APSectionFactors,int lengthInOut,double * FilterState)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
WebRtcIsac_DecimateAllpass(const double * in,double * state_in,int N,double * out)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 */
WebRtcIsac_Highpass(const double * in,double * out,double * state,int N)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
WebRtcIsac_Highpass_float(const float * in,double * out,double * state,int N)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