修复SDF标记阶段在Z方向应用安全间隙的问题
问题： - 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 <noreply@anthropic.com>
This commit is contained in:
parent
1e11f60042
commit
2f78b4e58e
@ -11,213 +11,10 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
/// <summary>
|
||||
/// 体素网格生成器 - 从 Navisworks BIM 模型生成 3D 体素网格
|
||||
/// 阶段 1 原型版本：使用简单的包围盒方法进行体素化
|
||||
/// 使用 SDF (Signed Distance Field) 方法进行精确体素化
|
||||
/// </summary>
|
||||
public class VoxelGridGenerator
|
||||
{
|
||||
/// <summary>
|
||||
/// 从 BIM 模型生成体素网格（简化版 - 使用包围盒）
|
||||
/// </summary>
|
||||
/// <param name="bounds">网格边界（世界坐标，模型单位）</param>
|
||||
/// <param name="voxelSizeMeters">体素尺寸（米）</param>
|
||||
/// <param name="vehicleRadiusMeters">车辆半径（米），用于膨胀障碍物</param>
|
||||
/// <param name="vehicleHeightMeters">车辆高度（米），用于检测通行空间</param>
|
||||
/// <param name="modelItems">BIM 模型元素列表</param>
|
||||
/// <returns>生成的体素网格</returns>
|
||||
public VoxelGrid GenerateFromBIM(
|
||||
BoundingBox3D bounds,
|
||||
double voxelSizeMeters,
|
||||
double vehicleRadiusMeters,
|
||||
double vehicleHeightMeters,
|
||||
IEnumerable<ModelItem> 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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用包围盒标记体素
|
||||
/// </summary>
|
||||
/// <param name="voxelGrid">体素网格</param>
|
||||
/// <param name="bbox">包围盒（模型单位）</param>
|
||||
/// <param name="isObstacle">是否为障碍物</param>
|
||||
/// <param name="elementType">元素类型</param>
|
||||
/// <param name="sourceItem">源模型元素</param>
|
||||
/// <param name="inflationRadius">膨胀半径（模型单位），用于障碍物</param>
|
||||
/// <returns>标记的体素数量</returns>
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查点是否在包围盒内
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取模型元素的物流类型
|
||||
@ -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);
|
||||
}
|
||||
|
||||
// 第七步：统计信息
|
||||
|
||||
Loading…
Reference in New Issue
Block a user