Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 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 | HandsĀfree 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 | } |