423 lines
17 KiB
C#
423 lines
17 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 基于通道的网格构建器
|
||
/// 实现A*算法改进方案中的通道优先策略和2.5D垂直扫描
|
||
///
|
||
/// 术语说明：
|
||
/// - "通道"指所有可通行的建筑结构（楼板、走廊、过道等）
|
||
/// - 楼板：物体在其顶面行驶，底面用于结构支撑
|
||
/// - 走廊/过道：传统意义的通道空间
|
||
/// </summary>
|
||
public class ChannelBasedGridBuilder
|
||
{
|
||
private ChannelHeightDetector _heightDetector;
|
||
private readonly ICoordinateSystem _coordinateSystem;
|
||
|
||
/// <summary>
|
||
/// 通道几何类型
|
||
/// </summary>
|
||
private enum ChannelGeometryType
|
||
{
|
||
/// <summary>
|
||
/// 水平通道（楼板）
|
||
/// </summary>
|
||
Horizontal,
|
||
|
||
/// <summary>
|
||
/// 倾斜通道（楼梯、坡道）
|
||
/// </summary>
|
||
Inclined,
|
||
|
||
/// <summary>
|
||
/// 复杂形状
|
||
/// </summary>
|
||
Complex
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造函数（自动使用当前坐标系）
|
||
/// </summary>
|
||
public ChannelBasedGridBuilder() : this(CoordinateSystemManager.Instance.Current)
|
||
{
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="coordinateSystem">坐标系</param>
|
||
public ChannelBasedGridBuilder(ICoordinateSystem coordinateSystem)
|
||
{
|
||
_coordinateSystem = coordinateSystem ?? throw new ArgumentNullException(nameof(coordinateSystem));
|
||
_heightDetector = new ChannelHeightDetector(_coordinateSystem);
|
||
LogManager.Info($"[通道网格构建器] 初始化完成，坐标系: {_coordinateSystem.Type}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构建基于通道的精确覆盖网格
|
||
/// </summary>
|
||
/// <param name="gridSize">网格单元格大小</param>
|
||
/// <param name="document">当前文档</param>
|
||
/// <returns>通道覆盖信息</returns>
|
||
public ChannelCoverage BuildChannelCoverage(double gridSize)
|
||
{
|
||
LogManager.Info("[通道网格构建器] 开始构建通道覆盖网格");
|
||
|
||
// 1. 获取所有通行相关的物品（通道 + 楼梯 + 电梯）
|
||
var channelItems = new List<ModelItem>();
|
||
|
||
// 获取通道
|
||
var channels = CategoryAttributeManager.GetLogisticsItemsByType(
|
||
"通道");
|
||
channelItems.AddRange(channels);
|
||
|
||
// 获取楼梯
|
||
var stairs = CategoryAttributeManager.GetLogisticsItemsByType(
|
||
"楼梯");
|
||
channelItems.AddRange(stairs);
|
||
|
||
// 获取电梯
|
||
var elevators = CategoryAttributeManager.GetLogisticsItemsByType(
|
||
"电梯");
|
||
channelItems.AddRange(elevators);
|
||
|
||
if (!channelItems.Any())
|
||
{
|
||
LogManager.Warning("[通道网格构建器] 未找到任何通行物品（通道/楼梯/电梯）");
|
||
return CreateEmptyChannelCoverage();
|
||
}
|
||
|
||
LogManager.Info($"[通道网格构建器] 找到通行物品: 通道{channels.Count}个, 楼梯{stairs.Count}个, 电梯{elevators.Count}个, 总计{channelItems.Count}个");
|
||
|
||
// 2. 计算通道总边界
|
||
var totalBounds = BoundingBoxGeometryUtils.CalculateTotalBounds(channelItems);
|
||
LogManager.Info($"[通道网格构建器] 通道总边界: {FormatBounds(totalBounds)}");
|
||
|
||
// 3. 创建网格地图
|
||
// 必须继续传递当前构建器绑定的坐标系，避免同一轮流程里
|
||
// ChannelBasedGridBuilder、GridMap、ChannelHeightDetector 各自重新抓取 Current。
|
||
var gridMap = new GridMap(totalBounds, gridSize, _coordinateSystem);
|
||
|
||
// 4. 为每个通道生成精确投影
|
||
var processedChannels = new List<ModelItem>();
|
||
foreach (var channel in channelItems)
|
||
{
|
||
try
|
||
{
|
||
ProjectChannelToGrid(channel, gridMap);
|
||
processedChannels.Add(channel);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[通道网格构建器] 投影通道失败 '{channel.DisplayName}': {ex.Message}");
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[通道网格构建器] 成功投影 {processedChannels.Count}/{channelItems.Count} 个通道");
|
||
|
||
// 5. 计算可通行网格的Z值范围
|
||
var (walkableMinZ, walkableMaxZ, walkableCount) = CalculateWalkableGridZRange(gridMap);
|
||
|
||
return new ChannelCoverage
|
||
{
|
||
GridMap = gridMap,
|
||
ChannelItems = processedChannels,
|
||
TotalBounds = totalBounds,
|
||
WalkableMinZ = walkableMinZ,
|
||
WalkableMaxZ = walkableMaxZ,
|
||
WalkableGridCount = walkableCount
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算可通行网格的Z值范围
|
||
/// 遍历所有可通行网格，基于实际WorldPosition.Z坐标计算最小值和最大值
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>包含最小Z值、最大Z值和可通行网格数量的元组</returns>
|
||
private (double minZ, double maxZ, int walkableCount) CalculateWalkableGridZRange(GridMap gridMap)
|
||
{
|
||
double minZ = double.MaxValue;
|
||
double maxZ = double.MinValue;
|
||
int walkableCount = 0;
|
||
|
||
LogManager.Info("[通道Z值计算] 开始计算可通行网格Z值范围");
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
// 只计算可通行的网格
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
walkableCount++;
|
||
double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0
|
||
? cell.HeightLayers[0].Z
|
||
: 0;
|
||
|
||
if (cellZ < minZ)
|
||
minZ = cellZ;
|
||
if (cellZ > maxZ)
|
||
maxZ = cellZ;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 如果没有可通行网格，返回默认值
|
||
if (walkableCount == 0)
|
||
{
|
||
LogManager.Warning("[通道Z值计算] 未找到可通行网格，使用默认Z值范围");
|
||
minZ = 0.0;
|
||
maxZ = 0.0;
|
||
}
|
||
|
||
LogManager.Info($"[通道Z值计算] 计算完成 - 可通行网格: {walkableCount}个, Z范围: [{minZ:F2}, {maxZ:F2}], 高度差: {maxZ - minZ:F2}");
|
||
|
||
return (minZ, maxZ, walkableCount);
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 将通道投影到网格
|
||
/// 统一投影方式：基于三角形几何的精确投影，解决楼板洞口和不规则形状问题
|
||
/// </summary>
|
||
/// <param name="channel">通道物品</param>
|
||
/// <param name="gridMap">目标网格地图</param>
|
||
private void ProjectChannelToGrid(ModelItem channel, GridMap gridMap)
|
||
{
|
||
LogManager.Info($"[通道网格构建器] 投影通道: {channel.DisplayName}");
|
||
|
||
// 获取通道限速（一次性查询，避免重复COM API调用）
|
||
double channelSpeedLimit = CategoryAttributeManager.GetSpeedLimit(channel);
|
||
|
||
// 提取三角形几何
|
||
var triangles = GeometryHelper.ExtractTriangles(new[] { channel });
|
||
if (triangles.Count == 0)
|
||
{
|
||
LogManager.Warning($"[通道网格构建器] 未能从 '{channel.DisplayName}' 提取到三角形");
|
||
return;
|
||
}
|
||
|
||
// 统一投影处理
|
||
int processedTriangles = 0;
|
||
foreach (var triangle in triangles)
|
||
{
|
||
var normal = ComputeTriangleNormal(triangle);
|
||
|
||
// 根据法向量决定处理方式（使用坐标系判断向上）
|
||
if (GetUpComponent(normal) > 0) // 朝上的面
|
||
{
|
||
RasterizeTriangleToGrid(gridMap, triangle, channel, channelSpeedLimit);
|
||
processedTriangles++;
|
||
}
|
||
// 垂直面和朝下的面不投影
|
||
}
|
||
|
||
LogManager.Info($"[通道网格构建器] 投影完成：{processedTriangles}/{triangles.Count} 个三角形");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算三角形法向量
|
||
/// </summary>
|
||
/// <param name="triangle">三角形</param>
|
||
/// <returns>法向量</returns>
|
||
private Point3D ComputeTriangleNormal(Triangle3D triangle)
|
||
{
|
||
var edge1 = new Point3D(
|
||
triangle.Point2.X - triangle.Point1.X,
|
||
triangle.Point2.Y - triangle.Point1.Y,
|
||
triangle.Point2.Z - triangle.Point1.Z);
|
||
|
||
var edge2 = new Point3D(
|
||
triangle.Point3.X - triangle.Point1.X,
|
||
triangle.Point3.Y - triangle.Point1.Y,
|
||
triangle.Point3.Z - triangle.Point1.Z);
|
||
|
||
// 计算叉积
|
||
var normal = new Point3D(
|
||
edge1.Y * edge2.Z - edge1.Z * edge2.Y,
|
||
edge1.Z * edge2.X - edge1.X * edge2.Z,
|
||
edge1.X * edge2.Y - edge1.Y * edge2.X);
|
||
|
||
// 归一化
|
||
double length = Math.Sqrt(normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z);
|
||
if (length > 0.0001)
|
||
{
|
||
normal = new Point3D(normal.X / length, normal.Y / length, normal.Z / length);
|
||
}
|
||
|
||
return normal;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取向上轴的分量（根据坐标系）
|
||
/// </summary>
|
||
private double GetUpComponent(Point3D vector)
|
||
{
|
||
return _coordinateSystem.GetElevation(vector);
|
||
}
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 将三角形光栅化到网格
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="triangle">三角形</param>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <param name="channelSpeedLimit">预计算的通道限速</param>
|
||
private void RasterizeTriangleToGrid(GridMap gridMap, Triangle3D triangle, ModelItem channel, double channelSpeedLimit)
|
||
{
|
||
// 获取三角形的2D投影边界框（使用水平坐标）
|
||
var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1);
|
||
var (p2h1, p2h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point2);
|
||
var (p3h1, p3h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point3);
|
||
|
||
var minH1 = Math.Min(p1h1, Math.Min(p2h1, p3h1));
|
||
var maxH1 = Math.Max(p1h1, Math.Max(p2h1, p3h1));
|
||
var minH2 = Math.Min(p1h2, Math.Min(p2h2, p3h2));
|
||
var maxH2 = Math.Max(p1h2, Math.Max(p2h2, p3h2));
|
||
|
||
var minGrid = gridMap.WorldToGrid(_coordinateSystem.CreatePoint(minH1, minH2, 0));
|
||
var maxGrid = gridMap.WorldToGrid(_coordinateSystem.CreatePoint(maxH1, maxH2, 0));
|
||
|
||
// 遍历边界框内的所有网格点
|
||
for (int x = minGrid.X; x <= maxGrid.X; x++)
|
||
{
|
||
for (int y = minGrid.Y; y <= maxGrid.Y; y++)
|
||
{
|
||
var gridPos = new GridPoint2D(x, y);
|
||
if (gridMap.IsValidGridPosition(gridPos))
|
||
{
|
||
var worldPos = gridMap.GridToWorld3D(gridPos);
|
||
|
||
// 检查点是否在三角形内部（使用2D水平投影）
|
||
if (IsPointInTriangleHorizontal(worldPos, triangle))
|
||
{
|
||
// 计算网格中心点在三角形平面上的精确高度
|
||
double gridCenterElevation = GetElevationAtPoint(triangle, worldPos);
|
||
|
||
// 创建高度层（追加模式，不覆盖）
|
||
var channelType = CategoryAttributeManager.GetCategoryId(channel);
|
||
var layer = new HeightLayer(
|
||
z: gridCenterElevation,
|
||
passableHeight: new HeightInterval(0, 0), // 初始化，后续由SetChannelPassableHeights设置
|
||
sourceItem: channel,
|
||
speedLimit: channelSpeedLimit,
|
||
type: channelType
|
||
);
|
||
|
||
// 追加高度层（支持同一网格多个高度）
|
||
gridMap.AddHeightLayer(gridPos, layer);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查点是否在三角形的水平投影内
|
||
/// </summary>
|
||
private bool IsPointInTriangleHorizontal(Point3D point, Triangle3D triangle)
|
||
{
|
||
var (ph1, ph2) = _coordinateSystem.GetHorizontalCoords(point);
|
||
var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1);
|
||
var (p2h1, p2h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point2);
|
||
var (p3h1, p3h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point3);
|
||
|
||
// 使用叉积法判断点是否在三角形内
|
||
double Cross(double ax, double ay, double bx, double by) => ax * by - ay * bx;
|
||
|
||
var d1 = Cross(ph1 - p1h1, ph2 - p1h2, p2h1 - p1h1, p2h2 - p1h2);
|
||
var d2 = Cross(ph1 - p2h1, ph2 - p2h2, p3h1 - p2h1, p3h2 - p2h2);
|
||
var d3 = Cross(ph1 - p3h1, ph2 - p3h2, p1h1 - p3h1, p1h2 - p3h2);
|
||
|
||
bool hasNeg = (d1 < 0) || (d2 < 0) || (d3 < 0);
|
||
bool hasPos = (d1 > 0) || (d2 > 0) || (d3 > 0);
|
||
|
||
return !(hasNeg && hasPos);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算点在三角形平面上的高度值（根据当前坐标系）
|
||
/// </summary>
|
||
private double GetElevationAtPoint(Triangle3D triangle, Point3D point)
|
||
{
|
||
var normal = ComputeTriangleNormal(triangle);
|
||
|
||
// 获取向上轴的分量
|
||
double upComponent = GetUpComponent(normal);
|
||
|
||
// 如果向上分量接近0，说明是垂直面，返回第一个点的Z值
|
||
if (Math.Abs(upComponent) < 0.0001)
|
||
{
|
||
return _coordinateSystem.GetElevation(triangle.Point1);
|
||
}
|
||
|
||
// 获取各点的水平坐标
|
||
var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1);
|
||
var (ph1, ph2) = _coordinateSystem.GetHorizontalCoords(point);
|
||
|
||
// 获取向上轴的索引来正确计算
|
||
int upAxis = _coordinateSystem.UpAxisIndex;
|
||
|
||
// 使用平面方程计算点的高度
|
||
// n_x(x-x0) + n_y(y-y0) + n_z(z-z0) = 0
|
||
// 解出高度值
|
||
double elevation;
|
||
if (upAxis == 2) // Z-up
|
||
{
|
||
elevation = triangle.Point1.Z - (normal.X * (ph1 - p1h1) + normal.Y * (ph2 - p1h2)) / normal.Z;
|
||
}
|
||
else // Y-up
|
||
{
|
||
elevation = triangle.Point1.Y - (normal.X * (ph1 - p1h1) + normal.Z * (ph2 - p1h2)) / normal.Y;
|
||
}
|
||
|
||
return elevation;
|
||
}
|
||
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 创建空的通道覆盖
|
||
/// </summary>
|
||
/// <returns>空的通道覆盖</returns>
|
||
private ChannelCoverage CreateEmptyChannelCoverage()
|
||
{
|
||
var emptyBounds = new BoundingBox3D(new Point3D(0, 0, 0), new Point3D(0, 0, 0));
|
||
return new ChannelCoverage
|
||
{
|
||
GridMap = new GridMap(emptyBounds, 1.0, _coordinateSystem),
|
||
ChannelItems = new List<ModelItem>(),
|
||
TotalBounds = emptyBounds
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 格式化边界框信息用于日志
|
||
/// </summary>
|
||
/// <param name="bounds">边界框</param>
|
||
/// <returns>格式化字符串</returns>
|
||
private string FormatBounds(BoundingBox3D bounds)
|
||
{
|
||
var (min1, max1, min2, max2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
var (minElev, maxElev) = _coordinateSystem.GetHeightRange(bounds);
|
||
return $"[H1:{min1:F2}-{max1:F2}, H2:{min2:F2}-{max2:F2}, Elev:{minElev:F2}-{maxElev:F2}]";
|
||
}
|
||
}
|
||
|
||
|
||
}
|