1033 lines
38 KiB
C#
1033 lines
38 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils;
|
||
using NavisworksTransport;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 网格地图数据结构
|
||
/// 用于将BIM模型转换为路径规划算法可使用的2D网格表示
|
||
/// </summary>
|
||
public class GridMap
|
||
{
|
||
/// <summary>
|
||
/// 网格宽度（单元格数量）
|
||
/// </summary>
|
||
public int Width { get; set; }
|
||
|
||
/// <summary>
|
||
/// 网格高度（单元格数量）
|
||
/// </summary>
|
||
public int Height { get; set; }
|
||
|
||
/// <summary>
|
||
/// 每个网格单元格的实际尺寸（模型单位）
|
||
/// </summary>
|
||
public double CellSize { get; set; }
|
||
|
||
/// <summary>
|
||
/// 网格原点（世界坐标系）
|
||
/// </summary>
|
||
public Point3D Origin { get; set; }
|
||
|
||
/// <summary>
|
||
/// 网格边界框（世界坐标系）
|
||
/// </summary>
|
||
public BoundingBox3D Bounds { get; set; }
|
||
|
||
/// <summary>
|
||
/// 路径规划的起点（用于Z坐标插值）
|
||
/// </summary>
|
||
public Point3D PlanningStartPoint { get; set; }
|
||
|
||
/// <summary>
|
||
/// 路径规划的终点（用于Z坐标插值）
|
||
/// </summary>
|
||
public Point3D PlanningEndPoint { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否已设置规划起点和终点
|
||
/// </summary>
|
||
public bool HasPlanningPoints { get; set; }
|
||
|
||
/// <summary>
|
||
/// 通道模型项集合（用于高度检测）
|
||
/// 包含所有可通行的建筑结构：楼板、走廊、过道等
|
||
/// 注意：楼板类通道的高度计算应使用顶面（车辆行驶面）
|
||
/// </summary>
|
||
public IEnumerable<ModelItem> ChannelItems { get; set; }
|
||
|
||
/// <summary>
|
||
/// 通道高度检测器
|
||
/// </summary>
|
||
public ChannelHeightDetector HeightDetector { get; set; }
|
||
|
||
/// <summary>
|
||
/// 坡度分析器
|
||
/// </summary>
|
||
public SlopeAnalyzer SlopeAnalyzer { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否启用精确高度计算（网格构建时禁用以提升性能）
|
||
/// </summary>
|
||
public bool EnablePreciseHeightCalculation { get; set; } = false;
|
||
|
||
/// <summary>
|
||
/// 网格单元格数组 [x, y]
|
||
/// </summary>
|
||
public GridCell[,] Cells { get; set; }
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="bounds">世界坐标边界框</param>
|
||
/// <param name="cellSize">网格单元格大小</param>
|
||
public GridMap(BoundingBox3D bounds, double cellSize)
|
||
{
|
||
if (cellSize <= 0)
|
||
throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize));
|
||
|
||
Bounds = bounds ?? throw new ArgumentNullException(nameof(bounds));
|
||
CellSize = cellSize;
|
||
|
||
// 计算网格尺寸
|
||
double worldWidth = bounds.Max.X - bounds.Min.X;
|
||
double worldHeight = bounds.Max.Y - bounds.Min.Y;
|
||
|
||
Width = (int)Math.Ceiling(worldWidth / cellSize);
|
||
Height = (int)Math.Ceiling(worldHeight / cellSize);
|
||
|
||
// 设置原点为边界框的最小点
|
||
Origin = bounds.Min;
|
||
|
||
// 初始化网格单元格
|
||
Cells = new GridCell[Width, Height];
|
||
InitializeCells();
|
||
|
||
// 初始化高度计算组件
|
||
InitializeHeightCalculation();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 初始化高度计算组件
|
||
/// </summary>
|
||
private void InitializeHeightCalculation()
|
||
{
|
||
try
|
||
{
|
||
HeightDetector = new ChannelHeightDetector();
|
||
SlopeAnalyzer = new SlopeAnalyzer();
|
||
LogManager.Info("[网格地图] 高度计算组件初始化完成");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[网格地图] 初始化高度计算组件失败: {ex.Message}");
|
||
EnablePreciseHeightCalculation = false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 初始化所有网格单元格为默认状态
|
||
/// </summary>
|
||
private void InitializeCells()
|
||
{
|
||
for (int x = 0; x < Width; x++)
|
||
{
|
||
for (int y = 0; y < Height; y++)
|
||
{
|
||
var cell = new GridCell
|
||
{
|
||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
|
||
IsInChannel = false,
|
||
HeightLayers = new List<HeightLayer>(),
|
||
ChannelType = ChannelType.Other
|
||
};
|
||
cell.Cost = cell.GetCost(); // 使用GetCost方法计算成本
|
||
Cells[x, y] = cell;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 世界坐标转换为网格坐标
|
||
/// </summary>
|
||
/// <param name="worldPosition">世界坐标点</param>
|
||
/// <returns>网格坐标（可能超出边界）</returns>
|
||
public GridPoint2D WorldToGrid(Point3D worldPosition)
|
||
{
|
||
double gridX = (worldPosition.X - Origin.X) / CellSize;
|
||
double gridY = (worldPosition.Y - Origin.Y) / CellSize;
|
||
|
||
return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算）
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>世界2D坐标点（网格左下角）</returns>
|
||
public Point3D GridToWorld2D(GridPoint2D gridPosition)
|
||
{
|
||
double worldX = Origin.X + gridPosition.X * CellSize;
|
||
double worldY = Origin.Y + gridPosition.Y * CellSize;
|
||
|
||
return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标）
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>世界3D坐标点</returns>
|
||
public Point3D GridToWorld3D(GridPoint2D gridPosition)
|
||
{
|
||
var world2D = GridToWorld2D(gridPosition);
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
|
||
// 使用最低层的Z坐标（HeightLayers[0]），如果没有层则用0
|
||
double z = 0;
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
z = cell.HeightLayers[0].Z;
|
||
}
|
||
|
||
return new Point3D(world2D.X, world2D.Y, z);
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 检查网格坐标是否在有效范围内
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>是否有效</returns>
|
||
public bool IsValidGridPosition(GridPoint2D gridPosition)
|
||
{
|
||
return gridPosition.X >= 0 && gridPosition.X < Width &&
|
||
gridPosition.Y >= 0 && gridPosition.Y < Height;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定网格位置是否可通行
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>是否可通行</returns>
|
||
public bool IsWalkable(GridPoint2D gridPosition)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return false;
|
||
|
||
return Cells[gridPosition.X, gridPosition.Y].HasAnyWalkableLayer();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置网格单元格的属性
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="isWalkable">是否可通行</param>
|
||
/// <param name="cost">遍历成本</param>
|
||
/// <param name="speedLimit">限速</param>
|
||
/// <param name="cellType">单元格类型</param>
|
||
public void SetCell(GridPoint2D gridPosition, bool isWalkable, double cost, double speedLimit, CategoryAttributeManager.LogisticsElementType cellType)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
var worldPos = GridToWorld3D(gridPosition);
|
||
var cell = new GridCell
|
||
{
|
||
CellType = cellType,
|
||
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
|
||
HeightLayers = new List<HeightLayer>(),
|
||
ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
|
||
SpeedLimit = speedLimit
|
||
};
|
||
|
||
// 应用验证规则并设置成本
|
||
ApplyCellValidationRules(ref cell, cost);
|
||
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将完整配置的GridCell放置到指定位置
|
||
/// 这是推荐的GridCell设置方式，避免了分步设置的问题
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格位置</param>
|
||
/// <param name="cell">完整配置的GridCell</param>
|
||
public void PlaceCell(GridPoint2D gridPosition, GridCell cell)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
// 应用验证规则（确保Unknown和障碍物类型的一致性）
|
||
ApplyCellValidationRules(ref cell, cell.Cost);
|
||
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为指定网格坐标添加高度层（追加模式，不覆盖）
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="layer">要添加的高度层</param>
|
||
public void AddHeightLayer(GridPoint2D gridPosition, HeightLayer layer)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
|
||
// 确保HeightLayers列表已初始化
|
||
if (cell.HeightLayers == null)
|
||
{
|
||
cell.HeightLayers = new List<HeightLayer>();
|
||
}
|
||
|
||
// 添加新的高度层
|
||
cell.HeightLayers.Add(layer);
|
||
|
||
// 更新单元格的基本属性（使用第一个层的属性）
|
||
if (cell.HeightLayers.Count == 1)
|
||
{
|
||
// IsWalkable由层决定，不再在网格级别设置
|
||
cell.CellType = layer.Type;
|
||
cell.IsInChannel = true;
|
||
cell.RelatedModelItem = layer.SourceItem;
|
||
cell.SpeedLimit = layer.SpeedLimit;
|
||
cell.Cost = cell.GetCost();
|
||
}
|
||
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 查找包含指定Z坐标的高度层
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="z">Z坐标（米）</param>
|
||
/// <param name="tolerance">容差（米），默认0.1m</param>
|
||
/// <returns>符合条件的高度层，如果没有则返回null</returns>
|
||
public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = 0.1)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return null;
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
|
||
return null;
|
||
|
||
// 查找Z坐标最接近的层
|
||
HeightLayer? bestLayer = null;
|
||
double minDistance = double.MaxValue;
|
||
|
||
foreach (var layer in cell.HeightLayers)
|
||
{
|
||
double distance = Math.Abs(layer.Z - z);
|
||
if (distance < minDistance && distance <= tolerance)
|
||
{
|
||
minDistance = distance;
|
||
bestLayer = layer;
|
||
}
|
||
}
|
||
|
||
return bestLayer;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取指定网格位置的所有高度层
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>高度层列表，如果没有则返回空列表</returns>
|
||
public List<HeightLayer> GetHeightLayers(GridPoint2D gridPosition)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return new List<HeightLayer>();
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
return cell.HeightLayers ?? new List<HeightLayer>();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 应用GridCell验证规则
|
||
/// 确保Unknown和障碍物类型永远不可通行，并设置合适的成本
|
||
/// </summary>
|
||
/// <param name="cell">要验证的GridCell（通过引用传递以便修改）</param>
|
||
/// <param name="originalCost">原始成本值</param>
|
||
private void ApplyCellValidationRules(ref GridCell cell, double originalCost)
|
||
{
|
||
// 🔥 关键验证：确保Unknown和障碍物类型永远不可通行
|
||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown ||
|
||
cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物)
|
||
{
|
||
// Unknown和障碍物没有可通行层，HasAnyWalkableLayer()自动返回false
|
||
cell.Cost = double.MaxValue;
|
||
}
|
||
else
|
||
{
|
||
// 优先使用传入的cost参数，如果为默认值则用GetCost
|
||
cell.Cost = (originalCost == double.MaxValue || originalCost == 0) ? cell.GetCost() : originalCost;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定位置在指定高度是否可通行
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="height">检查的高度</param>
|
||
/// <returns>是否可通行</returns>
|
||
public bool IsPassableAtHeight(GridPoint2D gridPosition, double height)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return false;
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
if (!cell.HasAnyWalkableLayer())
|
||
return false;
|
||
|
||
// 检查是否有任何高度层满足高度要求
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
foreach (var layer in cell.HeightLayers)
|
||
{
|
||
if (layer.PassableHeight.GetSpan() >= height)
|
||
return true;
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定3D点（包含Z坐标）是否在可通行的高度层范围内
|
||
/// 用于路径优化中的斜线连接检查，确保路径不穿过障碍物层
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="zCoord">Z坐标（模型单位）</param>
|
||
/// <param name="tolerance">Z坐标容差（模型单位），默认0.1</param>
|
||
/// <returns>是否可通行</returns>
|
||
public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = 0.1)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return false;
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
if (!cell.HasAnyWalkableLayer())
|
||
return false;
|
||
|
||
// 检查是否有任何可通行层包含这个Z坐标
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
foreach (var layer in cell.HeightLayers)
|
||
{
|
||
// 跳过不可通行的层
|
||
if (!layer.IsWalkable)
|
||
continue;
|
||
|
||
// 检查Z坐标是否在这一层的范围内
|
||
double layerMinZ = layer.Z;
|
||
double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();
|
||
|
||
if (zCoord >= layerMinZ - tolerance && zCoord <= layerMaxZ + tolerance)
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取所有通道类型的网格单元
|
||
/// </summary>
|
||
/// <returns>通道网格单元列表</returns>
|
||
public List<(GridPoint2D Position, GridCell Cell)> GetChannelCells()
|
||
{
|
||
var channelCells = new List<(GridPoint2D Position, GridCell Cell)>();
|
||
|
||
for (int x = 0; x < Width; x++)
|
||
{
|
||
for (int y = 0; y < Height; y++)
|
||
{
|
||
var cell = Cells[x, y];
|
||
if (cell.IsInChannel || cell.CellType == CategoryAttributeManager.LogisticsElementType.通道)
|
||
{
|
||
channelCells.Add((new GridPoint2D(x, y), cell));
|
||
}
|
||
}
|
||
}
|
||
|
||
return channelCells;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置通道模型项集合（用于精确高度计算）
|
||
/// </summary>
|
||
/// <param name="channelItems">通道模型项集合</param>
|
||
public void SetChannelItems(IEnumerable<ModelItem> channelItems)
|
||
{
|
||
try
|
||
{
|
||
ChannelItems = channelItems?.ToList() ?? new List<ModelItem>();
|
||
LogManager.Info($"[网格地图] 设置通道模型项: {(ChannelItems?.Count() ?? 0)} 个通道");
|
||
|
||
// 网格构建阶段不启用精确高度计算，仅存储通道数据供后续使用
|
||
LogManager.Info("[网格地图] 精确高度计算在网格构建时已禁用，将在路径校正时使用");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[网格地图] 设置通道模型项时发生错误: {ex.Message}");
|
||
ChannelItems = new List<ModelItem>();
|
||
EnablePreciseHeightCalculation = false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 切换高度计算模式
|
||
/// </summary>
|
||
/// <param name="enablePrecise">是否启用精确计算</param>
|
||
public void SetHeightCalculationMode(bool enablePrecise)
|
||
{
|
||
EnablePreciseHeightCalculation = enablePrecise && ChannelItems?.Any() == true;
|
||
LogManager.Info($"[网格地图] 高度计算模式: {(EnablePreciseHeightCalculation ? "精确模式" : "传统模式")}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除高度计算缓存
|
||
/// </summary>
|
||
public void ClearHeightCache()
|
||
{
|
||
try
|
||
{
|
||
HeightDetector?.ClearCache();
|
||
SlopeAnalyzer?.ClearCache();
|
||
LogManager.Info("[网格地图] 高度计算缓存已清除");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[网格地图] 清除高度缓存时发生错误: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取高度计算统计信息
|
||
/// </summary>
|
||
/// <returns>统计信息字符串</returns>
|
||
public string GetHeightCalculationStats()
|
||
{
|
||
try
|
||
{
|
||
var heightCacheCount = HeightDetector?.GetCacheCount() ?? 0;
|
||
var slopeCacheCount = SlopeAnalyzer?.GetCacheCount() ?? 0;
|
||
var channelCount = ChannelItems?.Count() ?? 0;
|
||
|
||
return $"精确计算: {(EnablePreciseHeightCalculation ? "启用" : "禁用")}, " +
|
||
$"通道数: {channelCount}, " +
|
||
$"高度缓存: {heightCacheCount}, " +
|
||
$"坡度缓存: {slopeCacheCount}";
|
||
}
|
||
catch
|
||
{
|
||
return "统计信息获取失败";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取网格单元格信息
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>单元格信息</returns>
|
||
public GridCell? GetCell(GridPoint2D gridPosition)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return null;
|
||
|
||
return Cells[gridPosition.X, gridPosition.Y];
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算网格内存使用量（估算）
|
||
/// </summary>
|
||
/// <returns>内存使用量（字节）</returns>
|
||
public long GetMemoryUsage()
|
||
{
|
||
// GridCell结构体大小估算：bool(1) + double(8) + enum(4) ≈ 16字节
|
||
return Width * Height * 16L;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取网格统计信息
|
||
/// </summary>
|
||
/// <returns>统计信息字符串</returns>
|
||
public string GetStatistics()
|
||
{
|
||
// 总计数
|
||
int totalWalkable = 0;
|
||
int totalNonWalkable = 0;
|
||
|
||
// 按类型统计 - 可通行的
|
||
int walkable_楼板 = 0;
|
||
int walkable_门 = 0;
|
||
int walkable_通道 = 0;
|
||
int walkable_装卸区 = 0;
|
||
int walkable_停车位 = 0;
|
||
int walkable_楼梯 = 0;
|
||
int walkable_电梯 = 0;
|
||
int walkable_走廊 = 0;
|
||
int walkable_Other = 0;
|
||
|
||
// 按类型统计 - 不可通行的
|
||
int nonWalkable_Unknown = 0;
|
||
int nonWalkable_障碍物 = 0;
|
||
int nonWalkable_楼板 = 0;
|
||
int nonWalkable_门 = 0;
|
||
int nonWalkable_通道 = 0;
|
||
int nonWalkable_Other = 0;
|
||
|
||
for (int x = 0; x < Width; x++)
|
||
{
|
||
for (int y = 0; y < Height; y++)
|
||
{
|
||
var cell = Cells[x, y];
|
||
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
totalWalkable++;
|
||
switch (cell.CellType)
|
||
{
|
||
case CategoryAttributeManager.LogisticsElementType.楼板:
|
||
walkable_楼板++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.门:
|
||
walkable_门++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.通道:
|
||
walkable_通道++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.装卸区:
|
||
walkable_装卸区++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.停车位:
|
||
walkable_停车位++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.楼梯:
|
||
walkable_楼梯++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.电梯:
|
||
walkable_电梯++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.走廊:
|
||
walkable_走廊++;
|
||
break;
|
||
default:
|
||
walkable_Other++;
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
totalNonWalkable++;
|
||
switch (cell.CellType)
|
||
{
|
||
case CategoryAttributeManager.LogisticsElementType.Unknown:
|
||
nonWalkable_Unknown++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.障碍物:
|
||
nonWalkable_障碍物++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.楼板:
|
||
nonWalkable_楼板++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.门:
|
||
nonWalkable_门++;
|
||
break;
|
||
case CategoryAttributeManager.LogisticsElementType.通道:
|
||
nonWalkable_通道++;
|
||
break;
|
||
default:
|
||
nonWalkable_Other++;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
var stats = $"=== 网格统计详细信息 ===\n";
|
||
stats += $"网格尺寸: {Width}x{Height} (总计 {Width * Height} 个单元格)\n";
|
||
stats += $"总体分布: 可通行 {totalWalkable} 个, 不可通行 {totalNonWalkable} 个\n";
|
||
|
||
stats += $"【可通行单元格详细统计】\n";
|
||
stats += $" 楼板: {walkable_楼板}, 通道: {walkable_通道}, 走廊: {walkable_走廊}\n";
|
||
stats += $" 门: {walkable_门}, 装卸区: {walkable_装卸区}, 停车位: {walkable_停车位}\n";
|
||
stats += $" 楼梯: {walkable_楼梯}, 电梯: {walkable_电梯}, 其他: {walkable_Other}\n";
|
||
|
||
stats += $"【不可通行单元格详细统计】\n";
|
||
stats += $" Unknown/空洞: {nonWalkable_Unknown}, 障碍物: {nonWalkable_障碍物}\n";
|
||
stats += $" 不可通行楼板: {nonWalkable_楼板}, 不可通行门: {nonWalkable_门}\n";
|
||
stats += $" 不可通行通道: {nonWalkable_通道}, 其他不可通行: {nonWalkable_Other}\n";
|
||
|
||
stats += $"内存使用: {GetMemoryUsage() / 1024.0:F1} KB";
|
||
|
||
return stats;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 网格单元格结构体
|
||
/// 存储单个网格单元格的状态信息
|
||
/// </summary>
|
||
public struct GridCell
|
||
{
|
||
/// <summary>
|
||
/// 遍历成本（1.0为标准成本）
|
||
/// </summary>
|
||
public double Cost { get; set; }
|
||
|
||
/// <summary>
|
||
/// 单元格类型
|
||
/// </summary>
|
||
public CategoryAttributeManager.LogisticsElementType CellType { get; set; }
|
||
|
||
/// <summary>
|
||
/// 相关的模型项引用（可选）
|
||
/// </summary>
|
||
public ModelItem RelatedModelItem { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否在通道内 - A*改进方案新增字段
|
||
/// </summary>
|
||
public bool IsInChannel { get; set; }
|
||
|
||
/// <summary>
|
||
/// 多高度层列表 - 支持同一网格坐标多个高度层（如楼梯下方通道）
|
||
/// </summary>
|
||
public List<HeightLayer> HeightLayers { get; set; }
|
||
|
||
/// <summary>
|
||
/// 通道类型 - 用于区分不同类型的通道
|
||
/// </summary>
|
||
public ChannelType ChannelType { get; set; }
|
||
|
||
/// <summary>
|
||
/// 限速（米/秒），0表示未设置限速
|
||
/// </summary>
|
||
public double SpeedLimit { get; set; }
|
||
|
||
/// <summary>
|
||
/// 检查是否至少有一个可通行的高度层
|
||
/// </summary>
|
||
/// <returns>如果至少有一层可通行返回true，否则返回false</returns>
|
||
public bool HasAnyWalkableLayer()
|
||
{
|
||
return HeightLayers != null && HeightLayers.Any(layer => layer.IsWalkable);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据物流类型获取通行成本
|
||
/// </summary>
|
||
/// <returns>通行成本</returns>
|
||
public double GetCost()
|
||
{
|
||
if (!HasAnyWalkableLayer())
|
||
return double.MaxValue;
|
||
|
||
switch (CellType)
|
||
{
|
||
case CategoryAttributeManager.LogisticsElementType.通道:
|
||
return 0.5;
|
||
case CategoryAttributeManager.LogisticsElementType.装卸区:
|
||
return 0.8;
|
||
case CategoryAttributeManager.LogisticsElementType.停车位:
|
||
return 0.9;
|
||
case CategoryAttributeManager.LogisticsElementType.楼板:
|
||
return 1.0;
|
||
case CategoryAttributeManager.LogisticsElementType.门:
|
||
return 1.2;
|
||
case CategoryAttributeManager.LogisticsElementType.楼梯:
|
||
return 3.0;
|
||
case CategoryAttributeManager.LogisticsElementType.电梯:
|
||
return 2.0;
|
||
case CategoryAttributeManager.LogisticsElementType.走廊:
|
||
return 0.6;
|
||
case CategoryAttributeManager.LogisticsElementType.Unknown:
|
||
// Unknown类型（空洞）永远不可通行
|
||
return double.MaxValue;
|
||
case CategoryAttributeManager.LogisticsElementType.障碍物:
|
||
// 障碍物永远不可通行
|
||
return double.MaxValue;
|
||
default:
|
||
return 1.0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建障碍物单元格
|
||
/// </summary>
|
||
/// <returns>障碍物单元格</returns>
|
||
public static GridCell CreateObstacle()
|
||
{
|
||
var cell = new GridCell
|
||
{
|
||
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
|
||
IsInChannel = false,
|
||
HeightLayers = new List<HeightLayer>(), // 空列表，HasAnyWalkableLayer()返回false
|
||
ChannelType = ChannelType.Other,
|
||
SpeedLimit = 0
|
||
};
|
||
cell.Cost = cell.GetCost();
|
||
return cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建门单元格
|
||
/// </summary>
|
||
/// <param name="isOpen">门是否打开</param>
|
||
/// <returns>门单元格</returns>
|
||
public static GridCell CreateDoor(bool isOpen = true)
|
||
{
|
||
var cell = new GridCell
|
||
{
|
||
CellType = CategoryAttributeManager.LogisticsElementType.门,
|
||
IsInChannel = isOpen,
|
||
HeightLayers = new List<HeightLayer>(),
|
||
ChannelType = ChannelType.Other,
|
||
SpeedLimit = 0
|
||
};
|
||
cell.Cost = cell.GetCost();
|
||
return cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建通道单元格
|
||
/// </summary>
|
||
/// <param name="channelType">通道类型</param>
|
||
/// <returns>通道单元格</returns>
|
||
public static GridCell CreateChannel(ChannelType channelType = ChannelType.Corridor)
|
||
{
|
||
var cell = new GridCell
|
||
{
|
||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||
IsInChannel = true,
|
||
HeightLayers = new List<HeightLayer>(),
|
||
ChannelType = channelType,
|
||
SpeedLimit = 0
|
||
};
|
||
cell.Cost = cell.GetCost();
|
||
return cell;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 高度层结构体
|
||
/// 用于支持多高度层的网格单元格
|
||
/// </summary>
|
||
public struct HeightLayer
|
||
{
|
||
/// <summary>
|
||
/// 该层的Z坐标（模型单位）
|
||
/// </summary>
|
||
public double Z { get; set; }
|
||
|
||
/// <summary>
|
||
/// 可通行高度范围（用于车辆检查）
|
||
/// </summary>
|
||
public HeightInterval PassableHeight { get; set; }
|
||
|
||
/// <summary>
|
||
/// 来源模型项
|
||
/// </summary>
|
||
public ModelItem SourceItem { get; set; }
|
||
|
||
/// <summary>
|
||
/// 限速（米/秒），0表示未设置限速
|
||
/// </summary>
|
||
public double SpeedLimit { get; set; }
|
||
|
||
/// <summary>
|
||
/// 层类型
|
||
/// </summary>
|
||
public CategoryAttributeManager.LogisticsElementType Type { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否是边界层（用于膨胀计算）
|
||
/// 边界定义：当前网格层数 > 邻居网格层数，且层间高度差超过阈值
|
||
/// </summary>
|
||
public bool IsBoundary { get; set; }
|
||
|
||
/// <summary>
|
||
/// 该层是否可通行（用于多层膨胀）
|
||
/// true: 该层可以通行
|
||
/// false: 该层被膨胀影响，不可通行
|
||
/// </summary>
|
||
public bool IsWalkable { get; set; }
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
public HeightLayer(double z, HeightInterval passableHeight, ModelItem sourceItem, double speedLimit, CategoryAttributeManager.LogisticsElementType type)
|
||
{
|
||
Z = z;
|
||
PassableHeight = passableHeight;
|
||
SourceItem = sourceItem;
|
||
SpeedLimit = speedLimit;
|
||
Type = type;
|
||
IsBoundary = false; // 默认不是边界
|
||
IsWalkable = true; // 默认可通行，通过膨胀算法修改
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 高度区间结构体 - 用于2.5D路径规划
|
||
/// 表示某个网格位置的可通行高度范围
|
||
/// </summary>
|
||
public struct HeightInterval
|
||
{
|
||
/// <summary>
|
||
/// 最小高度（米）
|
||
/// </summary>
|
||
public double MinZ { get; set; }
|
||
|
||
/// <summary>
|
||
/// 最大高度（米）
|
||
/// </summary>
|
||
public double MaxZ { get; set; }
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="minZ">最小高度</param>
|
||
/// <param name="maxZ">最大高度</param>
|
||
public HeightInterval(double minZ, double maxZ)
|
||
{
|
||
MinZ = minZ;
|
||
MaxZ = maxZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定高度是否在此区间内
|
||
/// </summary>
|
||
/// <param name="height">要检查的高度</param>
|
||
/// <returns>是否在区间内</returns>
|
||
public bool Contains(double height)
|
||
{
|
||
return height >= MinZ && height <= MaxZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定高度范围是否与此区间有重叠
|
||
/// </summary>
|
||
/// <param name="minHeight">要检查的最小高度</param>
|
||
/// <param name="maxHeight">要检查的最大高度</param>
|
||
/// <returns>是否有重叠</returns>
|
||
public bool Overlaps(double minHeight, double maxHeight)
|
||
{
|
||
return maxHeight >= MinZ && minHeight <= MaxZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取区间的高度跨度
|
||
/// </summary>
|
||
/// <returns>高度跨度</returns>
|
||
public double GetSpan()
|
||
{
|
||
return MaxZ - MinZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取区间的中心高度
|
||
/// </summary>
|
||
/// <returns>中心高度</returns>
|
||
public double GetCenter()
|
||
{
|
||
return (MinZ + MaxZ) / 2.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重写ToString方法
|
||
/// </summary>
|
||
/// <returns>字符串表示</returns>
|
||
public override string ToString()
|
||
{
|
||
return $"[{MinZ:F2}m - {MaxZ:F2}m]";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 通道覆盖信息
|
||
/// 存储通道构建器生成的网格地图和相关数据
|
||
/// </summary>
|
||
public class ChannelCoverage
|
||
{
|
||
/// <summary>
|
||
/// 网格地图
|
||
/// </summary>
|
||
public GridMap GridMap { get; set; }
|
||
|
||
/// <summary>
|
||
/// 通道物品列表
|
||
/// </summary>
|
||
public List<ModelItem> ChannelItems { get; set; } = new List<ModelItem>();
|
||
|
||
/// <summary>
|
||
/// 总边界框
|
||
/// </summary>
|
||
public BoundingBox3D TotalBounds { get; set; }
|
||
|
||
/// <summary>
|
||
/// 可通行网格的最小Z坐标（基于实际网格位置）
|
||
/// </summary>
|
||
public double WalkableMinZ { get; set; } = double.MaxValue;
|
||
|
||
/// <summary>
|
||
/// 可通行网格的最大Z坐标（基于实际网格位置）
|
||
/// </summary>
|
||
public double WalkableMaxZ { get; set; } = double.MinValue;
|
||
|
||
/// <summary>
|
||
/// 可通行网格数量
|
||
/// </summary>
|
||
public int WalkableGridCount { get; set; } = 0;
|
||
|
||
/// <summary>
|
||
/// 获取统计信息
|
||
/// </summary>
|
||
/// <returns>统计信息字符串</returns>
|
||
public string GetStatistics()
|
||
{
|
||
var channelCellCount = 0;
|
||
if (GridMap?.Cells != null)
|
||
{
|
||
for (int x = 0; x < GridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < GridMap.Height; y++)
|
||
{
|
||
if (GridMap.Cells[x, y].CellType == CategoryAttributeManager.LogisticsElementType.通道)
|
||
{
|
||
channelCellCount++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
var zRangeInfo = WalkableGridCount > 0
|
||
? $", Z范围: [{WalkableMinZ:F2}, {WalkableMaxZ:F2}], 可通行网格: {WalkableGridCount}个"
|
||
: ", Z范围: 未计算";
|
||
|
||
return $"通道覆盖统计: {ChannelItems.Count} 个通道物品, {channelCellCount} 个通道网格单元, " +
|
||
$"网格大小: {GridMap?.Width}x{GridMap?.Height}{zRangeInfo}";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自动路径规划异常
|
||
/// </summary>
|
||
public class AutoPathPlanningException : Exception
|
||
{
|
||
public AutoPathPlanningException(string message) : base(message) { }
|
||
public AutoPathPlanningException(string message, Exception innerException) : base(message, innerException) { }
|
||
}
|
||
}
|