git.sesse.net Git - nageru/blob - nageru/correlation_measurer.cpp

   1 // Adapted from Adriaensen's project Zita-mu1 (as of January 2016).
   2 // Original copyright follows:
   3 //
   4 //  Copyright (C) 2008-2015 Fons Adriaensen <fons@linuxaudio.org>
   5 //
   6 //  This program is free software; you can redistribute it and/or modify
   7 //  it under the terms of the GNU General Public License as published by
   8 //  the Free Software Foundation; either version 3 of the License, or
   9 //  (at your option) any later version.
  10 //
  11 //  This program is distributed in the hope that it will be useful,
  12 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 //  GNU General Public License for more details.
  15 //
  16 //  You should have received a copy of the GNU General Public License
  17 //  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 #include "correlation_measurer.h"
  20
  21 #include <assert.h>
  22 #include <cmath>
  23 #include <cstddef>
  24 #include <math.h>
  25 #include <vector>
  26
  27 using namespace std;
  28
  29 CorrelationMeasurer::CorrelationMeasurer(unsigned sample_rate,
  30                                          float lowpass_cutoff_hz,
  31                                          float falloff_seconds)
  32     : w1(2.0 * M_PI * lowpass_cutoff_hz / sample_rate),
  33       w2(1.0 / (falloff_seconds * sample_rate))
  34 {
  35 }
  36
  37 void CorrelationMeasurer::reset()
  38 {
  39         zl = zr = zll = zlr = zrr = 0.0f;
  40 }
  41
  42 void CorrelationMeasurer::process_samples(const std::vector<float> &samples)
  43 {
  44         assert(samples.size() % 2 == 0);
  45
  46         // The compiler isn't always happy about modifying members,
  47         // since it doesn't always know they can't alias on <samples>.
  48         // Help it out a bit.
  49         float l = zl, r = zr, ll = zll, lr = zlr, rr = zrr;
  50         const float w1c = w1, w2c = w2;
  51
  52         for (size_t i = 0; i < samples.size(); i += 2) {
  53                 // The 1e-15f epsilon is to avoid denormals.
  54                 // TODO: Just set the SSE flush-to-zero flags instead.
  55                 l += w1c * (samples[i + 0] - l) + 1e-15f;
  56                 r += w1c * (samples[i + 1] - r) + 1e-15f;
  57                 lr += w2c * (l * r - lr);
  58                 ll += w2c * (l * l - ll);
  59                 rr += w2c * (r * r - rr);
  60         }
  61
  62         zl = l;
  63         zr = r;
  64         zll = ll;
  65         zlr = lr;
  66         zrr = rr;
  67 }
  68
  69 float CorrelationMeasurer::get_correlation() const
  70 {
  71         // The 1e-12f epsilon is to avoid division by zero.
  72         // zll and zrr are both always non-negative, so we do not risk negative values.
  73         return zlr / sqrt(zll * zrr + 1e-12f);
  74 }