src/lib/doslib/ext/speex/scal.c

   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.14159261
  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       beta = 1.-.3*amount*amount;
 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 }