Add files via upload

UTSR/linterp.cpp

@@ -0,0 +1,73 @@
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+std::vector<std::vector<double>> linterp(const std::vector<double>& X, const std::vector<std::vector<double>>& V, const std::vector<std::vector<double>>& Xq) {
+    // Ensure X is a column vector
+    std::vector<double> X_col = X;
+
+    // Ensure V is a column vector if it's a 1D vector
+    std::vector<std::vector<double>> V_col = V;
+    if (V.size() == 1) {
+        V_col = std::vector<std::vector<double>>(V[0].size(), std::vector<double>(1));
+        for (size_t i = 0; i < V[0].size(); ++i) {
+            V_col[i][0] = V[0][i];
+        }
+    }
+
+    // Ensure Xq is a column vector if it's a 1D vector
+    std::vector<std::vector<double>> Xq_col = Xq;
+    if (Xq.size() == 1) {
+        Xq_col = std::vector<std::vector<double>>(Xq[0].size(), std::vector<double>(1));
+        for (size_t i = 0; i < Xq[0].size(); ++i) {
+            Xq_col[i][0] = Xq[0][i];
+        }
+    }
+
+    // Allocate output variable space
+    std::vector<std::vector<double>> Vq(Xq_col.size(), std::vector<double>(Xq_col[0].size(), 0.0));
+    double dX = X_col[1] - X_col[0];
+    size_t N = V_col.size();
+
+    // Loop every Xq column
+    for (size_t col = 0; col < Xq_col[0].size(); ++col) {
+        // Calculate Xq shifts
+        std::vector<double> f;
+        std::vector<bool> fMask(Xq_col.size(), false);
+        for (size_t i = 0; i < Xq_col.size(); ++i) {
+            if (Xq_col[i][col] >= X_col[0] && Xq_col[i][col] <= X_col.back()) {
+                f.push_back((Xq_col[i][col] - X_col[0]) / dX);
+                fMask[i] = true;
+            }
+        }
+
+        // Calculate Xq index
+        std::vector<size_t> im(f.size());
+        for (size_t i = 0; i < f.size(); ++i) {
+            im[i] = static_cast<size_t>(std::floor(f[i]));
+        }
+        size_t im2Inval = std::count_if(im.begin(), im.end(), [N](size_t i) { return i + 2 > N; });
+
+        // Calculate interpolation factors
+        std::vector<double> fx(f.size()), gx(f.size());
+        for (size_t i = 0; i < f.size(); ++i) {
+            fx[i] = f[i] - im[i];
+            gx[i] = 1 - fx[i];
+        }
+
+        // Interpolate Vq values
+        size_t j = 0;
+        for (size_t i = 0; i < Xq_col.size(); ++i) {
+            if (fMask[i]) {
+                if (im2Inval > 0 && j >= f.size() - im2Inval) {
+                    Vq[i][col] = gx[j] * V_col[im[j] + 1][col] + fx[j] * 0.0;
+                } else {
+                    Vq[i][col] = gx[j] * V_col[im[j] + 1][col] + fx[j] * V_col[im[j] + 2][col];
+                }
+                ++j;
+            }
+        }
+    }
+
+    return Vq;
+}