From 289eff555486f2d1659dc24b8929820ec86bd8b9 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Sat, 6 Sep 2025 19:13:58 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E7=A9=BA=E6=B4=9E?= =?UTF-8?q?=E5=92=8C=E9=9A=9C=E7=A2=8D=E7=89=A9=E7=BD=91=E6=A0=BC=E7=9A=84?= =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .claude/settings.local.json | 4 +- CLAUDE.md | 1 + doc/design/2026/GridCellTypeDesign.md | 259 ++++++++++++++++++ src/Core/PathPlanningManager.cs | 139 ++++++---- src/Core/PathPointRenderPlugin.cs | 44 ++- src/PathPlanning/AutoPathFinder.cs | 87 ++++-- src/PathPlanning/GridMap.cs | 31 ++- src/UI/WPF/ViewModels/PathEditingViewModel.cs | 10 +- 8 files changed, 481 insertions(+), 94 deletions(-) create mode 100644 doc/design/2026/GridCellTypeDesign.md diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 00fd7f1..5fe39c5 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -128,7 +128,9 @@ "Read(/C:\\Users\\Tellme\\apps\\NavisworksTransport\\src\\Core\\Collision/**)", "Read(/C:\\Users\\Tellme\\apps\\NavisworksTransport\\src\\Core\\Collision/**)", "Read(/C:\\Users\\Tellme\\apps\\NavisworksTransport\\src\\Core\\Collision/**)", - "Read(/C:\\Users\\Tellme\\apps\\NavisworksTransport\\src\\Core\\Collision/**)" + "Read(/C:\\Users\\Tellme\\apps\\NavisworksTransport\\src\\Core\\Collision/**)", + "WebSearch", + "Read(//c/Users/Tellme/apps/OpenSource/AStar-master/Roy-T.AStar/**)" ], "deny": [], "additionalDirectories": [ diff --git a/CLAUDE.md b/CLAUDE.md index 6c4f10c..02d0d3d 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -116,6 +116,7 @@ public class PathClickToolPlugin : ToolPlugin { } ### Development Principles - **不考虑向后兼容性**: 直接修改现有代码逻辑，无需保持向后兼容性，专注于解决当前问题和改进现有功能 +- **最小化修改原则**: 进行任何代码修改时，必须遵循最小化原则，只修改必要的部分，不添加额外的功能或逻辑，以确保问题能够清晰暴露和定位 ### Critical API Usage Patterns diff --git a/doc/design/2026/GridCellTypeDesign.md b/doc/design/2026/GridCellTypeDesign.md new file mode 100644 index 0000000..eae51ed --- /dev/null +++ b/doc/design/2026/GridCellTypeDesign.md @@ -0,0 +1,259 @@ +# 网格单元类型设计方案 + +**版本**: 1.0 +**日期**: 2025-09-06 +**作者**: Claude Code + +## 问题背景 + +### 当前问题 +在当前的自动寻路系统中，发现了路径会穿越空洞（没有几何覆盖的区域）的问题。从3D可视化中可以看到，黄色的寻路路径直接穿过了黑色的空洞区域，而不是沿着绿色的通行网格前进。 + +### 根本原因分析 +通过代码分析，发现问题出现在网格初始化和A*算法转换的逻辑中： + +1. **网格初始化问题** (`GridMap.InitializeCells`) + - 所有网格单元初始化时默认 `IsWalkable = false`（正确） + - 但类型被设置为 `ElementType.Obstacle`（问题所在） + +2. **通道投影只覆盖部分区域** (`ChannelBasedGridBuilder.ProjectChannelToGrid`) + - 只将通道几何覆盖的区域设置为可通行 + - 未被任何几何覆盖的空洞区域保持初始状态 + +3. **A*转换逻辑缺陷** (`AutoPathFinder.ConvertToAStarGridWith2_5D`) + - 只简单检查 `!cell.IsWalkable` 来判断是否阻塞 + - 对于空洞区域（未被几何覆盖），可能存在误判 + +## 新的网格类型设计 + +### 双属性设计理念 + +每个网格单元包含两个独立的属性： +1. **可通行性** (`IsWalkable`) - 布尔值，决定是否能够通行 +2. **物流类型** (`LogisticsType`) - 枚举值，决定通行成本和特殊处理 + +### LogisticsType 枚举定义 + +```csharp +public enum LogisticsType +{ + // 特殊类型 + Unknown = 0, // 未知/空洞 - 无几何覆盖（默认值） + + // 不可通行类型 + Obstacle = 1, // 障碍物 - 墙体、柱子、设备等 + + // 可通行类型（按优先级排序） + Floor = 10, // 普通楼板 - 权重 1.0 + Channel = 11, // 通道 - 权重 0.5（最优先） + Door = 12, // 门 - 权重 1.2 + Stairs = 13, // 楼梯 - 权重 3.0 + Elevator = 14, // 电梯 - 特殊处理 + LoadingZone = 15, // 装卸区 - 权重 0.8 + ParkingArea = 16, // 停车区 - 权重 0.9 + CheckPoint = 17, // 检查点 - 权重 1.5 +} +``` + +### GridCell 类设计 + +```csharp +public class GridCell +{ + // 双属性 + public bool IsWalkable { get; set; } + public LogisticsType Type { get; set; } + + // 成本计算 + public float GetCost() + { + if (!IsWalkable) + return float.MaxValue; + + switch (Type) + { + case LogisticsType.Channel: return 0.5f; + case LogisticsType.LoadingZone: return 0.8f; + case LogisticsType.ParkingArea: return 0.9f; + case LogisticsType.Floor: return 1.0f; + case LogisticsType.Door: return 1.2f; + case LogisticsType.CheckPoint: return 1.5f; + case LogisticsType.Stairs: return 3.0f; + default: return 1.0f; + } + } +} +``` + +### 属性组合逻辑 + +| LogisticsType | IsWalkable | 说明 | A*处理 | +|--------------|------------|------|--------| +| Unknown | false | 空洞区域 | 不可通行 | +| Obstacle | false | 障碍物 | 不可通行 | +| Floor | true | 普通楼板 | 可通行，权重1.0 | +| Channel | true | 通道 | 可通行，权重0.5 | +| Door | true | 门 | 可通行，权重1.2 | +| Stairs | true | 楼梯 | 可通行，权重3.0 | + +## 实现流程 + +### 网格生成流程 + +```csharp +// 步骤1：初始化（关键改进） +void InitializeGrid() +{ + for (int x = 0; x < width; x++) + { + for (int y = 0; y < height; y++) + { + grid[x, y] = new GridCell + { + IsWalkable = false, // 默认不可通行 + Type = LogisticsType.Unknown // 默认未知（空洞） + }; + } + } +} + +// 步骤2：投影楼板 +void ProjectFloors() +{ + foreach (var floor in floorGeometry) + { + var cells = GetCellsCoveredBy(floor); + foreach (var cell in cells) + { + cell.IsWalkable = true; + cell.Type = LogisticsType.Floor; + } + } +} + +// 步骤3：投影障碍物 +void ProjectObstacles() +{ + foreach (var obstacle in obstacleGeometry) + { + var cells = GetCellsCoveredBy(obstacle); + foreach (var cell in cells) + { + cell.IsWalkable = false; + cell.Type = LogisticsType.Obstacle; + } + } +} + +// 步骤4：应用物流属性（覆盖基础类型） +void ApplyLogisticsAttributes() +{ + // 通道（只覆盖可通行区域） + foreach (var channel in channelItems) + { + var cells = GetCellsCoveredBy(channel); + foreach (var cell in cells) + { + if (cell.IsWalkable) // 只覆盖可通行区域 + { + cell.Type = LogisticsType.Channel; + // IsWalkable 保持 true + } + } + } + + // 门、楼梯等同理 +} +``` + +### A*算法转换 + +```csharp +private Grid ConvertToAStarGrid(GridMap gridMap) +{ + var grid = CreateAStarGrid(); + + for (int x = 0; x < gridMap.Width; x++) + { + for (int y = 0; y < gridMap.Height; y++) + { + var cell = gridMap.Cells[x, y]; + + // 基于 IsWalkable 判断连通性 + if (!cell.IsWalkable) + { + grid.DisconnectNode(new GridPosition(x, y)); + } + else + { + // 基于 LogisticsType 设置权重 + var cost = cell.GetCost(); + grid.SetNodeWeight(new GridPosition(x, y), cost); + } + } + } + + return grid; +} +``` + +## 使用场景示例 + +### 场景1：最简单 - 只有楼板 +- **输入**: 只标记楼板几何，无物流属性 +- **结果**: + - 楼板区域 → `Floor` + `IsWalkable=true` (权重1.0) + - 其他区域 → `Unknown` + `IsWalkable=false` (不可通行) +- **路径**: 在楼板范围内找几何最短路径 + +### 场景2：楼板 + 通道 +- **输入**: 标记楼板几何 + 部分区域标记为通道 +- **结果**: + - 通道区域 → `Channel` + `IsWalkable=true` (权重0.5) + - 其他楼板 → `Floor` + `IsWalkable=true` (权重1.0) + - 空洞区域 → `Unknown` + `IsWalkable=false` (不可通行) +- **路径**: 优先走通道，必要时走普通楼板，绝不穿越空洞 + +### 场景3：复杂场景 +- **输入**: 楼板 + 通道 + 门 + 楼梯 + 障碍物等混合 +- **结果**: 各种类型有不同的权重 +- **路径**: A*算法综合考虑距离和成本找最优路径 + +## 关键改进点 + +### 1. 解决空洞穿越问题 +- **根本解决**: `Unknown` 类型 + `IsWalkable=false` 确保空洞区域不可通行 +- **可视化一致**: 黑色空洞区域在算法中也确实不可通行 + +### 2. 逻辑清晰 +- **职责分离**: 通行性和类型分开处理，逻辑更清晰 +- **易于调试**: 可以分别检查通行性和类型，快速定位问题 + +### 3. 灵活扩展 +- **新类型**: 轻松添加新的物流类型和权重 +- **特殊场景**: 支持"不可通行的通道"（施工中）等特殊组合 + +### 4. 向后兼容 +- **现有工作流**: 继续支持只设置通道的工作方式 +- **渐进迁移**: 可以逐步引入更复杂的几何和属性设置 + +## 实施优先级 + +### 第一阶段：修复空洞问题 +1. 修改 `GridMap.InitializeCells()` - 默认类型改为 `Unknown` +2. 修改 `AutoPathFinder.ConvertToAStarGridWith2_5D()` - 基于 `IsWalkable` 判断 +3. 测试验证空洞不再可穿越 + +### 第二阶段：完善类型系统 +1. 实现完整的 `LogisticsType` 枚举 +2. 实现权重计算系统 +3. 支持楼板几何投影 + +### 第三阶段：扩展功能 +1. 支持更多物流属性类型 +2. 实现复杂场景的自动分类 +3. 优化性能和内存使用 + +## 总结 + +这个双属性设计方案既解决了当前的空洞穿越问题，又为未来的功能扩展奠定了坚实基础。通过清晰的职责分离和灵活的类型系统，系统能够支持从简单楼板到复杂物流场景的各种需求。 \ No newline at end of file diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index d289df0..2d58214 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -590,7 +590,7 @@ namespace NavisworksTransport { _pathPointMarkers?.Clear(); // 清除路径标记但保留网格可视化 - _renderPlugin?.ClearPathsExcept("grid_visualization_all"); + _renderPlugin?.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); LogManager.Info("3D路径标记已清除（保留网格可视化）"); } catch (Exception ex) @@ -3099,80 +3099,120 @@ namespace NavisworksTransport // 清除之前的网格可视化 ClearGridVisualization(); - // 创建单一的网格可视化路径 - var gridVisualizationRoute = new PathRoute("GridVisualization") + // 创建三个独立的网格可视化路径 + var channelRoute = new PathRoute("GridVisualization_Channel") { - Id = "grid_visualization_all" + Id = "grid_visualization_channel" + }; + + var unknownRoute = new PathRoute("GridVisualization_Unknown") + { + Id = "grid_visualization_unknown" + }; + + var obstacleRoute = new PathRoute("GridVisualization_Obstacle") + { + Id = "grid_visualization_obstacle" }; // 统计变量 - int walkableCells = 0; int channelCells = 0; + int unknownCells = 0; + int obstacleCells = 0; int openSpaceCells = 0; - int visualizedCells = 0; - const int maxVisualizationPoints = 5000; // 限制最大可视化点数 + int totalVisualized = 0; - // 遍历所有网格单元格，批量收集可通行网格点 - for (int x = 0; x < gridMap.Width && visualizedCells < maxVisualizationPoints; x++) + // 遍历所有网格单元格，根据类型分别收集 + for (int x = 0; x < gridMap.Width; x++) { - for (int y = 0; y < gridMap.Height && visualizedCells < maxVisualizationPoints; y++) + for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - // 只处理可通行的单元格 - if (cell.IsWalkable) + // 计算网格单元格的世界中心位置 + var gridPos = new NavisworksTransport.PathPlanning.Point2D(x, y); + var worldCenter = gridMap.GridToWorld(gridPos); + + // 使用网格单元格中心的Z坐标，如果没有则使用网格边界的最大Z值 + double centerZ = cell.WorldPosition.Z != 0 ? cell.WorldPosition.Z : gridMap.Bounds.Max.Z; + var visualizationPoint = new Point3D(worldCenter.X, worldCenter.Y, centerZ); + + PathRoute targetRoute = null; + string gridTypeName = ""; + + // 根据网格类型确定目标路径和名称 + if (cell.IsWalkable && cell.CellType == ElementType.Channel) + { + targetRoute = channelRoute; + gridTypeName = "通道"; + channelCells++; + } + else if (cell.IsWalkable && cell.CellType == ElementType.OpenSpace) + { + targetRoute = channelRoute; // 开放空间也用绿色显示 + gridTypeName = "开放"; + openSpaceCells++; + } + else if (cell.CellType == ElementType.Unknown) + { + targetRoute = unknownRoute; + gridTypeName = "空洞"; + unknownCells++; + } + else if (cell.CellType == ElementType.Obstacle) + { + targetRoute = obstacleRoute; + gridTypeName = "障碍"; + obstacleCells++; + } + + // 如果有对应的路径，创建并添加网格点 + if (targetRoute != null) { - walkableCells++; - - // 根据单元格类型分类统计 - if (cell.CellType == ElementType.Channel) - channelCells++; - else if (cell.CellType == ElementType.OpenSpace) - openSpaceCells++; - - // 计算网格单元格的世界中心位置 - var gridPos = new NavisworksTransport.PathPlanning.Point2D(x, y); - var worldCenter = gridMap.GridToWorld(gridPos); - - // 使用网格单元格中心的Z坐标，如果没有则使用网格边界的最大Z值 - double centerZ = cell.WorldPosition.Z != 0 ? cell.WorldPosition.Z : gridMap.Bounds.Max.Z; - var visualizationPoint = new Point3D(worldCenter.X, worldCenter.Y, centerZ); - - // 创建网格可视化点 var gridPoint = new PathPoint { Position = visualizationPoint, - Name = $"网格({x},{y})", + Name = $"网格_{gridTypeName}({x},{y})", Type = PathPointType.WayPoint, - Index = visualizedCells // 设置索引 + Index = totalVisualized, + Notes = $"GridType:{cell.CellType}" }; - // 添加到统一的路径中 - gridVisualizationRoute.Points.Add(gridPoint); - visualizedCells++; + targetRoute.Points.Add(gridPoint); + totalVisualized++; } } } - // 批量渲染所有网格点 - if (gridVisualizationRoute.Points.Count > 0) + // 分别渲染三种类型的网格点 + if (channelRoute.Points.Count > 0) { - renderPlugin.RenderPointOnly(gridVisualizationRoute); + renderPlugin.RenderPointOnly(channelRoute); + LogManager.Info($"[网格可视化] 渲染通道/开放空间网格点: {channelRoute.Points.Count} 个"); + } + + if (unknownRoute.Points.Count > 0) + { + renderPlugin.RenderPointOnly(unknownRoute); + LogManager.Info($"[网格可视化] 渲染Unknown网格点: {unknownRoute.Points.Count} 个"); + } + + if (obstacleRoute.Points.Count > 0) + { + renderPlugin.RenderPointOnly(obstacleRoute); + LogManager.Info($"[网格可视化] 渲染障碍物网格点: {obstacleRoute.Points.Count} 个"); } // 输出统计信息 LogManager.Info($"[网格可视化] 可视化完成："); LogManager.Info($" - 总网格单元格数：{gridMap.Width * gridMap.Height}"); - LogManager.Info($" - 可通行单元格数：{walkableCells}"); - LogManager.Info($" - 其中通道单元格：{channelCells}"); - LogManager.Info($" - 其中开放空间单元格：{openSpaceCells}"); - LogManager.Info($" - 已可视化单元格数：{visualizedCells}"); - if (visualizedCells >= maxVisualizationPoints) - { - LogManager.Info($" - 注意：可视化点数已达到上限 {maxVisualizationPoints}"); - } + LogManager.Info($" - 通道单元格数：{channelCells}"); + LogManager.Info($" - 开放空间单元格数：{openSpaceCells}"); + LogManager.Info($" - Unknown单元格数：{unknownCells}"); + LogManager.Info($" - 障碍物单元格数：{obstacleCells}"); + LogManager.Info($" - 已可视化单元格数：{totalVisualized}"); - RaiseStatusChanged($"网格可视化完成：显示了 {visualizedCells} 个可通行网格单元", PathPlanningStatusType.Success); + RaiseStatusChanged($"网格可视化完成：显示了 {totalVisualized} 个网格单元 (通道:{channelCells}, Unknown:{unknownCells}, 障碍:{obstacleCells})", PathPlanningStatusType.Success); } catch (Exception ex) { @@ -3193,8 +3233,11 @@ namespace NavisworksTransport var renderPlugin = PathPointRenderPlugin.Instance; if (renderPlugin != null) { - // 只清除网格可视化路径 - renderPlugin.RemovePath("grid_visualization_all"); + // 清除所有网格可视化路径（包括旧的和新的） + renderPlugin.RemovePath("grid_visualization_all"); // 旧的单一路径 + renderPlugin.RemovePath("grid_visualization_channel"); // 新的通道路径 + renderPlugin.RemovePath("grid_visualization_unknown"); // 新的Unknown路径 + renderPlugin.RemovePath("grid_visualization_obstacle"); // 新的障碍物路径 LogManager.Info("[网格可视化] 网格可视化已清除"); } else diff --git a/src/Core/PathPointRenderPlugin.cs b/src/Core/PathPointRenderPlugin.cs index 9121607..937180c 100644 --- a/src/Core/PathPointRenderPlugin.cs +++ b/src/Core/PathPointRenderPlugin.cs @@ -306,7 +306,10 @@ namespace NavisworksTransport try { // 检查是否是网格可视化路径 - bool isGridVisualization = pathRoute.Id == "grid_visualization_all"; + bool isGridVisualization = pathRoute.Id == "grid_visualization_all" || + pathRoute.Id == "grid_visualization_channel" || + pathRoute.Id == "grid_visualization_unknown" || + pathRoute.Id == "grid_visualization_obstacle"; var visualization = new PathVisualization { @@ -353,7 +356,10 @@ namespace NavisworksTransport if (points.Count == 0) return; // 检查是否是网格可视化路径 - bool isGridVisualization = visualization.PathId == "grid_visualization_all"; + bool isGridVisualization = visualization.PathId == "grid_visualization_all" || + visualization.PathId == "grid_visualization_channel" || + visualization.PathId == "grid_visualization_unknown" || + visualization.PathId == "grid_visualization_obstacle"; // 按索引排序点 var sortedPoints = points.OrderBy(p => p.Index).ToList(); @@ -505,14 +511,44 @@ namespace NavisworksTransport private CircleMarker CreatePointMarker(PathPoint point) { // 检查是否是网格可视化点（通过名称判断） - bool isGridVisualization = point.Name.StartsWith("网格("); + bool isGridVisualization = point.Name.StartsWith("网格(") || point.Name.StartsWith("网格_"); + + // 确定网格点颜色 + Color gridColor = Color.Green; // 默认绿色 + if (isGridVisualization) + { + // 根据网格类型确定颜色 + if (point.Name.Contains("空洞") || point.Name.Contains("Unknown")) + { + gridColor = Color.Red; + } + else if (point.Name.Contains("障碍") || point.Name.Contains("Obstacle")) + { + gridColor = Color.FromByteRGB(128, 128, 128); // 灰色 + } + else if (point.Name.Contains("通道") || point.Name.Contains("Channel") || point.Name.Contains("开放")) + { + gridColor = Color.Green; + } + // 也可以通过Notes字段检查 + else if (!string.IsNullOrEmpty(point.Notes) && point.Notes.StartsWith("GridType:")) + { + var gridTypeStr = point.Notes.Substring(9); // 去掉"GridType:"前缀 + if (gridTypeStr == "Unknown") + gridColor = Color.Red; + else if (gridTypeStr == "Obstacle") + gridColor = Color.FromByteRGB(128, 128, 128); // 灰色 + else + gridColor = Color.Green; + } + } return new CircleMarker { Center = point.Position, Normal = new Vector3D(0, 0, 1), Radius = isGridVisualization ? GetRadiusForGridVisualization() : GetRadiusForPointType(point.Type), - Color = isGridVisualization ? Color.Green : GetColorForPointType(point.Type), + Color = isGridVisualization ? gridColor : GetColorForPointType(point.Type), Alpha = 1.0, Filled = true, PointType = point.Type, diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index 106be69..ac4f594 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -95,9 +95,10 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {rawPath.Count} 个点"); - // 4. 路径优化（添加缺失的步骤） - var optimizedPath = OptimizePath(rawPath, gridMap); - LogManager.Info($"[2.5D路径规划] 路径优化完成，从 {rawPath.Count} 个点优化到 {optimizedPath.Count} 个点"); + // 4. 路径优化（临时禁用以避免穿越问题） + // var optimizedPath = OptimizePath(rawPath, gridMap); + var optimizedPath = rawPath; // 直接使用原始A*路径，不进行优化 + LogManager.Info($"[2.5D路径规划] 路径优化已禁用，使用原始A*路径，包含 {optimizedPath.Count} 个点"); // 5. 对路径进行高度插值，利用2.5D高度区间信息 var enhancedPath = InterpolatePathHeights2_5D(optimizedPath, gridMap, vehicleHeight); @@ -146,45 +147,73 @@ namespace NavisworksTransport.PathPlanning var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, traversalVelocity); LogManager.Info($"[A*转换-2.5D] A*网格创建完成"); - int blockedCells = 0; - int heightConstrainedCells = 0; + // 先断开所有节点连接 + for (int x = 0; x < gridMap.Width; x++) + { + for (int y = 0; y < gridMap.Height; y++) + { + grid.DisconnectNode(new GridPosition(x, y)); + } + } - // 标记不可通行的单元格，考虑高度约束 + // 只连接可通行的节点 + int connectedCells = 0; for (int x = 0; x < gridMap.Width; x++) { for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - bool cellBlocked = false; - if (!cell.IsWalkable) + // 检查基本可通行性和高度约束 + bool isPassable = cell.IsWalkable; + if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any()) { - // 传统的不可行走单元格 - cellBlocked = true; - blockedCells++; + isPassable = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); } - else if (cell.PassableHeights != null && cell.PassableHeights.Any()) + + if (isPassable) { - // 检查是否有足够的高度空间 - bool hasValidHeight = cell.PassableHeights.Any(interval => - interval.GetSpan() >= vehicleHeight); + var pos = new GridPosition(x, y); + connectedCells++; - if (!hasValidHeight) + // 连接右侧邻居 + if (x + 1 < gridMap.Width) { - cellBlocked = true; - heightConstrainedCells++; + var rightCell = gridMap.Cells[x + 1, y]; + bool rightPassable = rightCell.IsWalkable; + if (rightPassable && rightCell.PassableHeights != null && rightCell.PassableHeights.Any()) + { + rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); + } + if (rightPassable) + { + var rightPos = new GridPosition(x + 1, y); + grid.AddEdge(pos, rightPos, traversalVelocity); + grid.AddEdge(rightPos, pos, traversalVelocity); + } + } + + // 连接下方邻居 + if (y + 1 < gridMap.Height) + { + var bottomCell = gridMap.Cells[x, y + 1]; + bool bottomPassable = bottomCell.IsWalkable; + if (bottomPassable && bottomCell.PassableHeights != null && bottomCell.PassableHeights.Any()) + { + bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); + } + if (bottomPassable) + { + var bottomPos = new GridPosition(x, y + 1); + grid.AddEdge(pos, bottomPos, traversalVelocity); + grid.AddEdge(bottomPos, pos, traversalVelocity); + } } - } - - if (cellBlocked) - { - var gridPos = new GridPosition(x, y); - grid.DisconnectNode(gridPos); } } } - LogManager.Info($"[A*转换-2.5D] 网格转换完成，阻塞单元格: {blockedCells}个，高度约束阻塞: {heightConstrainedCells}个"); + LogManager.Info($"[A*转换-2.5D] 网格转换完成，已连接 {connectedCells} 个可通行节点"); return grid; } catch (Exception ex) @@ -214,8 +243,8 @@ namespace NavisworksTransport.PathPlanning if (startNode != null) { // RoyT.AStar返回的Position是米坐标，需要转换为网格索引 - var startGridX = (int)Math.Round(startNode.Position.X / cellSizeInMeters); - var startGridY = (int)Math.Round(startNode.Position.Y / cellSizeInMeters); + var startGridX = (int)Math.Floor(startNode.Position.X / cellSizeInMeters); + var startGridY = (int)Math.Floor(startNode.Position.Y / cellSizeInMeters); var startGridPos = new Point2D(startGridX, startGridY); var startWorldPos = gridMap.GridToWorld(startGridPos); LogManager.Info($"[路径转换] 起始节点: A*米坐标({startNode.Position.X:F2}, {startNode.Position.Y:F2}) -> 网格索引({startGridPos.X}, {startGridPos.Y}) -> 世界坐标({startWorldPos.X:F2}, {startWorldPos.Y:F2}, {startWorldPos.Z:F2})"); @@ -227,8 +256,8 @@ namespace NavisworksTransport.PathPlanning foreach (var edge in astarPath.Edges) { // RoyT.AStar返回的Position是米坐标，需要转换为网格索引 - var gridX = (int)Math.Round(edge.End.Position.X / cellSizeInMeters); - var gridY = (int)Math.Round(edge.End.Position.Y / cellSizeInMeters); + var gridX = (int)Math.Floor(edge.End.Position.X / cellSizeInMeters); + var gridY = (int)Math.Floor(edge.End.Position.Y / cellSizeInMeters); var gridPos = new Point2D(gridX, gridY); var worldPos = gridMap.GridToWorld(gridPos); diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index ca931b0..a37fb70 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -159,7 +159,7 @@ namespace NavisworksTransport.PathPlanning { IsWalkable = false, Cost = double.MaxValue, - CellType = ElementType.Obstacle, + CellType = ElementType.Unknown, // 修改：默认为未知/空洞类型 IsInChannel = false, PassableHeights = new List(), ChannelType = ChannelType.Other, @@ -488,6 +488,7 @@ namespace NavisworksTransport.PathPlanning int obstacleCount = 0; int doorCount = 0; int channelCount = 0; + int unknownCount = 0; for (int x = 0; x < Width; x++) { @@ -511,13 +512,24 @@ namespace NavisworksTransport.PathPlanning } else { - obstacleCount++; + switch (cell.CellType) + { + case ElementType.Unknown: + unknownCount++; + break; + case ElementType.Obstacle: + obstacleCount++; + break; + default: + obstacleCount++; + break; + } } } } return $"网格统计: {Width}x{Height} ({Width * Height}个单元格), " + - $"可通行: {walkableCount}, 障碍物: {obstacleCount}, " + + $"可通行: {walkableCount}, 障碍物: {obstacleCount}, 未知: {unknownCount}, " + $"门: {doorCount}, 通道: {channelCount}, " + $"内存使用: {GetMemoryUsage() / 1024.0:F1} KB"; } @@ -646,25 +658,30 @@ namespace NavisworksTransport.PathPlanning /// public enum ElementType { + /// + /// 未知/空洞 - 没有任何几何覆盖，不可通行 + /// + Unknown = 0, + /// /// 开放空间 - 默认可通行区域 /// - OpenSpace = 0, + OpenSpace = 1, /// /// 障碍物 - 不可通行 /// - Obstacle = 1, + Obstacle = 2, /// /// 门 - 条件性可通行 /// - Door = 2, + Door = 3, /// /// 通道 - 优先通行路线 /// - Channel = 3 + Channel = 4 } /// diff --git a/src/UI/WPF/ViewModels/PathEditingViewModel.cs b/src/UI/WPF/ViewModels/PathEditingViewModel.cs index 9cb6551..401f808 100644 --- a/src/UI/WPF/ViewModels/PathEditingViewModel.cs +++ b/src/UI/WPF/ViewModels/PathEditingViewModel.cs @@ -153,7 +153,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels } // 1. 清理现有的路径可视化，但保留网格可视化 - PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all"); + PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); LogManager.WriteLog("[路径可视化] 已清理现有路径显示（保留网格可视化）"); // 2. 如果有选中的路径，则显示该路径 @@ -465,7 +465,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels { try { - PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all"); + PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); LogManager.Info("新建路径：已清除现有路径可视化显示（保留网格可视化）"); } catch (Exception ex) @@ -733,7 +733,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels ClearTemporaryAutoPathMarkers(); if (PathPointRenderPlugin.Instance != null) { - PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all"); // 清理历史路径但保留网格可视化 + PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); // 清理历史路径但保留网格可视化 LogManager.WriteLog("[自动路径规划] 已清理历史路径对象（保留网格可视化）"); } @@ -821,7 +821,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels // 确保失败后也清理所有可能的残留路径对象 if (PathPointRenderPlugin.Instance != null) { - PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all"); + PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); LogManager.WriteLog("[自动路径规划] 规划失败后已清理渲染对象（保留网格可视化）"); } } @@ -848,7 +848,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels try { // 清除路径可视化，但保留网格可视化 - PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all"); + PathPointRenderPlugin.Instance.ClearPathsExcept("grid_visualization_all", "grid_visualization_channel", "grid_visualization_unknown", "grid_visualization_obstacle"); LogManager.Info("重置参数：已清除路径可视化显示（保留网格可视化）"); } catch (Exception ex)