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 #define ALLPASS_ORDER 20
56
57 struct SpeexDecorrState_ {
58 int rate;
59 int channels;
60 int frame_size;
61 #ifdef VORBIS_PSYCHO
62 VorbisPsy *psy;
63 struct drft_lookup lookup;
64 float *wola_mem;
65 float *curve;
66 #endif
67 float *vorbis_win;
68 int seed;
69 float *y;
70
71 /* Per-channel stuff */
72 float *buff;
73 float (*ring)[ALLPASS_ORDER];
74 int *ringID;
75 int *order;
76 float *alpha;
77 };
78
79
80
speex_decorrelate_new(int rate,int channels,int frame_size)81 EXPORT SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
82 {
83 int i, ch;
84 SpeexDecorrState *st = speex_alloc(sizeof(SpeexDecorrState));
85 st->rate = rate;
86 st->channels = channels;
87 st->frame_size = frame_size;
88 #ifdef VORBIS_PSYCHO
89 st->psy = vorbis_psy_init(rate, 2*frame_size);
90 spx_drft_init(&st->lookup, 2*frame_size);
91 st->wola_mem = speex_alloc(frame_size*sizeof(float));
92 st->curve = speex_alloc(frame_size*sizeof(float));
93 #endif
94 st->y = speex_alloc(frame_size*sizeof(float));
95
96 st->buff = speex_alloc(channels*2*frame_size*sizeof(float));
97 st->ringID = speex_alloc(channels*sizeof(int));
98 st->order = speex_alloc(channels*sizeof(int));
99 st->alpha = speex_alloc(channels*sizeof(float));
100 st->ring = speex_alloc(channels*ALLPASS_ORDER*sizeof(float));
101
102 /*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
103 st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
104 for (i=0;i<2*frame_size;i++)
105 st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
106 st->seed = rand();
107
108 for (ch=0;ch<channels;ch++)
109 {
110 for (i=0;i<ALLPASS_ORDER;i++)
111 st->ring[ch][i] = 0;
112 st->ringID[ch] = 0;
113 st->alpha[ch] = 0;
114 st->order[ch] = 10;
115 }
116 return st;
117 }
118
uni_rand(int * seed)119 static float uni_rand(int *seed)
120 {
121 const unsigned int jflone = 0x3f800000;
122 const unsigned int jflmsk = 0x007fffff;
123 union {int i; float f;} ran;
124 *seed = 1664525 * *seed + 1013904223;
125 ran.i = jflone | (jflmsk & *seed);
126 ran.f -= 1.5;
127 return 2*ran.f;
128 }
129
irand(int * seed)130 static unsigned int irand(int *seed)
131 {
132 *seed = 1664525 * *seed + 1013904223;
133 return ((unsigned int)*seed)>>16;
134 }
135
136
speex_decorrelate(SpeexDecorrState * st,const spx_int16_t * in,spx_int16_t * out,int strength)137 EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
138 {
139 int ch;
140 float amount;
141
142 if (strength<0)
143 strength = 0;
144 if (strength>100)
145 strength = 100;
146
147 amount = .01*strength;
148 for (ch=0;ch<st->channels;ch++)
149 {
150 int i;
151 int N=2*st->frame_size;
152 float beta, beta2;
153 float *x;
154 float max_alpha = 0;
155
156 float *buff;
157 float *ring;
158 int ringID;
159 int order;
160 float alpha;
161
162 buff = st->buff+ch*2*st->frame_size;
163 ring = st->ring[ch];
164 ringID = st->ringID[ch];
165 order = st->order[ch];
166 alpha = st->alpha[ch];
167
168 for (i=0;i<st->frame_size;i++)
169 buff[i] = buff[i+st->frame_size];
170 for (i=0;i<st->frame_size;i++)
171 buff[i+st->frame_size] = in[i*st->channels+ch];
172
173 x = buff+st->frame_size;
174 beta = 1.-.3*amount*amount;
175 if (amount>1)
176 beta = 1-sqrt(.4*amount);
177 else
178 beta = 1-0.63246*amount;
179 if (beta<0)
180 beta = 0;
181
182 beta2 = beta;
183 for (i=0;i<st->frame_size;i++)
184 {
185 st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
186 + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
187 - alpha*(ring[ringID]
188 - beta*ring[ringID+1>=order?0:ringID+1]);
189 ring[ringID++]=st->y[i];
190 st->y[i] *= st->vorbis_win[st->frame_size+i];
191 if (ringID>=order)
192 ringID=0;
193 }
194 order = order+(irand(&st->seed)%3)-1;
195 if (order < 5)
196 order = 5;
197 if (order > 10)
198 order = 10;
199 /*order = 5+(irand(&st->seed)%6);*/
200 max_alpha = pow(.96+.04*(amount-1),order);
201 if (max_alpha > .98/(1.+beta2))
202 max_alpha = .98/(1.+beta2);
203
204 alpha = alpha + .4*uni_rand(&st->seed);
205 if (alpha > max_alpha)
206 alpha = max_alpha;
207 if (alpha < -max_alpha)
208 alpha = -max_alpha;
209 for (i=0;i<ALLPASS_ORDER;i++)
210 ring[i] = 0;
211 ringID = 0;
212 for (i=0;i<st->frame_size;i++)
213 {
214 float tmp = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
215 + x[i-ALLPASS_ORDER]*st->vorbis_win[i]
216 - alpha*(ring[ringID]
217 - beta*ring[ringID+1>=order?0:ringID+1]);
218 ring[ringID++]=tmp;
219 tmp *= st->vorbis_win[i];
220 if (ringID>=order)
221 ringID=0;
222 st->y[i] += tmp;
223 }
224
225 #ifdef VORBIS_PSYCHO
226 float frame[N];
227 float scale = 1./N;
228 for (i=0;i<2*st->frame_size;i++)
229 frame[i] = buff[i];
230 //float coef = .5*0.78130;
231 float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
232 compute_curve(st->psy, buff, st->curve);
233 for (i=1;i<st->frame_size;i++)
234 {
235 float x1,x2;
236 float gain;
237 do {
238 x1 = uni_rand(&st->seed);
239 x2 = uni_rand(&st->seed);
240 } while (x1*x1+x2*x2 > 1.);
241 gain = coef*sqrt(.1+st->curve[i]);
242 frame[2*i-1] = gain*x1;
243 frame[2*i] = gain*x2;
244 }
245 frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
246 frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
247 spx_drft_backward(&st->lookup,frame);
248 for (i=0;i<2*st->frame_size;i++)
249 frame[i] *= st->vorbis_win[i];
250 #endif
251
252 for (i=0;i<st->frame_size;i++)
253 {
254 #ifdef VORBIS_PSYCHO
255 float tmp = st->y[i] + frame[i] + st->wola_mem[i];
256 st->wola_mem[i] = frame[i+st->frame_size];
257 #else
258 float tmp = st->y[i];
259 #endif
260 if (tmp>32767)
261 tmp = 32767;
262 if (tmp < -32767)
263 tmp = -32767;
264 out[i*st->channels+ch] = tmp;
265 }
266
267 st->ringID[ch] = ringID;
268 st->order[ch] = order;
269 st->alpha[ch] = alpha;
270
271 }
272 }
273
speex_decorrelate_destroy(SpeexDecorrState * st)274 EXPORT void speex_decorrelate_destroy(SpeexDecorrState *st)
275 {
276 #ifdef VORBIS_PSYCHO
277 vorbis_psy_destroy(st->psy);
278 speex_free(st->wola_mem);
279 speex_free(st->curve);
280 #endif
281 speex_free(st->buff);
282 speex_free(st->ring);
283 speex_free(st->ringID);
284 speex_free(st->alpha);
285 speex_free(st->vorbis_win);
286 speex_free(st->order);
287 speex_free(st->y);
288 speex_free(st);
289 }
290