139 lines
3.7 KiB
C++
139 lines
3.7 KiB
C++
#ifndef PATH_CURVE_H
|
||
#define PATH_CURVE_H
|
||
|
||
#include <vector>
|
||
#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 获取路径点
|
||
*/
|
||
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
|