89 lines
2.9 KiB
C++
89 lines
2.9 KiB
C++
#include <vector>
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <numeric>
|
|
|
|
std::vector<std::vector<double>> linterpAdj(const std::vector<double>& X, const std::vector<std::vector<double>>& Xq, const std::vector<std::vector<double>>& Vq) {
|
|
// Ensure X is a column vector
|
|
std::vector<double> X_col = X;
|
|
|
|
// Ensure Vq is a column vector if it's a 1D vector
|
|
std::vector<std::vector<double>> Vq_col = Vq;
|
|
if (Vq.size() == 1) {
|
|
Vq_col = std::vector<std::vector<double>>(Vq[0].size(), std::vector<double>(1));
|
|
for (size_t i = 0; i < Vq[0].size(); ++i) {
|
|
Vq_col[i][0] = Vq[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>> V(X_col.size(), std::vector<double>(Xq_col[0].size(), 0.0));
|
|
double dX = X_col[1] - X_col[0];
|
|
size_t N = V.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;
|
|
for (size_t i = 0; i < Xq_col.size(); ++i) {
|
|
double shift = (Xq_col[i][col] - X_col[0]) / dX;
|
|
if (Xq_col[i][col] >= X_col[0] && Xq_col[i][col] <= X_col.back()) {
|
|
f.push_back(shift);
|
|
fMask.push_back(true);
|
|
} else {
|
|
fMask.push_back(false);
|
|
}
|
|
}
|
|
|
|
// 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];
|
|
}
|
|
|
|
// Calculate adjoint interpolation
|
|
std::vector<double> VqM;
|
|
for (size_t i = 0; i < fMask.size(); ++i) {
|
|
if (fMask[i]) {
|
|
VqM.push_back(Vq_col[i][col]);
|
|
}
|
|
}
|
|
|
|
std::vector<double> pG(N, 0.0), pF(N, 0.0);
|
|
for (size_t i = 0; i < im.size(); ++i) {
|
|
pG[im[i] + 1] += gx[i] * VqM[i];
|
|
if (im[i] + 2 <= N) {
|
|
pF[im[i] + 2] += fx[i] * VqM[i];
|
|
}
|
|
}
|
|
|
|
pF.resize(N - im2Inval);
|
|
|
|
for (size_t i = 0; i < N; ++i) {
|
|
V[i][col] = pG[i] + (i < pF.size() ? pF[i] : 0.0);
|
|
}
|
|
}
|
|
|
|
return V;
|
|
}
|
|
|
|
|