目录结构调整

src/control/path_curve_custom.cpp.backup

@@ -0,0 +1,190 @@
+#include "path_curve.h"
+#include <fstream>
+#include <sstream>
+#include <iostream>
+
+// CSV加载功能实现
+bool PathCurve::loadFromCSV(const std::string& filename, bool has_header) {
+    std::ifstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Error: Cannot open file " << filename << std::endl;
+        return false;
+    }
+
+    std::vector<PathPoint> points;
+    std::string line;
+    int line_num = 0;
+
+    // 跳过表头
+    if (has_header && std::getline(file, line)) {
+        line_num++;
+    }
+
+    while (std::getline(file, line)) {
+        line_num++;
+        // 跳过空行和注释行
+        if (line.empty() || line[0] == '#') {
+            continue;
+        }
+
+        std::stringstream ss(line);
+        std::string token;
+        std::vector<double> values;
+        bool parse_error = false;
+
+        // 解析CSV行
+        while (std::getline(ss, token, ',')) {
+            try {
+                // 去除前后空格
+                size_t start = token.find_first_not_of(" \t\r\n");
+                size_t end = token.find_last_not_of(" \t\r\n");
+                if (start == std::string::npos) {
+                    // 空token，跳过整行
+                    parse_error = true;
+                    break;
+                }
+                std::string trimmed = token.substr(start, end - start + 1);
+                values.push_back(std::stod(trimmed));
+            } catch (const std::exception&) {
+                std::cerr << "Error parsing line " << line_num << ": " << line << std::endl;
+                parse_error = true;
+                break;
+            }
+        }
+
+        // 如果解析出错或值数量不足，跳过整行
+        if (parse_error) {
+            continue;
+        }
+
+        // 根据列数创建路径点
+        if (values.size() >= 2) {
+            PathPoint p;
+            p.x = values[0];
+            p.y = values[1];
+            p.theta = (values.size() >= 3) ? values[2] : 0.0;
+            p.kappa = (values.size() >= 4) ? values[3] : 0.0;
+            points.push_back(p);
+        }
+    }
+
+    file.close();
+
+    if (points.empty()) {
+        std::cerr << "Error: No valid path points loaded from " << filename << std::endl;
+        return false;
+    }
+
+    // 设置路径点（会自动计算theta和kappa）
+    setPathPoints(points);
+    
+    std::cout << "Successfully loaded " << points.size() << " points from " << filename << std::endl;
+    return true;
+}
+
+// CSV保存功能实现
+bool PathCurve::saveToCSV(const std::string& filename) const {
+    std::ofstream file(filename);
+    if (!file.is_open()) {
+        std::cerr << "Error: Cannot create file " << filename << std::endl;
+        return false;
+    }
+
+    // 写入表头
+    file << "# Custom Path Data\n";
+    file << "# x(m), y(m), theta(rad), kappa(1/m)\n";
+
+    // 写入路径点
+    for (const auto& p : path_points_) {
+        file << p.x << ", " << p.y << ", " << p.theta << ", " << p.kappa << "\n";
+    }
+
+    file.close();
+    std::cout << "Successfully saved " << path_points_.size() << " points to " << filename << std::endl;
+    return true;
+}
+
+// Catmull-Rom样条插值实现
+void PathCurve::generateSpline(const std::vector<PathPoint>& key_points,
+                               int num_points,
+                               double tension) {
+    if (key_points.size() < 2) {
+        std::cerr << "Error: Need at least 2 key points for spline interpolation" << std::endl;
+        return;
+    }
+
+    path_points_.clear();
+    path_points_.reserve(num_points);
+
+    // 参数化张力 (0 = 最平滑, 1 = 最紧)
+    double s = (1.0 - tension) / 2.0;
+
+    // 对每一段进行插值
+    int segments = static_cast<int>(key_points.size()) - 1;
+    int points_per_segment = num_points / segments;
+
+    for (int seg = 0; seg < segments; ++seg) {
+        // 获取控制点（使用边界处理）
+        PathPoint p0 = (seg == 0) ? key_points[0] : key_points[seg - 1];
+        PathPoint p1 = key_points[seg];
+        PathPoint p2 = key_points[seg + 1];
+        PathPoint p3 = (seg == segments - 1) ? key_points[seg + 1] : key_points[seg + 2];
+
+        // 对每一段生成点
+        int points_in_this_segment = (seg == segments - 1) ? 
+            (num_points - seg * points_per_segment) : points_per_segment;
+
+        for (int i = 0; i < points_in_this_segment; ++i) {
+            double t = static_cast<double>(i) / points_per_segment;
+            double t2 = t * t;
+            double t3 = t2 * t;
+
+            // Catmull-Rom 基函数
+            double b0 = s * ((-t3 + 2.0*t2 - t));
+            double b1 = s * ((3.0*t3 - 5.0*t2) / 2.0) + 1.0;
+            double b2 = s * ((-3.0*t3 + 4.0*t2 + t) / 2.0);
+            double b3 = s * (t3 - t2);
+
+            // 调整系数以确保通过控制点
+            if (seg > 0 || i > 0) {
+                b0 *= 2.0;
+                b1 = b1 * 2.0 - b0 / 2.0;
+                b2 = b2 * 2.0 - b3 / 2.0;
+                b3 *= 2.0;
+            }
+
+            PathPoint p;
+            p.x = b0*p0.x + b1*p1.x + b2*p2.x + b3*p3.x;
+            p.y = b0*p0.y + b1*p1.y + b2*p2.y + b3*p3.y;
+            p.theta = 0.0;  // 将由setPathPoints计算
+            p.kappa = 0.0;  // 将由setPathPoints计算
+
+            path_points_.push_back(p);
+        }
+    }
+
+    // 重新计算所有点的theta和kappa
+    for (size_t i = 0; i < path_points_.size(); ++i) {
+        if (i == 0 && path_points_.size() > 1) {
+            double dx = path_points_[i + 1].x - path_points_[i].x;
+            double dy = path_points_[i + 1].y - path_points_[i].y;
+            path_points_[i].theta = std::atan2(dy, dx);
+        } else if (i == path_points_.size() - 1 && path_points_.size() > 1) {
+            double dx = path_points_[i].x - path_points_[i - 1].x;
+            double dy = path_points_[i].y - path_points_[i - 1].y;
+            path_points_[i].theta = std::atan2(dy, dx);
+        } else if (path_points_.size() > 2) {
+            double dx = path_points_[i + 1].x - path_points_[i - 1].x;
+            double dy = path_points_[i + 1].y - path_points_[i - 1].y;
+            path_points_[i].theta = std::atan2(dy, dx);
+
+            if (i > 0 && i < path_points_.size() - 1) {
+                path_points_[i].kappa = computeCurvature(
+                    path_points_[i - 1], path_points_[i], path_points_[i + 1]);
+            }
+        }
+    }
+
+    std::cout << "Generated spline with " << path_points_.size() 
+              << " points from " << key_points.size() << " key points" << std::endl;
+}