feat: pd_signal::interp()

google-tests/CMakeLists.txt

@@ -11,6 +11,7 @@ include_directories(${gtest_SOURCE_DIR}/include ${gtest_SOURCE_DIR} libnpy/inclu
 add_executable(Google_Tests_run
         test1.cpp
         test2.cpp
+        test3.cpp
 )
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)

google-tests/test3.cpp

@@ -0,0 +1,27 @@
+//
+// Created by david on 04.03.2026.
+//
+#include <gtest/gtest.h>
+#include "pd_signal.h"
+
+using namespace pd_signal;
+
+TEST(SignalTest, interp_t1) {
+    //EXPECT_EQ();
+    std::vector<double> xp     { 1.0,      2.0,      4.0, 5.0, 5.01, 6.0 };
+    std::vector<double> fp     { 1.0,      2.0,      4.0, 5.0, 5.01, 7.0 };
+
+    std::vector<double> x { 0.9, 1.0, 1.5, 2.0, 3.9,    4.1, 5.0, 5.5, 6.0, 6.1 };
+    std::vector<double>y_e{ 1.0, 1.0, 1.5, 2.0, 3.9,    4.1, 5.0, 5.99494949495, 7.0, 7.0 };
+    size_t N = x.size();
+    // 5.99494949495 = (5.5-5.01)/0.99*(7-5.01)+5.01
+
+    std::vector<double> y;
+    interp(x, xp, fp, y);
+
+    // assert y == y_e, nb. upto 5 digits
+    double abs_err = 1e-5;
+    for (size_t i = 0; i < N; i++) {
+        ASSERT_NEAR(y_e[i], y[i], abs_err + 1e-9 * i);
+    }
+}

pasada-lib/CMakeLists.txt

@@ -4,7 +4,7 @@ SET(PASADA_SRC
         library.cpp
         iir_filter.cpp
         ssf_filter.cpp
-        include/ssf_filter.h
+        pd_signal.cpp
 )
 
 if(PASADA_BUILD_TESTS)

pasada-lib/include/pd_signal.h

@@ -0,0 +1,18 @@
+//
+// Created by david on 04.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_SIGNAL_H
+#define PASADASUPERPROJECT_SIGNAL_H
+
+#include <vector>
+
+namespace pd_signal {
+    /**
+     * Evaluate at points x the function given by the samples fp[xp[n]].
+     * Returned in y.
+     */
+    void interp(std::vector<double>& x, std::vector<double>& xp, std::vector<double>& fp, std::vector<double>& y);
+}
+
+#endif //PASADASUPERPROJECT_SIGNAL_H

pasada-lib/pd_signal.cpp

@@ -0,0 +1,68 @@
+//
+// 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];
+            }
+        }
+    }
+}
+
+}