initial
This commit is contained in:
CaiXiang
216
include/path_curve.h.backup
Normal file
216
include/path_curve.h.backup
Normal file
@@ -0,0 +1,216 @@
|
||||
#ifndef PATH_CURVE_H
|
||||
#define PATH_CURVE_H
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <string>
|
||||
#include <string>
|
||||
#include <string>
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 路径点结构体
|
||||
*/
|
||||
struct PathPoint {
|
||||
double x; // x坐标
|
||||
double y; // y坐标
|
||||
double theta; // 切线方向角
|
||||
double kappa; // 曲率
|
||||
|
||||
PathPoint(double x_ = 0.0, double y_ = 0.0,
|
||||
double theta_ = 0.0, double kappa_ = 0.0)
|
||||
: x(x_), y(y_), theta(theta_), kappa(kappa_) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief 路径曲线类
|
||||
* 支持多种曲线类型：直线、圆弧、贝塞尔曲线等
|
||||
*/
|
||||
class PathCurve {
|
||||
public:
|
||||
enum CurveType {
|
||||
LINE, // 直线
|
||||
CIRCLE_ARC, // 圆弧
|
||||
CUBIC_BEZIER, // 三次贝塞尔曲线
|
||||
SPLINE // 样条曲线
|
||||
};
|
||||
|
||||
PathCurve() = default;
|
||||
|
||||
/**
|
||||
* @brief 生成直线路径
|
||||
* @param start 起点
|
||||
* @param end 终点
|
||||
* @param num_points 路径点数量
|
||||
*/
|
||||
void generateLine(const PathPoint& start, const PathPoint& end, int num_points = 100);
|
||||
|
||||
/**
|
||||
* @brief 生成圆弧路径
|
||||
* @param center_x 圆心x坐标
|
||||
* @param center_y 圆心y坐标
|
||||
* @param radius 半径
|
||||
* @param start_angle 起始角度 (rad)
|
||||
* @param end_angle 终止角度 (rad)
|
||||
* @param num_points 路径点数量
|
||||
*/
|
||||
void generateCircleArc(double center_x, double center_y, double radius,
|
||||
double start_angle, double end_angle, int num_points = 100);
|
||||
|
||||
/**
|
||||
* @brief 生成三次贝塞尔曲线
|
||||
* @param p0 起点
|
||||
* @param p1 第一个控制点
|
||||
* @param p2 第二个控制点
|
||||
* @param p3 终点
|
||||
* @param num_points 路径点数量
|
||||
*/
|
||||
void generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
|
||||
const PathPoint& p2, const PathPoint& p3,
|
||||
int num_points = 100);
|
||||
|
||||
/**
|
||||
* @brief 从路径点数组生成路径
|
||||
* @param points 路径点数组
|
||||
*/
|
||||
void setPathPoints(const std::vector<PathPoint>& points);
|
||||
|
||||
/**
|
||||
* @brief 从CSV文件加载路径点
|
||||
* @param filename CSV文件路径
|
||||
* @param has_header 是否包含表头（默认true）
|
||||
* @return 是否加载成功
|
||||
*/
|
||||
bool loadFromCSV(const std::string& filename, bool has_header = true);
|
||||
|
||||
/**
|
||||
* @brief 将路径点保存到CSV文件
|
||||
* @param filename CSV文件路径
|
||||
* @return 是否保存成功
|
||||
*/
|
||||
bool saveToCSV(const std::string& filename) const;
|
||||
|
||||
/**
|
||||
* @brief 使用样条插值生成路径
|
||||
* @param key_points 关键路径点
|
||||
* @param num_points 生成的路径点总数
|
||||
* @param tension 张力参数
|
||||
*/
|
||||
void generateSpline(const std::vector<PathPoint>& key_points,
|
||||
int num_points = 100,
|
||||
double tension = 0.5);
|
||||
|
||||
/**
|
||||
* @brief 从CSV文件加载路径点
|
||||
* @param filename CSV文件路径
|
||||
* @param has_header 是否包含表头（默认true）
|
||||
* @return 是否加载成功
|
||||
*/
|
||||
bool loadFromCSV(const std::string& filename, bool has_header = true);
|
||||
|
||||
/**
|
||||
* @brief 将路径点保存到CSV文件
|
||||
* @param filename CSV文件路径
|
||||
* @return 是否保存成功
|
||||
*/
|
||||
bool saveToCSV(const std::string& filename) const;
|
||||
|
||||
/**
|
||||
* @brief 使用样条插值生成路径
|
||||
* @param key_points 关键路径点
|
||||
* @param num_points 生成的路径点总数
|
||||
* @param tension 张力参数
|
||||
*/
|
||||
void generateSpline(const std::vector<PathPoint>& key_points,
|
||||
int num_points = 100,
|
||||
double tension = 0.5);
|
||||
|
||||
/**
|
||||
* @brief 从CSV文件加载路径点
|
||||
* @param filename CSV文件路径
|
||||
* @param has_header 是否包含表头（默认true）
|
||||
* @return 是否加载成功
|
||||
*/
|
||||
bool loadFromCSV(const std::string& filename, bool has_header = true);
|
||||
|
||||
/**
|
||||
* @brief 将路径点保存到CSV文件
|
||||
* @param filename CSV文件路径
|
||||
* @return 是否保存成功
|
||||
*/
|
||||
bool saveToCSV(const std::string& filename) const;
|
||||
|
||||
/**
|
||||
* @brief 使用样条插值生成路径
|
||||
* @param key_points 关键路径点
|
||||
* @param num_points 生成的路径点总数
|
||||
* @param tension 张力参数
|
||||
*/
|
||||
void generateSpline(const std::vector<PathPoint>& key_points,
|
||||
int num_points = 100,
|
||||
double tension = 0.5);
|
||||
|
||||
/**
|
||||
* @brief 从CSV文件加载路径点
|
||||
* @param filename CSV文件路径
|
||||
* @param has_header 是否包含表头（默认true）
|
||||
* @return 是否加载成功
|
||||
*/
|
||||
bool loadFromCSV(const std::string& filename, bool has_header = true);
|
||||
|
||||
/**
|
||||
* @brief 将路径点保存到CSV文件
|
||||
* @param filename CSV文件路径
|
||||
* @return 是否保存成功
|
||||
*/
|
||||
bool saveToCSV(const std::string& filename) const;
|
||||
|
||||
/**
|
||||
* @brief 使用样条插值生成路径
|
||||
* @param key_points 关键路径点
|
||||
* @param num_points 生成的路径点总数
|
||||
* @param tension 张力参数
|
||||
*/
|
||||
void generateSpline(const std::vector<PathPoint>& key_points,
|
||||
int num_points = 100,
|
||||
double tension = 0.5);
|
||||
|
||||
/**
|
||||
* @brief 获取路径点
|
||||
*/
|
||||
const std::vector<PathPoint>& getPathPoints() const { return path_points_; }
|
||||
|
||||
/**
|
||||
* @brief 获取指定参数t处的路径点（线性插值）
|
||||
* @param t 参数 [0, 1]
|
||||
* @return 插值后的路径点
|
||||
*/
|
||||
PathPoint getPointAt(double t) const;
|
||||
|
||||
/**
|
||||
* @brief 获取路径长度
|
||||
*/
|
||||
double getPathLength() const;
|
||||
|
||||
/**
|
||||
* @brief 找到最近的路径点索引
|
||||
* @param x 查询点x坐标
|
||||
* @param y 查询点y坐标
|
||||
* @return 最近点的索引
|
||||
*/
|
||||
int findNearestPoint(double x, double y) const;
|
||||
|
||||
private:
|
||||
std::vector<PathPoint> path_points_;
|
||||
|
||||
// 计算两点间的曲率
|
||||
double computeCurvature(const PathPoint& p1, const PathPoint& p2,
|
||||
const PathPoint& p3) const;
|
||||
};
|
||||
|
||||
#endif // PATH_CURVE_H
|
||||
Reference in New Issue
Block a user