From c5c0c984a1063e1505c5d0212492e31382d50b90 Mon Sep 17 00:00:00 2001
From: Paul Brossier <piem@piem.org>
Date: Sat, 12 Sep 2009 00:08:54 +0200
Subject: [PATCH] src/mathutils.c: merge and fix vec_quadint_min and _max into
 simpler and more exact vec_quadint, use in src/pitch algorithms

---
 src/mathutils.c         | 53 ++++++++---------------------------------
 src/mathutils.h         |  7 ++----
 src/pitch/pitchmcomb.c  |  2 +-
 src/pitch/pitchyin.c    |  4 ++--
 src/pitch/pitchyinfft.c |  6 ++---
 5 files changed, 18 insertions(+), 54 deletions(-)

diff --git a/src/mathutils.c b/src/mathutils.c
index 5a6e9bac..4c1d5379 100644
--- a/src/mathutils.c
+++ b/src/mathutils.c
@@ -285,49 +285,16 @@ smpl_t vec_median(fvec_t * input) {
   }
 }
 
-smpl_t vec_quadint(fvec_t * x,uint_t pos) {
-  uint_t span = 2;
-  smpl_t step = 1./200.;
-  /* hack : init resold to - something (in case x[pos+-span]<0)) */
-  smpl_t res, frac, s0, s1, s2, exactpos = (smpl_t)pos, resold = -1000.;
-  if ((pos > span) && (pos < x->length-span)) {
-    s0 = x->data[0][pos-span];
-    s1 = x->data[0][pos]     ;
-    s2 = x->data[0][pos+span];
-    /* increase frac */
-    for (frac = 0.; frac < 2.; frac = frac + step) {
-      res = aubio_quadfrac(s0, s1, s2, frac);
-      if (res > resold)
-        resold = res;
-      else {
-        exactpos += (frac-step)*2. - 1.;
-        break;
-      }
-    }
-  }
-  return exactpos;
-}
-
-smpl_t vec_quadint_min(fvec_t * x,uint_t pos, uint_t span) {
-  smpl_t step = 1./200.;
-  /* init resold to - something (in case x[pos+-span]<0)) */
-  smpl_t res, frac, s0, s1, s2, exactpos = (smpl_t)pos, resold = 100000.;
-  if ((pos > span) && (pos < x->length-span)) {
-    s0 = x->data[0][pos-span];
-    s1 = x->data[0][pos]     ;
-    s2 = x->data[0][pos+span];
-    /* increase frac */
-    for (frac = 0.; frac < 2.; frac = frac + step) {
-      res = aubio_quadfrac(s0, s1, s2, frac);
-      if (res < resold) {
-        resold = res;
-      } else {
-        exactpos += (frac-step)*span - span/2.;
-        break;
-      }
-    }
-  }
-  return exactpos;
+smpl_t vec_quadint(fvec_t * x,uint_t pos, uint_t span) {
+  smpl_t s0, s1, s2;
+  uint_t x0 = (pos < span) ? pos : pos - span;
+  uint_t x2 = (pos + span < x->length) ? pos + span : pos;
+  if (x0 == pos) return (x->data[0][pos] <= x->data[0][x2]) ? pos : x2;
+  if (x2 == pos) return (x->data[0][pos] <= x->data[0][x0]) ? pos : x0;
+  s0 = x->data[0][x0];
+  s1 = x->data[0][pos]     ;
+  s2 = x->data[0][x2];
+  return pos + 0.5 * (s2 - s0 ) / (s2 - 2.* s1 + s0);
 }
 
 smpl_t aubio_quadfrac(smpl_t s0, smpl_t s1, smpl_t s2, smpl_t pf) {
diff --git a/src/mathutils.h b/src/mathutils.h
index 9bf0f583..6e51586d 100644
--- a/src/mathutils.h
+++ b/src/mathutils.h
@@ -161,11 +161,8 @@ smpl_t vec_moving_thres(fvec_t * vec, fvec_t * tmp,
  */
 smpl_t vec_median(fvec_t * input);
 
-/** finds exact maximum position by quadratic interpolation*/
-smpl_t vec_quadint(fvec_t * x,uint_t pos);
-
-/** finds exact minimum position by quadratic interpolation*/
-smpl_t vec_quadint_min(fvec_t * x,uint_t pos, uint_t span);
+/** finds exact peak index by quadratic interpolation*/
+smpl_t vec_quadint(fvec_t * x, uint_t pos, uint_t span);
 
 /** Quadratic interpolation using Lagrange polynomial.
  *
diff --git a/src/pitch/pitchmcomb.c b/src/pitch/pitchmcomb.c
index f7435112..5cd23ff9 100644
--- a/src/pitch/pitchmcomb.c
+++ b/src/pitch/pitchmcomb.c
@@ -272,7 +272,7 @@ uint_t aubio_pitchmcomb_quadpick(aubio_spectralpeak_t * spectral_peaks, fvec_t *
       if (ispeak) {
         count += ispeak;
         spectral_peaks[count-1].bin = j;
-        spectral_peaks[count-1].ebin = vec_quadint(X,j) - 1.;
+        spectral_peaks[count-1].ebin = vec_quadint(X, j, 1) - 1.;
       }
     }
   return count;
diff --git a/src/pitch/pitchyin.c b/src/pitch/pitchyin.c
index 88b785f6..28da6ec4 100644
--- a/src/pitch/pitchyin.c
+++ b/src/pitch/pitchyin.c
@@ -106,10 +106,10 @@ smpl_t aubio_pitchyin_getpitchfast(fvec_t * input, fvec_t * yin, smpl_t tol){
     period = tau-3;
     if(tau > 4 && (yin->data[c][period] < tol) && 
         (yin->data[c][period] < yin->data[c][period+1])) {
-      return vec_quadint_min(yin,period,1);
+      return vec_quadint(yin,period,1);
     }
   }
-  return vec_quadint_min(yin,vec_min_elem(yin),1);
+  return vec_quadint(yin,vec_min_elem(yin),1);
   //return 0;
 }
 
diff --git a/src/pitch/pitchyinfft.c b/src/pitch/pitchyinfft.c
index 27279db2..b8c18e02 100644
--- a/src/pitch/pitchyinfft.c
+++ b/src/pitch/pitchyinfft.c
@@ -128,14 +128,14 @@ smpl_t aubio_pitchyinfft_detect(aubio_pitchyinfft_t * p, fvec_t * input, smpl_t
     //return vec_quadint_min(yin,tau,1);
     /* additional check for (unlikely) octave doubling in higher frequencies */
     if (tau>35) {
-      return vec_quadint_min(yin,tau,1);
+      return vec_quadint(yin,tau,1);
     } else {
       /* should compare the minimum value of each interpolated peaks */
       halfperiod = FLOOR(tau/2+.5);
       if (yin->data[0][halfperiod] < tol)
-        return vec_quadint_min(yin,halfperiod,1);
+        return vec_quadint(yin,halfperiod,1);
       else
-        return vec_quadint_min(yin,tau,1);
+        return vec_quadint(yin,tau,1);
     }
   } else
     return 0.;
-- 
2.26.2