修改网格生成和路径规划中的bug
This commit is contained in:
parent
9ea89aa8d0
commit
8b5e2baf23
@ -949,7 +949,7 @@ namespace NavisworksTransport
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"网格地图生成失败: {ex.Message}");
|
||||
throw new Exception($"网格地图生成失败: {ex.Message}", ex);
|
||||
throw; // 直接抛出原始异常，不包装
|
||||
}
|
||||
|
||||
// 根据用户设置决定是否进行网格可视化
|
||||
@ -1000,7 +1000,7 @@ namespace NavisworksTransport
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"路径查找失败: {ex.Message}");
|
||||
throw new Exception($"路径查找失败: {ex.Message}", ex);
|
||||
throw; // 直接抛出原始异常，不包装
|
||||
}
|
||||
|
||||
// 1. 检查路径查找是否完全失败
|
||||
@ -1083,9 +1083,8 @@ namespace NavisworksTransport
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
var errorMsg = $"自动路径规划失败: {ex.Message}";
|
||||
LogManager.Error(errorMsg, ex);
|
||||
RaiseErrorOccurred(errorMsg, ex);
|
||||
LogManager.Error($"自动路径规划失败: {ex.Message}", ex);
|
||||
RaiseErrorOccurred(ex.Message, ex); // 使用原始异常消息，不再包装
|
||||
return Task.FromResult<PathRoute>(null);
|
||||
}
|
||||
}
|
||||
@ -3391,7 +3390,7 @@ namespace NavisworksTransport
|
||||
// 清除之前的网格可视化
|
||||
ClearGridVisualization();
|
||||
|
||||
// 创建三个独立的网格可视化路径
|
||||
// 创建四种独立的网格可视化路径
|
||||
var channelRoute = new PathRoute("GridVisualization_Channel")
|
||||
{
|
||||
Id = "grid_visualization_channel"
|
||||
@ -3416,18 +3415,9 @@ namespace NavisworksTransport
|
||||
int channelCells = 0;
|
||||
int unknownCells = 0;
|
||||
int obstacleCells = 0;
|
||||
int openSpaceCells = 0;
|
||||
int doorCells = 0;
|
||||
int totalVisualized = 0;
|
||||
int multiLayerCells = 0;
|
||||
int totalLayersVisualized = 0;
|
||||
int heightInsufficientLayers = 0;
|
||||
|
||||
// 获取车辆高度（模型单位）用于判断层高是否足够
|
||||
double vehicleHeightInModelUnits = vehicleHeight *
|
||||
UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
|
||||
|
||||
LogManager.Info($"[网格可视化] 车辆高度: {vehicleHeight}m = {vehicleHeightInModelUnits:F2}模型单位");
|
||||
|
||||
// 遍历所有网格单元格，根据类型分别收集
|
||||
for (int x = 0; x < gridMap.Width; x++)
|
||||
@ -3442,44 +3432,23 @@ namespace NavisworksTransport
|
||||
double centerX = gridCorner.X + gridMap.CellSize / 2;
|
||||
double centerY = gridCorner.Y + gridMap.CellSize / 2;
|
||||
|
||||
// 检查是否有多个高度层
|
||||
// 所有网格都基于高度层，统一处理
|
||||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||||
{
|
||||
// 多层逻辑：遍历每一层，根据PassableHeight判断是否可通行
|
||||
multiLayerCells++;
|
||||
|
||||
foreach (var layer in cell.HeightLayers)
|
||||
{
|
||||
double layerZ = layer.Z;
|
||||
double passableHeight = layer.PassableHeight.GetSpan();
|
||||
|
||||
// 判断该层是否可通行（考虑膨胀影响和高度限制）
|
||||
bool isLayerWalkable = layer.IsWalkable; // 膨胀影响
|
||||
bool isHeightSufficient = passableHeight >= vehicleHeightInModelUnits; // 高度限制
|
||||
|
||||
// 调试：输出不可通行的层
|
||||
if ((!isLayerWalkable || !isHeightSufficient) && multiLayerCells < 5) // 只输出前5个网格的详情
|
||||
{
|
||||
LogManager.Debug($"[层通行性] 网格({x},{y}) Layer Z={layerZ:F2}, IsWalkable={isLayerWalkable}, PassableH={passableHeight:F2}, 车辆H={vehicleHeightInModelUnits:F2}, 高度足够={isHeightSufficient}");
|
||||
}
|
||||
|
||||
PathRoute targetRoute = null;
|
||||
string gridTypeName = "";
|
||||
|
||||
// 根据层可通行性、高度是否足够和网格类型决定渲染方式
|
||||
if (isLayerWalkable && isHeightSufficient)
|
||||
// 仅根据 layer.IsWalkable 决定渲染
|
||||
if (layer.IsWalkable)
|
||||
{
|
||||
// 高度足够 - 按原类型渲染
|
||||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道)
|
||||
{
|
||||
if (_showWalkableGrid)
|
||||
{
|
||||
targetRoute = channelRoute;
|
||||
gridTypeName = "通道";
|
||||
channelCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||||
// 可通行层 - 门特殊处理，其他统一样式
|
||||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||||
{
|
||||
if (_showDoorGrid)
|
||||
{
|
||||
@ -3488,38 +3457,25 @@ namespace NavisworksTransport
|
||||
doorCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.楼板)
|
||||
else
|
||||
{
|
||||
// 其他可通行类型统一为通道样式（绿色）
|
||||
if (_showWalkableGrid)
|
||||
{
|
||||
targetRoute = channelRoute;
|
||||
gridTypeName = "开放";
|
||||
openSpaceCells++;
|
||||
gridTypeName = "通道";
|
||||
channelCells++;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 层不可通行（被膨胀影响或高度不足）- 渲染为障碍物（灰色）
|
||||
// 不可通行层 - 渲染为障碍物（灰色）
|
||||
if (_showObstacleGrid)
|
||||
{
|
||||
targetRoute = obstacleRoute;
|
||||
|
||||
// 区分不可通行的原因
|
||||
if (!isLayerWalkable && !isHeightSufficient)
|
||||
{
|
||||
gridTypeName = "障碍_膨胀+高度不足";
|
||||
}
|
||||
else if (!isLayerWalkable)
|
||||
{
|
||||
gridTypeName = "障碍_膨胀影响";
|
||||
}
|
||||
else
|
||||
{
|
||||
gridTypeName = "障碍_高度不足";
|
||||
}
|
||||
|
||||
heightInsufficientLayers++;
|
||||
gridTypeName = "障碍";
|
||||
obstacleCells++;
|
||||
}
|
||||
}
|
||||
|
||||
@ -3534,83 +3490,30 @@ namespace NavisworksTransport
|
||||
Name = $"网格_{gridTypeName}({x},{y})_Z{layerZ:F2}",
|
||||
Type = PathPointType.WayPoint,
|
||||
Index = totalVisualized,
|
||||
Notes = $"GridType:{cell.CellType}, LayerZ={layerZ:F2}, PassableH={passableHeight:F2}m"
|
||||
Notes = $"GridType:{cell.CellType}, LayerZ={layerZ:F2}, IsWalkable={layer.IsWalkable}"
|
||||
};
|
||||
|
||||
targetRoute.Points.Add(gridPoint);
|
||||
totalVisualized++;
|
||||
totalLayersVisualized++;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||||
{
|
||||
// 单层逻辑：保持原有逻辑不变
|
||||
var gridCenter = new Point3D(centerX, centerY, gridCorner.Z);
|
||||
|
||||
PathRoute targetRoute = null;
|
||||
string gridTypeName = "";
|
||||
|
||||
// 根据网格类型和用户设置确定目标路径和名称
|
||||
if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.通道)
|
||||
{
|
||||
if (_showWalkableGrid)
|
||||
{
|
||||
targetRoute = channelRoute;
|
||||
gridTypeName = "通道";
|
||||
channelCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||||
{
|
||||
if (_showDoorGrid)
|
||||
{
|
||||
targetRoute = doorRoute;
|
||||
gridTypeName = "门";
|
||||
doorCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.楼板)
|
||||
{
|
||||
if (_showWalkableGrid)
|
||||
{
|
||||
targetRoute = channelRoute;
|
||||
gridTypeName = "开放";
|
||||
openSpaceCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||||
{
|
||||
if (_showUnknownGrid)
|
||||
{
|
||||
targetRoute = unknownRoute;
|
||||
gridTypeName = "空洞";
|
||||
unknownCells++;
|
||||
}
|
||||
}
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物)
|
||||
{
|
||||
if (_showObstacleGrid)
|
||||
{
|
||||
targetRoute = obstacleRoute;
|
||||
gridTypeName = "障碍";
|
||||
obstacleCells++;
|
||||
}
|
||||
}
|
||||
|
||||
// 如果有对应的路径，创建并添加网格点
|
||||
if (targetRoute != null)
|
||||
// Unknown网格没有高度层，用于调试
|
||||
if (_showUnknownGrid)
|
||||
{
|
||||
var gridCenter = new Point3D(centerX, centerY, gridCorner.Z);
|
||||
var gridPoint = new PathPoint
|
||||
{
|
||||
Position = gridCenter,
|
||||
Name = $"网格_{gridTypeName}({x},{y})",
|
||||
Name = $"网格_空洞({x},{y})",
|
||||
Type = PathPointType.WayPoint,
|
||||
Index = totalVisualized,
|
||||
Notes = $"GridType:{cell.CellType}"
|
||||
Notes = $"GridType:Unknown, NoLayers"
|
||||
};
|
||||
|
||||
targetRoute.Points.Add(gridPoint);
|
||||
unknownRoute.Points.Add(gridPoint);
|
||||
unknownCells++;
|
||||
totalVisualized++;
|
||||
}
|
||||
}
|
||||
@ -3621,41 +3524,38 @@ namespace NavisworksTransport
|
||||
if (channelRoute.Points.Count > 0)
|
||||
{
|
||||
renderPlugin.RenderPointOnly(channelRoute);
|
||||
LogManager.Info($"[网格可视化] 渲染通道/开放空间网格点: {channelRoute.Points.Count} 个");
|
||||
LogManager.Info($"[网格可视化] 渲染可通行网格层: {channelRoute.Points.Count} 个（绿色）");
|
||||
}
|
||||
|
||||
if (doorRoute.Points.Count > 0)
|
||||
{
|
||||
renderPlugin.RenderPointOnly(doorRoute);
|
||||
LogManager.Info($"[网格可视化] 渲染门网格点: {doorRoute.Points.Count} 个（50%透明绿色）");
|
||||
LogManager.Info($"[网格可视化] 渲染门网格层: {doorRoute.Points.Count} 个（50%透明绿色）");
|
||||
}
|
||||
|
||||
if (unknownRoute.Points.Count > 0)
|
||||
{
|
||||
renderPlugin.RenderPointOnly(unknownRoute);
|
||||
LogManager.Info($"[网格可视化] 渲染Unknown网格点: {unknownRoute.Points.Count} 个");
|
||||
LogManager.Info($"[网格可视化] 渲染Unknown网格: {unknownRoute.Points.Count} 个（红色，调试用）");
|
||||
}
|
||||
|
||||
if (obstacleRoute.Points.Count > 0)
|
||||
{
|
||||
renderPlugin.RenderPointOnly(obstacleRoute);
|
||||
LogManager.Info($"[网格可视化] 渲染障碍物网格点: {obstacleRoute.Points.Count} 个");
|
||||
LogManager.Info($"[网格可视化] 渲染障碍物网格层: {obstacleRoute.Points.Count} 个（灰色）");
|
||||
}
|
||||
|
||||
// 输出统计信息
|
||||
LogManager.Info($"[网格可视化] 可视化完成：");
|
||||
LogManager.Info($" - 总网格单元格数：{gridMap.Width * gridMap.Height}");
|
||||
LogManager.Info($" - 多层网格数：{multiLayerCells}");
|
||||
LogManager.Info($" - 通道单元格数：{channelCells}");
|
||||
LogManager.Info($" - 门单元格数：{doorCells}");
|
||||
LogManager.Info($" - 开放空间单元格数：{openSpaceCells}");
|
||||
LogManager.Info($" - 可通行层数：{channelCells}");
|
||||
LogManager.Info($" - 门层数：{doorCells}");
|
||||
LogManager.Info($" - Unknown单元格数：{unknownCells}");
|
||||
LogManager.Info($" - 障碍物单元格数：{obstacleCells}");
|
||||
LogManager.Info($" - 高度不足层数：{heightInsufficientLayers}");
|
||||
LogManager.Info($" - 已可视化单元格数：{totalVisualized}");
|
||||
LogManager.Info($" - 已可视化层数：{totalLayersVisualized}");
|
||||
LogManager.Info($" - 障碍物层数：{obstacleCells}");
|
||||
LogManager.Info($" - 已可视化点数：{totalVisualized}");
|
||||
|
||||
RaiseStatusChanged($"网格可视化完成：显示了 {totalVisualized} 个网格单元 (多层网格:{multiLayerCells}, 通道:{channelCells}, 门:{doorCells}[50%透明], Unknown:{unknownCells}, 障碍:{obstacleCells}, 高度不足:{heightInsufficientLayers})", PathPlanningStatusType.Success);
|
||||
RaiseStatusChanged($"网格可视化完成：显示了 {totalVisualized} 个网格层 (多层网格:{multiLayerCells}, 可通行:{channelCells}, 门:{doorCells}, Unknown:{unknownCells}, 障碍:{obstacleCells})", PathPlanningStatusType.Success);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
@ -3780,7 +3680,7 @@ namespace NavisworksTransport
|
||||
{
|
||||
_routes.Add(route);
|
||||
loadedCount++;
|
||||
LogManager.Info($"加载历史路径: {route.Name}, ID={route.Id}, 长度={route.TotalLength:F2}米");
|
||||
//LogManager.Info($"加载历史路径: {route.Name}, ID={route.Id}, 长度={route.TotalLength:F2}米");
|
||||
}
|
||||
}
|
||||
|
||||
@ -3870,4 +3770,4 @@ namespace NavisworksTransport
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -115,14 +115,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[3D路径规划] 发生异常: {ex.Message}\n{ex.StackTrace}");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
throw; // 直接抛出异常，不返回默认路径
|
||||
}
|
||||
}
|
||||
|
||||
@ -160,7 +153,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
if (!cell.IsWalkable || cell.HeightLayers.Count == 0)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
continue;
|
||||
|
||||
// 为每个高度层创建独立节点
|
||||
@ -212,7 +205,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
if (!cell.IsWalkable || cell.HeightLayers.Count == 0)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
continue;
|
||||
|
||||
for (int li = 0; li < cell.HeightLayers.Count; li++)
|
||||
@ -235,7 +228,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
continue;
|
||||
|
||||
var neighborCell = gridMap.Cells[nx, ny];
|
||||
if (!neighborCell.IsWalkable)
|
||||
if (!neighborCell.HasAnyWalkableLayer())
|
||||
continue;
|
||||
|
||||
// 查找邻居中所有满足高度约束的层
|
||||
@ -452,17 +445,58 @@ namespace NavisworksTransport.PathPlanning
|
||||
var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z);
|
||||
var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z);
|
||||
|
||||
if (startNode == null || endNode == null)
|
||||
// 起点和终点必须检查可达性
|
||||
bool startOnObstacle = (startNode == null);
|
||||
bool endOnObstacle = (endNode == null);
|
||||
|
||||
// 如果起点在障碍物上，直接报错
|
||||
if (startOnObstacle && endOnObstacle)
|
||||
{
|
||||
LogManager.Error($"[A*执行-3D] 无法找到起点或终点节点");
|
||||
return new PathFindingResult
|
||||
LogManager.Error($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})和终点网格({endGridPos.X},{endGridPos.Y})都在不可通行区域");
|
||||
throw new AutoPathPlanningException("起点和终点都在不可通行区域，无法生成路径");
|
||||
}
|
||||
else if (startOnObstacle)
|
||||
{
|
||||
LogManager.Error($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})在不可通行区域");
|
||||
throw new AutoPathPlanningException("起点在不可通行区域，无法生成路径");
|
||||
}
|
||||
|
||||
// 如果终点不可达，寻找最接近终点的可达节点（类似2.5D的ClosestApproach逻辑）
|
||||
if (endOnObstacle)
|
||||
{
|
||||
LogManager.Warning($"[A*执行-3D] 终点网格({endGridPos.X},{endGridPos.Y})不可达，寻找最接近的可达节点");
|
||||
|
||||
// 从所有节点中找到最接近终点的节点
|
||||
double minDistance = double.MaxValue;
|
||||
foreach (var node in graph3D.AllNodes)
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
var info = graph3D.NodeInfo[node];
|
||||
var nodeWorldPos = gridMap.GridToWorld3D(new GridPoint2D(info.X, info.Y));
|
||||
nodeWorldPos = new Point3D(nodeWorldPos.X, nodeWorldPos.Y, info.Z);
|
||||
|
||||
double distance = Math.Sqrt(
|
||||
Math.Pow(nodeWorldPos.X - end.X, 2) +
|
||||
Math.Pow(nodeWorldPos.Y - end.Y, 2) +
|
||||
Math.Pow(nodeWorldPos.Z - end.Z, 2)
|
||||
);
|
||||
|
||||
if (distance < minDistance)
|
||||
{
|
||||
minDistance = distance;
|
||||
endNode = node;
|
||||
}
|
||||
}
|
||||
|
||||
if (endNode != null)
|
||||
{
|
||||
var closestInfo = graph3D.NodeInfo[endNode];
|
||||
LogManager.Info($"[A*执行-3D] 找到最接近终点的可达节点: ({closestInfo.X},{closestInfo.Y},层{closestInfo.LayerIndex},Z={closestInfo.Z:F2})，距离原终点 {minDistance:F2}模型单位");
|
||||
}
|
||||
else
|
||||
{
|
||||
LogManager.Error($"[A*执行-3D] 无法找到任何可达节点（图中没有任何节点）");
|
||||
throw new AutoPathPlanningException("无法找到任何可达节点，网格地图可能完全被障碍物覆盖");
|
||||
}
|
||||
}
|
||||
|
||||
var startInfo = graph3D.NodeInfo[startNode];
|
||||
@ -476,24 +510,88 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
if (astarPath.Edges.Count == 0)
|
||||
{
|
||||
LogManager.Warning($"[A*执行-3D] 未找到路径");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
LogManager.Error($"[A*执行-3D] 未找到路径，起点({startInfo.X},{startInfo.Y})到终点({endInfo.X},{endInfo.Y})之间被障碍物完全阻断");
|
||||
throw new AutoPathPlanningException("起点和终点之间被障碍物完全阻断，无法找到可行路径");
|
||||
}
|
||||
|
||||
// 转换为世界坐标路径
|
||||
var path3D = ConvertGraphPathToWorld(astarPath, graph3D, gridMap, start, end);
|
||||
|
||||
// 计算路径完成度，检查是否到达终点
|
||||
double completionPercentage = 100.0;
|
||||
bool isComplete = true;
|
||||
Point3D actualEnd = path3D.Count > 0 ? path3D[path3D.Count - 1] : start;
|
||||
|
||||
// 如果终点在障碍物上，路径肯定不完整
|
||||
if (endOnObstacle)
|
||||
{
|
||||
isComplete = false;
|
||||
actualEnd = path3D[path3D.Count - 1];
|
||||
|
||||
// 计算实际距离和总距离
|
||||
double totalDistance = Math.Sqrt(
|
||||
Math.Pow(end.X - start.X, 2) +
|
||||
Math.Pow(end.Y - start.Y, 2) +
|
||||
Math.Pow(end.Z - start.Z, 2)
|
||||
);
|
||||
double actualDistance = Math.Sqrt(
|
||||
Math.Pow(actualEnd.X - start.X, 2) +
|
||||
Math.Pow(actualEnd.Y - start.Y, 2) +
|
||||
Math.Pow(actualEnd.Z - start.Z, 2)
|
||||
);
|
||||
|
||||
completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;
|
||||
|
||||
LogManager.Warning($"[3D路径规划] 终点在障碍物上，使用最接近的可达节点，路径不完整：完成度 {completionPercentage:F1}%");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 检查A*路径最后一个节点是否到达终点网格
|
||||
if (astarPath.Edges.Count > 0)
|
||||
{
|
||||
var lastEdge = astarPath.Edges[astarPath.Edges.Count - 1];
|
||||
var lastNode = lastEdge.End as Roy_T.AStar.Graphs.Node;
|
||||
|
||||
if (lastNode != null && graph3D.NodeInfo.ContainsKey(lastNode))
|
||||
{
|
||||
var lastInfo = graph3D.NodeInfo[lastNode];
|
||||
|
||||
// 检查最后节点是否到达终点网格（使用前面已定义的endGridPos）
|
||||
if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y)
|
||||
{
|
||||
// 到达终点网格，路径完整
|
||||
isComplete = true;
|
||||
actualEnd = path3D[path3D.Count - 1];
|
||||
completionPercentage = 100.0;
|
||||
LogManager.Info($"[3D路径规划] 找到完整路径，到达终点网格({endGridPos.X},{endGridPos.Y})");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 未到达终点，计算完成度
|
||||
isComplete = false;
|
||||
actualEnd = path3D[path3D.Count - 1];
|
||||
|
||||
// 计算实际距离和总距离
|
||||
double totalDistance = Math.Sqrt(
|
||||
Math.Pow(end.X - start.X, 2) +
|
||||
Math.Pow(end.Y - start.Y, 2) +
|
||||
Math.Pow(end.Z - start.Z, 2)
|
||||
);
|
||||
double actualDistance = Math.Sqrt(
|
||||
Math.Pow(actualEnd.X - start.X, 2) +
|
||||
Math.Pow(actualEnd.Y - start.Y, 2) +
|
||||
Math.Pow(actualEnd.Z - start.Z, 2)
|
||||
);
|
||||
|
||||
completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;
|
||||
|
||||
LogManager.Warning($"[3D路径规划] 部分路径：到达网格({lastInfo.X},{lastInfo.Y})，距离终点网格({endGridPos.X},{endGridPos.Y})还有距离");
|
||||
LogManager.Info($"[3D路径规划] 完成度: {completionPercentage:F1}%，实际终点: ({actualEnd.X:F2}, {actualEnd.Y:F2}, Z={actualEnd.Z:F2})");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LogManager.Info($"[3D路径规划] 路径生成完成，包含 {path3D.Count} 个路径点，完成度: {completionPercentage:F1}%");
|
||||
|
||||
return new PathFindingResult
|
||||
@ -579,8 +677,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
pointIndex++;
|
||||
}
|
||||
|
||||
path3D.Add(originalEnd);
|
||||
LogManager.Info($"[路径点{pointIndex}] 原始终点: ({originalEnd.X:F2}, {originalEnd.Y:F2}, Z={originalEnd.Z:F2})");
|
||||
// 🔥 修复：不再强制添加原始终点，由ExecuteAStarOnGraph根据完成度决定
|
||||
LogManager.Info($"[路径转换完成] 总共 {path3D.Count} 个路径点");
|
||||
|
||||
return path3D;
|
||||
@ -621,14 +718,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
if (pathResult == null || !pathResult.PathPoints.Any())
|
||||
{
|
||||
LogManager.Warning("[2.5D路径规划] 未找到有效路径");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D>(),
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
CompletionPercentage = 0.0
|
||||
};
|
||||
LogManager.Error("[2.5D路径规划] 未找到有效路径");
|
||||
throw new AutoPathPlanningException("未找到有效路径，起点和终点之间可能被障碍物阻断");
|
||||
}
|
||||
|
||||
LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
|
||||
@ -658,13 +749,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[2.5D路径规划] 路径查找失败: {ex.Message}");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D>(),
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
CompletionPercentage = 0.0
|
||||
};
|
||||
throw; // 直接抛出异常，不返回默认路径
|
||||
}
|
||||
}
|
||||
|
||||
@ -741,7 +826,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
// 统计基本可通行性
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
totalWalkableCells++;
|
||||
}
|
||||
@ -753,7 +838,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 检查是否有满足车辆高度的层
|
||||
if (!HasCompatibleLayer(cell, vehicleHeight))
|
||||
{
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
heightConstrainedCells++;
|
||||
if (heightConstrainedCells <= 10)
|
||||
@ -978,7 +1063,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 检查是否有兼容的高度层
|
||||
if (!HasCompatibleLayer(cell, vehicleHeight))
|
||||
{
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
heightConstrainedCells++;
|
||||
}
|
||||
@ -1200,7 +1285,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// <returns>是否可通行</returns>
|
||||
private bool IsPassableWithHeight(GridCell cell, double vehicleHeight)
|
||||
{
|
||||
if (!cell.IsWalkable)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
return false;
|
||||
|
||||
// 检查是否有任何高度层满足车辆高度要求
|
||||
@ -1225,7 +1310,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// <returns>是否有兼容的高度层</returns>
|
||||
private bool HasCompatibleLayer(GridCell cell, double vehicleHeight)
|
||||
{
|
||||
if (!cell.IsWalkable)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
return false;
|
||||
|
||||
if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
|
||||
@ -1583,29 +1668,32 @@ namespace NavisworksTransport.PathPlanning
|
||||
start.Z + t * (end.Z - start.Z)
|
||||
);
|
||||
|
||||
// 基础检查：采样点是否在可通行网格内
|
||||
// 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内
|
||||
// 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层
|
||||
var gridPos = gridMap.WorldToGrid(samplePoint);
|
||||
if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsWalkable(gridPos))
|
||||
double tolerance = 0.1; // Z坐标容差（模型单位）
|
||||
|
||||
if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAt3DPoint(gridPos, samplePoint.Z, tolerance))
|
||||
{
|
||||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||||
|
||||
|
||||
// 计算网格中心点世界坐标（基于左下角）
|
||||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||||
var gridCenterZ = samplePoint.Z; // Z保持不变
|
||||
|
||||
|
||||
// 计算偏差
|
||||
var deltaX = samplePoint.X - gridCenterX;
|
||||
var deltaY = samplePoint.Y - gridCenterY;
|
||||
|
||||
|
||||
// LogManager.Debug($"[斜线检查] 采样点{i}/{samples}失败：" +
|
||||
// $"采样点({samplePoint.X:F3},{samplePoint.Y:F3},{samplePoint.Z:F3})，" +
|
||||
// $"网格({gridPos.X},{gridPos.Y})左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||||
// $"网格中心({gridCenterX:F3},{gridCenterY:F3},{gridCenterZ:F3})，" +
|
||||
// $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
|
||||
// $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "不可通行")}");
|
||||
// $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "Z坐标不在可通行层范围")}");
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -1905,9 +1993,9 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
var cell = gridMap.Cells[gridX, gridY];
|
||||
// 检查基本可行走性
|
||||
if (!cell.IsWalkable)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
{
|
||||
LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - IsWalkable={cell.IsWalkable}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行");
|
||||
LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - HasWalkableLayer={cell.HasAnyWalkableLayer()}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行");
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -2275,14 +2363,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
else
|
||||
{
|
||||
LogManager.Warning("[A*执行] 未找到路径");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D>(),
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
CompletionPercentage = 0.0
|
||||
};
|
||||
LogManager.Error("[A*执行] 未找到路径");
|
||||
throw new AutoPathPlanningException("A*算法未找到路径，起点和终点之间被障碍物阻断");
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
@ -2508,7 +2590,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 检查是否有兼容的高度层
|
||||
if (!HasCompatibleLayer(cell, vehicleHeight))
|
||||
{
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
heightConstrainedCells++;
|
||||
}
|
||||
@ -2743,7 +2825,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
// 🔥 核心修改：直接使用IsWalkable判断，所有不可通行的网格都视为障碍物
|
||||
bool isUnsafeArea = !cell.IsWalkable;
|
||||
bool isUnsafeArea = !cell.HasAnyWalkableLayer();
|
||||
|
||||
distanceMap[x, y] = isUnsafeArea ? 0 : int.MaxValue;
|
||||
|
||||
@ -2752,7 +2834,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
obstacleCount++;
|
||||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||||
unknownCount++;
|
||||
else if (cell.IsWalkable)
|
||||
else if (cell.HasAnyWalkableLayer())
|
||||
walkableCount++;
|
||||
}
|
||||
}
|
||||
@ -2942,4 +3024,4 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -145,7 +145,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
// 只计算可通行的网格
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
walkableCount++;
|
||||
double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0
|
||||
@ -338,4 +338,4 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -21,7 +21,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
@ -44,7 +43,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.门,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
@ -65,7 +63,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = false, // 障碍物永远不可通行
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
@ -88,7 +85,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.电梯,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
@ -111,7 +107,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.楼梯,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
@ -131,7 +126,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
return new GridCell
|
||||
{
|
||||
IsWalkable = false, // Unknown类型默认不可通行
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
|
||||
@ -160,7 +160,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = false,
|
||||
// IsWalkable字段已删除，通行性由HeightLayers决定
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
|
||||
IsInChannel = false,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
@ -240,8 +240,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
if (!IsValidGridPosition(gridPosition))
|
||||
return false;
|
||||
|
||||
return Cells[gridPosition.X, gridPosition.Y].IsWalkable;
|
||||
|
||||
return Cells[gridPosition.X, gridPosition.Y].HasAnyWalkableLayer();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -260,7 +260,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
var worldPos = GridToWorld3D(gridPosition);
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = isWalkable,
|
||||
// IsWalkable字段已删除，通行性由HeightLayers决定
|
||||
CellType = cellType,
|
||||
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
@ -315,7 +315,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 更新单元格的基本属性（使用第一个层的属性）
|
||||
if (cell.HeightLayers.Count == 1)
|
||||
{
|
||||
cell.IsWalkable = true;
|
||||
// IsWalkable由层决定，不再在网格级别设置
|
||||
cell.CellType = layer.Type;
|
||||
cell.IsInChannel = true;
|
||||
cell.RelatedModelItem = layer.SourceItem;
|
||||
@ -385,7 +385,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown ||
|
||||
cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物)
|
||||
{
|
||||
cell.IsWalkable = false;
|
||||
// IsWalkable由层决定，不再在网格级别设置
|
||||
// Unknown和障碍物没有可通行层，HasAnyWalkableLayer()自动返回false
|
||||
cell.Cost = double.MaxValue;
|
||||
}
|
||||
else
|
||||
@ -407,7 +408,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
return false;
|
||||
|
||||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||||
if (!cell.IsWalkable)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
return false;
|
||||
|
||||
// 检查是否有任何高度层满足高度要求
|
||||
@ -422,6 +423,46 @@ namespace NavisworksTransport.PathPlanning
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查指定3D点（包含Z坐标）是否在可通行的高度层范围内
|
||||
/// 用于路径优化中的斜线连接检查，确保路径不穿过障碍物层
|
||||
/// </summary>
|
||||
/// <param name="gridPosition">网格坐标</param>
|
||||
/// <param name="zCoord">Z坐标（模型单位）</param>
|
||||
/// <param name="tolerance">Z坐标容差（模型单位），默认0.1</param>
|
||||
/// <returns>是否可通行</returns>
|
||||
public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = 0.1)
|
||||
{
|
||||
if (!IsValidGridPosition(gridPosition))
|
||||
return false;
|
||||
|
||||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
return false;
|
||||
|
||||
// 检查是否有任何可通行层包含这个Z坐标
|
||||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||||
{
|
||||
foreach (var layer in cell.HeightLayers)
|
||||
{
|
||||
// 跳过不可通行的层
|
||||
if (!layer.IsWalkable)
|
||||
continue;
|
||||
|
||||
// 检查Z坐标是否在这一层的范围内
|
||||
double layerMinZ = layer.Z;
|
||||
double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();
|
||||
|
||||
if (zCoord >= layerMinZ - tolerance && zCoord <= layerMaxZ + tolerance)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取所有通道类型的网格单元
|
||||
/// </summary>
|
||||
@ -575,7 +616,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = Cells[x, y];
|
||||
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
totalWalkable++;
|
||||
switch (cell.CellType)
|
||||
@ -663,11 +704,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// </summary>
|
||||
public struct GridCell
|
||||
{
|
||||
/// <summary>
|
||||
/// 是否可通行
|
||||
/// </summary>
|
||||
public bool IsWalkable { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 遍历成本（1.0为标准成本）
|
||||
/// </summary>
|
||||
@ -703,15 +739,24 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// </summary>
|
||||
public double SpeedLimit { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查是否至少有一个可通行的高度层
|
||||
/// </summary>
|
||||
/// <returns>如果至少有一层可通行返回true，否则返回false</returns>
|
||||
public bool HasAnyWalkableLayer()
|
||||
{
|
||||
return HeightLayers != null && HeightLayers.Any(layer => layer.IsWalkable);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据物流类型获取通行成本
|
||||
/// </summary>
|
||||
/// <returns>通行成本</returns>
|
||||
public double GetCost()
|
||||
{
|
||||
if (!IsWalkable)
|
||||
if (!HasAnyWalkableLayer())
|
||||
return double.MaxValue;
|
||||
|
||||
|
||||
switch (CellType)
|
||||
{
|
||||
case CategoryAttributeManager.LogisticsElementType.通道:
|
||||
@ -749,10 +794,10 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = false,
|
||||
// IsWalkable由层决定，障碍物没有层或所有层IsWalkable=false
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
|
||||
IsInChannel = false,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
HeightLayers = new List<HeightLayer>(), // 空列表，HasAnyWalkableLayer()返回false
|
||||
ChannelType = ChannelType.Other,
|
||||
SpeedLimit = 0
|
||||
};
|
||||
@ -768,7 +813,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
// IsWalkable字段已删除，通行性由HeightLayers决定
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.楼板,
|
||||
IsInChannel = false,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
@ -788,7 +833,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = isOpen,
|
||||
// IsWalkable字段已删除，通行性由HeightLayers决定
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.门,
|
||||
IsInChannel = isOpen,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
@ -808,7 +853,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
// IsWalkable字段已删除，通行性由HeightLayers决定
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
@ -1029,4 +1074,4 @@ namespace NavisworksTransport.PathPlanning
|
||||
public AutoPathPlanningException(string message) : base(message) { }
|
||||
public AutoPathPlanningException(string message, Exception innerException) : base(message, innerException) { }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -136,16 +136,16 @@ namespace NavisworksTransport.PathPlanning
|
||||
LogManager.Info($"【生成网格地图】智能收集到 {irrelevantRelatedItems.Count} 个无关项相关节点（将被排除）");
|
||||
var irrelevantItemsSet = irrelevantRelatedItems; // 使用收集后的完整集合
|
||||
|
||||
// 2. 直接遍历处理障碍物（包围盒检测） - 使用高性能优化版本
|
||||
LogManager.Info("【生成网格地图】步骤2: 高性能包围盒遍历处理障碍物");
|
||||
ProcessObstaclesWithBoundingBox(channelCoverage.GridMap, traversableRelatedItems, scanHeightInModelUnits, channelCoverage, irrelevantItemsSet);
|
||||
|
||||
LogManager.Info($"【阶段2完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||||
|
||||
// 2.5. 为所有可通行网格设置PassableHeights确保高度约束检查
|
||||
LogManager.Info("【生成网格地图】步骤2.5: 为可通行网格设置高度约束");
|
||||
// 2. 先设置PassableHeights，确保障碍物高度检查时有正确的高度范围
|
||||
LogManager.Info("【生成网格地图】步骤2: 为可通行网格设置高度约束");
|
||||
SetChannelPassableHeights(channelCoverage.GridMap, scanHeightInModelUnits);
|
||||
LogManager.Info($"【阶段2.5完成】高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||||
LogManager.Info($"【阶段2完成】高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||||
|
||||
// 2.5. 处理障碍物（包围盒检测） - 使用高性能优化版本
|
||||
LogManager.Info("【生成网格地图】步骤2.5: 高性能包围盒遍历处理障碍物");
|
||||
ProcessObstaclesWithBoundingBox(channelCoverage.GridMap, traversableRelatedItems, scanHeightInModelUnits, channelCoverage, irrelevantItemsSet);
|
||||
|
||||
LogManager.Info($"【阶段2.5完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||||
|
||||
// 2.6. 单独处理门元素，设置为可通行，并设置通行高度
|
||||
LogManager.Info("【生成网格地图】步骤2.6: 处理门元素");
|
||||
@ -367,7 +367,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
if (cell.IsWalkable && cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
// 检查是否是楼梯或电梯网格（有多层且包含楼梯/电梯层）
|
||||
bool hasStairsOrElevator = cell.HeightLayers.Any(layer =>
|
||||
@ -449,7 +449,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
// 只检查可通行且有高度层的网格
|
||||
if (!cell.IsWalkable || cell.HeightLayers == null || cell.HeightLayers.Count == 0)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
continue;
|
||||
|
||||
totalCheckedCells++;
|
||||
@ -461,14 +461,44 @@ namespace NavisworksTransport.PathPlanning
|
||||
int nx = x + dx[dir];
|
||||
int ny = y + dy[dir];
|
||||
|
||||
// 边界检查
|
||||
// 情况1：网格地图边界 - 标记当前网格所有层为边界
|
||||
if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
|
||||
{
|
||||
for (int li = 0; li < currentLayerCount; li++)
|
||||
{
|
||||
var layer = cell.HeightLayers[li];
|
||||
if (!layer.IsBoundary)
|
||||
{
|
||||
layer.IsBoundary = true;
|
||||
cell.HeightLayers[li] = layer;
|
||||
totalBoundaryLayers++;
|
||||
//LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（地图边界），方向={dirNames[dir]}");
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var neighbor = gridMap.Cells[nx, ny];
|
||||
int neighborLayerCount = (neighbor.HeightLayers != null) ? neighbor.HeightLayers.Count : 0;
|
||||
|
||||
// 层数梯度检测：当前层数 > 邻居层数
|
||||
// 情况2：邻居是Unknown - 标记当前网格所有层为边界
|
||||
if (neighbor.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||||
{
|
||||
for (int li = 0; li < currentLayerCount; li++)
|
||||
{
|
||||
var layer = cell.HeightLayers[li];
|
||||
if (!layer.IsBoundary)
|
||||
{
|
||||
layer.IsBoundary = true;
|
||||
cell.HeightLayers[li] = layer;
|
||||
totalBoundaryLayers++;
|
||||
//LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（邻居Unknown），方向={dirNames[dir]}");
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// 情况3：层数梯度检测：当前层数 > 邻居层数
|
||||
if (currentLayerCount <= neighborLayerCount)
|
||||
continue;
|
||||
|
||||
@ -514,9 +544,9 @@ namespace NavisworksTransport.PathPlanning
|
||||
cell.HeightLayers[currentLayerIndex] = currentLayer;
|
||||
totalBoundaryLayers++;
|
||||
|
||||
LogManager.Debug($"【边界检测】网格({x},{y}) Layer{currentLayerIndex} 标记为边界，" +
|
||||
$"方向={dirNames[dir]}, 当前Z={currentZ:F2}, 邻居Z={neighborZ:F2}, " +
|
||||
$"高度差={heightDiff:F2}, 梯度={gradient}");
|
||||
// LogManager.Debug($"【边界检测】网格({x},{y}) Layer{currentLayerIndex} 标记为边界，" +
|
||||
// $"方向={dirNames[dir]}, 当前Z={currentZ:F2}, 邻居Z={neighborZ:F2}, " +
|
||||
// $"高度差={heightDiff:F2}, 梯度={gradient}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -673,7 +703,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 创建更新后的通道GridCell，保持原有属性但更新位置和高度
|
||||
var updatedCell = new GridCell
|
||||
{
|
||||
IsWalkable = true,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
@ -694,7 +723,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 创建高度不足的通道GridCell，保持原有属性但设为不可通行
|
||||
var updatedCell = new GridCell
|
||||
{
|
||||
IsWalkable = false,
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
@ -748,7 +776,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
if (cell.IsWalkable)
|
||||
if (cell.HasAnyWalkableLayer())
|
||||
{
|
||||
walkableCount++;
|
||||
switch (cell.CellType)
|
||||
@ -866,30 +894,37 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 默认为无穷大
|
||||
distanceMap[x, y] = double.MaxValue;
|
||||
|
||||
// 1. 整个网格是障碍物 → 距离为0
|
||||
if (!cell.IsWalkable)
|
||||
{
|
||||
distanceMap[x, y] = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 2. Unknown网格 → 距离为0
|
||||
// 1. Unknown网格 → 距离为0（整个网格无法通行）
|
||||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||||
{
|
||||
distanceMap[x, y] = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 3. 该层索引存在且是边界 → 距离为0
|
||||
// 2. 检查该层索引是否存在
|
||||
if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
|
||||
{
|
||||
var layer = cell.HeightLayers[layerIndex];
|
||||
|
||||
// 2a. 该层不可通行 → 距离为0（障碍物层）
|
||||
if (!layer.IsWalkable)
|
||||
{
|
||||
distanceMap[x, y] = 0.0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// 2b. 该层是边界 → 距离为0
|
||||
if (layer.IsBoundary)
|
||||
{
|
||||
distanceMap[x, y] = 0.0;
|
||||
boundaryCount++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 3. 该层索引不存在（网格没有这一层）→ 距离为0（相当于障碍物）
|
||||
distanceMap[x, y] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -964,14 +999,12 @@ namespace NavisworksTransport.PathPlanning
|
||||
if (layer.Type == CategoryAttributeManager.LogisticsElementType.门)
|
||||
continue;
|
||||
|
||||
// 膨胀条件：距离 < 膨胀半径
|
||||
// 边界网格距离=0，算在膨胀半径内
|
||||
// 例如：半径=1 → 只膨胀边界本身（距离0）
|
||||
// 半径=2 → 膨胀边界+向内1格（距离0,1）
|
||||
// 半径=3 → 膨胀边界+向内2格（距离0,1,2）
|
||||
// 膨胀条件：距离>0（排除源点）且距离<=半径
|
||||
// 例如：半径=2 → 膨胀距离1和距离2的网格
|
||||
// 半径=3 → 膨胀距离1、2、3的网格
|
||||
bool shouldInflate = false;
|
||||
|
||||
if (distanceMap[x, y] < inflationRadius)
|
||||
if (distanceMap[x, y] > 0.0 && distanceMap[x, y] <= inflationRadius)
|
||||
{
|
||||
shouldInflate = true;
|
||||
}
|
||||
@ -993,33 +1026,11 @@ namespace NavisworksTransport.PathPlanning
|
||||
LogManager.Info($"【多层膨胀】Layer{layerIndex} 完成，边界: {boundaryCount}, 膨胀: {inflatedCount}");
|
||||
}
|
||||
|
||||
// 第三步：更新GridCell.IsWalkable（至少有一层可通行）
|
||||
int cellsBecameUnwalkable = 0;
|
||||
for (int x = 0; x < gridMap.Width; x++)
|
||||
{
|
||||
for (int y = 0; y < gridMap.Height; y++)
|
||||
{
|
||||
var cell = gridMap.Cells[x, y];
|
||||
bool wasWalkable = cell.IsWalkable;
|
||||
|
||||
// 检查是否至少有一层可通行
|
||||
bool hasWalkableLayer = false;
|
||||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||||
{
|
||||
hasWalkableLayer = cell.HeightLayers.Any(layer => layer.IsWalkable);
|
||||
}
|
||||
|
||||
cell.IsWalkable = hasWalkableLayer;
|
||||
gridMap.Cells[x, y] = cell;
|
||||
|
||||
if (wasWalkable && !hasWalkableLayer)
|
||||
cellsBecameUnwalkable++;
|
||||
}
|
||||
}
|
||||
// GridCell.IsWalkable已移除，通行性由HeightLayer.IsWalkable决定
|
||||
// 不再需要更新网格级别的IsWalkable
|
||||
|
||||
stopwatch.Stop();
|
||||
LogManager.Info($"【多层膨胀】完成，耗时: {stopwatch.ElapsedMilliseconds}ms");
|
||||
LogManager.Info($"【多层膨胀】网格变为不可通行: {cellsBecameUnwalkable}个");
|
||||
|
||||
foreach (var kvp in layerStats.OrderBy(k => k.Key))
|
||||
{
|
||||
@ -1052,7 +1063,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
var cell = gridMap.Cells[x, y];
|
||||
|
||||
// 膨胀源点：1.障碍物 2.Unknown网格 3.边界层网格
|
||||
if (!cell.IsWalkable)
|
||||
if (!cell.HasAnyWalkableLayer())
|
||||
{
|
||||
distanceMap[x, y] = 0.0;
|
||||
obstacleCount++;
|
||||
@ -1180,10 +1191,23 @@ namespace NavisworksTransport.PathPlanning
|
||||
continue;
|
||||
}
|
||||
|
||||
// 创建障碍物GridCell
|
||||
var worldPos = gridMap.GridToWorld3D(gridPos);
|
||||
var obstacleCell = GridCellBuilder.Obstacle(worldPos, null);
|
||||
gridMap.PlaceCell(gridPos, obstacleCell);
|
||||
// 膨胀处理：保留层结构，只将所有层标记为不可通行
|
||||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||||
{
|
||||
// 有高度层：将所有层标记为不可通行
|
||||
for (int i = 0; i < cell.HeightLayers.Count; i++)
|
||||
{
|
||||
var layer = cell.HeightLayers[i];
|
||||
layer.IsWalkable = false;
|
||||
cell.HeightLayers[i] = layer;
|
||||
}
|
||||
}
|
||||
|
||||
// 更新网格类型和属性
|
||||
cell.CellType = CategoryAttributeManager.LogisticsElementType.障碍物;
|
||||
cell.Cost = double.MaxValue;
|
||||
cell.IsInChannel = false;
|
||||
gridMap.Cells[x, y] = cell;
|
||||
inflatedCells++;
|
||||
}
|
||||
else if (distanceMap[x, y] == double.MaxValue || distanceMap[x, y] > inflationRadius)
|
||||
@ -1673,7 +1697,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
|
||||
var updatedCells = 0;
|
||||
var obstacleItems = 0;
|
||||
var layersRemoved = 0;
|
||||
var layersBlocked = 0;
|
||||
var cellsFullyBlocked = 0;
|
||||
var cellsWithLayers = 0;
|
||||
var totalLayersChecked = 0;
|
||||
@ -1694,11 +1718,13 @@ namespace NavisworksTransport.PathPlanning
|
||||
totalLayersChecked += cell.HeightLayers.Count;
|
||||
}
|
||||
|
||||
// 3D高度检查：只移除与障碍物高度重叠的层
|
||||
var remainingLayers = new List<HeightLayer>();
|
||||
// 3D高度检查：标记与障碍物重叠的层为不可通行（保留层结构）
|
||||
bool anyLayerModified = false;
|
||||
|
||||
foreach (var layer in cell.HeightLayers)
|
||||
for (int i = 0; i < cell.HeightLayers.Count; i++)
|
||||
{
|
||||
var layer = cell.HeightLayers[i];
|
||||
|
||||
// 计算该层的高度范围
|
||||
double layerMinZ = layer.Z;
|
||||
double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();
|
||||
@ -1710,34 +1736,25 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 检查是否重叠
|
||||
bool overlaps = !(obstacleMaxZ <= layerMinZ || obstacleMinZ > layerMaxZ);
|
||||
|
||||
if (!overlaps)
|
||||
if (overlaps && layer.IsWalkable)
|
||||
{
|
||||
// 不重叠，保留该层
|
||||
remainingLayers.Add(layer);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 重叠，移除该层
|
||||
layersRemoved++;
|
||||
LogManager.Debug($"[3D高度检查] 网格({update.X},{update.Y}) 层Z={layer.Z:F2}, 高度范围=[{layerMinZ:F2},{layerMaxZ:F2}], 障碍物范围=[{obstacleMinZ:F2},{obstacleMaxZ:F2}], 重叠=移除");
|
||||
// 重叠：标记为不可通行，但保留层结构
|
||||
layer.IsWalkable = false;
|
||||
cell.HeightLayers[i] = layer;
|
||||
layersBlocked++;
|
||||
anyLayerModified = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 更新网格的高度层列表
|
||||
cell.HeightLayers = remainingLayers;
|
||||
|
||||
// 如果所有层都被移除，标记整个网格为障碍物
|
||||
if (remainingLayers.Count == 0)
|
||||
// 如果所有层都被标记为不可通行，标记整个网格为障碍物
|
||||
if (anyLayerModified && !cell.HeightLayers.Any(l => l.IsWalkable))
|
||||
{
|
||||
cell.IsWalkable = false;
|
||||
cell.CellType = CategoryAttributeManager.LogisticsElementType.障碍物;
|
||||
cell.Cost = double.MaxValue;
|
||||
cell.IsInChannel = false;
|
||||
cell.RelatedModelItem = update.Item;
|
||||
cellsFullyBlocked++;
|
||||
}
|
||||
// 如果还有层保留，网格仍然可通行（只是少了某些层）
|
||||
// 保持 IsWalkable = true，路径规划时会基于剩余层进行
|
||||
|
||||
gridMap.Cells[update.X, update.Y] = cell;
|
||||
updatedCells++;
|
||||
@ -1747,7 +1764,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
obstacleItems = gridUpdates.GroupBy(u => u.Item).Count();
|
||||
var updateElapsed = (DateTime.Now - updateStart).TotalMilliseconds;
|
||||
LogManager.Info($"[高性能障碍物处理] 阶段5完成: 更新 {updatedCells} 个网格单元，涉及 {obstacleItems} 个障碍物项，耗时: {updateElapsed:F1}ms");
|
||||
LogManager.Info($"[3D高度检查] 有层的网格: {cellsWithLayers}, 检查的总层数: {totalLayersChecked}, 移除了 {layersRemoved} 个高度层");
|
||||
LogManager.Info($"[3D高度检查] 有层的网格: {cellsWithLayers}, 检查的总层数: {totalLayersChecked}, 阻挡了 {layersBlocked} 个高度层");
|
||||
LogManager.Info($"[3D高度检查] {cellsFullyBlocked} 个网格被完全阻挡，{updatedCells - cellsFullyBlocked} 个网格保留了部分层");
|
||||
|
||||
// 输出总体统计
|
||||
@ -1987,4 +2004,4 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user