← Back to branch summary

~noskcaj/ubuntu/saucy/sflphone/merge-1.2.3-2

« back to all changes in this revision

Viewing changes to daemon/libs/pjproject/third_party/speex/libspeex/vorbis_psy.c

  • Committer: Jackson Doak
  • Date: 2013-07-10 21:04:46 UTC
  • mfrom: (20.1.3 sid)
  • Revision ID: noskcaj@ubuntu.com-20130710210446-y8f587vza807icr9
https://launchpad.net/bugs/1196978
Properly merged from upstream.

Show diffs side-by-side

added added

removed removed

Lines of Context:
daemon/libs/pjproject/third_party/speex/libspeex/vorbis_psy.c
1
 
/* Copyright (C) 2005 Jean-Marc Valin, CSIRO, Christopher Montgomery
2
 
   File: vorbis_psy.c
3
 
 
4
 
   Redistribution and use in source and binary forms, with or without
5
 
   modification, are permitted provided that the following conditions
6
 
   are met:
7
 
   
8
 
   - Redistributions of source code must retain the above copyright
9
 
   notice, this list of conditions and the following disclaimer.
10
 
   
11
 
   - Redistributions in binary form must reproduce the above copyright
12
 
   notice, this list of conditions and the following disclaimer in the
13
 
   documentation and/or other materials provided with the distribution.
14
 
   
15
 
   - Neither the name of the Xiph.org Foundation nor the names of its
16
 
   contributors may be used to endorse or promote products derived from
17
 
   this software without specific prior written permission.
18
 
   
19
 
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20
 
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21
 
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22
 
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23
 
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24
 
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25
 
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26
 
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27
 
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28
 
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29
 
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
 
*/
31
 
 
32
 
#ifdef HAVE_CONFIG_H
33
 
#include "config.h"
34
 
#endif
35
 
 
36
 
#ifdef VORBIS_PSYCHO
37
 
 
38
 
#include "arch.h"
39
 
#include "smallft.h"
40
 
#include "lpc.h"
41
 
#include "vorbis_psy.h"
42
 
 
43
 
#include <stdlib.h>
44
 
#include <math.h>
45
 
#include <string.h>
46
 
 
47
 
/* psychoacoustic setup ********************************************/
48
 
 
49
 
static VorbisPsyInfo example_tuning = {
50
 
  
51
 
  .5,.5,  
52
 
  3,3,25,
53
 
  
54
 
  /*63     125     250     500      1k      2k      4k      8k     16k*/
55
 
  // vorbis mode 4 style
56
 
  //{-32,-32,-32,-32,-28,-24,-22,-20,-20, -20, -20, -8, -6, -6, -6, -6, -6},
57
 
  { -4, -6, -6, -6, -6, -6, -6, -6, -8, -8,-10,-10, -8, -6, -4, -4, -2},
58
 
  
59
 
  {
60
 
    0, 1, 2, 3, 4, 5, 5,  5,     /* 7dB */
61
 
    6, 6, 6, 5, 4, 4, 4,  4,     /* 15dB */
62
 
    4, 4, 5, 5, 5, 6, 6,  6,     /* 23dB */
63
 
    7, 7, 7, 8, 8, 8, 9, 10,     /* 31dB */
64
 
    11,12,13,14,15,16,17, 18,     /* 39dB */
65
 
  }
66
 
 
67
 
};
68
 
 
69
 
 
70
 
 
71
 
/* there was no great place to put this.... */
72
 
#include <stdio.h>
73
 
static void _analysis_output(char *base,int i,float *v,int n,int bark,int dB){
74
 
  int j;
75
 
  FILE *of;
76
 
  char buffer[80];
77
 
 
78
 
  sprintf(buffer,"%s_%d.m",base,i);
79
 
  of=fopen(buffer,"w");
80
 
  
81
 
  if(!of)perror("failed to open data dump file");
82
 
  
83
 
  for(j=0;j<n;j++){
84
 
    if(bark){
85
 
      float b=toBARK((4000.f*j/n)+.25);
86
 
      fprintf(of,"%f ",b);
87
 
    }else
88
 
      fprintf(of,"%f ",(double)j);
89
 
    
90
 
    if(dB){
91
 
      float val;
92
 
      if(v[j]==0.)
93
 
        val=-140.;
94
 
      else
95
 
        val=todB(v[j]);
96
 
      fprintf(of,"%f\n",val);
97
 
    }else{
98
 
      fprintf(of,"%f\n",v[j]);
99
 
    }
100
 
  }
101
 
  fclose(of);
102
 
}
103
 
 
104
 
