1 /* Copyright (C) 2006-2008 CSIRO, Jean-Marc Valin, Xiph.Org Foundation
2
3 File: scal.c
4 Shaped comb-allpass filter for channel decorrelation
5
6 Redistribution and use in source and binary forms, with or without
7 modification, are permitted provided that the following conditions are
8 met:
9
10 1. Redistributions of source code must retain the above copyright notice,
11 this list of conditions and the following disclaimer.
12
13 2. Redistributions in binary form must reproduce the above copyright
14 notice, this list of conditions and the following disclaimer in the
15 documentation and/or other materials provided with the distribution.
16
17 3. The name of the author may not be used to endorse or promote products
18 derived from this software without specific prior written permission.
19
20 THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
21 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
22 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
24 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28 STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 /*
34 The algorithm implemented here is described in:
35
36 * J.-M. Valin, Perceptually-Motivated Nonlinear Channel Decorrelation For
37 Stereo Acoustic Echo Cancellation, Accepted for Joint Workshop on
38 Handsfree Speech Communication and Microphone Arrays (HSCMA), 2008.
39 http://people.xiph.org/~jm/papers/valin_hscma2008.pdf
40
41 */
42
43 #ifdef HAVE_CONFIG_H
44 #include "config.h"
45 #endif
46
47 #include "speex/speex_echo.h"
48 #include "vorbis_psy.h"
49 #include "arch.h"
50 #include "os_support.h"
51 #include "smallft.h"
52 #include <math.h>
53 #include <stdlib.h>
54
55 #ifndef M_PI
56 #define M_PI 3.14159265358979323846 /* pi */
57 #endif
58
59 #define ALLPASS_ORDER 20
60
61 struct SpeexDecorrState_ {
62 int rate;
63 int channels;
64 int frame_size;
65 #ifdef VORBIS_PSYCHO
66 VorbisPsy *psy;
67 struct drft_lookup lookup;
68 float *wola_mem;
69 float *curve;
70 #endif
71 float *vorbis_win;
72 int seed;
73 float *y;
74
75 /* Per-channel stuff */
76 float *buff;
77 float (*ring)[ALLPASS_ORDER];
78 int *ringID;
79 int *order;
80 float *alpha;
81 };
82
83
84
speex_decorrelate_new(int rate,int channels,int frame_size)85 EXPORT SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
86 {
87 int i, ch;
88 SpeexDecorrState *st = speex_alloc(sizeof(SpeexDecorrState));
89 st->rate = rate;
90 st->channels = channels;
91 st->frame_size = frame_size;
92 #ifdef VORBIS_PSYCHO
93 st->psy = vorbis_psy_init(rate, 2*frame_size);
94 spx_drft_init(&st->lookup, 2*frame_size);
95 st->wola_mem = speex_alloc(frame_size*sizeof(float));
96 st->curve = speex_alloc(frame_size*sizeof(float));
97 #endif
98 st->y = speex_alloc(frame_size*sizeof(float));
99
100 st->buff = speex_alloc(channels*2*frame_size*sizeof(float));
101 st->ringID = speex_alloc(channels*sizeof(int));
102 st->order = speex_alloc(channels*sizeof(int));
103 st->alpha = speex_alloc(channels*sizeof(float));
104 st->ring = speex_alloc(channels*ALLPASS_ORDER*sizeof(float));
105
106 /*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
107 st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
108 for (i=0;i<2*frame_size;i++)
109 st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
110 st->seed = rand();
111
112 for (ch=0;ch<channels;ch++)
113 {
114 for (i=0;i<ALLPASS_ORDER;i++)
115 st->ring[ch][i] = 0;
116 st->ringID[ch] = 0;
117 st->alpha[ch] = 0;
118 st->order[ch] = 10;
119 }
120 return st;
121 }
122
uni_rand(int * seed)123 static float uni_rand(int *seed)
124 {
125 const unsigned int jflone = 0x3f800000;
126 const unsigned int jflmsk = 0x007fffff;
127 union {int i; float f;} ran;
128 *seed = 1664525 * *seed + 1013904223;
129 ran.i = jflone | (jflmsk & *seed);
130 ran.f -= 1.5;
131 return 2*ran.f;
132 }
133
irand(int * seed)134 static unsigned int irand(int *seed)
135 {
136 *seed = 1664525 * *seed + 1013904223;
137 return ((unsigned int)*seed)>>16;
138 }
139
140
speex_decorrelate(SpeexDecorrState * st,const spx_int16_t * in,spx_int16_t * out,int strength)141 EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
142 {
143 int ch;
144 float amount;
145
146 if (strength<0)
147 strength = 0;
148 if (strength>100)
149 strength = 100;
150
151 amount = .01*strength;
152 for (ch=0;ch<st->channels;ch++)
153 {
154 int i;
155 int N=2*st->frame_size;
156 float beta, beta2;
157 float *x;
158 float max_alpha = 0;
159
160 float *buff;
161 float *ring;
162 int ringID;
163 int order;
164 float alpha;
165
166 buff = st->buff+ch*2*st->frame_size;
167 ring = st->ring[ch];
168 ringID = st->ringID[ch];
169 order = st->order[ch];
170 alpha = st->alpha[ch];
171
172 for (i=0;i<st->frame_size;i++)
173 buff[i] = buff[i+st->frame_size];
174 for (i=0;i<st->frame_size;i++)
175 buff[i+st->frame_size] = in[i*st->channels+ch];
176
177 x = buff+st->frame_size;
178
179 if (amount>1)
180 beta = 1-sqrt(.4*amount);
181 else
182 beta = 1-0.63246*amount;
183 if (beta<0)
184 beta = 0;
185
186 beta2 = beta;
187 for (i=0;i<st->frame_size;i++)
188 {
189 st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
190 + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
191 - alpha*(ring[ringID]
192 - beta*ring[ringID+1>=order?0:ringID+1]);
193 ring[ringID++]=st->y[i];
194 st->y[i] *= st->vorbis_win[st->frame_size+i];
195 if (ringID>=order)
196 ringID=0;
197 }
198 order = order+(irand(&st->seed)%3)-1;
199 if (order < 5)
200 order = 5;
201 if (order > 10)
202 order = 10;
203 /*order = 5+(irand(&st->seed)%6);*/
204 max_alpha = pow(.96+.04*(amount-1),order);
205 if (max_alpha > .98/(1.+beta2))
206 max_alpha = .98/(1.+beta2);
207
208 alpha = alpha + .4*uni_rand(&st->seed);
209 if (alpha > max_alpha)
210 alpha = max_alpha;
211 if (alpha < -max_alpha)
212 alpha = -max_alpha;
213 for (i=0;i<ALLPASS_ORDER;i++)
214 ring[i] = 0;
215 ringID = 0;
216 for (i=0;i<st->frame_size;i++)
217 {
218 float tmp = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
219 + x[i-ALLPASS_ORDER]*st->vorbis_win[i]
220 - alpha*(ring[ringID]
221 - beta*ring[ringID+1>=order?0:ringID+1]);
222 ring[ringID++]=tmp;
223 tmp *= st->vorbis_win[i];
224 if (ringID>=order)
225 ringID=0;
226 st->y[i] += tmp;
227 }
228
229 #ifdef VORBIS_PSYCHO
230 float frame[N];
231 float scale = 1./N;
232 for (i=0;i<2*st->frame_size;i++)
233 frame[i] = buff[i];
234 //float coef = .5*0.78130;
235 float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
236 compute_curve(st->psy, buff, st->curve);
237 for (i=1;i<st->frame_size;i++)
238 {
239 float x1,x2;
240 float gain;
241 do {
242 x1 = uni_rand(&st->seed);
243 x2 = uni_rand(&st->seed);
244 } while (x1*x1+x2*x2 > 1.);
245 gain = coef*sqrt(.1+st->curve[i]);
246 frame[2*i-1] = gain*x1;
247 frame[2*i] = gain*x2;
248 }
249 frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
250 frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
251 spx_drft_backward(&st->lookup,frame);
252 for (i=0;i<2*st->frame_size;i++)
253 frame[i] *= st->vorbis_win[i];
254 #endif
255
256 for (i=0;i<st->frame_size;i++)
257 {
258 #ifdef VORBIS_PSYCHO
259 float tmp = st->y[i] + frame[i] + st->wola_mem[i];
260 st->wola_mem[i] = frame[i+st->frame_size];
261 #else
262 float tmp = st->y[i];
263 #endif
264 if (tmp>32767)
265 tmp = 32767;
266 if (tmp < -32767)
267 tmp = -32767;
268 out[i*st->channels+ch] = tmp;
269 }
270
271 st->ringID[ch] = ringID;
272 st->order[ch] = order;
273 st->alpha[ch] = alpha;
274
275 }
276 }
277
speex_decorrelate_destroy(SpeexDecorrState * st)278 EXPORT void speex_decorrelate_destroy(SpeexDecorrState *st)
279 {
280 #ifdef VORBIS_PSYCHO
281 vorbis_psy_destroy(st->psy);
282 speex_free(st->wola_mem);
283 speex_free(st->curve);
284 #endif
285 speex_free(st->buff);
286 speex_free(st->ring);
287 speex_free(st->ringID);
288 speex_free(st->alpha);
289 speex_free(st->vorbis_win);
290 speex_free(st->order);
291 speex_free(st->y);
292 speex_free(st);
293 }
294