diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index 1a3712f..033026f 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -99,28 +99,20 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
- // 4. 路径优化（临时禁用以避免穿越问题）
- // var optimizedPath = OptimizePath(pathResult.PathPoints, gridMap);
- var optimizedPath = pathResult.PathPoints; // 直接使用原始A*路径，不进行优化
- LogManager.Info($"[2.5D路径规划] 路径优化已禁用，使用原始A*路径，包含 {optimizedPath.Count} 个点");
-
- // 5. 对路径进行高度插值，利用2.5D高度区间信息
- var enhancedPath = InterpolatePathHeights2_5D(optimizedPath, gridMap, vehicleHeight);
+ // 5. 根据网格点通行高度区间对路径点Z坐标调整
+ var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
// 6. 精确高度修正
var heightCorrectedPath = ApplyPreciseHeightCorrection(enhancedPath, gridMap);
- // 7. 直接返回高度校正后的路径（不再修改XY坐标）
- var finalPath = heightCorrectedPath;
-
// 8. 验证路径中所有点的高度约束（特别是门网格）
- ValidatePathHeightConstraints(finalPath, gridMap, vehicleHeight);
+ ValidatePathHeightConstraints(heightCorrectedPath, gridMap, vehicleHeight);
// 更新PathFindingResult
- pathResult.PathPoints = finalPath;
+ pathResult.PathPoints = heightCorrectedPath;
pathResult.OriginalEndPoint = originalEnd;
- LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {finalPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
+ LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {heightCorrectedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
// 路径优化 - 去除共线点
pathResult = OptimizePath(pathResult, gridMap);
@@ -1232,8 +1224,6 @@ namespace NavisworksTransport.PathPlanning
}
var cell = gridMap.Cells[gridX, gridY];
- LogManager.Info($"[高度检查] 单元格状态：IsWalkable={cell.IsWalkable}, CellType={cell.CellType}, PassableHeights计数={cell.PassableHeights?.Count ?? 0}");
-
// 检查基本可行走性
if (!cell.IsWalkable)
{
@@ -1244,8 +1234,6 @@ namespace NavisworksTransport.PathPlanning
// 检查高度约束
if (cell.PassableHeights != null && cell.PassableHeights.Any())
{
- LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间，车辆高度：{vehicleHeight}模型单位)");
-
foreach (var interval in cell.PassableHeights)
{
bool spanOk = interval.GetSpan() >= vehicleHeight;
@@ -1256,7 +1244,6 @@ namespace NavisworksTransport.PathPlanning
if (spanOk && containsHeight)
{
- LogManager.Info($"[高度检查] ✅ 找到合适的高度区间，通过检查");
return true;
}
}
@@ -1275,101 +1262,44 @@ namespace NavisworksTransport.PathPlanning
}
///
- /// 使用2.5D高度区间信息对路径进行高度插值
+ /// 根据网格高度区间调整路径点Z坐标
///
/// 原始路径
/// 网格地图
/// 车辆高度（模型单位）
- /// 插值后的路径
- private List InterpolatePathHeights2_5D(List path, GridMap gridMap, double vehicleHeight)
+ /// 调整后的路径
+ private List ApplyGridHeightConstraints(List path, GridMap gridMap, double vehicleHeight)
{
- try
+ LogManager.Info($"[应用高度区间] 开始调整，路径点数: {path.Count}");
+
+ var adjustedPath = new List();
+ int adjustedCount = 0;
+
+ foreach (var point in path)
{
- LogManager.Info($"[2.5D高度插值] 开始插值，路径点数: {path.Count}");
-
- var interpolatedPath = new List();
- int validHeightPoints = 0;
-
- for (int i = 0; i < path.Count; i++)
- {
- var originalPoint = path[i];
-
- // 获取最优高度
- var optimalHeight = GetOptimalHeightFromGrid(originalPoint, gridMap, vehicleHeight);
-
- if (optimalHeight.HasValue)
- {
- var enhancedPoint = new Point3D(originalPoint.X, originalPoint.Y, optimalHeight.Value);
- interpolatedPath.Add(enhancedPoint);
- validHeightPoints++;
- }
- else
- {
- // 如果无法获取高度信息，使用原始点
- interpolatedPath.Add(originalPoint);
- }
- }
-
- LogManager.Info($"[2.5D高度插值] 完成，有效高度点: {validHeightPoints}/{path.Count}");
- return interpolatedPath;
- }
- catch (Exception ex)
- {
- LogManager.Error($"[2.5D高度插值] 插值失败: {ex.Message}");
- return path; // 返回原始路径
- }
- }
-
- ///
- /// 从网格中获取指定位置的最优高度
- ///
- /// 世界坐标点
- /// 网格地图
- /// 车辆高度（模型单位）
- /// 最优高度，如果无法确定则返回null
- private double? GetOptimalHeightFromGrid(Point3D point, GridMap gridMap, double vehicleHeight)
- {
- try
- {
- // 将世界坐标转换为网格坐标
var gridPos = gridMap.WorldToGrid(point);
- int gridX = gridPos.X;
- int gridY = gridPos.Y;
- // 确保坐标在有效范围内
- if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height)
+ // 检查网格有效性
+ if (!gridMap.IsValidGridPosition(gridPos))
{
- return null;
+ adjustedPath.Add(point);
+ continue;
}
- var cell = gridMap.Cells[gridX, gridY];
+ var cell = gridMap.Cells[gridPos.X, gridPos.Y];
- // 如果有高度区间信息
- if (cell.PassableHeights != null && cell.PassableHeights.Any())
- {
- // 查找满足车辆高度要求的最佳区间
- // vehicleHeight已经是模型单位，直接比较即可
- var validIntervals = cell.PassableHeights
- .Where(interval => interval.GetSpan() >= vehicleHeight)
- .OrderBy(interval => interval.MinZ)
- .ToList();
+ var bestInterval = cell.PassableHeights
+ .Where(interval => interval.GetSpan() >= vehicleHeight)
+ .OrderBy(interval => interval.MinZ)
+ .FirstOrDefault();
- if (validIntervals.Any())
- {
- // 选择最低的有效区间的底部高度，转换为绝对坐标
- var bestInterval = validIntervals.First();
- return cell.WorldPosition.Z + bestInterval.MinZ;
- }
- }
-
- // 如果没有高度信息，返回原始Z坐标
- return point.Z;
- }
- catch (Exception ex)
- {
- LogManager.Error($"[最优高度获取] 获取失败: {ex.Message}");
- return point.Z;
+ var optimalZ = cell.WorldPosition.Z + bestInterval.MinZ;
+ adjustedPath.Add(new Point3D(point.X, point.Y, optimalZ));
+ adjustedCount++;
}
+
+ LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{path.Count} 个点");
+ return adjustedPath;
}
///