vx32

Local 9vx git repository for patches.
git clone git://r-36.net/vx32
Log | Files | Refs

psy.c (30167B)


      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 1919 2005-07-24 14:18:04Z baford $
     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