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.69 2002/06/29 13:13:54 msmith 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     if(p->noiseoffset){
 340       for(i=0;i<P_NOISECURVES;i++){
 341         _ogg_free(p->noiseoffset[i]);
 342       }
 343       _ogg_free(p->noiseoffset);
 344     }
 345     memset(p,0,sizeof(*p));
 346   }
 347 }
 348
 349 /* octave/(8*eighth_octave_lines) x scale and dB y scale */
 350 static void seed_curve(float *seed,
 351                        const float **curves,
 352                        float amp,
 353                        int oc, int n,
 354                        int linesper,float dBoffset){
 355   int i,post1;
 356   int seedptr;
 357   const float *posts,*curve;
 358
 359   int choice=(int)((amp+dBoffset-P_LEVEL_0)*.1f);
 360   choice=max(choice,0);
 361   choice=min(choice,P_LEVELS-1);
 362   posts=curves[choice];
 363   curve=posts+2;
 364   post1=(int)posts[1];
 365   seedptr=oc+(posts[0]-EHMER_OFFSET)*linesper-(linesper>>1);
 366
 367   for(i=posts[0];i<post1;i++){
 368     if(seedptr>0){
 369       float lin=amp+curve[i];
 370       if(seed[seedptr]<lin)seed[seedptr]=lin;
 371     }
 372     seedptr+=linesper;
 373     if(seedptr>=n)break;
 374   }
 375 }
 376
 377 static void seed_loop(vorbis_look_psy *p,
 378                       const float ***curves,
 379                       const float *f,
 380                       const float *flr,
 381                       float *seed,
 382                       float specmax){
 383   vorbis_info_psy *vi=p->vi;
 384   long n=p->n,i;
 385   float dBoffset=vi->max_curve_dB-specmax;
 386
 387   /* prime the working vector with peak values */
 388
 389   for(i=0;i<n;i++){
 390     float max=f[i];
 391     long oc=p->octave[i];
 392     while(i+1<n && p->octave[i+1]==oc){
 393       i++;
 394       if(f[i]>max)max=f[i];
 395     }
 396
 397     if(max+6.f>flr[i]){
 398       oc=oc>>p->shiftoc;
 399       if(oc>=P_BANDS)oc=P_BANDS-1;
 400       if(oc<0)oc=0;
 401
 402       seed_curve(seed,
 403                  curves[oc],
 404                  max,
 405                  p->octave[i]-p->firstoc,
 406                  p->total_octave_lines,
 407                  p->eighth_octave_lines,
 408                  dBoffset);
 409     }
 410   }
 411 }
 412
 413 static void seed_chase(float *seeds, int linesper, long n){
 414   long  *posstack=alloca(n*sizeof(*posstack));
 415   float *ampstack=alloca(n*sizeof(*ampstack));
 416   long   stack=0;
 417   long   pos=0;
 418   long   i;
 419
 420   for(i=0;i<n;i++){
 421     if(stack<2){
 422       posstack[stack]=i;
 423       ampstack[stack++]=seeds[i];
 424     }else{
 425       while(1){
 426         if(seeds[i]<ampstack[stack-1]){
 427           posstack[stack]=i;
 428           ampstack[stack++]=seeds[i];
 429           break;
 430         }else{
 431           if(i<posstack[stack-1]+linesper){
 432             if(stack>1 && ampstack[stack-1]<=ampstack[stack-2] &&
 433                i<posstack[stack-2]+linesper){
 434               /* we completely overlap, making stack-1 irrelevant.  pop it */
 435               stack--;
 436               continue;
 437             }
 438           }
 439           posstack[stack]=i;
 440           ampstack[stack++]=seeds[i];
 441           break;
 442
 443         }
 444       }
 445     }
 446   }
 447
 448   /* the stack now contains only the positions that are relevant. Scan
 449      'em straight through */
 450
 451   for(i=0;i<stack;i++){
 452     long endpos;
 453     if(i<stack-1 && ampstack[i+1]>ampstack[i]){
 454       endpos=posstack[i+1];
 455     }else{
 456       endpos=posstack[i]+linesper+1; /* +1 is important, else bin 0 is
 457                                         discarded in short frames */
 458     }
 459     if(endpos>n)endpos=n;
 460     for(;pos<endpos;pos++)
 461       seeds[pos]=ampstack[i];
 462   }
 463
 464   /* there.  Linear time.  I now remember this was on a problem set I
 465      had in Grad Skool... I didn't solve it at the time ;-) */
 466
 467 }
 468
 469 /* bleaugh, this is more complicated than it needs to be */
 470 static void max_seeds(vorbis_look_psy *p,
 471                       float *seed,
 472                       float *flr){
 473   long   n=p->total_octave_lines;
 474   int    linesper=p->eighth_octave_lines;
 475   long   linpos=0;
 476   long   pos;
 477
 478   seed_chase(seed,linesper,n); /* for masking */
 479
 480   pos=p->octave[0]-p->firstoc-(linesper>>1);
 481   while(linpos+1<p->n){
 482     float minV=seed[pos];
 483     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
 484     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
 485     while(pos+1<=end){
 486       pos++;
 487       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
 488         minV=seed[pos];
 489     }
 490
 491     /* seed scale is log.  Floor is linear.  Map back to it */
 492     end=pos+p->firstoc;
 493     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
 494       if(flr[linpos]<minV)flr[linpos]=minV;
 495   }
 496
 497   {
 498     float minV=seed[p->total_octave_lines-1];
 499     for(;linpos<p->n;linpos++)
 500       if(flr[linpos]<minV)flr[linpos]=minV;
 501   }
 502
 503 }
 504
 505 static void bark_noise_hybridmp(int n,const long *b,
 506                                 const float *f,
 507                                 float *noise,
 508                                 const float offset,
 509                                 const int fixed){
 510
 511   float *N=alloca((n+1)*sizeof(*N));
 512   float *X=alloca((n+1)*sizeof(*N));
 513   float *XX=alloca((n+1)*sizeof(*N));
 514   float *Y=alloca((n+1)*sizeof(*N));
 515   float *XY=alloca((n+1)*sizeof(*N));
 516
 517   float tN, tX, tXX, tY, tXY;
 518   float fi;
 519   int i;
 520
 521   int lo, hi;
 522   float R, A, B, D;
 523
 524   tN = tX = tXX = tY = tXY = 0.f;
 525   for (i = 0, fi = 0.f; i < n; i++, fi += 1.f) {
 526     float w, x, y;
 527
 528     x = fi;
 529     y = f[i] + offset;
 530     if (y < 1.f) y = 1.f;
 531     w = y * y;
 532     N[i] = tN;
 533     X[i] = tX;
 534     XX[i] = tXX;
 535     Y[i] = tY;
 536     XY[i] = tXY;
 537     tN += w;
 538     tX += w * x;
 539     tXX += w * x * x;
 540     tY += w * y;
 541     tXY += w * x * y;
 542   }
 543   N[i] = tN;
 544   X[i] = tX;
 545   XX[i] = tXX;
 546   Y[i] = tY;
 547   XY[i] = tXY;
 548
 549   for (i = 0, fi = 0.f;; i++, fi += 1.f) {
 550
 551     lo = b[i] >> 16;
 552     if( lo>=0 ) break;
 553     hi = b[i] & 0xffff;
 554
 555     tN = N[hi] + N[-lo];
 556     tX = X[hi] - X[-lo];
 557     tXX = XX[hi] + XX[-lo];
 558     tY = Y[hi] + Y[-lo];
 559     tXY = XY[hi] - XY[-lo];
 560
 561     A = tY * tXX - tX * tXY;
 562     B = tN * tXY - tX * tY;
 563     D = tN * tXX - tX * tX;
 564     R = (A + fi * B) / D;
 565     if (R < 0.f)
 566       R = 0.f;
 567
 568     noise[i] = R - offset;
 569   }
 570
 571   for ( ; hi < n; i++, fi += 1.f) {
 572
 573     lo = b[i] >> 16;
 574     hi = b[i] & 0xffff;
 575
 576     tN = N[hi] - N[lo];
 577     tX = X[hi] - X[lo];
 578     tXX = XX[hi] - XX[lo];
 579     tY = Y[hi] - Y[lo];
 580     tXY = XY[hi] - XY[lo];
 581
 582     A = tY * tXX - tX * tXY;
 583     B = tN * tXY - tX * tY;
 584     D = tN * tXX - tX * tX;
 585     R = (A + fi * B) / D;
 586     if (R < 0.f) R = 0.f;
 587
 588     noise[i] = R - offset;
 589   }
 590   for ( ; i < n; i++, fi += 1.f) {
 591
 592     R = (A + fi * B) / D;
 593     if (R < 0.f) R = 0.f;
 594
 595     noise[i] = R - offset;
 596   }
 597
 598   if (fixed <= 0) return;
 599
 600   for (i = 0, fi = 0.f; i < (fixed + 1) / 2; i++, fi += 1.f) {
 601     hi = i + fixed / 2;
 602     lo = hi - fixed;
 603
 604     tN = N[hi] + N[-lo];
 605     tX = X[hi] - X[-lo];
 606     tXX = XX[hi] + XX[-lo];
 607     tY = Y[hi] + Y[-lo];
 608     tXY = XY[hi] - XY[-lo];
 609
 610
 611     A = tY * tXX - tX * tXY;
 612     B = tN * tXY - tX * tY;
 613     D = tN * tXX - tX * tX;
 614     R = (A + fi * B) / D;
 615
 616     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 617   }
 618   for ( ; hi < n; i++, fi += 1.f) {
 619
 620     hi = i + fixed / 2;
 621     lo = hi - fixed;
 622
 623     tN = N[hi] - N[lo];
 624     tX = X[hi] - X[lo];
 625     tXX = XX[hi] - XX[lo];
 626     tY = Y[hi] - Y[lo];
 627     tXY = XY[hi] - XY[lo];
 628
 629     A = tY * tXX - tX * tXY;
 630     B = tN * tXY - tX * tY;
 631     D = tN * tXX - tX * tX;
 632     R = (A + fi * B) / D;
 633
 634     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 635   }
 636   for ( ; i < n; i++, fi += 1.f) {
 637     R = (A + fi * B) / D;
 638     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 639   }
 640 }
 641
 642 static float FLOOR1_fromdB_INV_LOOKUP[256]={
 643   0.F, 8.81683e+06F, 8.27882e+06F, 7.77365e+06F,
 644   7.29930e+06F, 6.85389e+06F, 6.43567e+06F, 6.04296e+06F,
 645   5.67422e+06F, 5.32798e+06F, 5.00286e+06F, 4.69759e+06F,
 646   4.41094e+06F, 4.14178e+06F, 3.88905e+06F, 3.65174e+06F,
 647   3.42891e+06F, 3.21968e+06F, 3.02321e+06F, 2.83873e+06F,
 648   2.66551e+06F, 2.50286e+06F, 2.35014e+06F, 2.20673e+06F,
 649   2.07208e+06F, 1.94564e+06F, 1.82692e+06F, 1.71544e+06F,
 650   1.61076e+06F, 1.51247e+06F, 1.42018e+06F, 1.33352e+06F,
 651   1.25215e+06F, 1.17574e+06F, 1.10400e+06F, 1.03663e+06F,
 652   973377.F, 913981.F, 858210.F, 805842.F,
 653   756669.F, 710497.F, 667142.F, 626433.F,
 654   588208.F, 552316.F, 518613.F, 486967.F,
 655   457252.F, 429351.F, 403152.F, 378551.F,
 656   355452.F, 333762.F, 313396.F, 294273.F,
 657   276316.F, 259455.F, 243623.F, 228757.F,
 658   214798.F, 201691.F, 189384.F, 177828.F,
 659   166977.F, 156788.F, 147221.F, 138237.F,
 660   129802.F, 121881.F, 114444.F, 107461.F,
 661   100903.F, 94746.3F, 88964.9F, 83536.2F,
 662   78438.8F, 73652.5F, 69158.2F, 64938.1F,
 663   60975.6F, 57254.9F, 53761.2F, 50480.6F,
 664   47400.3F, 44507.9F, 41792.0F, 39241.9F,
 665   36847.3F, 34598.9F, 32487.7F, 30505.3F,
 666   28643.8F, 26896.0F, 25254.8F, 23713.7F,
 667   22266.7F, 20908.0F, 19632.2F, 18434.2F,
 668   17309.4F, 16253.1F, 15261.4F, 14330.1F,
 669   13455.7F, 12634.6F, 11863.7F, 11139.7F,
 670   10460.0F, 9821.72F, 9222.39F, 8659.64F,
 671   8131.23F, 7635.06F, 7169.17F, 6731.70F,
 672   6320.93F, 5935.23F, 5573.06F, 5232.99F,
 673   4913.67F, 4613.84F, 4332.30F, 4067.94F,
 674   3819.72F, 3586.64F, 3367.78F, 3162.28F,
 675   2969.31F, 2788.13F, 2617.99F, 2458.24F,
 676   2308.24F, 2167.39F, 2035.14F, 1910.95F,
 677   1794.35F, 1684.85F, 1582.04F, 1485.51F,
 678   1394.86F, 1309.75F, 1229.83F, 1154.78F,
 679   1084.32F, 1018.15F, 956.024F, 897.687F,
 680   842.910F, 791.475F, 743.179F, 697.830F,
 681   655.249F, 615.265F, 577.722F, 542.469F,
 682   509.367F, 478.286F, 449.101F, 421.696F,
 683   395.964F, 371.803F, 349.115F, 327.812F,
 684   307.809F, 289.026F, 271.390F, 254.830F,
 685   239.280F, 224.679F, 210.969F, 198.096F,
 686   186.008F, 174.658F, 164.000F, 153.993F,
 687   144.596F, 135.773F, 127.488F, 119.708F,
 688   112.404F, 105.545F, 99.1046F, 93.0572F,
 689   87.3788F, 82.0469F, 77.0404F, 72.3394F,
 690   67.9252F, 63.7804F, 59.8885F, 56.2341F,
 691   52.8027F, 49.5807F, 46.5553F, 43.7144F,
 692   41.0470F, 38.5423F, 36.1904F, 33.9821F,
 693   31.9085F, 29.9614F, 28.1332F, 26.4165F,
 694   24.8045F, 23.2910F, 21.8697F, 20.5352F,
 695   19.2822F, 18.1056F, 17.0008F, 15.9634F,
 696   14.9893F, 14.0746F, 13.2158F, 12.4094F,
 697   11.6522F, 10.9411F, 10.2735F, 9.64662F,
 698   9.05798F, 8.50526F, 7.98626F, 7.49894F,
 699   7.04135F, 6.61169F, 6.20824F, 5.82941F,
 700   5.47370F, 5.13970F, 4.82607F, 4.53158F,
 701   4.25507F, 3.99542F, 3.75162F, 3.52269F,
 702   3.30774F, 3.10590F, 2.91638F, 2.73842F,
 703   2.57132F, 2.41442F, 2.26709F, 2.12875F,
 704   1.99885F, 1.87688F, 1.76236F, 1.65482F,
 705   1.55384F, 1.45902F, 1.36999F, 1.28640F,
 706   1.20790F, 1.13419F, 1.06499F, 1.F
 707 };
 708
 709 void _vp_remove_floor(vorbis_look_psy *p,
 710                       float *mdct,
 711                       int *codedflr,
 712                       float *residue,
 713                       int sliding_lowpass){
 714
 715   int i,n=p->n;
 716
 717   if(sliding_lowpass>n)sliding_lowpass=n;
 718
 719   for(i=0;i<sliding_lowpass;i++){
 720     residue[i]=
 721       mdct[i]*FLOOR1_fromdB_INV_LOOKUP[codedflr[i]];
 722   }
 723
 724   for(;i<n;i++)
 725     residue[i]=0.;
 726 }
 727
 728 void _vp_noisemask(vorbis_look_psy *p,
 729                    float *logmdct,
 730                    float *logmask){
 731
 732   int i,n=p->n;
 733   float *work=alloca(n*sizeof(*work));
 734
 735   bark_noise_hybridmp(n,p->bark,logmdct,logmask,
 736                       140.,-1);
 737
 738   for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
 739
 740   bark_noise_hybridmp(n,p->bark,work,logmask,0.,
 741                       p->vi->noisewindowfixed);
 742
 743   for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
 744
 745   /* work[i] holds the median line (.5), logmask holds the upper
 746      envelope line (1.) */
 747
 748   for(i=0;i<n;i++){
 749     int dB=logmask[i]+.5;
 750     if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
 751     logmask[i]= work[i]+p->vi->noisecompand[dB];
 752   }
 753 }
 754
 755 void _vp_tonemask(vorbis_look_psy *p,
 756                   float *logfft,
 757                   float *logmask,
 758                   float global_specmax,
 759                   float local_specmax){
 760
 761   int i,n=p->n;
 762
 763   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
 764   float att=local_specmax+p->vi->ath_adjatt;
 765   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
 766
 767   /* set the ATH (floating below localmax, not global max by a
 768      specified att) */
 769   if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
 770
 771   for(i=0;i<n;i++)
 772     logmask[i]=p->ath[i]+att;
 773
 774   /* tone masking */
 775   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
 776   max_seeds(p,seed,logmask);
 777
 778 }
 779
 780 void _vp_offset_and_mix(vorbis_look_psy *p,
 781                         float *noise,
 782                         float *tone,
 783                         int offset_select,
 784                         float *logmask){
 785   int i,n=p->n;
 786   float toneatt=p->vi->tone_masteratt[offset_select];
 787
 788   for(i=0;i<n;i++){
 789     float val= noise[i]+p->noiseoffset[offset_select][i];
 790     if(val>p->vi->noisemaxsupp)val=p->vi->noisemaxsupp;
 791     logmask[i]=max(val,tone[i]+toneatt);
 792   }
 793 }
 794
 795 float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
 796   vorbis_info *vi=vd->vi;
 797   codec_setup_info *ci=vi->codec_setup;
 798   vorbis_info_psy_global *gi=&ci->psy_g_param;
 799
 800   int n=ci->blocksizes[vd->W]/2;
 801   float secs=(float)n/vi->rate;
 802
 803   amp+=secs*gi->ampmax_att_per_sec;
 804   if(amp<-9999)amp=-9999;
 805   return(amp);
 806 }
 807
 808 static void couple_lossless(float A, float B,
 809                             float *qA, float *qB){
 810   int test1=fabs(*qA)>fabs(*qB);
 811   test1-= fabs(*qA)<fabs(*qB);
 812
 813   if(!test1)test1=((fabs(A)>fabs(B))<<1)-1;
 814   if(test1==1){
 815     *qB=(*qA>0.f?*qA-*qB:*qB-*qA);
 816   }else{
 817     float temp=*qB;
 818     *qB=(*qB>0.f?*qA-*qB:*qB-*qA);
 819     *qA=temp;
 820   }
 821
 822   if(*qB>fabs(*qA)*1.9999f){
 823     *qB= -fabs(*qA)*2.f;
 824     *qA= -*qA;
 825   }
 826 }
 827
 828 static float hypot_lookup[32]={
 829   -0.009935, -0.011245, -0.012726, -0.014397,
 830   -0.016282, -0.018407, -0.020800, -0.023494,
 831   -0.026522, -0.029923, -0.033737, -0.038010,
 832   -0.042787, -0.048121, -0.054064, -0.060671,
 833   -0.068000, -0.076109, -0.085054, -0.094892,
 834   -0.105675, -0.117451, -0.130260, -0.144134,
 835   -0.159093, -0.175146, -0.192286, -0.210490,
 836   -0.229718, -0.249913, -0.271001, -0.292893};
 837
 838 static void precomputed_couple_point(float premag,
 839                                      int floorA,int floorB,
 840                                      float *mag, float *ang){
 841
 842   int test=(floorA>floorB)-1;
 843   int offset=31-abs(floorA-floorB);
 844   float floormag=hypot_lookup[((offset<0)-1)&offset]+1.f;
 845
 846   floormag*=FLOOR1_fromdB_INV_LOOKUP[(floorB&test)|(floorA&(~test))];
 847
 848   *mag=premag*floormag;
 849
 850   *ang=0.f;
 851 }
 852
 853 /* just like below, this is currently set up to only do
 854    single-step-depth coupling.  Otherwise, we'd have to do more
 855    copying (which will be inevitable later) */
 856
 857 /* doing the real circular magnitude calculation is audibly superior
 858    to (A+B)/sqrt(2) */
 859 static float cardoid_hypot(float a, float b){
 860   if(a>0.){
 861     if(b>0.)return sqrt(a*a+b*b);
 862     if(a>-b)return sqrt(a*a-b*b);
 863     return -sqrt(b*b-a*a);
 864   }
 865   if(b<0.)return -sqrt(a*a+b*b);
 866   if(-a>b)return -sqrt(a*a-b*b);
 867   return sqrt(b*b-a*a);
 868 }
 869
 870 float **_vp_quantize_couple_memo(vorbis_block *vb,
 871                                  vorbis_look_psy *p,
 872                                  vorbis_info_mapping0 *vi,
 873                                  float **mdct){
 874
 875   int i,j,n=p->n;
 876   float **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 877
 878   for(i=0;i<vi->coupling_steps;i++){
 879     float *mdctM=mdct[vi->coupling_mag[i]];
 880     float *mdctA=mdct[vi->coupling_ang[i]];
 881     ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 882     for(j=0;j<n;j++)
 883       ret[i][j]=cardoid_hypot(mdctM[j],mdctA[j]);
 884   }
 885
 886   return(ret);
 887 }
 888
 889 /* this is for per-channel noise normalization */
 890 static int apsort(const void *a, const void *b){
 891   if(fabs(**(float **)a)>fabs(**(float **)b))return -1;
 892   return 1;
 893 }
 894
 895 int **_vp_quantize_couple_sort(vorbis_block *vb,
 896                                  vorbis_look_psy *p,
 897                                  vorbis_info_mapping0 *vi,
 898                                  float **mags){
 899
 900
 901   if(p->vi->normal_point_p){
 902     int i,j,k,n=p->n;
 903     int **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 904     int partition=p->vi->normal_partition;
 905     float **work=alloca(sizeof(*work)*partition);
 906
 907     for(i=0;i<vi->coupling_steps;i++){
 908       ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 909
 910       for(j=0;j<n;j+=partition){
 911         for(k=0;k<partition;k++)work[k]=mags[i]+k+j;
 912         qsort(work,partition,sizeof(*work),apsort);
 913         for(k=0;k<partition;k++)ret[i][k+j]=work[k]-mags[i];
 914       }
 915     }
 916     return(ret);
 917   }
 918   return(NULL);
 919 }
 920
 921 void _vp_noise_normalize_sort(vorbis_look_psy *p,
 922                               float *magnitudes,int *sortedindex){
 923   int i,j,n=p->n;
 924   vorbis_info_psy *vi=p->vi;
 925   int partition=vi->normal_partition;
 926   float **work=alloca(sizeof(*work)*partition);
 927   int start=vi->normal_start;
 928
 929   for(j=start;j<n;j+=partition){
 930     if(j+partition>n)partition=n-j;
 931     for(i=0;i<partition;i++)work[i]=magnitudes+i+j;
 932     qsort(work,partition,sizeof(*work),apsort);
 933     for(i=0;i<partition;i++){
 934       sortedindex[i+j-start]=work[i]-magnitudes;
 935     }
 936   }
 937 }
 938
 939 void _vp_noise_normalize(vorbis_look_psy *p,
 940                          float *in,float *out,int *sortedindex){
 941   int flag=0,i,j=0,n=p->n;
 942   vorbis_info_psy *vi=p->vi;
 943   int partition=vi->normal_partition;
 944   int start=vi->normal_start;
 945
 946   if(vi->normal_channel_p){
 947     for(;j<start;j++)
 948       out[j]=rint(in[j]);
 949
 950     for(;j+partition<=n;j+=partition){
 951       float acc=0.;
 952       int k;
 953
 954       for(i=j;i<j+partition;i++)
 955         acc+=in[i]*in[i];
 956
 957       for(i=0;i<partition;i++){
 958         k=sortedindex[i+j-start];
 959
 960         if(in[k]*in[k]>=.25f){
 961           out[k]=rint(in[k]);
 962           acc-=in[k]*in[k];
 963           flag=1;
 964         }else{
 965           if(acc<vi->normal_thresh)break;
 966           out[k]=unitnorm(in[k]);
 967           acc-=1.;
 968         }
 969       }
 970
 971       //if(!flag && i<3)i=0;
 972       for(;i<partition;i++){
 973         k=sortedindex[i+j-start];
 974         out[k]=0.;
 975       }
 976     }
 977   }
 978
 979   for(;j<n;j++)
 980     out[j]=rint(in[j]);
 981
 982 }
 983
 984 void _vp_couple(int blobno,
 985                 vorbis_info_psy_global *g,
 986                 vorbis_look_psy *p,
 987                 vorbis_info_mapping0 *vi,
 988                 float **res,
 989                 float **mag_memo,
 990                 int   **mag_sort,
 991                 int   **ifloor,
 992                 int   *nonzero){
 993
 994   int i,j,k,n=p->n;
 995
 996   /* perform any requested channel coupling */
 997   /* point stereo can only be used in a first stage (in this encoder)
 998      because of the dependency on floor lookups */
 999   for(i=0;i<vi->coupling_steps;i++){
1000
1001     /* once we're doing multistage coupling in which a channel goes
1002        through more than one coupling step, the floor vector
1003        magnitudes will also have to be recalculated an propogated
1004        along with PCM.  Right now, we're not (that will wait until 5.1
1005        most likely), so the code isn't here yet. The memory management
1006        here is all assuming single depth couplings anyway. */
1007
1008     /* make sure coupling a zero and a nonzero channel results in two
1009        nonzero channels. */
1010     if(nonzero[vi->coupling_mag[i]] ||
1011        nonzero[vi->coupling_ang[i]]){
1012
1013
1014       float *rM=res[vi->coupling_mag[i]];
1015       float *rA=res[vi->coupling_ang[i]];
1016       float *qM=rM+n;
1017       float *qA=rA+n;
1018       int *floorM=ifloor[vi->coupling_mag[i]];
1019       int *floorA=ifloor[vi->coupling_ang[i]];
1020       int limit=g->coupling_pointlimit[p->vi->blockflag][blobno];
1021       float prepoint=stereo_threshholds[g->coupling_prepointamp[blobno]];
1022       float postpoint=stereo_threshholds[g->coupling_postpointamp[blobno]];
1023       int partition=(p->vi->normal_point_p?p->vi->normal_partition:p->n);
1024
1025       nonzero[vi->coupling_mag[i]]=1;
1026       nonzero[vi->coupling_ang[i]]=1;
1027
1028       for(j=0;j<p->n;j+=partition){
1029         float acc=0.f;
1030
1031         for(k=0;k<partition;k++){
1032           int l=k+j;
1033           if((l>=limit && fabs(rM[l])<postpoint && fabs(rA[l])<postpoint) ||
1034              (fabs(rM[l])<prepoint && fabs(rA[l])<prepoint)){
1035             precomputed_couple_point(mag_memo[i][l],
1036                                      floorM[l],floorA[l],
1037                                      qM+l,qA+l);
1038             if(rint(qM[l])==0.f)acc+=qM[l]*qM[l];
1039           }else{
1040             couple_lossless(rM[l],rA[l],qM+l,qA+l);
1041           }
1042         }
1043
1044         if(p->vi->normal_point_p)
1045           for(k=0;k<partition && acc>=p->vi->normal_thresh;k++){
1046             int l=mag_sort[i][j+k];
1047             if(l>=limit && rint(qM[l])==0.f){
1048               qM[l]=unitnorm(qM[l]);
1049               acc-=1.f;
1050             }
1051           }
1052       }
1053     }
1054   }
1055 }
1056