给网格全部加上高度,解决了门限高的问题。

This commit is contained in:
tian 2025-09-16 04:56:56 +08:00
parent 15a3a29a28
commit 97b9c2beca
2 changed files with 151 additions and 32 deletions

View File

@ -165,6 +165,9 @@ namespace NavisworksTransport.PathPlanning
// 7. 直接返回高度校正后的路径不再修改XY坐标
var finalPath = heightCorrectedPath;
// 8. 验证路径中所有点的高度约束(特别是门网格)
ValidatePathHeightConstraints(finalPath, gridMap, vehicleHeight);
// 更新PathFindingResult
pathResult.PathPoints = finalPath;
pathResult.OriginalEndPoint = originalEnd;
@ -251,24 +254,34 @@ namespace NavisworksTransport.PathPlanning
// 检查基本可通行性和高度约束
bool isPassable = cell.IsWalkable;
if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any())
if (isPassable)
{
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
if (!heightOk)
if (cell.PassableHeights != null && cell.PassableHeights.Any())
{
heightConstrainedCells++;
// 将车辆高度从米转换为模型单位
double vehicleHeightInModelUnits = vehicleHeight * UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits);
if (!heightOk)
{
heightConstrainedCells++;
// 详细日志记录被高度约束排除的单元格仅记录前10个避免日志过多
if (heightConstrainedCells <= 10)
{
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除:车辆高度{vehicleHeight}m可用区间{string.Join(", ", cell.PassableHeights.Select(i => $"{i.MinZ:F2}-{i.MaxZ:F2}({i.GetSpan():F2})"))}");
}
else if (heightConstrainedCells == 11)
{
LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除,不再详细记录...");
// 详细日志记录被高度约束排除的单元格仅记录前10个避免日志过多
if (heightConstrainedCells <= 10)
{
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除:车辆高度{vehicleHeight}m({vehicleHeightInModelUnits:F2}模型单位),可用区间:{string.Join(", ", cell.PassableHeights.Select(i => $"{i.MinZ:F2}-{i.MaxZ:F2}({i.GetSpan():F2})"))}");
}
else if (heightConstrainedCells == 11)
{
LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除,不再详细记录...");
}
}
isPassable = heightOk;
}
else
{
// 没有高度信息的网格直接标记为不可通行
isPassable = false;
}
isPassable = heightOk;
}
if (isPassable)
@ -284,7 +297,7 @@ namespace NavisworksTransport.PathPlanning
bool rightPassable = rightCell.IsWalkable;
if (rightPassable && rightCell.PassableHeights != null && rightCell.PassableHeights.Any())
{
rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits);
}
if (rightPassable)
{
@ -301,7 +314,7 @@ namespace NavisworksTransport.PathPlanning
bool bottomPassable = bottomCell.IsWalkable;
if (bottomPassable && bottomCell.PassableHeights != null && bottomCell.PassableHeights.Any())
{
bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits);
}
if (bottomPassable)
{
@ -550,7 +563,7 @@ namespace NavisworksTransport.PathPlanning
return cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
}
return true;
return false;
}
/// <summary>
@ -1282,13 +1295,17 @@ namespace NavisworksTransport.PathPlanning
// 检查高度约束
if (cell.PassableHeights != null && cell.PassableHeights.Any())
{
LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间,车辆高度:{vehicleHeight}m");
// 将车辆高度从米转换为模型单位
double vehicleHeightInModelUnits = vehicleHeight * UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间,车辆高度:{vehicleHeight}m ({vehicleHeightInModelUnits:F2}模型单位)");
foreach (var interval in cell.PassableHeights)
{
bool spanOk = interval.GetSpan() >= vehicleHeight;
bool containsHeight = interval.Contains(point.Z);
LogManager.Info($"[高度检查] 区间 {interval}: 跨度={interval.GetSpan():F2}m (需要≥{vehicleHeight}m): {spanOk}, 包含Z={point.Z:F2}: {containsHeight}");
bool spanOk = interval.GetSpan() >= vehicleHeightInModelUnits;
// 将绝对坐标转换为相对于网格底面的坐标
double relativeZ = point.Z - cell.WorldPosition.Z;
bool containsHeight = interval.Contains(relativeZ);
LogManager.Info($"[高度检查] 区间 {interval}: 跨度={interval.GetSpan():F2}模型单位 (需要≥{vehicleHeightInModelUnits:F2}模型单位): {spanOk}, 包含相对Z={relativeZ:F2}: {containsHeight}");
if (spanOk && containsHeight)
{
@ -1301,9 +1318,7 @@ namespace NavisworksTransport.PathPlanning
return false;
}
// 如果没有高度信息,认为可通行(向后兼容)
LogManager.Info($"[高度检查] 无高度信息,默认可通行(向后兼容模式)");
return true;
return false;
}
catch (Exception ex)
{
@ -1509,6 +1524,64 @@ namespace NavisworksTransport.PathPlanning
}
}
/// <summary>
/// 验证路径中所有点的高度约束(特别检查门网格)
/// </summary>
/// <param name="path">路径点列表</param>
/// <param name="gridMap">网格地图</param>
/// <param name="vehicleHeight">车辆高度</param>
private void ValidatePathHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
{
LogManager.Info($"[路径高度验证] 开始验证 {path.Count} 个路径点的高度约束");
int checkedPoints = 0;
int passedPoints = 0;
int failedPoints = 0;
int doorPoints = 0;
for (int i = 0; i < path.Count; i++)
{
var point = path[i];
var gridPos = gridMap.WorldToGrid(point);
if (gridMap.IsValidGridPosition(gridPos))
{
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
checkedPoints++;
// 特别标记门网格点
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.)
{
doorPoints++;
LogManager.Info($"[路径高度验证] 检查路径点{i}(门网格): ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})");
}
else
{
LogManager.Info($"[路径高度验证] 检查路径点{i}{cell.CellType}: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})");
}
// 进行高度约束检查
bool isPassable = IsPointPassableAtHeight(point, gridMap, vehicleHeight);
if (isPassable)
{
passedPoints++;
}
else
{
failedPoints++;
LogManager.Warning($"[路径高度验证] ❌ 路径点{i}高度约束检查失败!");
}
}
}
LogManager.Info($"[路径高度验证] 完成 - 检查点数: {checkedPoints}, 通过: {passedPoints}, 失败: {failedPoints}, 门网格点: {doorPoints}");
if (failedPoints > 0)
{
LogManager.Warning($"[路径高度验证] 警告:发现 {failedPoints} 个点不满足高度约束!");
}
}
/// <summary>
/// 计算两点间的3D距离
/// </summary>

