227 lines
7.1 KiB
Plaintext
227 lines
7.1 KiB
Plaintext
#include "path_curve.h"
|
|
#include <limits>
|
|
#include <algorithm>
|
|
|
|
#define _USE_MATH_DEFINES
|
|
#include <cmath>
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
|
|
void PathCurve::generateLine(const PathPoint& start, const PathPoint& end, int num_points) {
|
|
path_points_.clear();
|
|
path_points_.reserve(num_points);
|
|
|
|
for (int i = 0; i < num_points; ++i) {
|
|
double t = static_cast<double>(i) / (num_points - 1);
|
|
PathPoint p;
|
|
p.x = start.x + t * (end.x - start.x);
|
|
p.y = start.y + t * (end.y - start.y);
|
|
p.theta = std::atan2(end.y - start.y, end.x - start.x);
|
|
p.kappa = 0.0; // 直线曲率为0
|
|
|
|
path_points_.push_back(p);
|
|
}
|
|
}
|
|
|
|
void PathCurve::generateCircleArc(double center_x, double center_y, double radius,
|
|
double start_angle, double end_angle, int num_points) {
|
|
path_points_.clear();
|
|
path_points_.reserve(num_points);
|
|
|
|
double angle_diff = end_angle - start_angle;
|
|
// 归一化角度差
|
|
while (angle_diff > M_PI) angle_diff -= 2.0 * M_PI;
|
|
while (angle_diff < -M_PI) angle_diff += 2.0 * M_PI;
|
|
|
|
for (int i = 0; i < num_points; ++i) {
|
|
double t = static_cast<double>(i) / (num_points - 1);
|
|
double angle = start_angle + t * angle_diff;
|
|
|
|
PathPoint p;
|
|
p.x = center_x + radius * std::cos(angle);
|
|
p.y = center_y + radius * std::sin(angle);
|
|
|
|
// 切线方向垂直于半径方向
|
|
if (angle_diff > 0) {
|
|
p.theta = angle + M_PI / 2.0;
|
|
} else {
|
|
p.theta = angle - M_PI / 2.0;
|
|
}
|
|
|
|
// 圆的曲率是半径的倒数
|
|
p.kappa = 1.0 / radius;
|
|
if (angle_diff < 0) p.kappa = -p.kappa;
|
|
|
|
path_points_.push_back(p);
|
|
}
|
|
}
|
|
|
|
void PathCurve::generateCubicBezier(const PathPoint& p0, const PathPoint& p1,
|
|
const PathPoint& p2, const PathPoint& p3,
|
|
int num_points) {
|
|
path_points_.clear();
|
|
path_points_.reserve(num_points);
|
|
|
|
for (int i = 0; i < num_points; ++i) {
|
|
double t = static_cast<double>(i) / (num_points - 1);
|
|
double t2 = t * t;
|
|
double t3 = t2 * t;
|
|
double mt = 1.0 - t;
|
|
double mt2 = mt * mt;
|
|
double mt3 = mt2 * mt;
|
|
|
|
// 贝塞尔曲线公式
|
|
PathPoint p;
|
|
p.x = mt3 * p0.x + 3.0 * mt2 * t * p1.x + 3.0 * mt * t2 * p2.x + t3 * p3.x;
|
|
p.y = mt3 * p0.y + 3.0 * mt2 * t * p1.y + 3.0 * mt * t2 * p2.y + t3 * p3.y;
|
|
|
|
// 一阶导数（切线方向）
|
|
double dx = 3.0 * mt2 * (p1.x - p0.x) + 6.0 * mt * t * (p2.x - p1.x) +
|
|
3.0 * t2 * (p3.x - p2.x);
|
|
double dy = 3.0 * mt2 * (p1.y - p0.y) + 6.0 * mt * t * (p2.y - p1.y) +
|
|
3.0 * t2 * (p3.y - p2.y);
|
|
|
|
p.theta = std::atan2(dy, dx);
|
|
|
|
// 二阶导数（用于计算曲率）
|
|
double ddx = 6.0 * mt * (p2.x - 2.0 * p1.x + p0.x) +
|
|
6.0 * t * (p3.x - 2.0 * p2.x + p1.x);
|
|
double ddy = 6.0 * mt * (p2.y - 2.0 * p1.y + p0.y) +
|
|
6.0 * t * (p3.y - 2.0 * p2.y + p1.y);
|
|
|
|
// 曲率公式 κ = (x'y'' - y'x'') / (x'^2 + y'^2)^(3/2)
|
|
double velocity_squared = dx * dx + dy * dy;
|
|
if (velocity_squared > 1e-6) {
|
|
p.kappa = (dx * ddy - dy * ddx) / std::pow(velocity_squared, 1.5);
|
|
} else {
|
|
p.kappa = 0.0;
|
|
}
|
|
|
|
path_points_.push_back(p);
|
|
}
|
|
}
|
|
|
|
void PathCurve::setPathPoints(const std::vector<PathPoint>& points) {
|
|
path_points_ = points;
|
|
|
|
// 计算每个点的切线方向和曲率
|
|
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]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
PathPoint PathCurve::getPointAt(double t) const {
|
|
if (path_points_.empty()) {
|
|
return PathPoint();
|
|
}
|
|
|
|
if (path_points_.size() == 1) {
|
|
return path_points_[0];
|
|
}
|
|
|
|
// 限制t在[0, 1]范围内
|
|
t = std::max(0.0, std::min(1.0, t));
|
|
double index_float = t * (path_points_.size() - 1);
|
|
size_t index = static_cast<size_t>(index_float);
|
|
|
|
if (index >= path_points_.size() - 1) {
|
|
return path_points_.back();
|
|
}
|
|
|
|
// 线性插值
|
|
double alpha = index_float - index;
|
|
const PathPoint& p1 = path_points_[index];
|
|
const PathPoint& p2 = path_points_[index + 1];
|
|
|
|
PathPoint result;
|
|
result.x = p1.x + alpha * (p2.x - p1.x);
|
|
result.y = p1.y + alpha * (p2.y - p1.y);
|
|
result.theta = p1.theta + alpha * (p2.theta - p1.theta);
|
|
result.kappa = p1.kappa + alpha * (p2.kappa - p1.kappa);
|
|
|
|
return result;
|
|
}
|
|
|
|
double PathCurve::getPathLength() const {
|
|
double length = 0.0;
|
|
for (size_t i = 1; i < path_points_.size(); ++i) {
|
|
double dx = path_points_[i].x - path_points_[i-1].x;
|
|
double dy = path_points_[i].y - path_points_[i-1].y;
|
|
length += std::sqrt(dx * dx + dy * dy);
|
|
}
|
|
return length;
|
|
}
|
|
|
|
int PathCurve::findNearestPoint(double x, double y) const {
|
|
if (path_points_.empty()) {
|
|
return -1;
|
|
}
|
|
|
|
int nearest_index = 0;
|
|
double min_distance = std::numeric_limits<double>::max();
|
|
|
|
for (size_t i = 0; i < path_points_.size(); ++i) {
|
|
double dx = x - path_points_[i].x;
|
|
double dy = y - path_points_[i].y;
|
|
double distance = std::sqrt(dx * dx + dy * dy);
|
|
|
|
if (distance < min_distance) {
|
|
min_distance = distance;
|
|
nearest_index = static_cast<int>(i);
|
|
}
|
|
}
|
|
|
|
return nearest_index;
|
|
}
|
|
|
|
double PathCurve::computeCurvature(const PathPoint& p1, const PathPoint& p2,
|
|
const PathPoint& p3) const {
|
|
// 使用三点计算曲率
|
|
double dx1 = p2.x - p1.x;
|
|
double dy1 = p2.y - p1.y;
|
|
double dx2 = p3.x - p2.x;
|
|
double dy2 = p3.y - p2.y;
|
|
|
|
double cross = dx1 * dy2 - dy1 * dx2;
|
|
double d1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
|
|
double d2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
|
|
|
|
if (d1 < 1e-6 || d2 < 1e-6) {
|
|
return 0.0;
|
|
}
|
|
|
|
// Menger曲率公式
|
|
double area = std::abs(cross) / 2.0;
|
|
double d3_sq = (p3.x - p1.x) * (p3.x - p1.x) + (p3.y - p1.y) * (p3.y - p1.y);
|
|
double d3 = std::sqrt(d3_sq);
|
|
|
|
if (d3 < 1e-6) {
|
|
return 0.0;
|
|
}
|
|
|
|
return 4.0 * area / (d1 * d2 * d3);
|
|
}
|