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-2002             *
   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.75 2002/07/30 09:25:12 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       /* 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*.5/n)*(1<<(p->shiftoc+1))-gi->eighth_octave_lines;
 276   maxoc=toOC((n+.25f)*rate*.5/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 #include<stdio.h>
 498 static void max_seeds(vorbis_look_psy *p,
 499                       float *seed,
 500                       float *flr){
 501   long   n=p->total_octave_lines;
 502   int    linesper=p->eighth_octave_lines;
 503   long   linpos=0;
 504   long   pos;
 505
 506   seed_chase(seed,linesper,n); /* for masking */
 507
 508   pos=p->octave[0]-p->firstoc-(linesper>>1);
 509
 510   while(linpos+1<p->n){
 511     float minV=seed[pos];
 512     long end=((p->octave[linpos]+p->octave[linpos+1])>>1)-p->firstoc;
 513     if(minV>p->vi->tone_abs_limit)minV=p->vi->tone_abs_limit;
 514     while(pos+1<=end){
 515       pos++;
 516       if((seed[pos]>NEGINF && seed[pos]<minV) || minV==NEGINF)
 517         minV=seed[pos];
 518     }
 519
 520     end=pos+p->firstoc;
 521     for(;linpos<p->n && p->octave[linpos]<=end;linpos++)
 522       if(flr[linpos]<minV)flr[linpos]=minV;
 523   }
 524
 525   {
 526     float minV=seed[p->total_octave_lines-1];
 527     for(;linpos<p->n;linpos++)
 528       if(flr[linpos]<minV)flr[linpos]=minV;
 529   }
 530
 531 }
 532
 533 static void bark_noise_hybridmp(int n,const long *b,
 534                                 const float *f,
 535                                 float *noise,
 536                                 const float offset,
 537                                 const int fixed){
 538
 539   float *N=alloca((n+1)*sizeof(*N));
 540   float *X=alloca((n+1)*sizeof(*N));
 541   float *XX=alloca((n+1)*sizeof(*N));
 542   float *Y=alloca((n+1)*sizeof(*N));
 543   float *XY=alloca((n+1)*sizeof(*N));
 544
 545   float tN, tX, tXX, tY, tXY;
 546   float fi;
 547   int i;
 548
 549   int lo, hi;
 550   float R, A, B, D;
 551
 552   tN = tX = tXX = tY = tXY = 0.f;
 553   for (i = 0, fi = 0.f; i < n; i++, fi += 1.f) {
 554     float w, x, y;
 555
 556     x = fi;
 557     y = f[i] + offset;
 558     if (y < 1.f) y = 1.f;
 559     w = y * y;
 560     N[i] = tN;
 561     X[i] = tX;
 562     XX[i] = tXX;
 563     Y[i] = tY;
 564     XY[i] = tXY;
 565     tN += w;
 566     tX += w * x;
 567     tXX += w * x * x;
 568     tY += w * y;
 569     tXY += w * x * y;
 570   }
 571   N[i] = tN;
 572   X[i] = tX;
 573   XX[i] = tXX;
 574   Y[i] = tY;
 575   XY[i] = tXY;
 576
 577   for (i = 0, fi = 0.f;; i++, fi += 1.f) {
 578
 579     lo = b[i] >> 16;
 580     if( lo>=0 ) break;
 581     hi = b[i] & 0xffff;
 582
 583     tN = N[hi] + N[-lo];
 584     tX = X[hi] - X[-lo];
 585     tXX = XX[hi] + XX[-lo];
 586     tY = Y[hi] + Y[-lo];
 587     tXY = XY[hi] - XY[-lo];
 588
 589     A = tY * tXX - tX * tXY;
 590     B = tN * tXY - tX * tY;
 591     D = tN * tXX - tX * tX;
 592     R = (A + fi * B) / D;
 593     if (R < 0.f)
 594       R = 0.f;
 595
 596     noise[i] = R - offset;
 597   }
 598
 599   for ( ; hi < n; i++, fi += 1.f) {
 600
 601     lo = b[i] >> 16;
 602     hi = b[i] & 0xffff;
 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     A = tY * tXX - tX * tXY;
 611     B = tN * tXY - tX * tY;
 612     D = tN * tXX - tX * tX;
 613     R = (A + fi * B) / D;
 614     if (R < 0.f) R = 0.f;
 615
 616     noise[i] = R - offset;
 617   }
 618   for ( ; i < n; i++, fi += 1.f) {
 619
 620     R = (A + fi * B) / D;
 621     if (R < 0.f) R = 0.f;
 622
 623     noise[i] = R - offset;
 624   }
 625
 626   if (fixed <= 0) return;
 627
 628   for (i = 0, fi = 0.f; i < (fixed + 1) / 2; i++, fi += 1.f) {
 629     hi = i + fixed / 2;
 630     lo = hi - fixed;
 631
 632     tN = N[hi] + N[-lo];
 633     tX = X[hi] - X[-lo];
 634     tXX = XX[hi] + XX[-lo];
 635     tY = Y[hi] + Y[-lo];
 636     tXY = XY[hi] - XY[-lo];
 637
 638
 639     A = tY * tXX - tX * tXY;
 640     B = tN * tXY - tX * tY;
 641     D = tN * tXX - tX * tX;
 642     R = (A + fi * B) / D;
 643
 644     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 645   }
 646   for ( ; hi < n; i++, fi += 1.f) {
 647
 648     hi = i + fixed / 2;
 649     lo = hi - fixed;
 650
 651     tN = N[hi] - N[lo];
 652     tX = X[hi] - X[lo];
 653     tXX = XX[hi] - XX[lo];
 654     tY = Y[hi] - Y[lo];
 655     tXY = XY[hi] - XY[lo];
 656
 657     A = tY * tXX - tX * tXY;
 658     B = tN * tXY - tX * tY;
 659     D = tN * tXX - tX * tX;
 660     R = (A + fi * B) / D;
 661
 662     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 663   }
 664   for ( ; i < n; i++, fi += 1.f) {
 665     R = (A + fi * B) / D;
 666     if (R > 0.f && R - offset < noise[i]) noise[i] = R - offset;
 667   }
 668 }
 669
 670 static float FLOOR1_fromdB_INV_LOOKUP[256]={
 671   0.F, 8.81683e+06F, 8.27882e+06F, 7.77365e+06F,
 672   7.29930e+06F, 6.85389e+06F, 6.43567e+06F, 6.04296e+06F,
 673   5.67422e+06F, 5.32798e+06F, 5.00286e+06F, 4.69759e+06F,
 674   4.41094e+06F, 4.14178e+06F, 3.88905e+06F, 3.65174e+06F,
 675   3.42891e+06F, 3.21968e+06F, 3.02321e+06F, 2.83873e+06F,
 676   2.66551e+06F, 2.50286e+06F, 2.35014e+06F, 2.20673e+06F,
 677   2.07208e+06F, 1.94564e+06F, 1.82692e+06F, 1.71544e+06F,
 678   1.61076e+06F, 1.51247e+06F, 1.42018e+06F, 1.33352e+06F,
 679   1.25215e+06F, 1.17574e+06F, 1.10400e+06F, 1.03663e+06F,
 680   973377.F, 913981.F, 858210.F, 805842.F,
 681   756669.F, 710497.F, 667142.F, 626433.F,
 682   588208.F, 552316.F, 518613.F, 486967.F,
 683   457252.F, 429351.F, 403152.F, 378551.F,
 684   355452.F, 333762.F, 313396.F, 294273.F,
 685   276316.F, 259455.F, 243623.F, 228757.F,
 686   214798.F, 201691.F, 189384.F, 177828.F,
 687   166977.F, 156788.F, 147221.F, 138237.F,
 688   129802.F, 121881.F, 114444.F, 107461.F,
 689   100903.F, 94746.3F, 88964.9F, 83536.2F,
 690   78438.8F, 73652.5F, 69158.2F, 64938.1F,
 691   60975.6F, 57254.9F, 53761.2F, 50480.6F,
 692   47400.3F, 44507.9F, 41792.0F, 39241.9F,
 693   36847.3F, 34598.9F, 32487.7F, 30505.3F,
 694   28643.8F, 26896.0F, 25254.8F, 23713.7F,
 695   22266.7F, 20908.0F, 19632.2F, 18434.2F,
 696   17309.4F, 16253.1F, 15261.4F, 14330.1F,
 697   13455.7F, 12634.6F, 11863.7F, 11139.7F,
 698   10460.0F, 9821.72F, 9222.39F, 8659.64F,
 699   8131.23F, 7635.06F, 7169.17F, 6731.70F,
 700   6320.93F, 5935.23F, 5573.06F, 5232.99F,
 701   4913.67F, 4613.84F, 4332.30F, 4067.94F,
 702   3819.72F, 3586.64F, 3367.78F, 3162.28F,
 703   2969.31F, 2788.13F, 2617.99F, 2458.24F,
 704   2308.24F, 2167.39F, 2035.14F, 1910.95F,
 705   1794.35F, 1684.85F, 1582.04F, 1485.51F,
 706   1394.86F, 1309.75F, 1229.83F, 1154.78F,
 707   1084.32F, 1018.15F, 956.024F, 897.687F,
 708   842.910F, 791.475F, 743.179F, 697.830F,
 709   655.249F, 615.265F, 577.722F, 542.469F,
 710   509.367F, 478.286F, 449.101F, 421.696F,
 711   395.964F, 371.803F, 349.115F, 327.812F,
 712   307.809F, 289.026F, 271.390F, 254.830F,
 713   239.280F, 224.679F, 210.969F, 198.096F,
 714   186.008F, 174.658F, 164.000F, 153.993F,
 715   144.596F, 135.773F, 127.488F, 119.708F,
 716   112.404F, 105.545F, 99.1046F, 93.0572F,
 717   87.3788F, 82.0469F, 77.0404F, 72.3394F,
 718   67.9252F, 63.7804F, 59.8885F, 56.2341F,
 719   52.8027F, 49.5807F, 46.5553F, 43.7144F,
 720   41.0470F, 38.5423F, 36.1904F, 33.9821F,
 721   31.9085F, 29.9614F, 28.1332F, 26.4165F,
 722   24.8045F, 23.2910F, 21.8697F, 20.5352F,
 723   19.2822F, 18.1056F, 17.0008F, 15.9634F,
 724   14.9893F, 14.0746F, 13.2158F, 12.4094F,
 725   11.6522F, 10.9411F, 10.2735F, 9.64662F,
 726   9.05798F, 8.50526F, 7.98626F, 7.49894F,
 727   7.04135F, 6.61169F, 6.20824F, 5.82941F,
 728   5.47370F, 5.13970F, 4.82607F, 4.53158F,
 729   4.25507F, 3.99542F, 3.75162F, 3.52269F,
 730   3.30774F, 3.10590F, 2.91638F, 2.73842F,
 731   2.57132F, 2.41442F, 2.26709F, 2.12875F,
 732   1.99885F, 1.87688F, 1.76236F, 1.65482F,
 733   1.55384F, 1.45902F, 1.36999F, 1.28640F,
 734   1.20790F, 1.13419F, 1.06499F, 1.F
 735 };
 736
 737 void _vp_remove_floor(vorbis_look_psy *p,
 738                       float *mdct,
 739                       int *codedflr,
 740                       float *residue,
 741                       int sliding_lowpass){
 742
 743   int i,n=p->n;
 744
 745   if(sliding_lowpass>n)sliding_lowpass=n;
 746
 747   for(i=0;i<sliding_lowpass;i++){
 748     residue[i]=
 749       mdct[i]*FLOOR1_fromdB_INV_LOOKUP[codedflr[i]];
 750   }
 751
 752   for(;i<n;i++)
 753     residue[i]=0.;
 754 }
 755
 756 void _vp_noisemask(vorbis_look_psy *p,
 757                    float *logmdct,
 758                    float *logmask){
 759
 760   int i,n=p->n;
 761   float *work=alloca(n*sizeof(*work));
 762
 763   bark_noise_hybridmp(n,p->bark,logmdct,logmask,
 764                       140.,-1);
 765
 766   for(i=0;i<n;i++)work[i]=logmdct[i]-logmask[i];
 767
 768   bark_noise_hybridmp(n,p->bark,work,logmask,0.,
 769                       p->vi->noisewindowfixed);
 770
 771   for(i=0;i<n;i++)work[i]=logmdct[i]-work[i];
 772
 773 #if 0
 774   {
 775     static int seq=0;
 776
 777     float work2[n];
 778     for(i=0;i<n;i++){
 779       work2[i]=logmask[i]+work[i];
 780     }
 781
 782     if(seq&1)
 783       _analysis_output("medianR",seq/2,work,n,1,0,0);
 784     else
 785       _analysis_output("medianL",seq/2,work,n,1,0,0);
 786
 787     if(seq&1)
 788       _analysis_output("envelopeR",seq/2,work2,n,1,0,0);
 789     else
 790       _analysis_output("enveloperL",seq/2,work2,n,1,0,0);
 791     seq++;
 792   }
 793 #endif
 794
 795   for(i=0;i<n;i++){
 796     int dB=logmask[i]+.5;
 797     if(dB>=NOISE_COMPAND_LEVELS)dB=NOISE_COMPAND_LEVELS-1;
 798     logmask[i]= work[i]+p->vi->noisecompand[dB];
 799   }
 800
 801 }
 802
 803 void _vp_tonemask(vorbis_look_psy *p,
 804                   float *logfft,
 805                   float *logmask,
 806                   float global_specmax,
 807                   float local_specmax){
 808
 809   int i,n=p->n;
 810
 811   float *seed=alloca(sizeof(*seed)*p->total_octave_lines);
 812   float att=local_specmax+p->vi->ath_adjatt;
 813   for(i=0;i<p->total_octave_lines;i++)seed[i]=NEGINF;
 814
 815   /* set the ATH (floating below localmax, not global max by a
 816      specified att) */
 817   if(att<p->vi->ath_maxatt)att=p->vi->ath_maxatt;
 818
 819   for(i=0;i<n;i++)
 820     logmask[i]=p->ath[i]+att;
 821
 822   /* tone masking */
 823   seed_loop(p,(const float ***)p->tonecurves,logfft,logmask,seed,global_specmax);
 824   max_seeds(p,seed,logmask);
 825
 826 }
 827
 828 void _vp_offset_and_mix(vorbis_look_psy *p,
 829                         float *noise,
 830                         float *tone,
 831                         int offset_select,
 832                         float *logmask){
 833   int i,n=p->n;
 834   float toneatt=p->vi->tone_masteratt[offset_select];
 835
 836   for(i=0;i<n;i++){
 837     float val= noise[i]+p->noiseoffset[offset_select][i];
 838     if(val>p->vi->noisemaxsupp)val=p->vi->noisemaxsupp;
 839     logmask[i]=max(val,tone[i]+toneatt);
 840   }
 841 }
 842
 843 float _vp_ampmax_decay(float amp,vorbis_dsp_state *vd){
 844   vorbis_info *vi=vd->vi;
 845   codec_setup_info *ci=vi->codec_setup;
 846   vorbis_info_psy_global *gi=&ci->psy_g_param;
 847
 848   int n=ci->blocksizes[vd->W]/2;
 849   float secs=(float)n/vi->rate;
 850
 851   amp+=secs*gi->ampmax_att_per_sec;
 852   if(amp<-9999)amp=-9999;
 853   return(amp);
 854 }
 855
 856 static void couple_lossless(float A, float B,
 857                             float *qA, float *qB){
 858   int test1=fabs(*qA)>fabs(*qB);
 859   test1-= fabs(*qA)<fabs(*qB);
 860
 861   if(!test1)test1=((fabs(A)>fabs(B))<<1)-1;
 862   if(test1==1){
 863     *qB=(*qA>0.f?*qA-*qB:*qB-*qA);
 864   }else{
 865     float temp=*qB;
 866     *qB=(*qB>0.f?*qA-*qB:*qB-*qA);
 867     *qA=temp;
 868   }
 869
 870   if(*qB>fabs(*qA)*1.9999f){
 871     *qB= -fabs(*qA)*2.f;
 872     *qA= -*qA;
 873   }
 874 }
 875
 876 static float hypot_lookup[32]={
 877   -0.009935, -0.011245, -0.012726, -0.014397,
 878   -0.016282, -0.018407, -0.020800, -0.023494,
 879   -0.026522, -0.029923, -0.033737, -0.038010,
 880   -0.042787, -0.048121, -0.054064, -0.060671,
 881   -0.068000, -0.076109, -0.085054, -0.094892,
 882   -0.105675, -0.117451, -0.130260, -0.144134,
 883   -0.159093, -0.175146, -0.192286, -0.210490,
 884   -0.229718, -0.249913, -0.271001, -0.292893};
 885
 886 static void precomputed_couple_point(float premag,
 887                                      int floorA,int floorB,
 888                                      float *mag, float *ang){
 889
 890   int test=(floorA>floorB)-1;
 891   int offset=31-abs(floorA-floorB);
 892   float floormag=hypot_lookup[((offset<0)-1)&offset]+1.f;
 893
 894   floormag*=FLOOR1_fromdB_INV_LOOKUP[(floorB&test)|(floorA&(~test))];
 895
 896   *mag=premag*floormag;
 897   *ang=0.f;
 898 }
 899
 900 /* just like below, this is currently set up to only do
 901    single-step-depth coupling.  Otherwise, we'd have to do more
 902    copying (which will be inevitable later) */
 903
 904 /* doing the real circular magnitude calculation is audibly superior
 905    to (A+B)/sqrt(2) */
 906 static float dipole_hypot(float a, float b){
 907   if(a>0.){
 908     if(b>0.)return sqrt(a*a+b*b);
 909     if(a>-b)return sqrt(a*a-b*b);
 910     return -sqrt(b*b-a*a);
 911   }
 912   if(b<0.)return -sqrt(a*a+b*b);
 913   if(-a>b)return -sqrt(a*a-b*b);
 914   return sqrt(b*b-a*a);
 915 }
 916 static float round_hypot(float a, float b){
 917   if(a>0.){
 918     if(b>0.)return sqrt(a*a+b*b);
 919     if(a>-b)return sqrt(a*a+b*b);
 920     return -sqrt(b*b+a*a);
 921   }
 922   if(b<0.)return -sqrt(a*a+b*b);
 923   if(-a>b)return -sqrt(a*a+b*b);
 924   return sqrt(b*b+a*a);
 925 }
 926
 927 /* revert to round hypot for now */
 928 float **_vp_quantize_couple_memo(vorbis_block *vb,
 929                                  vorbis_info_psy_global *g,
 930                                  vorbis_look_psy *p,
 931                                  vorbis_info_mapping0 *vi,
 932                                  float **mdct){
 933
 934   int i,j,n=p->n;
 935   float **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 936   int limit=g->coupling_pointlimit[p->vi->blockflag][PACKETBLOBS/2];
 937
 938   for(i=0;i<vi->coupling_steps;i++){
 939     float *mdctM=mdct[vi->coupling_mag[i]];
 940     float *mdctA=mdct[vi->coupling_ang[i]];
 941     ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 942     for(j=0;j<limit;j++)
 943       ret[i][j]=dipole_hypot(mdctM[j],mdctA[j]);
 944     for(;j<n;j++)
 945       ret[i][j]=round_hypot(mdctM[j],mdctA[j]);
 946   }
 947
 948   return(ret);
 949 }
 950
 951 /* this is for per-channel noise normalization */
 952 static int apsort(const void *a, const void *b){
 953   float f1=fabsf(**(float**)a);
 954   float f2=fabsf(**(float**)b);
 955   if(f1>f2)return -1;
 956   else if(f1==f2)return 0;
 957   else return 1;
 958 }
 959
 960 int **_vp_quantize_couple_sort(vorbis_block *vb,
 961                                vorbis_look_psy *p,
 962                                vorbis_info_mapping0 *vi,
 963                                float **mags){
 964
 965
 966   if(p->vi->normal_point_p){
 967     int i,j,k,n=p->n;
 968     int **ret=_vorbis_block_alloc(vb,vi->coupling_steps*sizeof(*ret));
 969     int partition=p->vi->normal_partition;
 970     float **work=alloca(sizeof(*work)*partition);
 971
 972     for(i=0;i<vi->coupling_steps;i++){
 973       ret[i]=_vorbis_block_alloc(vb,n*sizeof(**ret));
 974
 975       for(j=0;j<n;j+=partition){
 976         for(k=0;k<partition;k++)work[k]=mags[i]+k+j;
 977         qsort(work,partition,sizeof(*work),apsort);
 978         for(k=0;k<partition;k++)ret[i][k+j]=work[k]-mags[i];
 979       }
 980     }
 981     return(ret);
 982   }
 983   return(NULL);
 984 }
 985
 986 void _vp_noise_normalize_sort(vorbis_look_psy *p,
 987                               float *magnitudes,int *sortedindex){
 988   int i,j,n=p->n;
 989   vorbis_info_psy *vi=p->vi;
 990   int partition=vi->normal_partition;
 991   float **work=alloca(sizeof(*work)*partition);
 992   int start=vi->normal_start;
 993
 994   for(j=start;j<n;j+=partition){
 995     if(j+partition>n)partition=n-j;
 996     for(i=0;i<partition;i++)work[i]=magnitudes+i+j;
 997     qsort(work,partition,sizeof(*work),apsort);
 998     for(i=0;i<partition;i++){
 999       sortedindex[i+j-start]=work[i]-magnitudes;
1000     }
1001   }
1002 }
1003
1004 void _vp_noise_normalize(vorbis_look_psy *p,
1005                          float *in,float *out,int *sortedindex){
1006   int flag=0,i,j=0,n=p->n;
1007   vorbis_info_psy *vi=p->vi;
1008   int partition=vi->normal_partition;
1009   int start=vi->normal_start;
1010
1011   if(start>n)start=n;
1012
1013   if(vi->normal_channel_p){
1014     for(;j<start;j++)
1015       out[j]=rint(in[j]);
1016
1017     for(;j+partition<=n;j+=partition){
1018       float acc=0.;
1019       int k;
1020
1021       for(i=j;i<j+partition;i++)
1022         acc+=in[i]*in[i];
1023
1024       for(i=0;i<partition;i++){
1025         k=sortedindex[i+j-start];
1026
1027         if(in[k]*in[k]>=.25f){
1028           out[k]=rint(in[k]);
1029           acc-=in[k]*in[k];
1030           flag=1;
1031         }else{
1032           if(acc<vi->normal_thresh)break;
1033           out[k]=unitnorm(in[k]);
1034           acc-=1.;
1035         }
1036       }
1037
1038       for(;i<partition;i++){
1039         k=sortedindex[i+j-start];
1040         out[k]=0.;
1041       }
1042     }
1043   }
1044
1045   for(;j<n;j++)
1046     out[j]=rint(in[j]);
1047
1048 }
1049
1050 void _vp_couple(int blobno,
1051                 vorbis_info_psy_global *g,
1052                 vorbis_look_psy *p,
1053                 vorbis_info_mapping0 *vi,
1054                 float **res,
1055                 float **mag_memo,
1056                 int   **mag_sort,
1057                 int   **ifloor,
1058                 int   *nonzero,
1059                 int  sliding_lowpass){
1060
1061   int i,j,k,n=p->n;
1062
1063   /* perform any requested channel coupling */
1064   /* point stereo can only be used in a first stage (in this encoder)
1065      because of the dependency on floor lookups */
1066   for(i=0;i<vi->coupling_steps;i++){
1067
1068     /* once we're doing multistage coupling in which a channel goes
1069        through more than one coupling step, the floor vector
1070        magnitudes will also have to be recalculated an propogated
1071        along with PCM.  Right now, we're not (that will wait until 5.1
1072        most likely), so the code isn't here yet. The memory management
1073        here is all assuming single depth couplings anyway. */
1074
1075     /* make sure coupling a zero and a nonzero channel results in two
1076        nonzero channels. */
1077     if(nonzero[vi->coupling_mag[i]] ||
1078        nonzero[vi->coupling_ang[i]]){
1079
1080
1081       float *rM=res[vi->coupling_mag[i]];
1082       float *rA=res[vi->coupling_ang[i]];
1083       float *qM=rM+n;
1084       float *qA=rA+n;
1085       int *floorM=ifloor[vi->coupling_mag[i]];
1086       int *floorA=ifloor[vi->coupling_ang[i]];
1087       float prepoint=stereo_threshholds[g->coupling_prepointamp[blobno]];
1088       float postpoint=stereo_threshholds[g->coupling_postpointamp[blobno]];
1089       int partition=(p->vi->normal_point_p?p->vi->normal_partition:p->n);
1090       int limit=g->coupling_pointlimit[p->vi->blockflag][blobno];
1091       int pointlimit=limit;
1092
1093       nonzero[vi->coupling_mag[i]]=1;
1094       nonzero[vi->coupling_ang[i]]=1;
1095
1096       for(j=0;j<p->n;j+=partition){
1097         float acc=0.f;
1098
1099         for(k=0;k<partition;k++){
1100           int l=k+j;
1101
1102           if(l<sliding_lowpass){
1103             if((l>=limit && fabs(rM[l])<postpoint && fabs(rA[l])<postpoint) ||
1104                (fabs(rM[l])<prepoint && fabs(rA[l])<prepoint)){
1105
1106
1107               precomputed_couple_point(mag_memo[i][l],
1108                                        floorM[l],floorA[l],
1109                                        qM+l,qA+l);
1110
1111               if(rint(qM[l])==0.f)acc+=qM[l]*qM[l];
1112             }else{
1113               couple_lossless(rM[l],rA[l],qM+l,qA+l);
1114             }
1115           }else{
1116             qM[l]=0.;
1117             qA[l]=0.;
1118           }
1119         }
1120
1121         if(p->vi->normal_point_p){
1122           for(k=0;k<partition && acc>=p->vi->normal_thresh;k++){
1123             int l=mag_sort[i][j+k];
1124             if(l<sliding_lowpass && l>=pointlimit && rint(qM[l])==0.f){
1125               qM[l]=unitnorm(qM[l]);
1126               acc-=1.f;
1127             }
1128           }
1129         }
1130       }
1131     }
1132   }
1133 }
1134