From 2f78b4e58eb977e65c852cb94f172a7bc3b4ae5d Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Sun, 12 Oct 2025 21:12:27 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8DSDF=E6=A0=87=E8=AE=B0?= =?UTF-8?q?=E9=98=B6=E6=AE=B5=E5=9C=A8Z=E6=96=B9=E5=90=91=E5=BA=94?= =?UTF-8?q?=E7=94=A8=E5=AE=89=E5=85=A8=E9=97=B4=E9=9A=99=E7=9A=84=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 问题： - SDF标记阶段使用了3D距离判断（distance < safetyMargin） - 导致薄楼板上下0.6米范围内的体素都被标记为障碍物 - 违反了"车辆只会侧面或顶部碰撞"的设计原则 解决方案： - SDF标记阶段只标记几何体内部（distance < 0）为障碍物 - 移除SDF阶段的安全间隙判断（minPassableDistance） - 安全间隙只在后续的XY平面膨胀阶段应用 效果： - 薄楼板只在其实际占据的Z层被标记为障碍物 - 楼板上下空间不会被错误标记 - 安全间隙仅在水平方向（XY平面）生效 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- src/PathPlanning/VoxelGridGenerator.cs | 241 +++---------------------- 1 file changed, 27 insertions(+), 214 deletions(-) diff --git a/src/PathPlanning/VoxelGridGenerator.cs b/src/PathPlanning/VoxelGridGenerator.cs index 95ab692..bd38eb2 100644 --- a/src/PathPlanning/VoxelGridGenerator.cs +++ b/src/PathPlanning/VoxelGridGenerator.cs @@ -11,213 +11,10 @@ namespace NavisworksTransport.PathPlanning { /// /// 体素网格生成器 - 从 Navisworks BIM 模型生成 3D 体素网格 - /// 阶段 1 原型版本：使用简单的包围盒方法进行体素化 + /// 使用 SDF (Signed Distance Field) 方法进行精确体素化 /// public class VoxelGridGenerator { - /// - /// 从 BIM 模型生成体素网格（简化版 - 使用包围盒） - /// - /// 网格边界（世界坐标，模型单位） - /// 体素尺寸（米） - /// 车辆半径（米），用于膨胀障碍物 - /// 车辆高度（米），用于检测通行空间 - /// BIM 模型元素列表 - /// 生成的体素网格 - public VoxelGrid GenerateFromBIM( - BoundingBox3D bounds, - double voxelSizeMeters, - double vehicleRadiusMeters, - double vehicleHeightMeters, - IEnumerable modelItems) - { - var stopwatch = Stopwatch.StartNew(); - LogManager.Info("=== 开始体素网格生成（简单包围盒方法） ==="); - - // 第一步：单位转换（米 → 模型单位） - double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor( - Autodesk.Navisworks.Api.Application.ActiveDocument.Units); - - double voxelSizeInModelUnits = voxelSizeMeters * metersToModelUnits; - double vehicleRadiusInModelUnits = vehicleRadiusMeters * metersToModelUnits; - double vehicleHeightInModelUnits = vehicleHeightMeters * metersToModelUnits; - - LogManager.Info($"单位转换系数: {metersToModelUnits:F4}"); - LogManager.Info($"体素尺寸: {voxelSizeMeters}米 = {voxelSizeInModelUnits:F2}模型单位"); - LogManager.Info($"车辆半径: {vehicleRadiusMeters}米 = {vehicleRadiusInModelUnits:F2}模型单位"); - LogManager.Info($"车辆高度: {vehicleHeightMeters}米 = {vehicleHeightInModelUnits:F2}模型单位"); - - // 第二步：创建体素网格 - var voxelGrid = new VoxelGrid(bounds, voxelSizeInModelUnits); - LogManager.Info($"创建体素网格: {voxelGrid.SizeX} × {voxelGrid.SizeY} × {voxelGrid.SizeZ} = {voxelGrid.TotalVoxels:N0} 个体素"); - - // 第三步：提取模型元素并分类 - var obstacleItems = new List<(ModelItem item, BoundingBox3D bbox, LogisticsElementType type)>(); - int processedCount = 0; - int skippedCount = 0; - - foreach (var item in modelItems) - { - processedCount++; - - // 跳过没有几何体的元素 - if (!item.HasGeometry) - { - skippedCount++; - continue; - } - - // 获取包围盒 - var bbox = item.BoundingBox(); - if (bbox == null) - { - skippedCount++; - continue; - } - - // 获取物流类型（从自定义属性或使用默认值） - var elementType = GetLogisticsElementType(item); - - obstacleItems.Add((item, bbox, elementType)); - } - - LogManager.Info($"模型元素统计: 总数={processedCount}, 有效={obstacleItems.Count}, 跳过={skippedCount}"); - - // 第四步：体素化 - 使用包围盒标记障碍物 - int obstacleVoxelCount = 0; - int passableVoxelCount = 0; - - foreach (var (item, bbox, elementType) in obstacleItems) - { - // 判断是否为障碍物 - bool isObstacle = IsObstacleType(elementType); - - // 体素化该包围盒 - int markedCount = VoxelizeBoundingBox( - voxelGrid, - bbox, - isObstacle, - elementType, - item, - vehicleRadiusInModelUnits); - - if (isObstacle) - obstacleVoxelCount += markedCount; - else - passableVoxelCount += markedCount; - } - - LogManager.Info($"体素标记完成: 障碍物体素={obstacleVoxelCount:N0}, 可通行体素={passableVoxelCount:N0}"); - - // 第五步：统计信息 - var (total, passable, obstacle) = voxelGrid.GetStatistics(); - double passableRatio = (double)passable / total * 100.0; - - stopwatch.Stop(); - LogManager.Info($"=== 体素网格生成完成 ==="); - LogManager.Info($"总体素数: {total:N0}"); - LogManager.Info($"可通行体素: {passable:N0} ({passableRatio:F1}%)"); - LogManager.Info($"障碍物体素: {obstacle:N0} ({100 - passableRatio:F1}%)"); - LogManager.Info($"生成耗时: {stopwatch.ElapsedMilliseconds} ms ({stopwatch.Elapsed.TotalSeconds:F2} 秒)"); - - return voxelGrid; - } - - /// - /// 使用包围盒标记体素 - /// - /// 体素网格 - /// 包围盒（模型单位） - /// 是否为障碍物 - /// 元素类型 - /// 源模型元素 - /// 膨胀半径（模型单位），用于障碍物 - /// 标记的体素数量 - private int VoxelizeBoundingBox( - VoxelGrid voxelGrid, - BoundingBox3D bbox, - bool isObstacle, - LogisticsElementType elementType, - ModelItem sourceItem, - double inflationRadius) - { - int markedCount = 0; - - // 膨胀包围盒（仅对障碍物） - BoundingBox3D inflatedBbox = bbox; - if (isObstacle && inflationRadius > 0) - { - double inflation = inflationRadius; - inflatedBbox = new BoundingBox3D( - new Point3D(bbox.Min.X - inflation, bbox.Min.Y - inflation, bbox.Min.Z - inflation), - new Point3D(bbox.Max.X + inflation, bbox.Max.Y + inflation, bbox.Max.Z + inflation) - ); - } - - // 计算包围盒覆盖的体素范围 - var (minX, minY, minZ) = voxelGrid.WorldToVoxel(inflatedBbox.Min); - var (maxX, maxY, maxZ) = voxelGrid.WorldToVoxel(inflatedBbox.Max); - - // 遍历该范围内的所有体素 - for (int x = Math.Max(0, minX); x <= Math.Min(voxelGrid.SizeX - 1, maxX); x++) - { - for (int y = Math.Max(0, minY); y <= Math.Min(voxelGrid.SizeY - 1, maxY); y++) - { - for (int z = Math.Max(0, minZ); z <= Math.Min(voxelGrid.SizeZ - 1, maxZ); z++) - { - // 检查体素中心是否在膨胀后的包围盒内 - Point3D voxelCenter = voxelGrid.VoxelToWorldCenter(x, y, z); - - if (IsPointInBoundingBox(voxelCenter, inflatedBbox)) - { - var cell = voxelGrid.GetCell(x, y, z); - - // 标记体素 - if (isObstacle) - { - // 障碍物：标记为不可通行 - cell.SetAsObstacle(sourceItem); - cell.Type = elementType; - } - else - { - // 可通行元素（门、通道等）：保持可通行，但设置类型 - // 注意：不覆盖已经标记为障碍物的体素 - if (cell.IsPassable) - { - cell.Type = elementType; - cell.SourceItem = sourceItem; - - // 设置速度限制（根据类型） - if (elementType == LogisticsElementType.门) - { - cell.SpeedLimit = 1.0; // 门的速度限制 1 m/s - } - else if (elementType == LogisticsElementType.楼梯) - { - cell.SpeedLimit = 0.5; // 楼梯速度限制 0.5 m/s - } - } - } - - markedCount++; - } - } - } - } - - return markedCount; - } - - /// - /// 检查点是否在包围盒内 - /// - private bool IsPointInBoundingBox(Point3D point, BoundingBox3D bbox) - { - return point.X >= bbox.Min.X && point.X <= bbox.Max.X && - point.Y >= bbox.Min.Y && point.Y <= bbox.Max.Y && - point.Z >= bbox.Min.Z && point.Z <= bbox.Max.Z; - } /// /// 获取模型元素的物流类型 @@ -327,11 +124,28 @@ namespace NavisworksTransport.PathPlanning var voxelGrid = new VoxelGrid(bounds, voxelSizeInModelUnits); LogManager.Info($"创建体素网格: {voxelGrid.SizeX} × {voxelGrid.SizeY} × {voxelGrid.SizeZ} = {voxelGrid.TotalVoxels:N0} 个体素"); - // 第三步：提取障碍物几何体并转换为 DMesh3 + // 第三步：过滤模型元素 - 排除门，只保留真正的障碍物 + LogManager.Info("开始过滤模型元素（排除门）..."); + var filterStopwatch = Stopwatch.StartNew(); + + int totalItemsCount = obstacleItems.Count(); + var filteredItems = obstacleItems.Where(item => + { + var elementType = GetLogisticsElementType(item); + // 排除门：门留出空洞，不作为障碍物 + // 保留真正的障碍物 + return elementType != LogisticsElementType.门 && IsObstacleType(elementType); + }).ToList(); + + filterStopwatch.Stop(); + LogManager.Info($"元素过滤完成，耗时: {filterStopwatch.ElapsedMilliseconds} ms"); + LogManager.Info($"元素统计: 总数={totalItemsCount}, 过滤后={filteredItems.Count}, 排除的门={totalItemsCount - filteredItems.Count}"); + + // 第四步：提取障碍物几何体并转换为 DMesh3 LogManager.Info("开始提取障碍物几何体..."); var geometryStopwatch = Stopwatch.StartNew(); - DMesh3 obstacleMesh = NavisworksToDMesh3Converter.ConvertFromModelItems(obstacleItems); + DMesh3 obstacleMesh = NavisworksToDMesh3Converter.ConvertFromModelItems(filteredItems); geometryStopwatch.Stop(); LogManager.Info($"几何体提取完成，耗时: {geometryStopwatch.ElapsedMilliseconds} ms"); @@ -375,7 +189,8 @@ namespace NavisworksTransport.PathPlanning int passableCount = 0; int boundaryCount = 0; - double minPassableDistance = safetyMarginInModelUnits; + // SDF阶段不使用安全间隙，只标记几何体内部为障碍物 + // 安全间隙在后续的XY平面膨胀阶段应用 // 获取 SDF 网格的原点和单元尺寸 Vector3f sdfOrigin = sdf.GridOrigin; @@ -416,11 +231,10 @@ namespace NavisworksTransport.PathPlanning var cell = voxelGrid.GetCell(x, y, z); cell.Distance = distance; - // 判断可通行性 + // 判断可通行性 - SDF阶段只标记几何体内部为障碍物 // 距离 < 0: 在障碍物内部 → 不可通行 - // 距离 < minPassableDistance: 在安全间隙内 → 不可通行 - // 距离 >= minPassableDistance: 安全区域 → 可通行 - if (distance < minPassableDistance) + // 距离 >= 0: 在障碍物外部 → 可通行 + if (distance < 0) { cell.SetAsObstacle(); cell.Type = LogisticsElementType.障碍物; @@ -467,10 +281,9 @@ namespace NavisworksTransport.PathPlanning { LogManager.Error($"MeshSignedDistanceGrid 计算失败: {ex.Message}"); LogManager.Error($"堆栈跟踪: {ex.StackTrace}"); - LogManager.Warning("回退到简单包围盒方法"); - // 如果 SDF 失败，回退到包围盒方法 - return GenerateFromBIM(bounds, voxelSizeMeters, vehicleRadiusMeters, vehicleHeightMeters, obstacleItems); + // 不使用 fallback，直接抛出异常让问题暴露 + throw new InvalidOperationException("SDF 体素网格生成失败，无法继续", ex); } // 第七步：统计信息