static void bark_noise_hybridmp(int n,const long *b,
105
 
                                const float *f,
106
 
                                float *noise,
107
 
                                const float offset,
108
 
                                const int fixed){
109
 
  
110
 
  float *N=alloca(n*sizeof(*N));
111
 
  float *X=alloca(n*sizeof(*N));
112
 
  float *XX=alloca(n*sizeof(*N));
113
 
  float *Y=alloca(n*sizeof(*N));
114
 
  float *XY=alloca(n*sizeof(*N));
115
 
 
116
 
  float tN, tX, tXX, tY, tXY;
117
 
  int i;
118
 
 
119
 
  int lo, hi;
120
 
  float R, A, B, D;
121
 
  float w, x, y;
122
 
 
123
 
  tN = tX = tXX = tY = tXY = 0.f;
124
 
 
125
 
  y = f[0] + offset;
126
 
  if (y < 1.f) y = 1.f;
127
 
 
128
 
  w = y * y * .5;
129
 
    
130
 
  tN += w;
131
 
  tX += w;
132
 
  tY += w * y;
133
 
 
134
 
  N[0] = tN;
135
 
  X[0] = tX;
136
 
  XX[0] = tXX;
137
 
  Y[0] = tY;
138
 
  XY[0] = tXY;
139
 
 
140
 
  for (i = 1, x = 1.f; i < n; i++, x += 1.f) {
141
 
    
142
 
    y = f[i] + offset;
143
 
    if (y < 1.f) y = 1.f;
144
 
 
145
 
    w = y * y;
146
 
    
147
 
    tN += w;
148
 
    tX += w * x;
149
 
    tXX += w * x * x;
150
 
    tY += w * y;
151
 
    tXY += w * x * y;
152
 
 
153
 
    N[i] = tN;
154
 
    X[i] = tX;
155
 
    XX[i] = tXX;
156
 
    Y[i] = tY;
157
 
    XY[i] = tXY;
158
 
  }
159
 
  
160
 
  for (i = 0, x = 0.f;; i++, x += 1.f) {
161
 
    
162
 
    lo = b[i] >> 16;
163
 
    if( lo>=0 ) break;
164
 
    hi = b[i] & 0xffff;
165
 
    
166
 
    tN = N[hi] + N[-lo];
167
 
    tX = X[hi] - X[-lo];
168
 
    tXX = XX[hi] + XX[-lo];
169
 
    tY = Y[hi] + Y[-lo];    
170
 
    tXY = XY[hi] - XY[-lo];
171
 
    
172
 
    A = tY * tXX - tX * tXY;
173
 
    B = tN * tXY - tX * tY;
174
 
    D = tN * tXX - tX * tX;
175
 
    R = (A + x * B) / D;
176
 
    if (R < 0.f)
177
 
      R = 0.f;
178
 
    
179
 
    noise[i] = R - offset;
180
 
  }
181
 
  
182
 
  for ( ;; i++, x += 1.f) {
183
 
    
184
 
    lo = b[i] >> 16;
185
 
    hi = b[i] & 0xffff;
186
 
    if(hi>=n)break;
187
 
    
188
 
    tN = N[hi] - N[lo];
189
 
    tX = X[hi] - X[lo];
190
 
    tXX = XX[hi] - XX[lo];
191
 
    tY = Y[hi] - Y[lo];
192
 
    tXY = XY[hi] - XY[lo];
193
 
    
194
 
    A = tY * tXX - tX * tXY;
195
 
    B = tN * tXY - tX * tY;
196
 
    D = tN * tXX - tX * tX;
197
 
    R = (A + x * B) / D;
198
 
    if (R < 0.f) R = 0.f;
199
 
    
200
 
    noise[i] = R - offset;
201
 
  }
202
 
  for ( ; i < n; i++, x += 1.f) {
203
 
    
204
 
    R = (A + x * B) / D;
205
 
    if (R < 0.f) R = 0.f;
206
 
    
207
 
    noise[i] = R - offset;
208
 
  }
209
 
  
210
 
  if (fixed <= 0) return;
211
 
  
212
 
  for (i = 0, x = 0.f;; i++, x += 1.f) {
213
 
    hi = i + fixed / 2;
214
 
    lo = hi - fixed;
215
 
    if(lo>=0)break;
216
 
 
217
 
    tN = N[hi] + N[-lo];
218
 
    tX = X[hi] - X[-lo];
219
 
    tXX = XX[hi] + XX[-lo];
220
 
    tY = Y[hi] + Y[-lo];
221
 
    tXY = XY[hi] - XY[-lo];
222
 
    
223
 
    
224
 
    A = tY * tXX - tX * tXY;
225
 
    B = tN * tXY - tX * tY;
226
 
    D = tN * tXX - tX * tX;
227
 
    R = (A + x * B) / D;
228
 
 
229
 
    if (R - offset < noise[i]) noise[i] = R - offset;
230
 
  }
231
 
  for ( ;; i++, x += 1.f) {
232
 
    
233
 
    hi = i + fixed / 2;
234
 
    lo = hi - fixed;
235
 
    if(hi>=n)break;
236
 
    
237
 
    tN = N[hi] - N[lo];
238
 
    tX = X[hi] - X[lo];
239
 
    tXX = XX[hi] - XX[lo];
240
 
    tY = Y[hi] - Y[lo];
241
 
    tXY = XY[hi] - XY[lo];
242
 
    
243
 
    A = tY * tXX - tX * tXY;
244
 
    B = tN * tXY - tX * tY;
245
 
    D = tN * tXX - tX * tX;
246
 
    R = (A + x * B) / D;
247
 
    
248
 
    if (R - offset < noise[i]) noise[i] = R - offset;
249
 
  }
250
 
  for ( ; i < n; i++, x += 1.f) {
251
 
    R = (A + x * B) / D;
252
 
    if (R - offset < noise[i]) noise[i] = R - offset;
253
 
  }
254
 
}
255
 
 
256
 
