379 lines
17 KiB
C#
379 lines
17 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils;
|
||
using NavisworksTransport.Core.Config;
|
||
using static NavisworksTransport.CategoryAttributeManager;
|
||
using g4;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 体素网格生成器 - 从 Navisworks BIM 模型生成 3D 体素网格
|
||
/// 使用 SDF (Signed Distance Field) 方法进行精确体素化
|
||
/// </summary>
|
||
public class VoxelGridGenerator
|
||
{
|
||
|
||
/// <summary>
|
||
/// 获取模型元素的物流类型
|
||
/// </summary>
|
||
private LogisticsElementType GetLogisticsElementType(ModelItem item)
|
||
{
|
||
// 尝试从自定义属性读取
|
||
try
|
||
{
|
||
var categories = item.PropertyCategories;
|
||
foreach (var category in categories)
|
||
{
|
||
if (category.DisplayName == LogisticsCategories.LOGISTICS ||
|
||
category.Name == LogisticsCategories.CATEGORY_INTERNAL_NAME)
|
||
{
|
||
foreach (var prop in category.Properties)
|
||
{
|
||
if (prop.DisplayName == LogisticsProperties.TYPE)
|
||
{
|
||
string typeStr = prop.Value.ToDisplayString();
|
||
if (Enum.TryParse<LogisticsElementType>(typeStr, out var parsedType))
|
||
{
|
||
return parsedType;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"读取物流属性失败: {ex.Message}");
|
||
}
|
||
|
||
// 默认：根据元素名称推断类型
|
||
string displayName = item.DisplayName?.ToLower() ?? "";
|
||
|
||
if (displayName.Contains("门") || displayName.Contains("door"))
|
||
return LogisticsElementType.门;
|
||
else if (displayName.Contains("楼梯") || displayName.Contains("stair"))
|
||
return LogisticsElementType.楼梯;
|
||
else if (displayName.Contains("电梯") || displayName.Contains("elevator"))
|
||
return LogisticsElementType.电梯;
|
||
else if (displayName.Contains("通道") || displayName.Contains("corridor"))
|
||
return LogisticsElementType.通道;
|
||
else
|
||
return LogisticsElementType.障碍物; // 默认为障碍物
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断物流类型是否为障碍物
|
||
/// </summary>
|
||
private bool IsObstacleType(LogisticsElementType type)
|
||
{
|
||
switch (type)
|
||
{
|
||
case LogisticsElementType.障碍物:
|
||
return true;
|
||
|
||
case LogisticsElementType.门:
|
||
case LogisticsElementType.通道:
|
||
case LogisticsElementType.楼梯:
|
||
case LogisticsElementType.电梯:
|
||
case LogisticsElementType.装卸区:
|
||
case LogisticsElementType.停车位:
|
||
return false;
|
||
|
||
default:
|
||
return true; // 未知类型默认为障碍物
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用 MeshSignedDistanceGrid 从 BIM 模型生成体素网格（精确几何体方法）
|
||
/// 阶段 1.4 版本：使用 geometry4Sharp 的签名距离场进行精确体素化
|
||
/// </summary>
|
||
/// <param name="bounds">网格边界（世界坐标，模型单位）</param>
|
||
/// <param name="voxelSizeMeters">体素尺寸（米）</param>
|
||
/// <param name="objectRadiusMeters">物体半径（米），用于安全间隙</param>
|
||
/// <param name="objectHeightMeters">物体高度（米）</param>
|
||
/// <param name="obstacleItems">障碍物模型元素列表</param>
|
||
/// <returns>生成的体素网格</returns>
|
||
public VoxelGrid GenerateFromBIMWithSDF(
|
||
BoundingBox3D bounds,
|
||
double voxelSizeMeters,
|
||
double objectRadiusMeters,
|
||
double objectHeightMeters,
|
||
IEnumerable<ModelItem> obstacleItems)
|
||
{
|
||
var stopwatch = Stopwatch.StartNew();
|
||
LogManager.Info("=== 开始体素网格生成（MeshSignedDistanceGrid 方法） ===");
|
||
|
||
// 第一步：单位转换（米 → 模型单位）
|
||
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(
|
||
Autodesk.Navisworks.Api.Application.ActiveDocument.Units);
|
||
|
||
double voxelSizeInModelUnits = voxelSizeMeters * metersToModelUnits;
|
||
double objectRadiusInModelUnits = objectRadiusMeters * metersToModelUnits;
|
||
double objectHeightInModelUnits = objectHeightMeters * metersToModelUnits;
|
||
double safetyMarginInModelUnits = objectRadiusInModelUnits; // 安全间隙 = 物体半径
|
||
|
||
LogManager.Info($"单位转换系数: {metersToModelUnits:F4}");
|
||
LogManager.Info($"体素尺寸: {voxelSizeMeters}米 = {voxelSizeInModelUnits:F2}模型单位");
|
||
LogManager.Info($"安全间隙: {objectRadiusMeters}米 = {safetyMarginInModelUnits:F2}模型单位");
|
||
|
||
// 第二步：创建体素网格
|
||
var voxelGrid = new VoxelGrid(bounds, voxelSizeInModelUnits);
|
||
LogManager.Info($"创建体素网格: {voxelGrid.SizeX} × {voxelGrid.SizeY} × {voxelGrid.SizeZ} = {voxelGrid.TotalVoxels:N0} 个体素");
|
||
|
||
// 第三步：过滤模型元素 - 排除门，只保留真正的障碍物
|
||
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 = null;
|
||
var cacheManager = new GeometryCacheManager(Application.ActiveDocument.FileName);
|
||
try
|
||
{
|
||
LogManager.Info($"[体素网格] 从缓存中提取 {filteredItems.Count} 个过滤后项的几何体");
|
||
var allTriangles = new List<Triangle3D>();
|
||
|
||
int processedItems = 0;
|
||
foreach (var item in filteredItems)
|
||
{
|
||
var itemTriangles = cacheManager.GetTrianglesForModelItem(item);
|
||
allTriangles.AddRange(itemTriangles);
|
||
processedItems++;
|
||
}
|
||
|
||
LogManager.Info($"[体素网格] 从缓存中提取完成，总三角形数: {allTriangles.Count}");
|
||
obstacleMesh = NavisworksToDMesh3Converter.Convert(allTriangles);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[体素网格] 使用几何体缓存失败: {ex.Message}，回退到传统方法");
|
||
|
||
// 如果缓存不可用，则使用传统方法提取几何体
|
||
obstacleMesh = NavisworksToDMesh3Converter.ConvertFromModelItems(filteredItems);
|
||
}
|
||
|
||
geometryStopwatch.Stop();
|
||
LogManager.Info($"几何体提取完成，耗时: {geometryStopwatch.ElapsedMilliseconds} ms");
|
||
|
||
if (obstacleMesh.TriangleCount == 0)
|
||
{
|
||
LogManager.Warning("警告：没有提取到任何障碍物几何体，所有体素将标记为可通行");
|
||
return voxelGrid;
|
||
}
|
||
|
||
// 第四步：使用 MeshSignedDistanceGrid 计算签名距离场
|
||
LogManager.Info("开始计算签名距离场（Signed Distance Field）...");
|
||
var sdfStopwatch = Stopwatch.StartNew();
|
||
|
||
try
|
||
{
|
||
// 计算需要的网格尺寸（基于包围盒和体素大小）
|
||
var meshBounds = obstacleMesh.GetBounds();
|
||
int numCellsX = (int)Math.Ceiling(meshBounds.Width / voxelSizeInModelUnits);
|
||
int numCellsY = (int)Math.Ceiling(meshBounds.Height / voxelSizeInModelUnits);
|
||
int numCellsZ = (int)Math.Ceiling(meshBounds.Depth / voxelSizeInModelUnits);
|
||
|
||
LogManager.Info($"SDF 网格尺寸: {numCellsX} × {numCellsY} × {numCellsZ}");
|
||
|
||
// 创建签名距离场
|
||
MeshSignedDistanceGrid sdf = new MeshSignedDistanceGrid(
|
||
obstacleMesh,
|
||
voxelSizeInModelUnits);
|
||
|
||
// 计算距离场（这可能耗时较长）
|
||
sdf.Compute();
|
||
|
||
sdfStopwatch.Stop();
|
||
LogManager.Info($"距离场计算完成，耗时: {sdfStopwatch.ElapsedMilliseconds} ms ({sdfStopwatch.Elapsed.TotalSeconds:F2} 秒)");
|
||
|
||
// 第五步：使用距离场标记体素
|
||
LogManager.Info("开始根据距离场标记体素...");
|
||
var markingStopwatch = Stopwatch.StartNew();
|
||
|
||
int obstacleCount = 0;
|
||
int passableCount = 0;
|
||
int boundaryCount = 0;
|
||
|
||
// SDF阶段不使用安全间隙，只标记几何体内部为障碍物
|
||
// 安全间隙在后续的XY平面膨胀阶段应用
|
||
|
||
// 获取 SDF 网格的原点和单元尺寸
|
||
Vector3f sdfOrigin = sdf.GridOrigin;
|
||
float sdfCellSize = sdf.CellSize;
|
||
var sdfDimensions = sdf.Dimensions;
|
||
|
||
LogManager.Info($"SDF 网格信息: 原点=({sdfOrigin.x:F2}, {sdfOrigin.y:F2}, {sdfOrigin.z:F2}), " +
|
||
$"单元尺寸={sdfCellSize:F4}, 维度={sdfDimensions.x}×{sdfDimensions.y}×{sdfDimensions.z}");
|
||
|
||
for (int x = 0; x < voxelGrid.SizeX; x++)
|
||
{
|
||
for (int y = 0; y < voxelGrid.SizeY; y++)
|
||
{
|
||
for (int z = 0; z < voxelGrid.SizeZ; z++)
|
||
{
|
||
// 体素中心的世界坐标
|
||
Point3D worldPos = voxelGrid.VoxelToWorldCenter(x, y, z);
|
||
|
||
// 将世界坐标转换为 SDF 网格索引
|
||
int sdfX = (int)Math.Floor((worldPos.X - sdfOrigin.x) / sdfCellSize);
|
||
int sdfY = (int)Math.Floor((worldPos.Y - sdfOrigin.y) / sdfCellSize);
|
||
int sdfZ = (int)Math.Floor((worldPos.Z - sdfOrigin.z) / sdfCellSize);
|
||
|
||
// 查询距离场（使用整数索引）
|
||
double distance = 0.0;
|
||
if (sdfX >= 0 && sdfX < sdfDimensions.x &&
|
||
sdfY >= 0 && sdfY < sdfDimensions.y &&
|
||
sdfZ >= 0 && sdfZ < sdfDimensions.z)
|
||
{
|
||
distance = sdf[sdfX, sdfY, sdfZ];
|
||
}
|
||
else
|
||
{
|
||
// 超出 SDF 范围，认为是远离障碍物
|
||
distance = double.MaxValue;
|
||
}
|
||
|
||
var cell = voxelGrid.GetCell(x, y, z);
|
||
cell.Distance = distance;
|
||
|
||
// 判断可通行性 - SDF阶段只标记几何体内部为障碍物
|
||
// 距离 < 0: 在障碍物内部 → 不可通行
|
||
// 距离 >= 0: 在障碍物外部 → 可通行
|
||
if (distance < 0)
|
||
{
|
||
cell.SetAsObstacle();
|
||
cell.Type = LogisticsElementType.障碍物;
|
||
obstacleCount++;
|
||
|
||
// 统计边界体素（距离接近0的体素）
|
||
if (Math.Abs(distance) < voxelSizeInModelUnits * 0.5)
|
||
{
|
||
boundaryCount++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
cell.IsPassable = true;
|
||
cell.Type = LogisticsElementType.通道;
|
||
passableCount++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
markingStopwatch.Stop();
|
||
LogManager.Info($"体素标记完成，耗时: {markingStopwatch.ElapsedMilliseconds} ms");
|
||
LogManager.Info($"标记统计: 障碍物={obstacleCount:N0}, 可通行={passableCount:N0}, 边界={boundaryCount:N0}");
|
||
|
||
// 第六步：障碍物膨胀（可选）
|
||
if (objectRadiusMeters > 0)
|
||
{
|
||
LogManager.Info("开始障碍物膨胀...");
|
||
var inflationStopwatch = Stopwatch.StartNew();
|
||
|
||
int inflatedCount = voxelGrid.InflateObstacles(objectRadiusMeters, metersToModelUnits);
|
||
|
||
inflationStopwatch.Stop();
|
||
LogManager.Info($"障碍物膨胀完成，耗时: {inflationStopwatch.ElapsedMilliseconds} ms");
|
||
LogManager.Info($"膨胀统计: 新增障碍物体素={inflatedCount:N0}");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("跳过障碍物膨胀（物体半径为0）");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"MeshSignedDistanceGrid 计算失败: {ex.Message}");
|
||
LogManager.Error($"堆栈跟踪: {ex.StackTrace}");
|
||
|
||
// 不使用 fallback，直接抛出异常让问题暴露
|
||
throw new InvalidOperationException("SDF 体素网格生成失败，无法继续", ex);
|
||
}
|
||
|
||
// 第七步：统计信息
|
||
var (total, passable, obstacle) = voxelGrid.GetStatistics();
|
||
double passableRatio = (double)passable / total * 100.0;
|
||
|
||
stopwatch.Stop();
|
||
LogManager.Info($"=== 体素网格生成完成（SDF 方法） ===");
|
||
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>
|
||
/// <returns>测试用体素网格</returns>
|
||
public static VoxelGrid CreateTestGrid()
|
||
{
|
||
// 创建 10m × 10m × 3m 的测试房间
|
||
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(
|
||
Autodesk.Navisworks.Api.Application.ActiveDocument.Units);
|
||
|
||
var bounds = new BoundingBox3D(
|
||
new Point3D(0, 0, 0),
|
||
new Point3D(10 * metersToModelUnits, 10 * metersToModelUnits, 3 * metersToModelUnits)
|
||
);
|
||
|
||
double voxelSize = ConfigManager.Instance.Current.PathEditing.CellSizeMeters * metersToModelUnits; // 使用配置的网格大小作为体素大小
|
||
|
||
var grid = new VoxelGrid(bounds, voxelSize);
|
||
|
||
LogManager.Info($"创建测试网格: {grid}");
|
||
|
||
// 添加一个简单的障碍物（中心位置的柱子）
|
||
int centerX = grid.SizeX / 2;
|
||
int centerY = grid.SizeY / 2;
|
||
|
||
for (int z = 0; z < grid.SizeZ; z++)
|
||
{
|
||
for (int dx = -1; dx <= 1; dx++)
|
||
{
|
||
for (int dy = -1; dy <= 1; dy++)
|
||
{
|
||
int x = centerX + dx;
|
||
int y = centerY + dy;
|
||
|
||
if (grid.IsValidIndex(x, y, z))
|
||
{
|
||
var cell = grid.GetCell(x, y, z);
|
||
cell.SetAsObstacle();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
var (total, passable, obstacle) = grid.GetStatistics();
|
||
LogManager.Info($"测试网格统计: 总={total}, 可通行={passable}, 障碍={obstacle}");
|
||
|
||
return grid;
|
||
}
|
||
}
|
||
}
|