934 lines
35 KiB
C#
934 lines
35 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);
|
||
|
||
// 检查网格大小是否超出限制
|
||
const int MAX_GRID_SIZE = 10000; // 最大10000x10000网格
|
||
const long MAX_TOTAL_CELLS = 50000000; // 最大5000万个单元格
|
||
|
||
if (Width > MAX_GRID_SIZE || Height > MAX_GRID_SIZE)
|
||
{
|
||
throw new ArgumentException($"网格尺寸过大: {Width}x{Height}，最大允许{MAX_GRID_SIZE}x{MAX_GRID_SIZE}。请增加网格单元格大小或减小搜索区域。");
|
||
}
|
||
|
||
long totalCells = (long)Width * Height;
|
||
if (totalCells > MAX_TOTAL_CELLS)
|
||
{
|
||
throw new ArgumentException($"网格单元格总数过多: {totalCells:N0}，最大允许{MAX_TOTAL_CELLS:N0}。请增加网格单元格大小或减小搜索区域。");
|
||
}
|
||
|
||
// 设置原点为边界框的最小点
|
||
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++)
|
||
{
|
||
// 只计算2D世界坐标，Z坐标将在有规划点信息后单独设置
|
||
var worldPos = GridToWorld2D(new GridPoint2D(x, y)); // 直接使用返回的Point3D，Z=0
|
||
|
||
var cell = new GridCell
|
||
{
|
||
IsWalkable = false,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
|
||
IsInChannel = false,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = ChannelType.Other,
|
||
WorldPosition = worldPos
|
||
};
|
||
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.Floor(gridX), (int)Math.Floor(gridY));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算）
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <returns>世界2D坐标点</returns>
|
||
public Point3D GridToWorld2D(GridPoint2D gridPosition)
|
||
{
|
||
double worldX = Origin.X + (gridPosition.X + 0.5) * CellSize;
|
||
double worldY = Origin.Y + (gridPosition.Y + 0.5) * CellSize;
|
||
|
||
return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格坐标转换为世界3D坐标（需要显式提供Z坐标）
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="z">Z坐标值</param>
|
||
/// <returns>世界3D坐标点</returns>
|
||
public Point3D GridToWorld3D(GridPoint2D gridPosition, double z)
|
||
{
|
||
var world2D = GridToWorld2D(gridPosition);
|
||
return new Point3D(world2D.X, world2D.Y, z);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 线性插值Z坐标计算
|
||
/// </summary>
|
||
/// <param name="worldX">世界坐标X</param>
|
||
/// <param name="worldY">世界坐标Y</param>
|
||
/// <returns>插值后的Z坐标</returns>
|
||
private double CalculateInterpolatedZ(double worldX, double worldY)
|
||
{
|
||
// 必须先设置规划点才能进行Z坐标插值计算
|
||
if (!HasPlanningPoints)
|
||
{
|
||
throw new InvalidOperationException("必须先设置PlanningStartPoint和PlanningEndPoint才能计算网格Z坐标");
|
||
}
|
||
|
||
var start = PlanningStartPoint;
|
||
var end = PlanningEndPoint;
|
||
|
||
// 计算当前点到起点和终点的距离
|
||
double distanceToStart = Math.Sqrt(Math.Pow(worldX - start.X, 2) + Math.Pow(worldY - start.Y, 2));
|
||
double distanceToEnd = Math.Sqrt(Math.Pow(worldX - end.X, 2) + Math.Pow(worldY - end.Y, 2));
|
||
double totalDistance = Math.Sqrt(Math.Pow(end.X - start.X, 2) + Math.Pow(end.Y - start.Y, 2));
|
||
|
||
// 防止除零错误
|
||
if (totalDistance < 0.001)
|
||
{
|
||
return (start.Z + end.Z) / 2.0; // 返回平均高度
|
||
}
|
||
|
||
// 线性插值：根据到起点的距离比例计算Z坐标
|
||
double ratio = distanceToStart / totalDistance;
|
||
double interpolatedZ = start.Z + (end.Z - start.Z) * ratio;
|
||
|
||
return interpolatedZ;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新所有网格单元格的WorldPosition.Z坐标
|
||
/// 必须在设置PlanningStartPoint和PlanningEndPoint之后调用
|
||
/// </summary>
|
||
public void UpdateCellWorldPositionZ()
|
||
{
|
||
if (!HasPlanningPoints)
|
||
{
|
||
throw new InvalidOperationException("必须先设置PlanningStartPoint和PlanningEndPoint才能更新网格Z坐标");
|
||
}
|
||
|
||
for (int x = 0; x < Width; x++)
|
||
{
|
||
for (int y = 0; y < Height; y++)
|
||
{
|
||
var cell = Cells[x, y];
|
||
var world2D = GridToWorld2D(new GridPoint2D(x, y));
|
||
double interpolatedZ = CalculateInterpolatedZ(world2D.X, world2D.Y);
|
||
|
||
// 更新单元格的WorldPosition.Z坐标
|
||
cell.WorldPosition = new Point3D(world2D.X, world2D.Y, interpolatedZ);
|
||
Cells[x, y] = cell;
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[网格Z坐标更新] 已更新所有 {Width}x{Height} 个网格单元格的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].IsWalkable;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置网格单元格的属性
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="isWalkable">是否可通行</param>
|
||
/// <param name="cost">遍历成本</param>
|
||
/// <param name="cellType">单元格类型</param>
|
||
public void SetCell(GridPoint2D gridPosition, bool isWalkable, double cost, CategoryAttributeManager.LogisticsElementType cellType)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
var existingZ = Cells[gridPosition.X, gridPosition.Y].WorldPosition.Z;
|
||
var worldPos = GridToWorld3D(gridPosition, existingZ);
|
||
var cell = new GridCell
|
||
{
|
||
IsWalkable = isWalkable,
|
||
CellType = cellType,
|
||
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
|
||
WorldPosition = worldPos
|
||
};
|
||
|
||
// 🔥 关键验证：确保Unknown和障碍物类型永远不可通行
|
||
if (cellType == CategoryAttributeManager.LogisticsElementType.Unknown ||
|
||
cellType == CategoryAttributeManager.LogisticsElementType.障碍物)
|
||
{
|
||
cell.IsWalkable = false;
|
||
cell.Cost = double.MaxValue;
|
||
}
|
||
else
|
||
{
|
||
// 优先使用传入的cost参数，如果为默认值则用GetCost
|
||
cell.Cost = (cost == double.MaxValue || cost == 0) ? cell.GetCost() : cost;
|
||
}
|
||
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置网格单元为通道类型 - A*改进方案新增方法
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="channelType">通道类型</param>
|
||
/// <param name="relatedItem">关联的模型物品</param>
|
||
public void SetCellAsChannel(GridPoint2D gridPosition, ChannelType channelType = ChannelType.Corridor, ModelItem relatedItem = null)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
var existingZ = Cells[gridPosition.X, gridPosition.Y].WorldPosition.Z;
|
||
var worldPos = GridToWorld3D(gridPosition, existingZ);
|
||
var cell = new GridCell
|
||
{
|
||
IsWalkable = true,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||
IsInChannel = true,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = channelType,
|
||
WorldPosition = worldPos,
|
||
RelatedModelItem = relatedItem
|
||
};
|
||
cell.Cost = cell.GetCost(); // 用GetCost方法计算通道成本
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 添加可通行高度区间到指定网格单元
|
||
/// </summary>
|
||
/// <param name="gridPosition">网格坐标</param>
|
||
/// <param name="heightInterval">高度区间</param>
|
||
public void AddPassableHeight(GridPoint2D gridPosition, HeightInterval heightInterval)
|
||
{
|
||
if (!IsValidGridPosition(gridPosition))
|
||
return;
|
||
|
||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||
if (cell.PassableHeights == null)
|
||
{
|
||
cell.PassableHeights = new List<HeightInterval>();
|
||
}
|
||
cell.PassableHeights.Add(heightInterval);
|
||
Cells[gridPosition.X, gridPosition.Y] = cell;
|
||
}
|
||
|
||
/// <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.IsWalkable)
|
||
return false;
|
||
|
||
// 如果没有设置高度区间，使用传统的可通行性判断
|
||
if (cell.PassableHeights == null || !cell.PassableHeights.Any())
|
||
return true;
|
||
|
||
// 检查是否有任何高度区间包含指定高度
|
||
return cell.PassableHeights.Any(interval => interval.Contains(height));
|
||
}
|
||
|
||
/// <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.IsWalkable)
|
||
{
|
||
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>
|
||
/// 是否可通行
|
||
/// </summary>
|
||
public bool IsWalkable { get; set; }
|
||
|
||
/// <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>
|
||
/// 可通行高度区间列表 - 支持2.5D路径规划
|
||
/// </summary>
|
||
public List<HeightInterval> PassableHeights { get; set; }
|
||
|
||
/// <summary>
|
||
/// 通道类型 - 用于区分不同类型的通道
|
||
/// </summary>
|
||
public ChannelType ChannelType { get; set; }
|
||
|
||
/// <summary>
|
||
/// 世界坐标位置 - 用于高度计算和空间查询
|
||
/// </summary>
|
||
public Point3D WorldPosition { get; set; }
|
||
|
||
/// <summary>
|
||
/// 根据物流类型获取通行成本
|
||
/// </summary>
|
||
/// <returns>通行成本</returns>
|
||
public double GetCost()
|
||
{
|
||
if (!IsWalkable)
|
||
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
|
||
{
|
||
IsWalkable = false,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
|
||
IsInChannel = false,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = ChannelType.Other
|
||
};
|
||
cell.Cost = cell.GetCost();
|
||
return cell;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建开放空间单元格
|
||
/// </summary>
|
||
/// <returns>开放空间单元格</returns>
|
||
public static GridCell CreateOpenSpace()
|
||
{
|
||
var cell = new GridCell
|
||
{
|
||
IsWalkable = true,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.楼板,
|
||
IsInChannel = false,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = ChannelType.Other
|
||
};
|
||
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
|
||
{
|
||
IsWalkable = isOpen,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.门,
|
||
IsInChannel = isOpen,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = ChannelType.Other
|
||
};
|
||
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
|
||
{
|
||
IsWalkable = true,
|
||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||
IsInChannel = true,
|
||
PassableHeights = new List<HeightInterval>(),
|
||
ChannelType = channelType
|
||
};
|
||
cell.Cost = cell.GetCost();
|
||
return cell;
|
||
}
|
||
}
|
||
|
||
|
||
/// <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>
|
||
/// 获取统计信息
|
||
/// </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++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return $"通道覆盖统计: {ChannelItems.Count} 个通道物品, {channelCellCount} 个通道网格单元, " +
|
||
$"网格大小: {GridMap?.Width}x{GridMap?.Height}";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自动路径规划异常
|
||
/// </summary>
|
||
public class AutoPathPlanningException : Exception
|
||
{
|
||
public AutoPathPlanningException(string message) : base(message) { }
|
||
public AutoPathPlanningException(string message, Exception innerException) : base(message, innerException) { }
|
||
}
|
||
} |