From 163986f9e56c9afe6852ef2b6e99dbc5305c1d8b Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Fri, 10 Oct 2025 02:45:20 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E4=BA=86=E5=AE=89=E5=85=A8?=
=?UTF-8?q?=E4=BC=98=E5=85=88=E5=92=8C=E7=9B=B4=E7=BA=BF=E4=BC=98=E5=85=88?=
=?UTF-8?q?=E7=9A=84=E7=AE=97=E6=B3=95=EF=BC=8C=E5=8E=BB=E6=8E=89=E4=BA=86?=
=?UTF-8?q?cell.WorldPosition=EF=BC=8C=E4=BD=86=E9=AB=98=E5=BA=A6=E5=B1=82?=
=?UTF-8?q?=E5=BC=95=E8=B5=B7=E7=9A=84=E9=97=AE=E9=A2=98=E8=BF=98=E9=9C=80?=
=?UTF-8?q?=E8=A6=81=E8=BF=9B=E4=B8=80=E6=AD=A5=E5=AE=8C=E5=96=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/PathPlanning/AutoPathFinder.cs | 100 +++++++++++++++++++-
src/PathPlanning/ChannelBasedGridBuilder.cs | 4 +-
src/PathPlanning/GridCellBuilder.cs | 6 --
src/PathPlanning/GridMap.cs | 21 ++--
src/PathPlanning/GridMapGenerator.cs | 6 +-
5 files changed, 110 insertions(+), 27 deletions(-)
diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index ed9a3e3..2a70a1d 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -126,10 +126,19 @@ namespace NavisworksTransport.PathPlanning
}
}
+ ///
+ /// 速度计算委托：根据当前节点、邻居节点和其他上下文信息计算边的速度
+ ///
+ private delegate float SpeedCalculator(
+ int currentX, int currentY, int currentLayerIndex, double currentZ,
+ int neighborX, int neighborY, int neighborLayerIndex, double neighborZ,
+ double heightDiff, double baseSpeed, double maxHeightDiffInModelUnits,
+ Point3D startPos, Point3D endPos);
+
///
/// 构建3D图（每个(x,y,layerIndex)作为独立节点）
///
- private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
+ private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, SpeedCalculator speedCalculator = null)
{
LogManager.Info("[3D图构建] 开始构建真3D图");
@@ -239,7 +248,18 @@ namespace NavisworksTransport.PathPlanning
if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
continue;
- float speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
+ // 使用策略特定的速度计算器，如果未提供则使用默认的高度差计算
+ float speed;
+ if (speedCalculator != null)
+ {
+ speed = speedCalculator(x, y, li, currentZ, nx, ny, nli, neighborLayer.Z,
+ heightDiff, baseSpeed, maxHeightDiffInModelUnits, startPos, endPos);
+ }
+ else
+ {
+ speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
+ }
+
maxSpeed = Math.Max(maxSpeed, speed);
var velocity = Velocity.FromKilometersPerHour(speed);
@@ -347,7 +367,73 @@ namespace NavisworksTransport.PathPlanning
///
private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
{
- return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
+ SpeedCalculator speedCalc = null;
+
+ switch (strategy)
+ {
+ case PathStrategy.Shortest:
+ LogManager.Info($"[3D图构建] 使用最短路径策略");
+ // 默认策略，speedCalc = null，使用高度差计算
+ break;
+
+ case PathStrategy.Straightest:
+ LogManager.Info($"[3D图构建] 使用直线优先策略");
+ // 直线优先：计算沿边方向能够直走的距离，距离越长速度越高
+ {
+ speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
+ {
+ // 基础速度（根据高度差）
+ float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
+
+ // 计算边的方向
+ var direction = new GridPoint2D(nx - cx, ny - cy);
+ var currentPos = new GridPoint2D(cx, cy);
+
+ // 计算沿该方向能够直走的距离
+ int straightDistance = CalculateStraightDistance(currentPos, direction, gridMap, vehicleHeight);
+
+ // 直走距离越长，速度加成越高（最多+30 km/h）
+ float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
+ float finalSpeed = baseSpeedValue + bonusSpeed;
+
+ return finalSpeed;
+ };
+ }
+ break;
+
+ case PathStrategy.SafestCenter:
+ LogManager.Info($"[3D图构建] 使用安全优先策略");
+ // 安全优先：计算安全距离图，基于到障碍物的距离调整速度
+ {
+ var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
+ LogManager.Info($"[3D图构建] 安全距离图计算完成");
+
+ speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
+ {
+ // 基础速度（根据高度差）
+ float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
+
+ // 获取两个节点的安全距离，取较大值（更安全的距离）
+ int currentSafety = safetyDistanceMap[cx, cy];
+ int neighborSafety = safetyDistanceMap[nx, ny];
+ int maxSafety = Math.Max(currentSafety, neighborSafety);
+
+ // 根据安全距离计算速度系数
+ float safetySpeed = CalculateSpeedBySafetyDistance(maxSafety);
+
+ // 返回基础速度和安全速度的加权组合
+ // 高度差影响基础速度，安全距离影响额外速度
+ return baseSpeedValue * 0.3f + safetySpeed * 0.7f;
+ };
+ }
+ break;
+
+ default:
+ LogManager.Warning($"[3D图构建] 未知策略 {strategy}，使用默认最短路径");
+ break;
+ }
+
+ return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos, speedCalc);
}
///
@@ -1329,11 +1415,15 @@ namespace NavisworksTransport.PathPlanning
if (endCell.HasValue)
{
// 调试：检查门网格的Z坐标
+ double endZ = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
+ ? endCell.Value.HeightLayers[0].Z
+ : 0;
+
if (endCell.Value.CellType == CategoryAttributeManager.LogisticsElementType.门)
{
- LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) WorldPosition.Z={endCell.Value.WorldPosition.Z:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
+ LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].Z={endZ:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
}
- endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endCell.Value.WorldPosition.Z);
+ endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endZ);
}
else
{
diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs
index baaac6f..f77ceee 100644
--- a/src/PathPlanning/ChannelBasedGridBuilder.cs
+++ b/src/PathPlanning/ChannelBasedGridBuilder.cs
@@ -148,7 +148,9 @@ namespace NavisworksTransport.PathPlanning
if (cell.IsWalkable)
{
walkableCount++;
- double cellZ = cell.WorldPosition.Z;
+ double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0
+ ? cell.HeightLayers[0].Z
+ : 0;
if (cellZ < minZ)
minZ = cellZ;
diff --git a/src/PathPlanning/GridCellBuilder.cs b/src/PathPlanning/GridCellBuilder.cs
index c1d19dc..284e964 100644
--- a/src/PathPlanning/GridCellBuilder.cs
+++ b/src/PathPlanning/GridCellBuilder.cs
@@ -25,7 +25,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.通道,
IsInChannel = true,
ChannelType = ChannelType.Corridor,
- WorldPosition = worldPosition,
RelatedModelItem = channel,
SpeedLimit = speedLimit,
HeightLayers = new System.Collections.Generic.List(),
@@ -49,7 +48,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.门,
IsInChannel = false,
ChannelType = ChannelType.Other,
- WorldPosition = worldPosition,
RelatedModelItem = door,
SpeedLimit = speedLimit,
HeightLayers = new System.Collections.Generic.List(),
@@ -71,7 +69,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
IsInChannel = false,
ChannelType = ChannelType.Other,
- WorldPosition = worldPosition,
RelatedModelItem = obstacle,
SpeedLimit = 0,
HeightLayers = new System.Collections.Generic.List(),
@@ -95,7 +92,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.电梯,
IsInChannel = false,
ChannelType = ChannelType.Other,
- WorldPosition = worldPosition,
RelatedModelItem = elevator,
SpeedLimit = speedLimit,
HeightLayers = new System.Collections.Generic.List(),
@@ -119,7 +115,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.楼梯,
IsInChannel = false,
ChannelType = ChannelType.Other,
- WorldPosition = worldPosition,
RelatedModelItem = stairs,
SpeedLimit = speedLimit,
HeightLayers = new System.Collections.Generic.List(),
@@ -140,7 +135,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.Unknown,
IsInChannel = false,
ChannelType = ChannelType.Other,
- WorldPosition = worldPosition,
RelatedModelItem = null,
SpeedLimit = 0,
HeightLayers = new System.Collections.Generic.List(),
diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs
index 828be01..6c8cc23 100644
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@@ -164,8 +164,7 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
IsInChannel = false,
HeightLayers = new List(),
- ChannelType = ChannelType.Other,
- WorldPosition = worldPos
+ ChannelType = ChannelType.Other
};
cell.Cost = cell.GetCost(); // 使用GetCost方法计算成本
Cells[x, y] = cell;
@@ -200,16 +199,23 @@ namespace NavisworksTransport.PathPlanning
}
///
- /// 将网格坐标转换为世界3D坐标（需要显式提供Z坐标）
+ /// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标）
///
/// 网格坐标
- /// Z坐标值
/// 世界3D坐标点
public Point3D GridToWorld3D(GridPoint2D gridPosition)
{
var world2D = GridToWorld2D(gridPosition);
var cell = Cells[gridPosition.X, gridPosition.Y];
- return new Point3D(world2D.X, world2D.Y, cell.WorldPosition.Z);
+
+ // 使用最低层的Z坐标（HeightLayers[0]），如果没有层则用0
+ double z = 0;
+ if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
+ {
+ z = cell.HeightLayers[0].Z;
+ }
+
+ return new Point3D(world2D.X, world2D.Y, z);
}
@@ -259,7 +265,6 @@ namespace NavisworksTransport.PathPlanning
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
HeightLayers = new List(),
ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
- WorldPosition = worldPos,
SpeedLimit = speedLimit
};
@@ -693,10 +698,6 @@ namespace NavisworksTransport.PathPlanning
///
public ChannelType ChannelType { get; set; }
- ///
- /// 世界坐标位置 - 用于高度计算和空间查询
- ///
- public Point3D WorldPosition { get; set; }
///
/// 限速（米/秒），0表示未设置限速
///
diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs
index 379b43d..825023b 100644
--- a/src/PathPlanning/GridMapGenerator.cs
+++ b/src/PathPlanning/GridMapGenerator.cs
@@ -520,7 +520,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.通道,
IsInChannel = true,
ChannelType = ChannelType.Corridor,
- WorldPosition = point,
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
SpeedLimit = cell.SpeedLimit, // 保持原有限速
HeightLayers = cell.HeightLayers, // 保持高度层
@@ -542,7 +541,6 @@ namespace NavisworksTransport.PathPlanning
CellType = CategoryAttributeManager.LogisticsElementType.通道,
IsInChannel = true,
ChannelType = ChannelType.Corridor,
- WorldPosition = point,
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
SpeedLimit = cell.SpeedLimit, // 保持原有限速
HeightLayers = cell.HeightLayers, // 保持高度层
@@ -560,9 +558,7 @@ namespace NavisworksTransport.PathPlanning
// 非通道单元格：不应该通过垂直扫描改变其状态
// 这种情况理论上不应该发生，因为只对通道单元格进行扫描
LogManager.Warning($"[通道2.5D模式] 警告：非通道单元格 ({gridX},{gridY}) 收到高度数据，类型：{cell.CellType}, IsInChannel：{cell.IsInChannel}");
-
- // 仍然更新世界位置，但保持原有状态
- cell.WorldPosition = point;
+
// 多层架构：高度区间在HeightLayer中设置
// cell.PassableHeight = interval;
gridMap.Cells[gridX, gridY] = cell;