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 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 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 130 static unsigned int irand(int *seed) 131 { 132 *seed = 1664525 * *seed + 1013904223; 133 return ((unsigned int)*seed)>>16; 134 } 135 136 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 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
