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 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 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 134 static unsigned int irand(int *seed) 135 { 136 *seed = 1664525 * *seed + 1013904223; 137 return ((unsigned int)*seed)>>16; 138 } 139 140 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 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
