5fe03a5b0694804378e7c72d1bcaa022e2e4c170
[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    for (i=0;i<st->frame_size;i++)
136       st->buff[i] = st->buff[i+st->frame_size];
137    for (i=0;i<st->frame_size;i++)
138       st->buff[i+st->frame_size] = in[i];
139    if (amount < 0)
140    {
141       amount = -amount;
142       var_order = 0;
143    }
144
145    x = st->buff+st->frame_size;
146    beta = 1.-.3*amount*amount;
147    if (amount>1)
148       beta = 1-sqrt(.4*amount);
149    else
150       beta = 1-0.63246*amount;
151    if (beta<0)
152       beta = 0;
153    
154    beta2 = beta;
155    if (!var_order)
156       beta=0;
157    for (i=0;i<st->frame_size;i++)
158    {
159       y[i] = st->vorbis_win[st->frame_size+i] * 
160             (st->alpha*(x[i-ALLPASS_ORDER+st->order]-beta*x[i-ALLPASS_ORDER+st->order-1])*st->vorbis_win[st->frame_size+i+st->order] + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i] - st->alpha*(st->ring[st->ringID]-beta*st->ring[st->ringID+1>=st->order?0:st->ringID+1]));
161       st->ring[st->ringID++]=y[i];
162       if (st->ringID>=st->order)
163          st->ringID=0;
164    }
165    st->order = st->order+(irand(&st->seed)%3)-1;
166    if (st->order < 5)
167       st->order = 5;
168    if (st->order > 10)
169       st->order = 10;
170    st->order = 5+(irand(&st->seed)%6);
171    if (!var_order)
172       st->order = 7;
173    max_alpha = pow(.96+.04*(amount-1),st->order);
174    if (max_alpha > .98/(1.+beta2))
175       max_alpha = .98/(1.+beta2);
176    
177    st->alpha = st->alpha + .5*uni_rand(&st->seed);
178    if (st->alpha > max_alpha)
179       st->alpha = max_alpha;
180    if (st->alpha < -max_alpha)
181       st->alpha = -max_alpha;
182    for (i=0;i<ALLPASS_ORDER;i++)
183       st->ring[i] = 0;
184    st->ringID = 0;
185    for (i=0;i<st->frame_size;i++)
186    {
187       float tmp = st->vorbis_win[i] * 
188             (st->alpha*(x[i-ALLPASS_ORDER+st->order]-beta*x[i-ALLPASS_ORDER+st->order-1])*st->vorbis_win[i+st->order] + x[i-ALLPASS_ORDER]*st->vorbis_win[i] - st->alpha*(st->ring[st->ringID]-beta*st->ring[st->ringID+1>=st->order?0:st->ringID+1]));
189       st->ring[st->ringID++]=tmp;
190       if (st->ringID>=st->order)
191          st->ringID=0;
192       y[i] += tmp;
193    }
194    
195 #ifdef VORBIS_PSYCHO
196    float frame[N];
197    float scale = 1./N;
198    for (i=0;i<2*st->frame_size;i++)
199       frame[i] = st->buff[i];
200    //float coef = .5*0.78130;
201    float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
202    compute_curve(st->psy, st->buff, st->curve);
203    for (i=1;i<st->frame_size;i++)
204    {
205       float x1,x2;
206       float gain;
207       do {
208          x1 = uni_rand(&st->seed);
209          x2 = uni_rand(&st->seed);
210       } while (x1*x1+x2*x2 > 1.);
211       gain = coef*sqrt(.1+st->curve[i]);
212       frame[2*i-1] = gain*x1;
213       frame[2*i] = gain*x2;
214    }
215    frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
216    frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
217    spx_drft_backward(&st->lookup,frame);
218    for (i=0;i<2*st->frame_size;i++)
219       frame[i] *= st->vorbis_win[i];
220 #endif
221    
222    for (i=0;i<st->frame_size;i++)
223    {
224 #ifdef VORBIS_PSYCHO
225       float tmp = y[i] + frame[i] + st->wola_mem[i];
226       st->wola_mem[i] = frame[i+st->frame_size];
227 #else
228       float tmp = y[i];
229 #endif
230       if (tmp>32767)
231          tmp = 32767;
232       if (tmp < -32767)
233          tmp = -32767;
234       out[i] = tmp;
235    }
236 }
237
238 void speex_decorrelate_destroy(DecorrState *st)
239 {
240 #ifdef VORBIS_PSYCHO
241    vorbis_psy_destroy(st->psy);
242    speex_free(st->wola_mem);
243    speex_free(st->curve);
244 #endif
245    speex_free(st->buff);
246    speex_free(st);
247 }