74 lines
2.6 KiB
C++
74 lines
2.6 KiB
C++
#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;
|
|
}
|