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