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);
}
// 第七步：统计信息