commit for push
This commit is contained in:
14
src/path_curve_fix.txt
Normal file
14
src/path_curve_fix.txt
Normal file
@@ -0,0 +1,14 @@
|
||||
} 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]);
|
||||
}
|
||||
}
|
||||
// 单点情况:保持原有的theta和kappa值(通常为0)
|
||||
// 不需要额外计算,避免越界访问
|
||||
Reference in New Issue
Block a user