Chris@39: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@39: /*
Chris@39:     This file is Copyright (c) 2012 Chris Cannam
Chris@39:   
Chris@39:     Permission is hereby granted, free of charge, to any person
Chris@39:     obtaining a copy of this software and associated documentation
Chris@39:     files (the "Software"), to deal in the Software without
Chris@39:     restriction, including without limitation the rights to use, copy,
Chris@39:     modify, merge, publish, distribute, sublicense, and/or sell copies
Chris@39:     of the Software, and to permit persons to whom the Software is
Chris@39:     furnished to do so, subject to the following conditions:
Chris@39: 
Chris@39:     The above copyright notice and this permission notice shall be
Chris@39:     included in all copies or substantial portions of the Software.
Chris@39: 
Chris@39:     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
Chris@39:     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
Chris@39:     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
Chris@39:     NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR
Chris@39:     ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
Chris@39:     CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
Chris@39:     WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Chris@39: */
Chris@39: 
Chris@39: #include "PeakInterpolator.h"
Chris@39: 
Chris@40: #include <iostream>
Chris@40: 
Chris@39: static double cubicInterpolate(const double y[4], double x)
Chris@39: {
Chris@39:     double a0 = y[3] - y[2] - y[0] + y[1];
Chris@39:     double a1 = y[0] - y[1] - a0;
Chris@39:     double a2 = y[2] - y[0];
Chris@39:     double a3 = y[1];
Chris@39:     return
Chris@39:         a0 * x * x * x +
Chris@39:         a1 * x * x +
Chris@39:         a2 * x +
Chris@39:         a3;
Chris@39: }
Chris@39: 
Chris@39: double
Chris@39: PeakInterpolator::findPeakLocation(const double *data, int size, int peakIndex)
Chris@39: {
Chris@40:     std::cerr << "findPeakLocation: size " << size << ", peakIndex " << peakIndex << std::endl;
Chris@40: 
Chris@40:     if (peakIndex < 1 || peakIndex > size - 2) {
Chris@40:         std::cerr << "returning " << peakIndex << ", data too short" << std::endl;
Chris@39:         return peakIndex;
Chris@39:     }
Chris@39: 
Chris@39:     double maxval = 0.0;
Chris@39:     double location = peakIndex;
Chris@39: 
Chris@39:     const int divisions = 10;
Chris@39:     double y[4];
Chris@39: 
Chris@39:     y[0] = data[peakIndex-1];
Chris@39:     y[1] = data[peakIndex];
Chris@39:     y[2] = data[peakIndex+1];
Chris@40:     if (peakIndex < size - 2) {
Chris@40:         y[3] = data[peakIndex+2];
Chris@40:     } else {
Chris@40:         y[3] = y[2];
Chris@40:     }
Chris@40:     std::cerr << "a y: " << y[0] << " " << y[1] << " " << y[2] << " " << y[3] << std::endl;
Chris@39:     for (int i = 0; i < divisions; ++i) {
Chris@39:         double probe = double(i) / double(divisions);
Chris@39:         double value = cubicInterpolate(y, probe);
Chris@40:         std::cerr << "probe = " << probe << ", value = " << value << " for location " << peakIndex + probe << std::endl;
Chris@39:         if (value > maxval) {
Chris@39:             maxval = value; 
Chris@39:             location = peakIndex + probe;
Chris@39:         }
Chris@39:     }
Chris@39: 
Chris@39:     y[3] = y[2];
Chris@39:     y[2] = y[1];
Chris@39:     y[1] = y[0];
Chris@40:     if (peakIndex > 1) {
Chris@40:         y[0] = data[peakIndex-2];
Chris@40:     } else {
Chris@40:         y[0] = y[1];
Chris@40:     }
Chris@40:     std::cerr << "b y: " << y[0] << " " << y[1] << " " << y[2] << " " << y[3] << std::endl;
Chris@39:     for (int i = 0; i < divisions; ++i) {
Chris@39:         double probe = double(i) / double(divisions);
Chris@39:         double value = cubicInterpolate(y, probe);
Chris@40:         std::cerr << "probe = " << probe << ", value = " << value << " for location " << peakIndex - 1 + probe << std::endl;
Chris@39:         if (value > maxval) {
Chris@39:             maxval = value; 
Chris@39:             location = peakIndex - 1 + probe;
Chris@39:         }
Chris@39:     }
Chris@39: 
Chris@40:     std::cerr << "returning " << location << std::endl;
Chris@40: 
Chris@39:     return location;
Chris@39: }
Chris@39: