← Back to branch summary

~ubuntu-branches/ubuntu/wily/sflphone/wily

« back to all changes in this revision

Viewing changes to daemon/libs/pjproject-2.1.0/third_party/speex/libspeex/scal.c

  • Committer: Package Import Robot
  • Author(s): Jonathan Riddell
  • Date: 2015-01-07 14:51:16 UTC
  • mfrom: (4.3.5 sid)
  • Revision ID: package-import@ubuntu.com-20150107145116-yxnafinf4lrdvrmx
Tags: 1.4.1-0.1ubuntu1
* Merge with Debian, remaining changes:
 - Drop soprano, nepomuk build-dep
* Drop ubuntu patches, now upstream

Show diffs side-by-side

added added

removed removed

Lines of Context:
daemon/libs/pjproject-2.1.0/third_party/speex/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 "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
 
}