tian/NavisworksTransport
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

实现了路径在斜面（如楼梯）上的规划

Browse Source
This commit is contained in:
tian 2025-09-28 17:46:16 +08:00
parent 8a95820fca
commit 3b1fae6a1e
5 changed files with 107 additions and 216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
src/PathPlanning/AutoPathFinder.cs
View File

@ -101,17 +101,14 @@ namespace NavisworksTransport.PathPlanning
// 5. 根据网格点通行高度区间对路径点Z坐标调整
var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
// 6. 精确高度修正
var heightCorrectedPath = ApplyPreciseHeightCorrection(enhancedPath, gridMap);
// 8. 验证路径中所有点的高度约束（特别是门网格）
ValidatePathHeightConstraints(heightCorrectedPath, gridMap, vehicleHeight);
// 6. 验证路径中所有点的高度约束（特别是门网格）
ValidatePathHeightConstraints(enhancedPath, gridMap, vehicleHeight);
// 更新PathFindingResult
pathResult.PathPoints = heightCorrectedPath;
pathResult.PathPoints = enhancedPath;
pathResult.OriginalEndPoint = originalEnd;
LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {heightCorrectedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {enhancedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
// 路径优化 - 去除共线点
pathResult = OptimizePath(pathResult, gridMap);
@ -198,10 +195,10 @@ namespace NavisworksTransport.PathPlanning
if (isPassable)
{
if (cell.PassableHeights != null && cell.PassableHeights.Any())
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
{
// vehicleHeight已经是模型单位，直接比较即可
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
if (!heightOk)
{
heightConstrainedCells++;
@ -209,7 +206,7 @@ namespace NavisworksTransport.PathPlanning
// 详细日志：记录被高度约束排除的单元格（仅记录前10个避免日志过多）
if (heightConstrainedCells <= 10)
{
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：车辆高度{vehicleHeight:F2}模型单位，可用区间：{string.Join(", ", cell.PassableHeights.Select(i => $"{i.MinZ:F2}-{i.MaxZ:F2}(跨度{i.GetSpan():F2})"))}");
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：车辆高度{vehicleHeight:F2}模型单位，可用区间：{cell.PassableHeight.MinZ:F2}-{cell.PassableHeight.MaxZ:F2}(跨度{cell.PassableHeight.GetSpan():F2})");
}
else if (heightConstrainedCells == 11)
{
@ -236,9 +233,9 @@ namespace NavisworksTransport.PathPlanning
{
var rightCell = gridMap.Cells[x + 1, y];
bool rightPassable = rightCell.IsWalkable;
if (rightPassable && rightCell.PassableHeights != null && rightCell.PassableHeights.Any())
if (rightPassable && rightCell.PassableHeight.MaxZ > rightCell.PassableHeight.MinZ)
{
rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
rightPassable = rightCell.PassableHeight.GetSpan() >= vehicleHeight;
}
if (rightPassable)
{
@ -253,9 +250,9 @@ namespace NavisworksTransport.PathPlanning
{
var bottomCell = gridMap.Cells[x, y + 1];
bool bottomPassable = bottomCell.IsWalkable;
if (bottomPassable && bottomCell.PassableHeights != null && bottomCell.PassableHeights.Any())
if (bottomPassable && bottomCell.PassableHeight.MaxZ > bottomCell.PassableHeight.MinZ)
{
bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
bottomPassable = bottomCell.PassableHeight.GetSpan() >= vehicleHeight;
}
if (bottomPassable)
{
@ -368,10 +365,10 @@ namespace NavisworksTransport.PathPlanning
// 检查基本可通行性和高度约束
bool isPassable = cell.IsWalkable;
if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any())
if (isPassable && cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
{
// vehicleHeight已经是模型单位，直接比较即可
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
if (!heightOk)
{
heightConstrainedCells++;
@ -500,10 +497,10 @@ namespace NavisworksTransport.PathPlanning
if (!cell.IsWalkable)
return false;
if (cell.PassableHeights != null && cell.PassableHeights.Any())
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
{
// vehicleHeight已经是模型单位，直接比较即可
return cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
return cell.PassableHeight.GetSpan() >= vehicleHeight;
}
return false;
@ -1074,80 +1071,6 @@ namespace NavisworksTransport.PathPlanning
return turns;
}
/// <summary>
/// 对最终路径点应用精确高度校正，使路径点贴合通道表面
///
/// 重要：使用GetChannelTopHeight确保车辆路径点位于正确的行驶表面
/// - 楼板类通道：校正到楼板顶面（行驶面）而非底面
/// - 走廊类通道：校正到可行驶的表面高度
/// </summary>
/// <param name="path">原始路径点</param>
/// <param name="gridMap">网格地图（包含通道信息，通道可能是楼板、走廊等）</param>
/// <returns>高度校正后的路径点</returns>
private List<Point3D> ApplyPreciseHeightCorrection(List<Point3D> path, GridMap gridMap)
{
try
{
LogManager.Info($"[高度校正] 开始对 {path.Count} 个路径点进行精确高度校正");
// 如果没有通道数据，直接返回原路径
if (gridMap.ChannelItems == null || !gridMap.ChannelItems.Any())
{
LogManager.Info("[高度校正] 无通道数据，跳过高度校正");
return path;
}
// 确保高度检测器可用
if (gridMap.HeightDetector == null)
{
LogManager.Warning("[高度校正] 高度检测器不可用，跳过高度校正");
return path;
}
var correctedPath = new List<Point3D>();
int correctedCount = 0;
for (int i = 0; i < path.Count; i++)
{
var originalPoint = path[i];
try
{
// 🔥 关键修改：使用GetChannelTopHeight获取通道顶面高度，而不是底面高度
// 因为通道是楼板，车辆应该在楼板顶面行驶
var preciseHeight = gridMap.HeightDetector.GetChannelTopHeight(originalPoint, gridMap.ChannelItems);
// 检查高度是否需要校正（容差1cm）
const double heightTolerance = 0.01; // 1cm
if (Math.Abs(originalPoint.Z - preciseHeight) > heightTolerance)
{
var correctedPoint = new Point3D(originalPoint.X, originalPoint.Y, preciseHeight);
correctedPath.Add(correctedPoint);
correctedCount++;
}
else
{
// 高度已经正确，无需校正
correctedPath.Add(originalPoint);
}
}
catch (Exception ex)
{
// 如果某个点的高度检测失败，使用原始点
LogManager.Warning($"[高度校正] 点{i}高度校正失败: {ex.Message}，使用原始高度");
correctedPath.Add(originalPoint);
}
}
LogManager.Info($"[高度校正] 高度校正完成，共校正了 {correctedCount}/{path.Count} 个点");
return correctedPath;
}
catch (Exception ex)
{
LogManager.Error($"[高度校正] 路径高度校正失败: {ex.Message}，返回原始路径");
return path;
}
}
/// <summary>
/// 修正路径的起点和终点为原始用户指定的坐标，避免网格转换造成的错位
@ -1232,22 +1155,19 @@ namespace NavisworksTransport.PathPlanning
}
// 检查高度约束
if (cell.PassableHeights != null && cell.PassableHeights.Any())
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
{
foreach (var interval in cell.PassableHeights)
{
bool spanOk = interval.GetSpan() >= vehicleHeight;
// 将绝对坐标转换为相对于网格底面的坐标
double relativeZ = point.Z - cell.WorldPosition.Z;
bool containsHeight = interval.Contains(relativeZ);
bool spanOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
// 将绝对坐标转换为相对于网格底面的坐标
double relativeZ = point.Z - cell.WorldPosition.Z;
bool containsHeight = cell.PassableHeight.Contains(relativeZ);
if (spanOk && containsHeight)
{
return true;
}
if (spanOk && containsHeight)
{
return true;
}
LogManager.Warning($"[高度检查] ❌ 所有高度区间都不满足要求");
LogManager.Warning($"[高度检查] ❌ 高度区间不满足要求");
return false;
}
@ -1278,7 +1198,7 @@ namespace NavisworksTransport.PathPlanning
{
var gridPos = gridMap.WorldToGrid(point);
// 检查网格有效性
// 检查网格位置有效性
if (!gridMap.IsValidGridPosition(gridPos))
{
adjustedPath.Add(point);
@ -1287,13 +1207,18 @@ namespace NavisworksTransport.PathPlanning
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
var bestInterval = cell.PassableHeights
.Where(interval => interval.GetSpan() >= vehicleHeight)
.OrderBy(interval => interval.MinZ)
.FirstOrDefault();
var optimalZ = cell.WorldPosition.Z + bestInterval.MinZ;
adjustedPath.Add(new Point3D(point.X, point.Y, optimalZ));
// 检查单个高度区间是否满足车辆高度要求
if (cell.PassableHeight.GetSpan() >= vehicleHeight)
{
// 使用高度区间的底部作为最优Z坐标
var optimalZ = cell.WorldPosition.Z + cell.PassableHeight.MinZ;
adjustedPath.Add(new Point3D(point.X, point.Y, optimalZ));
}
else
{
// 高度不足，保持原始点
adjustedPath.Add(point);
}
adjustedCount++;
}
@ -1600,10 +1525,10 @@ namespace NavisworksTransport.PathPlanning
// 检查基本可通行性和高度约束
bool isPassable = cell.IsWalkable;
if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any())
if (isPassable && cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
{
// vehicleHeight已经是模型单位，直接比较即可
bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight);
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
if (!heightOk)
{
heightConstrainedCells++;

2
src/PathPlanning/ChannelBasedGridBuilder.cs
View File

@ -225,7 +225,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = true,
CellType = CategoryAttributeManager.LogisticsElementType.通道,
IsInChannel = true,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = ChannelType.Corridor,
WorldPosition = new Point3D(worldPos.X, worldPos.Y, gridCenterHeight),
RelatedModelItem = null

40
src/PathPlanning/GridMap.cs
View File

@ -163,7 +163,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = false,
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
IsInChannel = false,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = ChannelType.Other,
WorldPosition = worldPos
};
@ -256,7 +256,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = isWalkable,
CellType = cellType,
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
WorldPosition = worldPos
};
@ -277,26 +277,6 @@ namespace NavisworksTransport.PathPlanning
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>
@ -312,12 +292,8 @@ namespace NavisworksTransport.PathPlanning
if (!cell.IsWalkable)
return false;
// 如果没有设置高度区间，使用传统的可通行性判断
if (cell.PassableHeights == null || !cell.PassableHeights.Any())
return true;
// 检查是否有任何高度区间包含指定高度
return cell.PassableHeights.Any(interval => interval.Contains(height));
return (cell.PassableHeight.MaxZ - cell.PassableHeight.MinZ) >= height;
}
/// <summary>
@ -589,7 +565,7 @@ namespace NavisworksTransport.PathPlanning
/// <summary>
/// 可通行高度区间列表 - 支持2.5D路径规划
/// </summary>
public List<HeightInterval> PassableHeights { get; set; }
public HeightInterval PassableHeight { get; set; }
/// <summary>
/// 通道类型 - 用于区分不同类型的通道
@ -650,7 +626,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = false,
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
IsInChannel = false,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = ChannelType.Other
};
cell.Cost = cell.GetCost();
@ -668,7 +644,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = true,
CellType = CategoryAttributeManager.LogisticsElementType.楼板,
IsInChannel = false,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = ChannelType.Other
};
cell.Cost = cell.GetCost();
@ -687,7 +663,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = isOpen,
CellType = CategoryAttributeManager.LogisticsElementType.门,
IsInChannel = isOpen,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = ChannelType.Other
};
cell.Cost = cell.GetCost();
@ -706,7 +682,7 @@ namespace NavisworksTransport.PathPlanning
IsWalkable = true,
CellType = CategoryAttributeManager.LogisticsElementType.通道,
IsInChannel = true,
PassableHeights = new List<HeightInterval>(),
PassableHeight = new HeightInterval(),
ChannelType = channelType
};
cell.Cost = cell.GetCost();

57
src/PathPlanning/GridMapGenerator.cs
View File

@ -298,10 +298,7 @@ namespace NavisworksTransport.PathPlanning
// 将门的高度范围存储在PassableHeights中供后续使用（使用相对坐标）
if (doorHeight > 0)
{
cell.PassableHeights = new List<HeightInterval>
{
new HeightInterval(0, doorHeight)
};
cell.PassableHeight = new HeightInterval(0, doorHeight);
}
// 将修改后的结构体设置回数组
@ -343,10 +340,7 @@ namespace NavisworksTransport.PathPlanning
var cell = gridMap.Cells[x, y];
if (cell.IsWalkable)
{
cell.PassableHeights = new List<HeightInterval>
{
new HeightInterval(0, scanHeightInModelUnits)
};
cell.PassableHeight = new HeightInterval(0, scanHeightInModelUnits);
gridMap.Cells[x, y] = cell;
processedCount++;
}
@ -461,12 +455,7 @@ namespace NavisworksTransport.PathPlanning
/// </summary>
/// <param name="gridMap">网格地图</param>
/// <param name="heightIntervals">高度区间数据</param>
/// <summary>
/// 将高度区间信息集成到网格地图中
/// </summary>
/// <param name="gridMap">网格地图</param>
/// <param name="heightIntervals">高度区间数据</param>
private void IntegrateHeightIntervalsToGrid(GridMap gridMap, Dictionary<Point3D, List<HeightInterval>> heightIntervals)
private void IntegrateHeightIntervalsToGrid(GridMap gridMap, Dictionary<Point3D, HeightInterval> heightIntervals)
{
int updatedCells = 0;
int channelCellsWithHeightData = 0;
@ -475,44 +464,39 @@ namespace NavisworksTransport.PathPlanning
foreach (var kvp in heightIntervals)
{
var point = kvp.Key;
var intervals = kvp.Value;
var interval = kvp.Value;
// 将世界坐标转换为网格坐标
var gridPos = gridMap.WorldToGrid(new Point3D(point.X, point.Y, 0));
int gridX = gridPos.X;
int gridY = gridPos.Y;
// 确保坐标在有效范围内
if (gridX >= 0 && gridX < gridMap.Width && gridY >= 0 && gridY < gridMap.Height)
{
// 更新网格单元格的高度区间信息
var cell = gridMap.Cells[gridX, gridY];
if (cell.PassableHeights == null)
{
cell.PassableHeights = new List<HeightInterval>();
}
cell.PassableHeights.Clear();
cell.PassableHeights.AddRange(intervals);
// 直接设置高度区间
cell.PassableHeight = interval;
// 关键修复：只处理通道单元格的高度信息集成
// 非通道单元格不应该通过高度扫描而改变其类型或可通行性
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道 && cell.IsInChannel)
{
channelCellsWithHeightData++;
if (intervals.Any())
if (interval.MaxZ > interval.MinZ)
{
// 通道区域有足够净空高度，设置为可通行
// 🔥 关键修复：使用SetCell方法正确更新网格统计
var cellPos = new GridPoint2D(gridX, gridY);
gridMap.SetCell(cellPos, true, 1.0, CategoryAttributeManager.LogisticsElementType.通道);
// 设置世界坐标和高度信息
var updatedCell = gridMap.Cells[gridX, gridY];
updatedCell.WorldPosition = point;
updatedCell.PassableHeights = new List<HeightInterval>(intervals);
updatedCell.PassableHeight = interval;
updatedCell.IsInChannel = true;
gridMap.Cells[gridX, gridY] = updatedCell;
}
@ -521,14 +505,14 @@ namespace NavisworksTransport.PathPlanning
// 通道区域但净空高度不足，标记为不可通行但保持通道类型
var cellPos = new GridPoint2D(gridX, gridY);
gridMap.SetCell(cellPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.通道);
// 设置世界坐标和高度信息
var updatedCell = gridMap.Cells[gridX, gridY];
updatedCell.WorldPosition = point;
updatedCell.PassableHeights = new List<HeightInterval>(intervals);
updatedCell.PassableHeight = interval;
updatedCell.IsInChannel = true;
gridMap.Cells[gridX, gridY] = updatedCell;
channelCellsBecomingUnwalkable++;
}
}
@ -540,7 +524,7 @@ namespace NavisworksTransport.PathPlanning
// 仍然更新高度信息，但保持原有状态
cell.WorldPosition = point;
cell.PassableHeights = new List<HeightInterval>(intervals);
cell.PassableHeight = interval;
gridMap.Cells[gridX, gridY] = cell;
}
updatedCells++;
@ -1259,14 +1243,11 @@ namespace NavisworksTransport.PathPlanning
cell.RelatedModelItem = update.Item;
// 记录高度信息
if (cell.PassableHeights == null)
cell.PassableHeights = new List<HeightInterval>();
var groundHeight = cell.WorldPosition.Z;
cell.PassableHeights.Add(new HeightInterval(
cell.PassableHeight = new HeightInterval(
update.BoundingBox.Min.Z - groundHeight,
update.BoundingBox.Max.Z - groundHeight
));
);
gridMap.Cells[update.X, update.Y] = cell;
updatedCells++;

71
src/PathPlanning/VerticalScanProcessor.cs
View File

@ -280,8 +280,8 @@ namespace NavisworksTransport.PathPlanning
/// <param name="gridPoints">网格点列表</param>
/// <param name="scanHeight">扫描高度（从地面向上的距离）</param>
/// <param name="vehicleHeight">车辆高度（用于通行检查）</param>
/// <returns>每个网格点的高度区间列表</returns>
public Dictionary<Point3D, List<HeightInterval>> ParallelScanHeightIntervals(
/// <returns>每个网格点的高度区间</returns>
public Dictionary<Point3D, HeightInterval> ParallelScanHeightIntervals(
IEnumerable<Point3D> gridPoints,
double scanHeight = 20.0,
double vehicleHeight = 3.0)
@ -289,7 +289,7 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[垂直扫描处理器] 开始并行扫描高度区间，扫描高度: {scanHeight}m, 车辆高度: {vehicleHeight}m");
var startTime = DateTime.Now;
var results = new ConcurrentDictionary<Point3D, List<HeightInterval>>();
var results = new ConcurrentDictionary<Point3D, HeightInterval>();
var pointsList = gridPoints?.ToList() ?? new List<Point3D>();
LogManager.Info($"[垂直扫描处理器] 准备处理{pointsList.Count}个点");
@ -307,8 +307,8 @@ namespace NavisworksTransport.PathPlanning
{
if (gridPoint != null)
{
var intervals = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
results[gridPoint] = intervals ?? new List<HeightInterval>();
var interval = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
results[gridPoint] = interval;
}
}
catch (OutOfMemoryException)
@ -316,7 +316,7 @@ namespace NavisworksTransport.PathPlanning
LogManager.Error($"[垂直扫描处理器] 内存不足，跳过点 {gridPoint}");
if (gridPoint != null)
{
results[gridPoint] = new List<HeightInterval>();
results[gridPoint] = new HeightInterval();
}
throw; // 内存不足需要重新抛出
}
@ -325,7 +325,7 @@ namespace NavisworksTransport.PathPlanning
LogManager.Error($"[垂直扫描处理器] 扫描点 {gridPoint} 失败: {ex.Message}");
if (gridPoint != null)
{
results[gridPoint] = new List<HeightInterval>();
results[gridPoint] = new HeightInterval();
}
}
});
@ -342,8 +342,8 @@ namespace NavisworksTransport.PathPlanning
{
if (gridPoint != null && !results.ContainsKey(gridPoint))
{
var intervals = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
results[gridPoint] = intervals ?? new List<HeightInterval>();
var interval = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
results[gridPoint] = interval;
}
}
catch (Exception serialEx)
@ -351,14 +351,14 @@ namespace NavisworksTransport.PathPlanning
LogManager.Error($"[垂直扫描处理器] 串行扫描点 {gridPoint} 也失败: {serialEx.Message}");
if (gridPoint != null)
{
results[gridPoint] = new List<HeightInterval>();
results[gridPoint] = new HeightInterval();
}
}
}
}
var elapsed = (DateTime.Now - startTime).TotalMilliseconds;
var totalIntervals = results.Values.Sum(list => list.Count);
var validIntervals = results.Values.Count(interval => interval.MaxZ > interval.MinZ);
// 输出缓存统计信息
if (_cacheHits + _cacheMisses > 0)
@ -367,14 +367,14 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[垂直扫描处理器] 缓存统计 - 命中: {_cacheHits}, 未命中: {_cacheMisses}, 命中率: {hitRate:F1}%, 缓存大小: {_candidateCache.Count}");
}
LogManager.Info($"[垂直扫描处理器] 并行扫描完成，耗时: {elapsed:F1}ms, 扫描点: {pointsList.Count}, 总区间: {totalIntervals}");
LogManager.Info($"[垂直扫描处理器] 并行扫描完成，耗时: {elapsed:F1}ms, 扫描点: {pointsList.Count}, 有效区间: {validIntervals}");
// 清理缓存，为下次扫描做准备
_candidateCache.Clear();
_cacheHits = 0;
_cacheMisses = 0;
return new Dictionary<Point3D, List<HeightInterval>>(results);
return new Dictionary<Point3D, HeightInterval>(results);
}
/// <summary>
@ -383,8 +383,8 @@ namespace NavisworksTransport.PathPlanning
/// <param name="basePoint">基础点（X,Y坐标）</param>
/// <param name="scanHeight">扫描高度</param>
/// <param name="vehicleHeight">车辆高度</param>
/// <returns>可通行的高度区间列表</returns>
public List<HeightInterval> ScanVerticalLine(Point3D basePoint, double scanHeight, double vehicleHeight)
/// <returns>可通行的高度区间</returns>
public HeightInterval ScanVerticalLine(Point3D basePoint, double scanHeight, double vehicleHeight)
{
// 第一级筛选：邻域筛选 - 获取空间哈希邻域内的候选项
var candidateItems = GetCandidateItemsFromSpatialHash(basePoint);
@ -396,9 +396,9 @@ namespace NavisworksTransport.PathPlanning
var intersectionResults = PerformIntersectionTests(heightFilteredItems, basePoint, scanHeight);
// 计算可通行区间
var passableIntervals = CalculatePassableIntervals(intersectionResults, basePoint.Z, scanHeight, vehicleHeight);
var passableInterval = CalculatePassableInterval(intersectionResults, basePoint.Z, scanHeight, vehicleHeight);
return passableIntervals;
return passableInterval;
}
@ -625,8 +625,8 @@ namespace NavisworksTransport.PathPlanning
/// <param name="baseZ">基础Z坐标</param>
/// <param name="scanHeight">扫描高度</param>
/// <param name="vehicleHeight">车辆高度</param>
/// <returns>可通行区间列表</returns>
private List<HeightInterval> CalculatePassableIntervals(
/// <returns>可通行区间</returns>
private HeightInterval CalculatePassableInterval(
List<IntersectionResult> intersectionResults,
double baseZ,
double scanHeight,
@ -664,8 +664,8 @@ namespace NavisworksTransport.PathPlanning
// 合并重叠的障碍物区间
var mergedObstacles = MergeOverlappingIntervals(obstacles);
// 计算可通行区间
var passableIntervals = new List<HeightInterval>();
// 计算最优可通行区间（取最大的一个）
HeightInterval bestInterval = new HeightInterval();
var scanRange = new HeightInterval(baseZ, baseZ + scanHeight);
// 如果没有找到地面，使用基础Z坐标作为默认地面
@ -674,7 +674,7 @@ namespace NavisworksTransport.PathPlanning
floors.Add(new HeightInterval(baseZ - 0.1, baseZ + 0.1));
}
// 对每个潜在的地面，计算其上方的可通行空间
// 对每个潜在的地面，计算其上方的可通行空间，取最大的
foreach (var floor in floors)
{
double floorTop = floor.MaxZ;
@ -694,18 +694,21 @@ namespace NavisworksTransport.PathPlanning
if (clearHeight >= MIN_PASSABLE_HEIGHT)
{
var interval = new HeightInterval(floorTop, availableTop);
passableIntervals.Add(interval);
if (interval.MaxZ - interval.MinZ > bestInterval.MaxZ - bestInterval.MinZ)
{
bestInterval = interval;
}
}
}
else
{
// 有障碍物，计算障碍物之间的可通行空间
// 有障碍物，计算障碍物之间的可通行空间，取最大的
double currentBottom = floorTop;
foreach (var obstacle in conflictingObstacles)
{
double obstacleBottom = Math.Max(obstacle.MinZ, currentBottom);
if (obstacleBottom > currentBottom)
{
double clearHeight = obstacleBottom - currentBottom;
@ -713,13 +716,16 @@ namespace NavisworksTransport.PathPlanning
if (clearHeight >= vehicleHeight)
{
var interval = new HeightInterval(currentBottom, obstacleBottom);
passableIntervals.Add(interval);
if (interval.MaxZ - interval.MinZ > bestInterval.MaxZ - bestInterval.MinZ)
{
bestInterval = interval;
}
}
}
currentBottom = Math.Max(currentBottom, obstacle.MaxZ);
}
// 检查最后一个障碍物之后的空间
if (currentBottom < availableTop)
{
@ -728,13 +734,16 @@ namespace NavisworksTransport.PathPlanning
if (clearHeight >= vehicleHeight)
{
var interval = new HeightInterval(currentBottom, availableTop);
passableIntervals.Add(interval);
if (interval.MaxZ - interval.MinZ > bestInterval.MaxZ - bestInterval.MinZ)
{
bestInterval = interval;
}
}
}
}
}
return passableIntervals;
return bestInterval;
}
#endregion