diff --git a/include/utils/base/abstract_interpolation.h b/include/utils/base/abstract_interpolation.h new file mode 100644 index 00000000..97c84f3f --- /dev/null +++ b/include/utils/base/abstract_interpolation.h @@ -0,0 +1,31 @@ +#ifndef INTERPOLATION_BASE_H +#define INTERPOLATION_BASE_H + +#include +#include + +// 插值基类 +class AbstractInterpolation { +protected: + std::vector x_points; // 已知点的x坐标 + std::vector y_points; // 已知点的y坐标 + + // 查找x所在的区间索引(二分查找) + int findInterval(double x) const; + +public: + // 构造函数 + AbstractInterpolation(const std::vector& x, const std::vector& y); + + // 析构函数 + virtual ~AbstractInterpolation() = default; + + // 纯虚函数:单个点插值 + virtual double interpolate(double x) const = 0; + + // 批量插值(默认实现) + virtual std::vector interpolate(const std::vector& x_values) const; +}; + +#endif // INTERPOLATION_BASE_H + \ No newline at end of file diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 64341ad7..85a60678 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -10,6 +10,7 @@ include_directories(${TINYXML2_INCLUDE_DIRS}) add_library(utils STATIC base/thread_pool.cpp base/timer.cpp + base/abstract_interpolation.cpp dynamics/inertial.cpp dynamics/joint.cpp dynamics/link.cpp diff --git a/src/utils/base/abstract_interpolation.cpp b/src/utils/base/abstract_interpolation.cpp new file mode 100644 index 00000000..65d645f4 --- /dev/null +++ b/src/utils/base/abstract_interpolation.cpp @@ -0,0 +1,73 @@ +#include "utils/base/abstract_interpolation.h" +#include +#include + +// 构造函数实现 +AbstractInterpolation::AbstractInterpolation(const std::vector& x, const std::vector& y) { + // 验证输入 + if (x.size() != y.size()) { + throw std::invalid_argument("x和y的长度必须相同"); + } + if (x.size() < 2) { + throw std::invalid_argument("至少需要2个已知点才能进行插值"); + } + + // 复制并排序数据点 + std::vector> points; + points.reserve(x.size()); + + for (size_t i = 0; i < x.size(); ++i) { + points.emplace_back(x[i], y[i]); + } + + // 按x坐标排序 + std::sort(points.begin(), points.end()); + + // 分离排序后的x和y + x_points.reserve(points.size()); + y_points.reserve(points.size()); + + for (const auto& p : points) { + x_points.push_back(p.first); + y_points.push_back(p.second); + } +} + +// 查找区间索引实现 +int AbstractInterpolation::findInterval(double x) const { + // 处理边界情况 + if (x <= x_points[0]) { + return 0; + } + if (x >= x_points.back()) { + return static_cast(x_points.size()) - 2; + } + + // 二分查找 + int left = 0; + int right = static_cast(x_points.size()) - 1; + + while (right - left > 1) { + int mid = (left + right) / 2; + if (x < x_points[mid]) { + right = mid; + } else { + left = mid; + } + } + + return left; +} + +// 批量插值实现 +std::vector AbstractInterpolation::interpolate(const std::vector& x_values) const { + std::vector results; + results.reserve(x_values.size()); + + for (double x : x_values) { + results.push_back(interpolate(x)); + } + + return results; +} + \ No newline at end of file