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