add abstract_interpolation.h

include/utils/base/abstract_interpolation.h

@@ -0,0 +1,31 @@
+#ifndef INTERPOLATION_BASE_H
+#define INTERPOLATION_BASE_H
+
+#include <vector>
+#include <stdexcept>
+
+// 插值基类
+class AbstractInterpolation {
+protected:
+    std::vector<double> x_points;  // 已知点的x坐标
+    std::vector<double> y_points;  // 已知点的y坐标
+
+    // 查找x所在的区间索引（二分查找）
+    int findInterval(double x) const;
+
+public:
+    // 构造函数
+    AbstractInterpolation(const std::vector<double>& x, const std::vector<double>& y);
+    
+    // 析构函数
+    virtual ~AbstractInterpolation() = default;
+
+    // 纯虚函数：单个点插值
+    virtual double interpolate(double x) const = 0;
+
+    // 批量插值（默认实现）
+    virtual std::vector<double> interpolate(const std::vector<double>& x_values) const;
+};
+
+#endif // INTERPOLATION_BASE_H
+    

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

src/utils/base/abstract_interpolation.cpp

@@ -0,0 +1,73 @@
+#include "utils/base/abstract_interpolation.h"
+#include <algorithm>
+#include <stdexcept>
+
+// 构造函数实现
+AbstractInterpolation::AbstractInterpolation(const std::vector<double>& x, const std::vector<double>& y) {
+    // 验证输入
+    if (x.size() != y.size()) {
+        throw std::invalid_argument("x和y的长度必须相同");
+    }
+    if (x.size() < 2) {
+        throw std::invalid_argument("至少需要2个已知点才能进行插值");
+    }
+
+    // 复制并排序数据点
+    std::vector<std::pair<double, double>> 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<int>(x_points.size()) - 2;
+    }
+
+    // 二分查找
+    int left = 0;
+    int right = static_cast<int>(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<double> AbstractInterpolation::interpolate(const std::vector<double>& x_values) const {
+    std::vector<double> results;
+    results.reserve(x_values.size());
+    
+    for (double x : x_values) {
+        results.push_back(interpolate(x));
+    }
+    
+    return results;
+}
+