View File

@ -243,7 +243,12 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info("【包围盒2.5D模式】步骤2.5: 处理门元素");
ProcessDoorElements(channelCoverage.GridMap, allChannelItems);
LogManager.Info($"【阶段2.5完成】门元素处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
// 2.6. 为通道网格设置PassableHeights确保高度约束检查
LogManager.Info("【包围盒2.5D模式】步骤2.6: 为通道网格设置高度约束");
SetChannelPassableHeights(channelCoverage.GridMap, scanHeight);
LogManager.Info($"【阶段2.6完成】通道高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}");
// 3. 应用车辆尺寸膨胀(如果需要)
if (vehicleRadius > 0 || safetyMargin > 0)
{
@ -335,20 +340,27 @@ namespace NavisworksTransport.PathPlanning
// 获取门的限高属性
string heightLimitStr = CategoryAttributeManager.GetLogisticsPropertyValue(doorItem, CategoryAttributeManager.LogisticsProperties.HEIGHT_LIMIT);
double doorHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高");
// 如果没有限高属性或解析失败,使用门包围盒的高度
if (doorHeight <= 0)
double configuredHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高");
// 计算门的实际物理高度
double actualDoorHeight = bbox.Max.Z - bbox.Min.Z;
double doorHeight;
if (configuredHeight <= 0)
{
doorHeight = bbox.Max.Z - bbox.Min.Z;
// 没有限高配置,使用实际门高
doorHeight = actualDoorHeight;
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 未设置限高,使用包围盒高度: {doorHeight:F2}");
}
else
{
// 转换限高到模型单位
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
doorHeight = doorHeight * metersToModelUnits;
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 限高: {doorHeight:F2} 模型单位");
double configuredHeightInModelUnits = configuredHeight * metersToModelUnits;
// 取实际门高和配置限高的最小值
doorHeight = Math.Min(actualDoorHeight, configuredHeightInModelUnits);
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 实际高度: {actualDoorHeight:F2}, 限高:{configuredHeightInModelUnits:F2}, 有效高度: {doorHeight:F2}");
}
// 获取门底部的Z坐标
@ -368,7 +380,7 @@ namespace NavisworksTransport.PathPlanning
var worldPos = gridMap.GridToWorld3D(gridPos, doorBottomZ);
cell.WorldPosition = worldPos;
// 将门的高度范围存储在PassableHeights中供后续使用
// 将门的高度范围存储在PassableHeights中供后续使用(使用相对坐标)
if (doorHeight > 0)
{
cell.PassableHeights = new List<HeightInterval>
@ -399,6 +411,40 @@ namespace NavisworksTransport.PathPlanning
}
}
/// <summary>
/// 为通道网格设置PassableHeights确保高度约束检查
/// </summary>
/// <param name="gridMap">网格地图</param>
/// <param name="scanHeight">扫描高度(车辆高度+安全间隙)</param>
private void SetChannelPassableHeights(GridMap gridMap, double scanHeight)
{
// scanHeight是米需要转换为模型单位
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
double scanHeightInModelUnits = scanHeight * metersToModelUnits;
LogManager.Info($"【通道高度设置】开始为通道网格设置高度约束,扫描高度: {scanHeight:F2}米 ({scanHeightInModelUnits:F2}模型单位)");
int processedCount = 0;
for (int x = 0; x < gridMap.Width; x++)
{
for (int y = 0; y < gridMap.Height; y++)
{
var cell = gridMap.Cells[x, y];
if (cell.CellType == CategoryAttributeManager.LogisticsElementType. && cell.IsWalkable)
{
cell.PassableHeights = new List<HeightInterval>
{
new HeightInterval(0, scanHeightInModelUnits)
};
gridMap.Cells[x, y] = cell;
processedCount++;
}
}
}
LogManager.Info($"【通道高度设置】完成,已处理 {processedCount} 个通道网格");
}
/// <summary>
/// 从物流元素集合中过滤出门类型的元素
/// </summary>