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