增加了空洞和障碍物网格的可视化
This commit is contained in:
parent
e73cd2113e
commit
289eff5554
@ -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": [
|
||||
|
||||
@ -116,6 +116,7 @@ public class PathClickToolPlugin : ToolPlugin { }
|
||||
### Development Principles
|
||||
|
||||
- **不考虑向后兼容性**: 直接修改现有代码逻辑，无需保持向后兼容性，专注于解决当前问题和改进现有功能
|
||||
- **最小化修改原则**: 进行任何代码修改时，必须遵循最小化原则，只修改必要的部分，不添加额外的功能或逻辑，以确保问题能够清晰暴露和定位
|
||||
|
||||
### Critical API Usage Patterns
|
||||
|
||||
|
||||
259
doc/design/2026/GridCellTypeDesign.md
Normal file
259
doc/design/2026/GridCellTypeDesign.md
Normal file
@ -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. 优化性能和内存使用
|
||||
|
||||
## 总结
|
||||
|
||||
这个双属性设计方案既解决了当前的空洞穿越问题，又为未来的功能扩展奠定了坚实基础。通过清晰的职责分离和灵活的类型系统，系统能够支持从简单楼板到复杂物流场景的各种需求。
|
||||
@ -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
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -159,7 +159,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
IsWalkable = false,
|
||||
Cost = double.MaxValue,
|
||||
CellType = ElementType.Obstacle,
|
||||
CellType = ElementType.Unknown, // 修改：默认为未知/空洞类型
|
||||
IsInChannel = false,
|
||||
PassableHeights = new List<HeightInterval>(),
|
||||
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
|
||||
/// </summary>
|
||||
public enum ElementType
|
||||
{
|
||||
/// <summary>
|
||||
/// 未知/空洞 - 没有任何几何覆盖，不可通行
|
||||
/// </summary>
|
||||
Unknown = 0,
|
||||
|
||||
/// <summary>
|
||||
/// 开放空间 - 默认可通行区域
|
||||
/// </summary>
|
||||
OpenSpace = 0,
|
||||
OpenSpace = 1,
|
||||
|
||||
/// <summary>
|
||||
/// 障碍物 - 不可通行
|
||||
/// </summary>
|
||||
Obstacle = 1,
|
||||
Obstacle = 2,
|
||||
|
||||
/// <summary>
|
||||
/// 门 - 条件性可通行
|
||||
/// </summary>
|
||||
Door = 2,
|
||||
Door = 3,
|
||||
|
||||
/// <summary>
|
||||
/// 通道 - 优先通行路线
|
||||
/// </summary>
|
||||
Channel = 3
|
||||
Channel = 4
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user