lib/psy.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
   5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
   6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
   7  *                                                                  *
   8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001             *
   9  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
  10  *                                                                  *
  11  ********************************************************************
  12
  13  function: psychoacoustics not including preecho
  14  last mod: $Id: psy.c,v 1.68 2002/06/28 22:19:37 xiphmont Exp $
  15
  16  ********************************************************************/
  17
  18 #include <stdlib.h>
  19 #include <math.h>
  20 #include <string.h>
  21 #include "vorbis/codec.h"
  22 #include "codec_internal.h"
  23
  24 #include "masking.h"
  25 #include "psy.h"
  26 #include "os.h"
  27 #include "lpc.h"
  28 #include "smallft.h"
  29 #include "scales.h"
  30 #include "misc.h"
  31
  32 #define NEGINF -9999.f
  33 static double stereo_threshholds[]={0.0, .5, 1.0, 1.5, 2.5, 4.5, 8.5, 16.5, 9e10};
  34
  35 vorbis_look_psy_global *_vp_global_look(vorbis_info *vi){
  36   codec_setup_info *ci=vi->codec_setup;
  37   vorbis_info_psy_global *gi=&ci->psy_g_param;
  38   vorbis_look_psy_global *look=_ogg_calloc(1,sizeof(*look));
  39
  40   look->channels=vi->channels;
  41
  42   look->ampmax=-9999.;
  43   look->gi=gi;
  44   return(look);
  45 }
  46
  47 void _vp_global_free(vorbis_look_psy_global *look){
  48   if(look){
  49     memset(look,0,sizeof(*look));
  50     _ogg_free(look);
  51   }
  52 }
  53
  54 void _vi_gpsy_free(vorbis_info_psy_global *i){
  55   if(i){
  56     memset(i,0,sizeof(*i));
  57     _ogg_free(i);
  58   }
  59 }
  60
  61 void _vi_psy_free(vorbis_info_psy *i){
  62   if(i){
  63     memset(i,0,sizeof(*i));
  64     _ogg_free(i);
  65   }
  66 }
  67
  68 static void min_curve(float *c,
  69                        float *c2){
  70   int i;
  71   for(i=0;i<EHMER_MAX;i++)if(c2[i]<c[i])c[i]=c2[i];
  72 }
  73 static void max_curve(float *c,
  74                        float *c2){
  75   int i;
  76   for(i=0;i<EHMER_MAX;i++)if(c2[i]>c[i])c[i]=c2[i];
  77 }
  78
  79 static void attenuate_curve(float *c,float att){
  80   int i;
  81   for(i=0;i<EHMER_MAX;i++)
  82     c[i]+=att;
  83 }
  84
  85 static float ***setup_tone_curves(float curveatt_dB[P_BANDS],float binHz,int n,
  86                                   float center_boost, float center_decay_rate){
  87   int i,j,k,m;
  88   float ath[EHMER_MAX];
  89   float workc[P_BANDS][P_LEVELS][EHMER_MAX];
  90   float athc[P_LEVELS][EHMER_MAX];
  91   float *brute_buffer=alloca(n*sizeof(*brute_buffer));
  92
  93   float ***ret=_ogg_malloc(sizeof(*ret)*P_BANDS);
  94
  95   memset(workc,0,sizeof(workc));
  96
  97   for(i=0;i<P_BANDS;i++){
  98     /* we add back in the ATH to avoid low level curves falling off to
  99        -infinity and unnecessarily cutting off high level curves in the
 100        curve limiting (last step). */
 101
 102     /* A half-band's settings must be valid over the whole band, and
 103        it's better to mask too little than too much */
 104     int ath_offset=i*4;
 105     for(j=0;j<EHMER_MAX;j++){
 106       float min=999.;
 107       for(k=0;k<4;k++)
 108         if(j+k+ath_offset<MAX_ATH){
 109           if(min>ATH[j+k+ath_offset])min=ATH[j+k+ath_offset];
 110         }else{
 111           if(min>ATH[MAX_ATH-1])min=ATH[MAX_ATH-1];
 112         }
 113       ath[j]=min;
 114     }
 115
 116     /* copy curves into working space, replicate the 50dB curve to 30
 117        and 40, replicate the 100dB curve to 110 */
 118     for(j=0;j<6;j++)
 119       memcpy(workc[i][j+2],tonemasks[i][j],EHMER_MAX*sizeof(*tonemasks[i][j]));
 120     memcpy(workc[i][0],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
 121     memcpy(workc[i][1],tonemasks[i][0],EHMER_MAX*sizeof(*tonemasks[i][0]));
 122
 123     /* apply centered curve boost/decay */
 124     for(j=0;j<P_LEVELS;j++){
 125       for(k=0;k<EHMER_MAX;k++){
 126         float adj=center_boost+abs(EHMER_OFFSET-k)*center_decay_rate;
 127         if(adj<0. && center_boost>0)adj=0.;
 128         if(adj>0. && center_boost<0)adj=0.;
 129         workc[i][j][k]+=adj;
 130       }
 131     }
 132
 133     /* normalize curves so the driving amplitude is 0dB */
 134     /* make temp curves with the ATH overlayed */
 135     for(j=0;j<P_LEVELS;j++){
 136       attenuate_curve(workc[i][j],curveatt_dB[i]+100.-(j<2?2:j)*10.-P_LEVEL_0);
 137       memcpy(athc[j],ath,EHMER_MAX*sizeof(**athc));
 138       attenuate_curve(athc[j],+100.-j*10.f-P_LEVEL_0);
 139       max_curve(athc[j],workc[i][j]);
 140     }
 141
 142     /* Now limit the louder curves.
 143
 144        the idea is this: We don't know what the playback attenuation
 145        will be; 0dB SL moves every time the user twiddles the volume
 146        knob. So that means we have to use a single 'most pessimal' curve
 147        for all masking amplitudes, right?  Wrong.  The *loudest* sound
 148        can be in (we assume) a range of ...+100dB] SL.  However, sounds
 149        20dB down will be in a range ...+80], 40dB down is from ...+60],
 150        etc... */
 151
 152     for(j=1;j<P_LEVELS;j++){
 153       min_curve(athc[j],athc[j-1]);
 154       min_curve(workc[i][j],athc[j]);
 155     }
 156   }
 157
 158   for(i=0;i<P_BANDS;i++){
 159     int hi_curve,lo_curve,bin;
 160     ret[i]=_ogg_malloc(sizeof(**ret)*P_LEVELS);
 161
 162     /* low frequency curves are measured with greater resolution than
 163        the MDCT/FFT will actually give us; we want the curve applied
 164        to the tone data to be pessimistic and thus apply the minimum
 165        masking possible for a given bin.  That means that a single bin
 166        could span more than one octave and that the curve will be a
 167        composite of multiple octaves.  It also may mean that a single
 168        bin may span > an eighth of an octave and that the eighth
 169        octave values may also be composited. */
 170
 171     /* which octave curves will we be compositing? */
 172     bin=floor(fromOC(i*.5)/binHz);
 173     lo_curve=  ceil(toOC(bin*binHz+1)*2);
 174     hi_curve=  floor(toOC((bin+1)*binHz)*2);
 175     if(lo_curve>i)lo_curve=i;
 176     if(lo_curve<0)lo_curve=0;
 177     if(hi_curve>=P_BANDS)hi_curve=P_BANDS-1;
 178
 179     for(m=0;m<P_LEVELS;m++){
 180       ret[i][m]=_ogg_malloc(sizeof(***ret)*(EHMER_MAX+2));
 181
 182       for(j=0;j<n;j++)brute_buffer[j]=999.;
 183
 184       /* render the curve into bins, then pull values back into curve.
 185          The point is that any inherent subsampling aliasing results in
 186          a safe minimum */
 187       for(k=lo_curve;k<=hi_curve;k++){
 188         int l=0;
 189
 190         for(j=0;j<EHMER_MAX;j++){
 191           int lo_bin= fromOC(j*.125+k*.5-2.0625)/binHz;
 192           int hi_bin= fromOC(j*.125+k*.5-1.9375)/binHz+1;
 193
 194           if(lo_bin<0)lo_bin=0;
 195           if(lo_bin>n)lo_bin=n;
 196           if(lo_bin<l)l=lo_bin;
 197           if(hi_bin<0)hi_bin=0;
 198           if(hi_bin>n)hi_bin=n;
 199
 200           for(;l<hi_bin && l<n;l++)
 201             if(brute_buffer[l]>workc[k][m][j])
 202               brute_buffer[l]=workc[k][m][j];
 203         }
 204
 205         for(;l<n;l++)
 206           if(brute_buffer[l]>workc[k][m][EHMER_MAX-1])
 207             brute_buffer[l]=workc[k][m][EHMER_MAX-1];
 208
 209       }
 210
 211       for(j=0;j<EHMER_MAX;j++){
 212         int bin=fromOC(j*.125+i*.5-2.)/binHz;
 213         if(bin<0){
 214           ret[i][m][j+2]=-999.;
 215         }else{
 216           if(bin>=n){
 217             ret[i][m][j+2]=-999.;
 218           }else{
 219             ret[i][m][j+2]=brute_buffer[bin];
 220           }
 221         }
 222       }
 223
 224       /* add fenceposts */
 225       for(j=0;j<EHMER_OFFSET;j++)
 226         if(ret[i][m][j+2]>-200.f)break;
 227       ret[i][m][0]=j;
 228
 229       for(j=EHMER_MAX-1;j>EHMER_OFFSET+1;j--)
 230         if(ret[i][m][j+2]>-200.f)
 231           break;
 232       ret[i][m][1]=j;
 233
 234     }
 235   }
 236
 237   return(ret);
 238 }
 239
 240 void _vp_psy_init(vorbis_look_psy *p,vorbis_info_psy *vi,
 241                   vorbis_info_psy_global *gi,int n,long rate){
 242   long i,j,lo=-99,hi=0;
 243   long maxoc;
 244   memset(p,0,sizeof(*p));
 245
 246   p->eighth_octave_lines=gi->eighth_octave_lines;
 247   p->shiftoc=rint(log(gi->eighth_octave_lines*8.f)/log(2.f))-1;
 248
 249   p->firstoc=toOC(.25f*rate/n)*(1<<(p->shiftoc+1))-gi->eighth_octave_lines;
 250   maxoc=toOC((n*.5f-.25f)*rate/n)*(1<<(p->shiftoc+1))+.5f;
 251   p->total_octave_lines=maxoc-p->firstoc+1;
 252   p->ath=_ogg_malloc(n*sizeof(*p->ath));
 253
 254   p->octave=_ogg_malloc(n*sizeof(*p->octave));
 255   p->bark=_ogg_malloc(n*sizeof(*p->bark));
 256   p->vi=vi;
 257   p->n=n;
 258   p->rate=rate;
 259
 260   /* set up the lookups for a given blocksize and sample rate */
 261
 262   for(i=0,j=0;i<MAX_ATH-1;i++){
 263     int endpos=rint(fromOC((i+1)*.125-2.)*2*n/rate);
 264     float base=ATH[i];
 265     if(j<endpos){
 266       float delta=(ATH[i+1]-base)/(endpos-j);
 267       for(;j<endpos && j<n;j++){
 268         p->ath[j]=base+100.;
 269         base+=delta;
 270       }
 271     }
 272   }
 273
 274   for(i=0;i<n;i++){
 275     float bark=toBARK(rate/(2*n)*i);
 276
 277     for(;lo+vi->noisewindowlomin<i &&
 278           toBARK(rate/(2*n)*lo)<(bark-vi->noisewindowlo);lo++);
 279
 280     for(;hi<n && (hi<i+vi->noisewindowhimin ||
 281           toBARK(rate/(2*n)*hi)<(bark+vi->noisewindowhi));hi++);
 282
 283     p->bark[i]=(lo<<16)+hi;
 284
 285   }
 286
 287   for(i=0;i<n;i++)
 288     p->octave[i]=toOC((i*.5f+.25f)*rate/n)*(1<<(p->shiftoc+1))+.5f;
 289
 290   p->tonecurves=setup_tone_curves(vi->toneatt,rate*.5/n,n,
 291                                   vi->tone_centerboost,vi->tone_decay);
 292
 293   /* set up rolling noise median */
 294   p->noiseoffset=_ogg_malloc(P_NOISECURVES*sizeof(*p->noiseoffset));
 295   for(i=0;i<P_NOISECURVES;i++)
 296     p->noiseoffset[i]=_ogg_malloc(n*sizeof(**p->noiseoffset));
 297
 298   for(i=0;i<n;i++){
 299     float halfoc=toOC((i+.5)*rate/(2.*n))*2.;
 300     int inthalfoc;
 301     float del;
 302
 303     if(halfoc<0)halfoc=0;
 304     if(halfoc>=P_BANDS-1)halfoc=P_BANDS-1;
 305     inthalfoc=(int)halfoc;
 306     del=halfoc-inthalfoc;
 307
 308     for(j=0;j<P_NOISECURVES;j++)
 309       p->noiseoffset[j][i]=
 310         p->vi->noiseoff[j][inthalfoc]*(1.-del) +
 311         p->vi->noiseoff[j][inthalfoc+1]*del;
 312
 313   }
 314 #if 0
 315   {
 316     static int ls=0;
 317     _analysis_output_always("noiseoff0",ls,p->noiseoffset[0],n,1,0,0);
 318     _analysis_output_always("noiseoff1",ls,p->noiseoffset[1],n,1,0,0);
 319     _analysis_output_always("noiseoff2",ls++,p->noiseoffset[2],n,1,0,0);
 320   }
 321 #endif
 322 }
 323
 324 void _vp_psy_clear(vorbis_look_psy *p){
 325   int i,j;
 326   if(p){
 327     if(p->ath)_ogg_free(p->ath);
 328     if(p->octave)_ogg_free(p->octave);
 329     if(p->bark)_ogg_free(p->bark);
 330     if(p->tonecurves){
 331       for(i=0;i<P_BANDS;i++){
 332         for(j=0;j<P_LEVELS;j++){
 333           _ogg_free(p->tonecurves[i][j]);
 334         }
 335         _ogg_free(p->tonecurves[i]);
 336       }
 337       _ogg_free(p->tonecurves);
 338     }
 339     _ogg_free(p->noiseoffset);
 340     memset(p,0,sizeof(*p));
 341   }
 342 }
 343
 344 /* octave/(8*eighth_octave_lines) x scale and dB y scale */
 345 static void seed_curve(float *seed,
 346                        const float **curves,
 347                        float amp,
 348                        int oc, int n,
 349                        int linesper,float dBoffset){
 350   int i,post1;
 351   int seedptr;
 352   const float *posts,*curve;
 353
 354   int choice=(int)((amp+dBoffset-P_LEVEL_0)*.1f);
 355   choice=max(choice,0);
 356   choice=min(choice,P_LEVELS-1);
 357   posts=curves[choice];
 358   curve=posts+2;
 359   post1=(int)posts[1];
 360   seedptr=oc+(posts[0]-EHMER_OFFSET)*linesper-(linesper>>1);
 361
 362   for(i=posts[0];i<post1;i++){
 363     if(seedptr>0){
 364       float lin=amp+curve[i];
 365       if(seed[seedptr]<lin)seed[seedptr]=lin;
 366     }
 367     seedptr+=linesper;
 368     if(seedptr>=n)break;
 369   }
 370 }
 371
 372 static void seed_loop(vorbis_look_psy *p,
 373                       const float ***curves,
 374                       const float *f,
 375                       const float *flr,
 376                       float *seed,
 377                       float specmax){
 378   vorbis_info_psy *vi=p->vi;
 379   long n=p->n,i;
 380   float dBoffset=vi->max_curve_dB-specmax;
 381
 382   /* prime the working vector with peak values */
 383
 384   for(i=0;i<n;i++){
 385     float max=f[i];
 386     long oc=p->octave[i];
 387     while(i+1<n && p->octave[i+1]==oc){
 388       i++;
 389       if(f[i]>max)max=f[i];
 390     }
 391
 392     if(max+6.f>flr[i]){
 393       oc=oc>>p->shiftoc;
 394       if(oc>=P_BANDS)oc=P_BANDS-1;
 395       if(oc<0)oc=0;
 396
 397       seed_curve(seed,
 398                  curves[oc],
 399                  max,
 400                  p->octave[i]-p->firstoc,
 401                  p->total_octave_lines,
 402                  p->eighth_octave_lines,
 403                  dBoffset);
 404     }
 405   }
 406 }
 407
 408 static void seed_chase(float *seeds, int linesper, long n){
 409   long  *posstack=alloca(n*sizeof(*posstack));
 410   float *ampstack=alloca(n*sizeof(*ampstack));
 411   long   stack=0;
 412   long   pos=0;
 413   long   i;
 414
 415   for(i=0;i<n;i++){
 416     if(stack<2){
 417       posstack[stack]=i;
 418       ampstack[stack++]=seeds[i];
 419     }else{
 420       while(1){
 421         if(seeds[i]<ampstack[stack-1]){
 422           posstack[stack]=i;
 423           ampstack[stack++]=seeds[i];
 424           break;
 425         }else{
 426           if(i<posstack[stack-1]+linesper){
 427             if(stack>1 && ampstack[stack-1]<=ampstack[stack-2] &&
 428                i<posstack[stack-2]+linesper){
 429               /* we completely overlap, making stack-1 irrelevant.  pop it */
 430               stack--;
 431               continue;
 432             }
 433           }
 434           posstack[stack]=i;
 435           ampstack[stack++]=seeds[i];
 436           break;
 437
 438         }
 439       }
 440     }
 441   }
 442
 443   /* the stack now contains only the positions that are relevant. Scan
 444      'em straight through */
 445
 446   for(i=0;i<stack;i++){
 447     long endpos;
 448     if(i<stack-1 && ampstack[i+1]>ampstack[i]){
 449       endpos=posstack[i+1];
 450     }else{
 451       endpos=posstack[i]+linesper+1; /* +1 is important, else bin 0 is
 452                                         discarded in short frames */
 453     }
 454     if(endpos>n)endpos=n;
 455     for(;pos<endpos;pos++)
 456       seeds[pos]=ampstack[i];
 457   }
 458
 459   /* there.  Linear time.  I now remember this was on a problem set I
 460      had in Grad Skool... I didn't solve it at the time ;-) */
 461
 462 }
 463
 464 /* bleaugh, this is more complicated than it needs to be */
 465 static void max_seeds(vorbis_look_psy *p,
 466                       float *seed,
 467                       float *flr){
 468   long   n=p->total_octave_lines;
 469   int    linesper=p->eighth_octave_lines;
 470   long   linpos=0;
 471   long   pos;
 472
 473   seed_chase(seed,linesper,n); /* for masking */
 474
 475   pos=p->octave[0]-p->firstoc-(linesper>>1);
 476   while(linpos+1<p->n){
 477     float minV=seed[pos];
 478     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
 479     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
 480     while(pos+1<=end){
 481       pos++;
 482       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
 483         minV=seed[pos];
 484     }
 485
 486     /* seed scale is log.  Floor is linear.  Map back to it */
 487     end=pos+p->firstoc;
 488     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
 489       if(flr[linpos]<minV)flr[linpos]=minV;
 490   }
 491
 492   {
 493     float minV=seed[p->total_octave_lines-1];
 494     for(;linpos<p->n;linpos++)
 495       if(flr[linpos]<minV)flr[linpos]=minV;
 496   }
 497
 498 }
 499
 500 static void bark_noise_hybridmp(int n,const long *b,
 501                                 const float *f,
 502                                 float *noise,
 503                                 const float offset,
 504                                 const int fixed){
 505
 506   float *N=alloca((n+1)*sizeof(*N));
 507   float *X=alloca((n+1)*sizeof(*N));
 508   float *XX=alloca((n+1)*sizeof(*N));
 509   float *Y=alloca((n+1)*sizeof(*N));
 510   float *XY=alloca((n+1)*sizeof(*N));
 511
 512   float tN, tX, tXX, tY, tXY;
 513   float fi;
 514   int i;
 515
 516   int lo, hi;
 517   float R, A, B, D;
 518
 519   tN = tX = tXX = tY = tXY = 0.f;
 520   for (i = 0, fi = 0.f; i < n; i++, fi += 1.f) {
 521     float w, x, y;
 522
 523     x = fi;
 524     y = f[i] + offset;
 525     if (y < 1.f) y = 1.f;
 526     w = y * y;
 527     N[i] = tN;
 528     X[i] = tX;
 529     XX[i] = tXX;
 530     Y[i] = tY;
 531     XY[i] = tXY;
 532     tN += w;
 533     tX += w * x;
 534     tXX += w * x * x;
 535     tY += w * y;
 536     tXY += w * x * y;
 537   }
 538   N[i] = tN;
 539   X[i] = tX;
 540   XX[i] = tXX;
 541   Y[i] = tY;
 542   XY[i] = tXY;
 543
 544   for (i = 0, fi = 0.f;; i++, fi += 1.f) {
 545
 546     lo = b[i] >> 16;
 547     if( lo>=0 ) break;
 548     hi = b[i] & 0xffff;
 549
 550     tN = N[hi] + N[-lo];
 551     tX = X[hi] - X[-lo];
 552     tXX = XX[hi] + XX[-lo];
 553     tY = Y[hi] + Y[-lo];
 554     tXY = XY[hi] - XY[-lo];
 555
 556     A = tY * tXX - tX * tXY;
 557     B = tN * tXY - tX * tY;
 558     D = tN * tXX - tX * tX;
 559     R = (A + fi * B) / D;
 560     if (R < 0.f)
 561       R = 0.f;
 562
 563     noise[i] = R - offset;
 564   }
 565
 566   for ( ; hi < n; i++, fi += 1.f) {
 567
 568     lo = b[i] >> 16;
 569     hi = b[i] & 0xffff;
 570
 571     tN = N[hi] - N[lo];
 572     tX = X[hi] - X[lo];
 573     tXX = XX[hi] - XX[lo];
 574     tY = Y[hi] - Y[lo];
 575     tXY = XY[hi] - XY[lo];
 576
 577     A = tY * tXX - tX * tXY;
 578     B = tN * tXY - tX * tY;
 579     D = tN * tXX - tX * tX;
 580     R = (A + fi * B) / D;
 581     if (R < 0.f) R = 0.f;
 582
 583     noise[i] = R - offset;
 584   }
 585   for ( ; i < n; i++, fi += 1.f) {
 586
 587     R = (A + fi * B) / D;
 588     if (R < 0.f) R = 0.f;
 589
 590     noise[i] = R - offset;
 591   }
 592
 593   if (fixed <= 0) return;
 594
 595   for (i = 0, fi = 0.f; i < (fixed + 1) / 2; i++, fi += 1.f) {
 596     hi = i + fixed / 2;
 597     lo = hi - fixed;
 598
 599     tN = N[hi] + N[-lo];
 600     tX = X[hi] - X[-lo];
 601     tXX = XX[hi] + XX[-lo];
 602     tY = Y[hi] + Y[-lo];
 603     tXY = XY[hi] - XY[-lo];
 604
 605
 606     A = tY * tXX - tX * tXY;
 607     B = tN * tXY - tX * tY;
 608     D = tN * tXX - tX * tX;
 609     R = (A + fi * B) / D;
 610
 611     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 612   }
 613   for ( ; hi < n; i++, fi += 1.f) {
 614
 615     hi = i + fixed / 2;
 616     lo = hi - fixed;
 617
 618     tN = N[hi] - N[lo];
 619     tX = X[hi] - X[lo];
 620     tXX = XX[hi] - XX[lo];
 621     tY = Y[hi] - Y[lo];
 622     tXY = XY[hi] - XY[lo];
 623
 624     A = tY * tXX - tX * tXY;
 625     B = tN * tXY - tX * tY;
 626     D = tN * tXX - tX * tX;
 627     R = (A + fi * B) / D;
 628
 629     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 630   }
 631   for ( ; i < n; i++, fi += 1.f) {
 632     R = (A + fi * B) / D;
 633     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 634   }
 635 }
 636
 637 static float FLOOR1_fromdB_INV_LOOKUP[256]={
 638   0.F, 8.81683e+06F, 8.27882e+06F, 7.77365e+06F,
 639   7.29930e+06F, 6.85389e+06F, 6.43567e+06F, 6.04296e+06F,
 640   5.67422e+06F, 5.32798e+06F, 5.00286e+06F, 4.69759e+06F,
 641   4.41094e+06F, 4.14178e+06F, 3.88905e+06F, 3.65174e+06F,
 642   3.42891e+06F, 3.21968e+06F, 3.02321e+06F, 2.83873e+06F,
 643   2.66551e+06F, 2.50286e+06F, 2.35014e+06F, 2.20673e+06F,
 644   2.07208e+06F, 1.94564e+06F, 1.82692e+06F, 1.71544e+06F,
 645   1.61076e+06F, 1.51247e+06F, 1.42018e+06F, 1.33352e+06F,
 646   1.25215e+06F, 1.17574e+06F, 1.10400e+06F, 1.03663e+06F,
 647   973377.F, 913981.F, 858210.F, 805842.F,
 648   756669.F, 710497.F, 667142.F, 626433.F,
 649   588208.F, 552316.F, 518613.F, 486967.F,
 650   457252.F, 429351.F, 403152.F, 378551.F,
 651   355452.F, 333762.F, 313396.F, 294273.F,
 652   276316.F, 259455.F, 243623.F, 228757.F,
 653   214798.F, 201691.F, 189384.F, 177828.F,
 654   166977.F, 156788.F, 147221.F, 138237.F,
 655   129802.F, 121881.F, 114444.F, 107461.F,
 656   100903.F, 94746.3F, 88964.9F, 83536.2F,
 657   78438.8F, 73652.5F, 69158.2F, 64938.1F,
 658   60975.6F, 57254.9F, 53761.2F, 50480.6F,
 659   47400.3F, 44507.9F, 41792.0F, 39241.9F,
 660   36847.3F, 34598.9F, 32487.7F, 30505.3F,
 661   28643.8F, 26896.0F, 25254.8F, 23713.7F,
 662   22266.7F, 20908.0F, 19632.2F, 18434.2F,
 663   17309.4F, 16253.1F, 15261.4F, 14330.1F,
 664   13455.7F, 12634.6F, 11863.7F, 11139.7F,
 665   10460.0F, 9821.72F, 9222.39F, 8659.64F,
 666   8131.23F, 7635.06F, 7169.17F, 6731.70F,
 667   6320.93F, 5935.23F, 5573.06F, 5232.99F,
 668   4913.67F, 4613.84F, 4332.30F, 4067.94F,
 669   3819.72F, 3586.64F, 3367.78F, 3162.28F,
 670   2969.31F, 2788.13F, 2617.99F, 2458.24F,
 671   2308.24F, 2167.39F, 2035.14F, 1910.95F,
 672   1794.35F, 1684.85F, 1582.04F, 1485.51F,
 673   1394.86F, 1309.75F, 1229.83F, 1154.78F,
 674   1084.32F, 1018.15F, 956.024F, 897.687F,
 675   842.910F, 791.475F, 743.179F, 697.830F,
 676   655.249F, 615.265F, 577.722F, 542.469F,
 677   509.367F, 478.286F, 449.101F, 421.696F,
 678   395.964F, 371.803F, 349.115F, 327.812F,
 679   307.809F, 289.026F, 271.390F, 254.830F,
 680   239.280F, 224.679F, 210.969F, 198.096F,
 681   186.008F, 174.658F, 164.000F, 153.993F,
 682   144.596F, 135.773F, 127.488F, 119.708F,
 683   112.404F, 105.545F, 99.1046F, 93.0572F,
 684   87.3788F, 82.0469F, 77.0404F, 72.3394F,
 685   67.9252F, 63.7804F, 59.8885F, 56.2341F,
 686   52.8027F, 49.5807F, 46.5553F, 43.7144F,
 687   41.0470F, 38.5423F, 36.1904F, 33.9821F,
 688   31.9085F, 29.9614F, 28.1332F, 26.4165F,
 689   24.8045F, 23.2910F, 21.8697F, 20.5352F,
 690   19.2822F, 18.1056F, 17.0008F, 15.9634F,
 691   14.9893F, 14.0746F, 13.2158F, 12.4094F,
 692   11.6522F, 10.9411F, 10.2735F, 9.64662F,
 693   9.05798F, 8.50526F, 7.98626F, 7.49894F,
 694   7.04135F, 6.61169F, 6.20824F, 5.82941F,
 695   5.47370F, 5.13970F, 4.82607F, 4.53158F,
 696   4.25507F, 3.99542F, 3.75162F, 3.52269F,
 697   3.30774F, 3.10590F, 2.91638F, 2.73842F,
 698   2.57132F, 2.41442F, 2.26709F, 2.12875F,
 699   1.99885F, 1.87688F, 1.76236F, 1.65482F,
 700   1.55384F, 1.45902F, 1.36999F, 1.28640F,
 701   1.20790F, 1.13419F, 1.06499F, 1.F
 702 };
 703
 704 void _vp_remove_floor(vorbis_look_psy *p,
 705                       float *mdct,
 706                       int *codedflr,
 707                       float *residue,
 708                       int sliding_lowpass){
 709
 710   int i,n=p->n;
 711
 712   if(sliding_lowpass>n)sliding_lowpass=n;
 713
 714   for(i=0;i<sliding_lowpass;i++){
 715     residue[i]=
 716       mdct[i]*FLOOR1_fromdB_INV_LOOKUP[codedflr[i]];
 717   }
 718
 719   for(;i<n;i++)
 720     residue[i]=0.;
 721 }
 722
 723 void _vp_noisemask(vorbis_look_psy *p,
 724                    float *logmdct,
 725                    float *logmask){
 726
 727   int i,n=p->n;
 728   float *work=alloca(n*sizeof(*work));
 729
 730   bark_noise_hybridmp(n,p->bark,logmdct,logmask,
 731                       140.,-1);
 732
 733   for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
 734
 735   bark_noise_hybridmp(n,p->bark,work,logmask,0.,
 736                       p->vi->noisewindowfixed);
 737
 738   for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
 739
 740   /* work[i] holds the median line (.5), logmask holds the upper
 741      envelope line (1.) */
 742
 743   for(i=0;i<n;i++){
 744     int dB=logmask[i]+.5;
 745     if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
 746     logmask[i]= work[i]+p->vi->noisecompand[dB];
 747   }
 748 }
 749
 750 void _vp_tonemask(vorbis_look_psy *p,
 751                   float *logfft,
 752                   float *logmask,
 753                   float global_specmax,
 754                   float local_specmax){
 755
 756   int i,n=p->n;
 757
 758   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
 759   float att=local_specmax+p->vi->ath_adjatt;
 760   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
 761
 762   /* set the ATH (floating below localmax, not global max by a
 763      specified att) */
 764   if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
 765
 766   for(i=0;i<n;i++)
 767     logmask[i]=p->ath[i]+att;
 768
 769   /* tone masking */
 770   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
 771   max_seeds(p,seed,logmask);
 772
 773 }
 774
 775 void _vp_offset_and_mix(vorbis_look_psy *p,
 776                         float *noise,
 777                         float *tone,
 778                         int offset_select,
 779                         float *logmask){
 780   int i,n=p->n;
 781   float toneatt=p->vi->tone_masteratt[offset_select];
 782
 783   for(i=0;i<n;i++){
 784     float val= noise[i]+p->noiseoffset[offset_select][i];
 785     if(val>p->vi->noisemaxsupp)val=p->vi->noisemaxsupp;
 786     logmask[i]=max(val,tone[i]+toneatt);
 787   }
 788 }
 789
 790 float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
 791   vorbis_info *vi=vd->vi;
 792   codec_setup_info *ci=vi->codec_setup;
 793   vorbis_info_psy_global *gi=&ci->psy_g_param;
 794
 795   int n=ci->blocksizes[vd->W]/2;
 796   float secs=(float)n/vi->rate;
 797
 798   amp+=secs*gi->ampmax_att_per_sec;
 799   if(amp<-9999)amp=-9999;
 800   return(amp);
 801 }
 802
 803 static void couple_lossless(float A, float B,
 804                             float *qA, float *qB){
 805   int test1=fabs(*qA)>fabs(*qB);
 806   test1-= fabs(*qA)<fabs(*qB);
 807
 808   if(!test1)test1=((fabs(A)>fabs(B))<<1)-1;
 809   if(test1==1){
 810     *qB=(*qA>0.f?*qA-*qB:*qB-*qA);
 811   }else{
 812     float temp=*qB;
 813     *qB=(*qB>0.f?*qA-*qB:*qB-*qA);
 814     *qA=temp;
 815   }
 816
 817   if(*qB>fabs(*qA)*1.9999f){
 818     *qB= -fabs(*qA)*2.f;
 819     *qA= -*qA;
 820   }
 821 }
 822
 823 static float hypot_lookup[32]={
 824   -0.009935, -0.011245, -0.012726, -0.014397,
 825   -0.016282, -0.018407, -0.020800, -0.023494,
 826   -0.026522, -0.029923, -0.033737, -0.038010,
 827   -0.042787, -0.048121, -0.054064, -0.060671,
 828   -0.068000, -0.076109, -0.085054, -0.094892,
 829   -0.105675, -0.117451, -0.130260, -0.144134,
 830   -0.159093, -0.175146, -0.192286, -0.210490,
 831   -0.229718, -0.249913, -0.271001, -0.292893};
 832
 833 static void precomputed_couple_point(float premag,
 834                                      int floorA,int floorB,
 835                                      float *mag, float *ang){
 836
 837   int test=(floorA>floorB)-1;
 838   int offset=31-abs(floorA-floorB);
 839   float floormag=hypot_lookup[((offset<0)-1)&offset]+1.f;
 840
 841   floormag*=FLOOR1_fromdB_INV_LOOKUP[(floorB&test)|(floorA&(~test))];
 842
 843   *mag=premag*floormag;
 844
 845   *ang=0.f;
 846 }
 847
 848 /* just like below, this is currently set up to only do
 849    single-step-depth coupling.  Otherwise, we'd have to do more
 850    copying (which will be inevitable later) */
 851
 852 /* doing the real circular magnitude calculation is audibly superior
 853    to (A+B)/sqrt(2) */
 854 static float cardoid_hypot(float a, float b){
 855   if(a>0.){
 856     if(b>0.)return sqrt(a*a+b*b);
 857     if(a>-b)return sqrt(a*a-b*b);
 858     return -sqrt(b*b-a*a);
 859   }
 860   if(b<0.)return -sqrt(a*a+b*b);
 861   if(-a>b)return -sqrt(a*a-b*b);
 862   return sqrt(b*b-a*a);
 863 }
 864
 865 float **_vp_quantize_couple_memo(vorbis_block *vb,
 866                                  vorbis_look_psy *p,
 867                                  vorbis_info_mapping0 *vi,
 868                                  float **mdct){
 869
 870   int i,j,n=p->n;
 871   float **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 872
 873   for(i=0;i<vi->coupling_steps;i++){
 874     float *mdctM=mdct[vi->coupling_mag[i]];
 875     float *mdctA=mdct[vi->coupling_ang[i]];
 876     ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 877     for(j=0;j<n;j++)
 878       ret[i][j]=cardoid_hypot(mdctM[j],mdctA[j]);
 879   }
 880
 881   return(ret);
 882 }
 883
 884 /* this is for per-channel noise normalization */
 885 static int apsort(const void *a, const void *b){
 886   if(fabs(**(float **)a)>fabs(**(float **)b))return -1;
 887   return 1;
 888 }
 889
 890 int **_vp_quantize_couple_sort(vorbis_block *vb,
 891                                  vorbis_look_psy *p,
 892                                  vorbis_info_mapping0 *vi,
 893                                  float **mags){
 894
 895
 896   if(p->vi->normal_point_p){
 897     int i,j,k,n=p->n;
 898     int **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 899     int partition=p->vi->normal_partition;
 900     float **work=alloca(sizeof(*work)*partition);
 901
 902     for(i=0;i<vi->coupling_steps;i++){
 903       ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 904
 905       for(j=0;j<n;j+=partition){
 906         for(k=0;k<partition;k++)work[k]=mags[i]+k+j;
 907         qsort(work,partition,sizeof(*work),apsort);
 908         for(k=0;k<partition;k++)ret[i][k+j]=work[k]-mags[i];
 909       }
 910     }
 911     return(ret);
 912   }
 913   return(NULL);
 914 }
 915
 916 void _vp_noise_normalize_sort(vorbis_look_psy *p,
 917                               float *magnitudes,int *sortedindex){
 918   int i,j,n=p->n;
 919   vorbis_info_psy *vi=p->vi;
 920   int partition=vi->normal_partition;
 921   float **work=alloca(sizeof(*work)*partition);
 922   int start=vi->normal_start;
 923
 924   for(j=start;j<n;j+=partition){
 925     if(j+partition>n)partition=n-j;
 926     for(i=0;i<partition;i++)work[i]=magnitudes+i+j;
 927     qsort(work,partition,sizeof(*work),apsort);
 928     for(i=0;i<partition;i++){
 929       sortedindex[i+j-start]=work[i]-magnitudes;
 930     }
 931   }
 932 }
 933
 934 void _vp_noise_normalize(vorbis_look_psy *p,
 935                          float *in,float *out,int *sortedindex){
 936   int flag=0,i,j=0,n=p->n;
 937   vorbis_info_psy *vi=p->vi;
 938   int partition=vi->normal_partition;
 939   int start=vi->normal_start;
 940
 941   if(vi->normal_channel_p){
 942     for(;j<start;j++)
 943       out[j]=rint(in[j]);
 944
 945     for(;j+partition<=n;j+=partition){
 946       float acc=0.;
 947       int k;
 948
 949       for(i=j;i<j+partition;i++)
 950         acc+=in[i]*in[i];
 951
 952       for(i=0;i<partition;i++){
 953         k=sortedindex[i+j-start];
 954
 955         if(in[k]*in[k]>=.25f){
 956           out[k]=rint(in[k]);
 957           acc-=in[k]*in[k];
 958           flag=1;
 959         }else{
 960           if(acc<vi->normal_thresh)break;
 961           out[k]=unitnorm(in[k]);
 962           acc-=1.;
 963         }
 964       }
 965
 966       //if(!flag && i<3)i=0;
 967       for(;i<partition;i++){
 968         k=sortedindex[i+j-start];
 969         out[k]=0.;
 970       }
 971     }
 972   }
 973
 974   for(;j<n;j++)
 975     out[j]=rint(in[j]);
 976
 977 }
 978
 979 void _vp_couple(int blobno,
 980                 vorbis_info_psy_global *g,
 981                 vorbis_look_psy *p,
 982                 vorbis_info_mapping0 *vi,
 983                 float **res,
 984                 float **mag_memo,
 985                 int   **mag_sort,
 986                 int   **ifloor,
 987                 int   *nonzero){
 988
 989   int i,j,k,n=p->n;
 990
 991   /* perform any requested channel coupling */
 992   /* point stereo can only be used in a first stage (in this encoder)
 993      because of the dependency on floor lookups */
 994   for(i=0;i<vi->coupling_steps;i++){
 995
 996     /* once we're doing multistage coupling in which a channel goes
 997        through more than one coupling step, the floor vector
 998        magnitudes will also have to be recalculated an propogated
 999        along with PCM.  Right now, we're not (that will wait until 5.1
1000        most likely), so the code isn't here yet. The memory management
1001        here is all assuming single depth couplings anyway. */
1002
1003     /* make sure coupling a zero and a nonzero channel results in two
1004        nonzero channels. */
1005     if(nonzero[vi->coupling_mag[i]] ||
1006        nonzero[vi->coupling_ang[i]]){
1007
1008
1009       float *rM=res[vi->coupling_mag[i]];
1010       float *rA=res[vi->coupling_ang[i]];
1011       float *qM=rM+n;
1012       float *qA=rA+n;
1013       int *floorM=ifloor[vi->coupling_mag[i]];
1014       int *floorA=ifloor[vi->coupling_ang[i]];
1015       int limit=g->coupling_pointlimit[p->vi->blockflag][blobno];
1016       float prepoint=stereo_threshholds[g->coupling_prepointamp[blobno]];
1017       float postpoint=stereo_threshholds[g->coupling_postpointamp[blobno]];
1018       int partition=(p->vi->normal_point_p?p->vi->normal_partition:p->n);
1019
1020       nonzero[vi->coupling_mag[i]]=1;
1021       nonzero[vi->coupling_ang[i]]=1;
1022
1023       for(j=0;j<p->n;j+=partition){
1024         float acc=0.f;
1025
1026         for(k=0;k<partition;k++){
1027           int l=k+j;
1028           if((l>=limit && fabs(rM[l])<postpoint && fabs(rA[l])<postpoint) ||
1029              (fabs(rM[l])<prepoint && fabs(rA[l])<prepoint)){
1030             precomputed_couple_point(mag_memo[i][l],
1031                                      floorM[l],floorA[l],
1032                                      qM+l,qA+l);
1033             if(rint(qM[l])==0.f)acc+=qM[l]*qM[l];
1034           }else{
1035             couple_lossless(rM[l],rA[l],qM+l,qA+l);
1036           }
1037         }
1038
1039         if(p->vi->normal_point_p)
1040           for(k=0;k<partition && acc>=p->vi->normal_thresh;k++){
1041             int l=mag_sort[i][j+k];
1042             if(l>=limit && rint(qM[l])==0.f){
1043               qM[l]=unitnorm(qM[l]);
1044               acc-=1.f;
1045             }
1046           }
1047       }
1048     }
1049   }
1050 }
1051