apps/codecs/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-speex.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