static void _vp_noisemask(VorbisPsy *p,
257
 
                          float *logfreq, 
258
 
                          float *logmask){
259
 
 
260
 
  int i,n=p->n/2;
261
 
  float *work=alloca(n*sizeof(*work));
262
 
 
263
 
  bark_noise_hybridmp(n,p->bark,logfreq,logmask,
264
 
                      140.,-1);
265
 
 
266
 
  for(i=0;i<n;i++)work[i]=logfreq[i]-logmask[i];
267
 
 
268
 
  bark_noise_hybridmp(n,p->bark,work,logmask,0.,
269
 
                      p->vi->noisewindowfixed);
270
 
 
271
 
  for(i=0;i<n;i++)work[i]=logfreq[i]-work[i];
272
 
  
273
 
  {
274
 
    static int seq=0;
275
 
    
276
 
    float work2[n];
277
 
    for(i=0;i<n;i++){
278
 
      work2[i]=logmask[i]+work[i];
279
 
    }
280
 
    
281
 
    //_analysis_output("logfreq",seq,logfreq,n,0,0);
282
 
    //_analysis_output("median",seq,work,n,0,0);
283
 
    //_analysis_output("envelope",seq,work2,n,0,0);
284
 
    seq++;
285
 
  }
286
 
 
287
 
  for(i=0;i<n;i++){
288
 
    int dB=logmask[i]+.5;
289
 
    if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
290
 
    if(dB<0)dB=0;
291
 
    logmask[i]= work[i]+p->vi->noisecompand[dB]+p->noiseoffset[i];
292
 
  }
293
 
 
294
 
}
295
 
 
296
 
VorbisPsy *vorbis_psy_init(int rate, int n)
297
 
