cleaning up the SCAL code
[speexdsp.git] / libspeex / 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   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 "vorbis_psy.h"
48 #include "arch.h"
49 #include "os_support.h"
50 #include "smallft.h"
51 #include <math.h>
52 #include <stdlib.h>
53
54 #define ALLPASS_ORDER 20
55
56 struct DecorrState_ {
57    int rate;
58    int channels;
59    int frame_size;
60 #ifdef VORBIS_PSYCHO
61    VorbisPsy *psy;
62    struct drft_lookup lookup;
63    float *wola_mem;
64    float *curve;
65 #endif
66    float *buff;
67    float *vorbis_win;
68    int    seed;
69    
70    float ring[ALLPASS_ORDER];
71    int ringID;
72    int order;
73    float alpha;
74 };
75
76 typedef struct DecorrState_ DecorrState;
77
78 DecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
79 {
80    int i;
81    DecorrState *st = speex_alloc(sizeof(DecorrState));
82    st->rate = rate;
83    st->channels = channels;
84    st->frame_size = frame_size;
85 #ifdef VORBIS_PSYCHO
86    st->psy = vorbis_psy_init(rate, 2*frame_size);
87    spx_drft_init(&st->lookup, 2*frame_size);
88    st->wola_mem = speex_alloc(frame_size*sizeof(float));
89    st->curve = speex_alloc(frame_size*sizeof(float));
90 #endif
91    st->buff = speex_alloc(2*frame_size*sizeof(float));
92    /*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
93    st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
94    for (i=0;i<2*frame_size;i++)
95       st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
96    st->seed = rand();
97    
98    for (i=0;i<ALLPASS_ORDER;i++)
99       st->ring[i] = 0;
100    st->ringID = 0;
101    st->alpha = 0;
102    st->order = 10;
103    
104    return st;
105 }
106
107 float uni_rand(int *seed)
108 {
109    const unsigned int jflone = 0x3f800000;
110    const unsigned int jflmsk = 0x007fffff;
111    union {int i; float f;} ran;
112    *seed = 1664525 * *seed + 1013904223;
113    ran.i = jflone | (jflmsk & *seed);
114    ran.f -= 1.5;
115    return 2*ran.f;
116 }
117
118 unsigned int irand(int *seed)
119 {
120    *seed = 1664525 * *seed + 1013904223;
121    return ((unsigned int)*seed)>>16;
122 }
123
124
125 void speex_decorrelate(DecorrState *st, const short *in, short *out, float amount)
126 {
127    int i;
128    int N=2*st->frame_size;
129    int var_order = 1;
130    float beta, beta2;
131    float *x;
132    float y[st->frame_size];
133    float max_alpha = 0;
134    
135    {
136       float *buff;
137       float *ring;
138       int ringID;
139       int order;
140       float alpha;
141
142       buff = st->buff;
143       ring = st->ring;
144       ringID = st->ringID;
145       order = st->order;
146       alpha = st->alpha;
147       
148       for (i=0;i<st->frame_size;i++)
149          buff[i] = buff[i+st->frame_size];
150       for (i=0;i<st->frame_size;i++)
151          buff[i+st->frame_size] = in[i];
152       if (amount < 0)
153       {
154          amount = -amount;
155          var_order = 0;
156       }
157
158       x = buff+st->frame_size;
159       beta = 1.-.3*amount*amount;
160       if (amount>1)
161          beta = 1-sqrt(.4*amount);
162       else
163          beta = 1-0.63246*amount;
164       if (beta<0)
165          beta = 0;
166    
167       beta2 = beta;
168       if (!var_order)
169          beta=0;
170       for (i=0;i<st->frame_size;i++)
171       {
172          y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order] 
173                + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i] 
174                - alpha*(ring[ringID]
175                - beta*ring[ringID+1>=order?0:ringID+1]);
176          ring[ringID++]=y[i];
177          y[i] *= st->vorbis_win[st->frame_size+i];
178          if (ringID>=order)
179             ringID=0;
180       }
181       order = order+(irand(&st->seed)%3)-1;
182       if (order < 5)
183          order = 5;
184       if (order > 10)
185          order = 10;
186       order = 5+(irand(&st->seed)%6);
187       if (!var_order)
188          order = 7;
189       max_alpha = pow(.96+.04*(amount-1),order);
190       if (max_alpha > .98/(1.+beta2))
191          max_alpha = .98/(1.+beta2);
192    
193       alpha = alpha + .5*uni_rand(&st->seed);
194       if (alpha > max_alpha)
195          alpha = max_alpha;
196       if (alpha < -max_alpha)
197          alpha = -max_alpha;
198       for (i=0;i<ALLPASS_ORDER;i++)
199          ring[i] = 0;
200       ringID = 0;
201       for (i=0;i<st->frame_size;i++)
202       {
203          float tmp =  alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order] 
204                + x[i-ALLPASS_ORDER]*st->vorbis_win[i] 
205                - alpha*(ring[ringID]
206                - beta*ring[ringID+1>=order?0:ringID+1]);
207          ring[ringID++]=tmp;
208          tmp *= st->vorbis_win[i];
209          if (ringID>=order)
210             ringID=0;
211          y[i] += tmp;
212       }
213    
214 #ifdef VORBIS_PSYCHO
215       float frame[N];
216       float scale = 1./N;
217       for (i=0;i<2*st->frame_size;i++)
218          frame[i] = buff[i];
219    //float coef = .5*0.78130;
220       float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
221       compute_curve(st->psy, buff, st->curve);
222       for (i=1;i<st->frame_size;i++)
223       {
224          float x1,x2;
225          float gain;
226          do {
227             x1 = uni_rand(&st->seed);
228             x2 = uni_rand(&st->seed);
229          } while (x1*x1+x2*x2 > 1.);
230          gain = coef*sqrt(.1+st->curve[i]);
231          frame[2*i-1] = gain*x1;
232          frame[2*i] = gain*x2;
233       }
234       frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
235       frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
236       spx_drft_backward(&st->lookup,frame);
237       for (i=0;i<2*st->frame_size;i++)
238          frame[i] *= st->vorbis_win[i];
239 #endif
240    
241       for (i=0;i<st->frame_size;i++)
242       {
243 #ifdef VORBIS_PSYCHO
244          float tmp = y[i] + frame[i] + st->wola_mem[i];
245          st->wola_mem[i] = frame[i+st->frame_size];
246 #else
247          float tmp = y[i];
248 #endif
249          if (tmp>32767)
250             tmp = 32767;
251          if (tmp < -32767)
252             tmp = -32767;
253          out[i] = tmp;
254       }
255       
256       st->ringID = ringID;
257       st->order = order;
258       st->alpha = alpha;
259
260    }
261 }
262
263 void speex_decorrelate_destroy(DecorrState *st)
264 {
265 #ifdef VORBIS_PSYCHO
266    vorbis_psy_destroy(st->psy);
267    speex_free(st->wola_mem);
268    speex_free(st->curve);
269 #endif
270    speex_free(st->buff);
271    speex_free(st);
272 }