69 lines
2.2 KiB
C++
69 lines
2.2 KiB
C++
//
|
|
// Created by david on 04.03.2026.
|
|
//
|
|
|
|
#include "include/pd_signal.h"
|
|
#include <stdexcept>
|
|
#include <algorithm>
|
|
|
|
namespace pd_signal {
|
|
|
|
static void throwIfNotAscending(std::vector<double>& xp) {
|
|
size_t N = xp.size();
|
|
for(int i = 0; i < N - 1; i++)
|
|
if((xp[i+1] - xp[i]) <= 0.0)
|
|
throw std::invalid_argument("xp is not strictly monotonically ascending");
|
|
}
|
|
|
|
static int binarySearch(std::vector<double>& data, double x) {
|
|
auto p = std::equal_range(data.begin(), data.end(), x);
|
|
if(p.first == data.end()) return -((int) data.size()) - 1; // out of range on upper end
|
|
int insertion_point = (int) std::distance(data.begin(), p.first);
|
|
if(p.first != p.second) return insertion_point; // element found
|
|
return -(insertion_point) - 1; // no element found directly
|
|
}
|
|
|
|
// Evaluate at points x the function given by the samples fp[xp[n]].
|
|
void interp(std::vector<double>& x, std::vector<double>& xp, std::vector<double>& fp, std::vector<double>& y) {
|
|
if (xp.size() != fp.size()) throw std::invalid_argument("xp.size() != fp.size()");
|
|
size_t N = xp.size();
|
|
size_t M = x.size();
|
|
if (N < 2) throw std::invalid_argument("N < 2");
|
|
throwIfNotAscending(xp);
|
|
|
|
std::vector<double> slope, intercept;
|
|
y.resize(M);
|
|
slope.resize(N-1);
|
|
intercept.resize(N-1);
|
|
|
|
for(int i = 0; i < N-1; i++) {
|
|
double dxp = xp[i+1] - xp[i];
|
|
double dfp = fp[i+1] - fp[i];
|
|
slope[i] = dfp / dxp;
|
|
intercept[i] = fp[i] - slope[i] * xp[i];
|
|
}
|
|
|
|
for(int i = 0; i < M; i++) {
|
|
if(x[i] < xp[0]) {
|
|
y[i] = fp[0];
|
|
} else if(x[i] > xp[N-1]) {
|
|
y[i] = fp[N-1];
|
|
} else {
|
|
int idx = binarySearch(xp, x[i]);
|
|
if (idx <= -1) {
|
|
// not found precisely. interpolate
|
|
idx = -idx - 1; // convert back to array index (insertion point)
|
|
// idx==0 or idx==M cannot happen, handled above.
|
|
//assert(idx > 0 && idx < N);
|
|
idx = idx - 1; // one below insertion point (left point)
|
|
y[i] = slope[idx] * x[i] + intercept[idx];
|
|
} else {
|
|
// found precisely
|
|
y[i] = fp[idx];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|