{
298
 
  long i,j,lo=-99,hi=1;
299
 
  VorbisPsy *p = speex_alloc(sizeof(VorbisPsy));
300
 
  memset(p,0,sizeof(*p));
301
 
  
302
 
  p->n = n;
303
 
  spx_drft_init(&p->lookup, n);
304
 
  p->bark = speex_alloc(n*sizeof(*p->bark));
305
 
  p->rate=rate;
306
 
  p->vi = &example_tuning;
307
 
 
308
 
  /* BH4 window */
309
 
  p->window = speex_alloc(sizeof(*p->window)*n);
310
 
  float a0 = .35875f;
311
 
  float a1 = .48829f;
312
 
  float a2 = .14128f;
313
 
  float a3 = .01168f;
314
 
  for(i=0;i<n;i++)
315
 
    p->window[i] = //a0 - a1*cos(2.*M_PI/n*(i+.5)) + a2*cos(4.*M_PI/n*(i+.5)) - a3*cos(6.*M_PI/n*(i+.5));
316
 
      sin((i+.5)/n * M_PI)*sin((i+.5)/n * M_PI);
317
 
  /* bark scale lookups */
318
 
  for(i=0;i<n;i++){
319
 
    float bark=toBARK(rate/(2*n)*i); 
320
 
    
321
 
    for(;lo+p->vi->noisewindowlomin<i && 
322
 
          toBARK(rate/(2*n)*lo)<(bark-p->vi->noisewindowlo);lo++);
323
 
    
324
 
    for(;hi<=n && (hi<i+p->vi->noisewindowhimin ||
325
 
          toBARK(rate/(2*n)*hi)<(bark+p->vi->noisewindowhi));hi++);
326
 
    
327
 
    p->bark[i]=((lo-1)<<16)+(hi-1);
328
 
 
329
 
  }
330
 
 
331
 
  /* set up rolling noise median */
332
 
  p->noiseoffset=speex_alloc(n*sizeof(*p->noiseoffset));
333
 
  
334
 
  for(i=0;i<n;i++){
335
 
    float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
336
 
    int inthalfoc;
337
 
    float del;
338
 
    
339
 
    if(halfoc<0)halfoc=0;
340
 
    if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
341
 
    inthalfoc=(int)halfoc;
342
 
    del=halfoc-inthalfoc;
343
 
    
344
 
    p->noiseoffset[i]=
345
 
      p->vi->noiseoff[inthalfoc]*(1.-del) + 
346
 
      p->vi->noiseoff[inthalfoc+1]*del;
347
 
    
348
 
  }
349
 
#if 0
350
 
  _analysis_output_always("noiseoff0",ls,p->noiseoffset,n,1,0,0);
351
 
#endif
352
 
 
353
 
   return p;
354
 
}
355
 
 
356
 
void vorbis_psy_destroy(VorbisPsy *p)
357
 
{
358
 
  if(p){
359
 
    spx_drft_clear(&p->lookup);
360
 
    if(p->bark)
361
 
      speex_free(p->bark);
362
 
    if(p->noiseoffset)
363
 
      speex_free(p->noiseoffset);
364
 
    if(p->window)
365
 
      speex_free(p->window);
366
 
    memset(p,0,sizeof(*p));
367
 
    speex_free(p);
368
 
  }
369
 
}
370
 
 
371
 
void compute_curve(VorbisPsy *psy, float *audio, float *curve)
372
 
{
373
 
   int i;
374
 
   float work[psy->n];
375
 
 
376
 
   float scale=4.f/psy->n;
377
 
   float scale_dB;
378
 
 
379
 
   scale_dB=todB(scale);
380
 
  
381
 
   /* window the PCM data; use a BH4 window, not vorbis */
382
 
   for(i=0;i<psy->n;i++)
383
 
     work[i]=audio[i] * psy->window[i];
384
 
 
385
 
   {
386
 
     static int seq=0;
387
 
     
388
 
     //_analysis_output("win",seq,work,psy->n,0,0);
389
 
 
390
 
     seq++;
391
 
   }
392
 
 
393
 
    /* FFT yields more accurate tonal estimation (not phase sensitive) */
394
 
    spx_drft_forward(&psy->lookup,work);
395
 
 
396
 
    /* magnitudes */
397
 
    work[0]=scale_dB+todB(work[0]);
398
 
    for(i=1;i<psy->n-1;i+=2){
399
 
      float temp = work[i]*work[i] + work[i+1]*work[i+1];
400
 
      work[(i+1)>>1] = scale_dB+.5f * todB(temp);
401
 
    }
402
 
 
403
 
    /* derive a noise curve */
404
 
    _vp_noisemask(psy,work,curve);
405
 
#define SIDEL 12
406
 
    for (i=0;i<SIDEL;i++)
407
 
    {
408
 
       curve[i]=curve[SIDEL];
409
 
    }
410
 
#define SIDEH 12
411
 
    for (i=0;i<SIDEH;i++)
412
 
    {
413
 
       curve[(psy->n>>1)-i-1]=curve[(psy->n>>1)-SIDEH];
414
 
    }
415
 
    for(i=0;i<((psy->n)>>1);i++)
416
 
       curve[i] = fromdB(1.2*curve[i]+.2*i);
417
 
       //curve[i] = fromdB(0.8*curve[i]+.35*i);
418
 
       //curve[i] = fromdB(0.9*curve[i])*pow(1.0*i+45,1.3);
419
 
}
420
 
 
421
 
