resample.cpp

Go to the documentation of this file.

//=============================================================================================================
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "resample.h"
 
//=============================================================================================================
// EIGEN INCLUDES
//=============================================================================================================
 
#include <Eigen/Core>
 
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================
 
#include <QDebug>
 
//=============================================================================================================
// C++ INCLUDES
//=============================================================================================================
 
#include <cmath>
#include <algorithm>
 
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================
 
using namespace UTILSLIB;
using namespace Eigen;
 
//=============================================================================================================
// PRIVATE
//=============================================================================================================
 
int Resample::gcd(int a, int b)
{
    while (b) {
        int t = b;
        b = a % b;
        a = t;
    }
    return a;
}
 
//=============================================================================================================
 
RowVectorXd Resample::buildKernel(int p, int q, int iNZeros)
{
    int M = std::max(p, q);
    int halfLen = iNZeros * M;
    int L = 2 * halfLen + 1;
 
    // Cutoff as a fraction of the sampling frequency at the upsampled rate p*oldSFreq.
    // We want to cut at the lower Nyquist: min(oldSFreq, newSFreq)/2.
    // In normalised terms (fraction of upsampled fs):
    //   cutoff_norm = min(p,q) / (2.0 * max(p,q))  ... but the conventional sinc parameterisation
    //   uses cutoff as a fraction of fs (not Nyquist), i.e. in [0,0.5].
    // So fc_fs = min(p,q) / (2.0 * max(p,q)).
    const double fc = static_cast<double>(std::min(p, q)) / (2.0 * static_cast<double>(M));
 
    RowVectorXd h(L);
    for (int k = 0; k < L; ++k) {
        double n = k - halfLen;
        double win = 0.54 - 0.46 * std::cos(2.0 * M_PI * k / (L - 1)); // Hamming
 
        if (std::abs(n) < 1e-10) {
            h(k) = 2.0 * fc * win;
        } else {
            // sinc(2*fc*n) * 2*fc  =  sin(2π*fc*n) / (π*n)
            h(k) = std::sin(2.0 * M_PI * fc * n) / (M_PI * n) * win;
        }
    }
 
    // Scale by p to restore unity gain after the conceptual upsampling-by-p step.
    h *= static_cast<double>(p);
    return h;
}
 
//=============================================================================================================
 
RowVectorXd Resample::polyphaseConv(const RowVectorXd& vecX,
                                    const RowVectorXd& vecH,
                                    int p,
                                    int q,
                                    int halfLen)
{
    const long long nIn = static_cast<long long>(vecX.size());
    const long long L = static_cast<long long>(vecH.size()); // = 2*halfLen + 1
 
    // Output length: ceil(nIn * p / q)
    const long long nOut = (nIn * p + q - 1) / q;
 
    RowVectorXd y(nOut);
 
    for (long long m = 0; m < nOut; ++m) {
        // The filter delay (halfLen upsampled samples) is absorbed into the center index so that
        // output sample 0 aligns with input sample 0.
        long long center = m * static_cast<long long>(q) + halfLen;
 
        // Iterate over all input indices j whose upsampled position j*p lies within
        // the filter window [center - L + 1, center].
        // That is: j*p in [center - L + 1, center]
        //       => j in [ceil((center-L+1)/p), floor(center/p)]  ∩ [0, nIn-1]
        long long j_min = (center - L + 1 + p - 1) / p; // ceil division
        if (j_min < 0)
            j_min = 0;
        long long j_max = center / p;
        if (j_max >= nIn)
            j_max = nIn - 1;
 
        double val = 0.0;
        for (long long j = j_min; j <= j_max; ++j) {
            long long tap = center - j * static_cast<long long>(p);
            if (tap >= 0 && tap < L) {
                val += vecH(static_cast<int>(tap)) * vecX(static_cast<int>(j));
            }
        }
        y(static_cast<int>(m)) = val;
    }
 
    return y;
}
 
//=============================================================================================================
// PUBLIC
//=============================================================================================================
 
RowVectorXd Resample::resample(const RowVectorXd& vecData,
                               double dNewSFreq,
                               double dOldSFreq,
                               int iNZeros)
{
    if (vecData.size() == 0) {
        qWarning() << "Resample::resample: empty input.";
        return vecData;
    }
    if (dNewSFreq <= 0.0 || dOldSFreq <= 0.0) {
        qWarning() << "Resample::resample: sampling frequencies must be positive.";
        return vecData;
    }
 
    // Represent ratio as integers to avoid floating-point drift.
    // Multiply both rates by 1000 and reduce by GCD (handles e.g. 600.0/1000.0 → 3/5).
    const int scale = 1000;
    int p_raw = static_cast<int>(std::round(dNewSFreq * scale));
    int q_raw = static_cast<int>(std::round(dOldSFreq * scale));
    int g = gcd(p_raw, q_raw);
    int p = p_raw / g;
    int q = q_raw / g;
 
    if (p == q) {
        return vecData; // Same rate after reduction
    }
 
    const int halfLen = iNZeros * std::max(p, q);
    RowVectorXd h = buildKernel(p, q, iNZeros);
 
    return polyphaseConv(vecData, h, p, q, halfLen);
}
 
//=============================================================================================================
 
MatrixXd Resample::resampleMatrix(const MatrixXd& matData,
                                  double dNewSFreq,
                                  double dOldSFreq,
                                  const RowVectorXi& vecPicks,
                                  int iNZeros)
{
    if (matData.size() == 0)
        return matData;
 
    // Pre-build kernel once for all channels
    const int scale = 1000;
    int p_raw = static_cast<int>(std::round(dNewSFreq * scale));
    int q_raw = static_cast<int>(std::round(dOldSFreq * scale));
    int g = gcd(p_raw, q_raw);
    int p = p_raw / g;
    int q = q_raw / g;
 
    if (p == q)
        return matData;
 
    const int halfLen = iNZeros * std::max(p, q);
    RowVectorXd h = buildKernel(p, q, iNZeros);
 
    const int nIn = static_cast<int>(matData.cols());
    const int nOut = static_cast<int>((static_cast<long long>(nIn) * p + q - 1) / q);
    const int nCh = static_cast<int>(matData.rows());
 
    MatrixXd result(nCh, nOut);
 
    if (vecPicks.size() == 0) {
        for (int i = 0; i < nCh; ++i) {
            result.row(i) = polyphaseConv(matData.row(i), h, p, q, halfLen);
        }
    } else {
        // Initialise to zero then fill picked rows (non-picked rows remain zero —
        // callers using picks should be aware rows are not copied; use full resampling
        // if a copy of non-MEG channels is needed).
        result.setZero();
        for (int k = 0; k < vecPicks.size(); ++k) {
            int i = vecPicks(k);
            if (i >= 0 && i < nCh) {
                result.row(i) = polyphaseConv(matData.row(i), h, p, q, halfLen);
            }
        }
    }
 
    return result;
}