scal.c revision 98913fed6520d8849fb2e246be943e04474aefa4 
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/*
34The 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
57struct 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
81EXPORT 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
119static 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
130static unsigned int irand(int *seed)
131{
132   *seed = 1664525 * *seed + 1013904223;
133   return ((unsigned int)*seed)>>16;
134}
135
136
137EXPORT 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
274EXPORT 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