/* Transform a masking curve (power spectrum) into a pole-zero filter */
422
 
void curve_to_lpc(VorbisPsy *psy, float *curve, float *awk1, float *awk2, int ord)
423
 
{
424
 
   int i;
425
 
   float ac[psy->n];
426
 
   float tmp;
427
 
   int len = psy->n >> 1;
428
 
   for (i=0;i<2*len;i++)
429
 
      ac[i] = 0;
430
 
   for (i=1;i<len;i++)
431
 
      ac[2*i-1] = curve[i];
432
 
   ac[0] = curve[0];
433
 
   ac[2*len-1] = curve[len-1];
434
 
   
435
 
   spx_drft_backward(&psy->lookup, ac);
436
 
   _spx_lpc(awk1, ac, ord);
437
 
   tmp = 1.;
438
 
   for (i=0;i<ord;i++)
439
 
   {
440
 
      tmp *= .99;
441
 
      awk1[i] *= tmp;
442
 
   }
443
 
#if 0
444
 
   for (i=0;i<ord;i++)
445
 
      awk2[i] = 0;
446
 
#else
447
 
   /* Use the second (awk2) filter to correct the first one */
448
 
   for (i=0;i<2*len;i++)
449
 
      ac[i] = 0;   
450
 
   for (i=0;i<ord;i++)
451
 
      ac[i+1] = awk1[i];
452
 
   ac[0] = 1;
453
 
   spx_drft_forward(&psy->lookup, ac);
454
 
   /* Compute (power) response of awk1 (all zero) */
455
 
   ac[0] *= ac[0];
456
 
   for (i=1;i<len;i++)
457
 
      ac[i] = ac[2*i-1]*ac[2*i-1] + ac[2*i]*ac[2*i];
458
 
   ac[len] = ac[2*len-1]*ac[2*len-1];
459
 
   /* Compute correction required */
460
 
   for (i=0;i<len;i++)
461
 
      curve[i] = 1. / (1e-6f+curve[i]*ac[i]);
462
 
 
463
 
   for (i=0;i<2*len;i++)
464
 
      ac[i] = 0;
465
 
   for (i=1;i<len;i++)
466
 
      ac[2*i-1] = curve[i];
467
 
   ac[0] = curve[0];
468
 
   ac[2*len-1] = curve[len-1];
469
 
   
470
 
   spx_drft_backward(&psy->lookup, ac);
471
 
   _spx_lpc(awk2, ac, ord);
472
 
   tmp = 1;
473
 
   for (i=0;i<ord;i++)
474
 
   {
475
 
      tmp *= .99;
476
 
      awk2[i] *= tmp;
477
 
   }
478
 
#endif
479
 
}
480
 
 
481
 
#if 0
482
 
#include <stdio.h>
483
 
#include <math.h>
484
 
 
485
 
#define ORDER 10
486
 
#define CURVE_SIZE 24
487
 
 
488
 
int main()
489
 
{
490
 
   int i;
491
 
   float curve[CURVE_SIZE];
492
 
   float awk1[ORDER], awk2[ORDER];
493
 
   for (i=0;i<CURVE_SIZE;i++)
494
 
      scanf("%f ", &curve[i]);
495
 
   for (i=0;i<CURVE_SIZE;i++)
496
 
      curve[i] = pow(10.f, .1*curve[i]);
497
 
   curve_to_lpc(curve, CURVE_SIZE, awk1, awk2, ORDER);
498
 
   for (i=0;i<ORDER;i++)
499
 
      printf("%f ", awk1[i]);
500
 
   printf ("\n");
501
 
   for (i=0;i<ORDER;i++)
502
 
      printf("%f ", awk2[i]);
503
 
   printf ("\n");
504
 
   return 0;
505
 
}
506
 
#endif
507
 
 
508
 
#endif