src/mathutils.c

   1 /*
   2    Copyright (C) 2003 Paul Brossier
   3
   4    This program is free software; you can redistribute it and/or modify
   5    it under the terms of the GNU General Public License as published by
   6    the Free Software Foundation; either version 2 of the License, or
   7    (at your option) any later version.
   8
   9    This program is distributed in the hope that it will be useful,
  10    but WITHOUT ANY WARRANTY; without even the implied warranty of
  11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12    GNU General Public License for more details.
  13
  14    You should have received a copy of the GNU General Public License
  15    along with this program; if not, write to the Free Software
  16    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17
  18 */
  19
  20 /* see in mathutils.h for doc */
  21
  22 #include "aubio_priv.h"
  23 #include "fvec.h"
  24 #include "mathutils.h"
  25 #include "config.h"
  26
  27 void aubio_window(smpl_t *w, uint_t size, aubio_window_type wintype) {
  28   uint_t i;
  29   switch(wintype) {
  30     case aubio_win_rectangle:
  31       for (i=0;i<size;i++)
  32         w[i] = 0.5;
  33       break;
  34     case aubio_win_hamming:
  35       for (i=0;i<size;i++)
  36         w[i] = 0.54 - 0.46 * COS(TWO_PI * i / (size));
  37       break;
  38     case aubio_win_hanning:
  39       for (i=0;i<size;i++)
  40         w[i] = 0.5 - (0.5 * COS(TWO_PI * i / (size)));
  41       break;
  42     case aubio_win_hanningz:
  43       for (i=0;i<size;i++)
  44         w[i] = 0.5 * (1.0 - COS(TWO_PI * i / (size)));
  45       break;
  46     case aubio_win_blackman:
  47       for (i=0;i<size;i++)
  48         w[i] = 0.42
  49           - 0.50 * COS(    TWO_PI*i/(size-1.0))
  50           + 0.08 * COS(2.0*TWO_PI*i/(size-1.0));
  51       break;
  52     case aubio_win_blackman_harris:
  53       for (i=0;i<size;i++)
  54         w[i] = 0.35875
  55           - 0.48829 * COS(    TWO_PI*i/(size-1.0))
  56           + 0.14128 * COS(2.0*TWO_PI*i/(size-1.0))
  57           - 0.01168 * COS(3.0*TWO_PI*i/(size-1.0));
  58       break;
  59     case aubio_win_gaussian:
  60       for (i=0;i<size;i++)
  61         w[i] = EXP(- 1.0 / SQR(size) * SQR(2.0*i-size));
  62       break;
  63     case aubio_win_welch:
  64       for (i=0;i<size;i++)
  65         w[i] = 1.0 - SQR((2*i-size)/(size+1.0));
  66       break;
  67     case aubio_win_parzen:
  68       for (i=0;i<size;i++)
  69         w[i] = 1.0 - ABS((2*i-size)/(size+1.0));
  70       break;
  71     default:
  72       break;
  73   }
  74 }
  75
  76 smpl_t aubio_unwrap2pi(smpl_t phase) {
  77   /* mod(phase+pi,-2pi)+pi */
  78   return phase + TWO_PI * (1. + FLOOR(-(phase+PI)/TWO_PI));
  79 }
  80
  81 smpl_t vec_mean(fvec_t *s) {
  82   uint_t i,j;
  83   smpl_t tmp = 0.0f;
  84   for (i=0; i < s->channels; i++)
  85     for (j=0; j < s->length; j++)
  86       tmp += s->data[i][j];
  87   return tmp/(smpl_t)(s->length);
  88 }
  89
  90 smpl_t vec_sum(fvec_t *s) {
  91   uint_t i,j;
  92   smpl_t tmp = 0.0f;
  93   for (i=0; i < s->channels; i++)
  94     for (j=0; j < s->length; j++)
  95       tmp += s->data[i][j];
  96   return tmp;
  97 }
  98
  99 smpl_t vec_max(fvec_t *s) {
 100   uint_t i,j;
 101   smpl_t tmp = 0.0f;
 102   for (i=0; i < s->channels; i++)
 103     for (j=0; j < s->length; j++)
 104       tmp = (tmp > s->data[i][j])? tmp : s->data[i][j];
 105   return tmp;
 106 }
 107
 108 smpl_t vec_min(fvec_t *s) {
 109   uint_t i,j;
 110   smpl_t tmp = s->data[0][0];
 111   for (i=0; i < s->channels; i++)
 112     for (j=0; j < s->length; j++)
 113       tmp = (tmp < s->data[i][j])? tmp : s->data[i][j]  ;
 114   return tmp;
 115 }
 116
 117 uint_t vec_min_elem(fvec_t *s) {
 118   uint_t i,j=0, pos=0.;
 119   smpl_t tmp = s->data[0][0];
 120   for (i=0; i < s->channels; i++)
 121     for (j=0; j < s->length; j++) {
 122       pos = (tmp < s->data[i][j])? pos : j;
 123       tmp = (tmp < s->data[i][j])? tmp : s->data[i][j]  ;
 124     }
 125   return pos;
 126 }
 127
 128 uint_t vec_max_elem(fvec_t *s) {
 129   uint_t i,j=0, pos=0.;
 130   smpl_t tmp = 0.0f;
 131   for (i=0; i < s->channels; i++)
 132     for (j=0; j < s->length; j++) {
 133       pos = (tmp > s->data[i][j])? pos : j;
 134       tmp = (tmp > s->data[i][j])? tmp : s->data[i][j]  ;
 135     }
 136   return pos;
 137 }
 138
 139 void vec_shift(fvec_t *s) {
 140   uint_t i,j;
 141   //smpl_t tmp = 0.0f;
 142   for (i=0; i < s->channels; i++)
 143     for (j=0; j < s->length / 2 ; j++) {
 144       //tmp = s->data[i][j];
 145       //s->data[i][j] = s->data[i][j+s->length/2];
 146       //s->data[i][j+s->length/2] = tmp;
 147       ELEM_SWAP(s->data[i][j],s->data[i][j+s->length/2]);
 148     }
 149 }
 150
 151 smpl_t vec_local_energy(fvec_t * f) {
 152   smpl_t locE = 0.;
 153   uint_t i,j;
 154   for (i=0;i<f->channels;i++)
 155     for (j=0;j<f->length;j++)
 156       locE+=SQR(f->data[i][j]);
 157   return locE;
 158 }
 159
 160 smpl_t vec_local_hfc(fvec_t * f) {
 161   smpl_t locE = 0.;
 162   uint_t i,j;
 163   for (i=0;i<f->channels;i++)
 164     for (j=0;j<f->length;j++)
 165       locE+=(i+1)*f->data[i][j];
 166   return locE;
 167 }
 168
 169 smpl_t vec_alpha_norm(fvec_t * DF, smpl_t alpha) {
 170   smpl_t tmp = 0.;
 171   uint_t i,j;
 172   for (i=0;i<DF->channels;i++)
 173     for (j=0;j<DF->length;j++)
 174       tmp += POW(ABS(DF->data[i][j]),alpha);
 175   return POW(tmp/DF->length,1./alpha);
 176 }
 177
 178 void vec_dc_removal(fvec_t * mag) {
 179     smpl_t mini = 0.;
 180     uint_t length = mag->length, i=0, j;
 181     mini = vec_min(mag);
 182     for (j=0;j<length;j++) {
 183       mag->data[i][j] -= mini;
 184     }
 185 }
 186
 187 void vec_alpha_normalise(fvec_t * mag, uint_t alpha) {
 188   smpl_t alphan = 1.;
 189   uint_t length = mag->length, i=0, j;
 190   alphan = vec_alpha_norm(mag,alpha);
 191   for (j=0;j<length;j++){
 192     mag->data[i][j] /= alphan;
 193   }
 194 }
 195
 196 void vec_add(fvec_t * mag, smpl_t threshold) {
 197   uint_t length = mag->length, i=0, j;
 198   for (j=0;j<length;j++) {
 199     mag->data[i][j] += threshold;
 200   }
 201 }
 202
 203 void vec_adapt_thres(fvec_t * vec, fvec_t * tmp,
 204     uint_t post, uint_t pre) {
 205   uint_t length = vec->length, i=0, j;
 206   for (j=0;j<length;j++) {
 207     vec->data[i][j] -= vec_moving_thres(vec, tmp, post, pre, j);
 208   }
 209 }
 210
 211 smpl_t vec_moving_thres(fvec_t * vec, fvec_t * tmpvec,
 212     uint_t post, uint_t pre, uint_t pos) {
 213   smpl_t * medar = (smpl_t *)tmpvec->data[0];
 214   uint_t k;
 215   uint_t win_length =  post+pre+1;
 216   uint_t length =  vec->length;
 217   /* post part of the buffer does not exist */
 218   if (pos<post+1) {
 219     for (k=0;k<post+1-pos;k++)
 220       medar[k] = 0.; /* 0-padding at the beginning */
 221     for (k=post+1-pos;k<win_length;k++)
 222       medar[k] = vec->data[0][k+pos-post];
 223   /* the buffer is fully defined */
 224   } else if (pos+pre<length) {
 225     for (k=0;k<win_length;k++)
 226       medar[k] = vec->data[0][k+pos-post];
 227   /* pre part of the buffer does not exist */
 228   } else {
 229     for (k=0;k<length-pos+post;k++)
 230       medar[k] = vec->data[0][k+pos-post];
 231     for (k=length-pos+post;k<win_length;k++)
 232       medar[k] = 0.; /* 0-padding at the end */
 233   }
 234   return vec_median(tmpvec);
 235 }
 236
 237 smpl_t vec_median(fvec_t * input) {
 238   uint_t n = input->length;
 239   smpl_t * arr = (smpl_t *) input->data[0];
 240   uint_t low, high ;
 241   uint_t median;
 242   uint_t middle, ll, hh;
 243
 244   low = 0 ; high = n-1 ; median = (low + high) / 2;
 245   for (;;) {
 246     if (high <= low) /* One element only */
 247       return arr[median] ;
 248
 249     if (high == low + 1) {  /* Two elements only */
 250       if (arr[low] > arr[high])
 251         ELEM_SWAP(arr[low], arr[high]) ;
 252       return arr[median] ;
 253     }
 254
 255     /* Find median of low, middle and high items; swap into position low */
 256     middle = (low + high) / 2;
 257     if (arr[middle] > arr[high])    ELEM_SWAP(arr[middle], arr[high]);
 258     if (arr[low]    > arr[high])    ELEM_SWAP(arr[low],    arr[high]);
 259     if (arr[middle] > arr[low])     ELEM_SWAP(arr[middle], arr[low]) ;
 260
 261     /* Swap low item (now in position middle) into position (low+1) */
 262     ELEM_SWAP(arr[middle], arr[low+1]) ;
 263
 264     /* Nibble from each end towards middle, swapping items when stuck */
 265     ll = low + 1;
 266     hh = high;
 267     for (;;) {
 268       do ll++; while (arr[low] > arr[ll]) ;
 269       do hh--; while (arr[hh]  > arr[low]) ;
 270
 271       if (hh < ll)
 272         break;
 273
 274       ELEM_SWAP(arr[ll], arr[hh]) ;
 275     }
 276
 277     /* Swap middle item (in position low) back into correct position */
 278     ELEM_SWAP(arr[low], arr[hh]) ;
 279
 280     /* Re-set active partition */
 281     if (hh <= median)
 282       low = ll;
 283     if (hh >= median)
 284       high = hh - 1;
 285   }
 286 }
 287
 288 smpl_t vec_quadint(fvec_t * x,uint_t pos) {
 289   uint_t span = 2;
 290   smpl_t step = 1./200.;
 291   /* hack : init resold to - something (in case x[pos+-span]<0)) */
 292   smpl_t res, frac, s0, s1, s2, exactpos = (smpl_t)pos, resold = -1000.;
 293   if ((pos > span) && (pos < x->length-span)) {
 294     s0 = x->data[0][pos-span];
 295     s1 = x->data[0][pos]     ;
 296     s2 = x->data[0][pos+span];
 297     /* increase frac */
 298     for (frac = 0.; frac < 2.; frac = frac + step) {
 299       res = aubio_quadfrac(s0, s1, s2, frac);
 300       if (res > resold)
 301         resold = res;
 302       else {
 303         exactpos += (frac-step)*2. - 1.;
 304         break;
 305       }
 306     }
 307   }
 308   return exactpos;
 309 }
 310
 311 smpl_t vec_quadint_min(fvec_t * x,uint_t pos, uint_t span) {
 312   smpl_t step = 1./200.;
 313   /* init resold to - something (in case x[pos+-span]<0)) */
 314   smpl_t res, frac, s0, s1, s2, exactpos = (smpl_t)pos, resold = 100000.;
 315   if ((pos > span) && (pos < x->length-span)) {
 316     s0 = x->data[0][pos-span];
 317     s1 = x->data[0][pos]     ;
 318     s2 = x->data[0][pos+span];
 319     /* increase frac */
 320     for (frac = 0.; frac < 2.; frac = frac + step) {
 321       res = aubio_quadfrac(s0, s1, s2, frac);
 322       if (res < resold) {
 323         resold = res;
 324       } else {
 325         exactpos += (frac-step)*span - span/2.;
 326         break;
 327       }
 328     }
 329   }
 330   return exactpos;
 331 }
 332
 333 smpl_t aubio_quadfrac(smpl_t s0, smpl_t s1, smpl_t s2, smpl_t pf) {
 334   smpl_t tmp = s0 + (pf/2.) * (pf * ( s0 - 2.*s1 + s2 ) - 3.*s0 + 4.*s1 - s2);
 335   return tmp;
 336 }
 337
 338 uint_t vec_peakpick(fvec_t * onset, uint_t pos) {
 339   uint_t i=0, tmp=0;
 340   /*for (i=0;i<onset->channels;i++)*/
 341   tmp = (onset->data[i][pos] > onset->data[i][pos-1]
 342       &&  onset->data[i][pos] > onset->data[i][pos+1]
 343       &&  onset->data[i][pos] > 0.);
 344   return tmp;
 345 }
 346
 347 smpl_t aubio_freqtomidi(smpl_t freq) {
 348   /* log(freq/A-2)/log(2) */
 349   smpl_t midi = freq/6.875;
 350   midi = LOG(midi)/0.69314718055995;
 351   midi *= 12;
 352   midi -= 3;
 353   return midi;
 354 }
 355
 356 smpl_t aubio_miditofreq(smpl_t midi) {
 357   smpl_t freq = (midi+3.)/12.;
 358   freq = EXP(freq*0.69314718055995);
 359   freq *= 6.875;
 360   return freq;
 361 }
 362
 363 smpl_t aubio_bintofreq(smpl_t bin, smpl_t samplerate, smpl_t fftsize) {
 364   smpl_t freq = samplerate/fftsize;
 365   return freq*bin;
 366 }
 367
 368 smpl_t aubio_bintomidi(smpl_t bin, smpl_t samplerate, smpl_t fftsize) {
 369   smpl_t midi = aubio_bintofreq(bin,samplerate,fftsize);
 370   return aubio_freqtomidi(midi);
 371 }
 372
 373 smpl_t aubio_freqtobin(smpl_t freq, smpl_t samplerate, smpl_t fftsize) {
 374   smpl_t bin = fftsize/samplerate;
 375   return freq*bin;
 376 }
 377
 378 smpl_t aubio_miditobin(smpl_t midi, smpl_t samplerate, smpl_t fftsize) {
 379   smpl_t freq = aubio_miditofreq(midi);
 380   return aubio_freqtobin(freq,samplerate,fftsize);
 381 }
 382
 383 /** returns 1 if wassilence is 0 and RMS(ibuf)<threshold
 384  * \bug mono
 385  */
 386 uint_t aubio_silence_detection(fvec_t * ibuf, smpl_t threshold) {
 387   smpl_t loudness = 0;
 388   uint_t i=0,j;
 389   for (j=0;j<ibuf->length;j++) {
 390     loudness += SQR(ibuf->data[i][j]);
 391   }
 392   loudness = SQRT(loudness);
 393   loudness /= (smpl_t)ibuf->length;
 394   loudness = LIN2DB(loudness);
 395
 396   return (loudness < threshold);
 397 }
 398
 399 /** returns level log(RMS(ibuf)) if < threshold, 1 otherwise
 400  * \bug mono
 401  */
 402 smpl_t aubio_level_detection(fvec_t * ibuf, smpl_t threshold) {
 403   smpl_t loudness = 0;
 404   uint_t i=0,j;
 405   for (j=0;j<ibuf->length;j++) {
 406     loudness += SQR(ibuf->data[i][j]);
 407   }
 408   loudness = SQRT(loudness);
 409   loudness /= (smpl_t)ibuf->length;
 410   loudness = LIN2DB(loudness);
 411
 412   if (loudness < threshold)
 413     return 1.;
 414   else
 415     return loudness;
 416 }
 417
 418 smpl_t aubio_zero_crossing_rate(fvec_t * input) {
 419   uint_t i=0,j;
 420   uint_t zcr = 0;
 421   for ( j = 1; j < input->length; j++ ) {
 422     // previous was negative
 423     if( input->data[i][j-1] <= 0. ) {
 424       if ( input->data[i][j] > 0. ) {
 425         zcr += 1;
 426       }
 427     //previous was positive
 428     } else if ( input->data[i][j] <= 0. ) {
 429       zcr += 1;
 430     }
 431   }
 432   return zcr/(smpl_t)input->length;
 433 }
 434
 435 void aubio_autocorr(fvec_t * input, fvec_t * output) {
 436   uint_t i = 0, j = 0, length = input->length;
 437   smpl_t * data = input->data[0];
 438   smpl_t * acf = output->data[0];
 439   smpl_t tmp =0.;
 440   for(i=0;i<length;i++){
 441     for(j=i;j<length;j++){
 442       tmp += data[j-i]*data[j];
 443     }
 444     acf[i] = tmp /(smpl_t)(length-i);
 445     tmp = 0.0;
 446   }
 447 }
 448
 449 void aubio_cleanup(void) {
 450 #if FFTW3_SUPPORT
 451   fftw_cleanup();
 452 #else
 453 #if FFTW3F_SUPPORT
 454   fftwf_cleanup();
 455 #endif
 456 #endif
 457 }