完善了安全优先和直线优先的算法，去掉了cell.WorldPosition，但高度层引起的问题还需要进一步完善
This commit is contained in:
parent
88712cc156
commit
163986f9e5
@ -126,10 +126,19 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 速度计算委托：根据当前节点、邻居节点和其他上下文信息计算边的速度
|
||||
/// </summary>
|
||||
private delegate float SpeedCalculator(
|
||||
int currentX, int currentY, int currentLayerIndex, double currentZ,
|
||||
int neighborX, int neighborY, int neighborLayerIndex, double neighborZ,
|
||||
double heightDiff, double baseSpeed, double maxHeightDiffInModelUnits,
|
||||
Point3D startPos, Point3D endPos);
|
||||
|
||||
/// <summary>
|
||||
/// 构建3D图（每个(x,y,layerIndex)作为独立节点）
|
||||
/// </summary>
|
||||
private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
|
||||
private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, SpeedCalculator speedCalculator = null)
|
||||
{
|
||||
LogManager.Info("[3D图构建] 开始构建真3D图");
|
||||
|
||||
@ -239,7 +248,18 @@ namespace NavisworksTransport.PathPlanning
|
||||
if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
|
||||
continue;
|
||||
|
||||
float speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
|
||||
// 使用策略特定的速度计算器，如果未提供则使用默认的高度差计算
|
||||
float speed;
|
||||
if (speedCalculator != null)
|
||||
{
|
||||
speed = speedCalculator(x, y, li, currentZ, nx, ny, nli, neighborLayer.Z,
|
||||
heightDiff, baseSpeed, maxHeightDiffInModelUnits, startPos, endPos);
|
||||
}
|
||||
else
|
||||
{
|
||||
speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
|
||||
}
|
||||
|
||||
maxSpeed = Math.Max(maxSpeed, speed);
|
||||
var velocity = Velocity.FromKilometersPerHour(speed);
|
||||
|
||||
@ -347,7 +367,73 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// </summary>
|
||||
private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
|
||||
{
|
||||
return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
|
||||
SpeedCalculator speedCalc = null;
|
||||
|
||||
switch (strategy)
|
||||
{
|
||||
case PathStrategy.Shortest:
|
||||
LogManager.Info($"[3D图构建] 使用最短路径策略");
|
||||
// 默认策略，speedCalc = null，使用高度差计算
|
||||
break;
|
||||
|
||||
case PathStrategy.Straightest:
|
||||
LogManager.Info($"[3D图构建] 使用直线优先策略");
|
||||
// 直线优先：计算沿边方向能够直走的距离，距离越长速度越高
|
||||
{
|
||||
speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
|
||||
{
|
||||
// 基础速度（根据高度差）
|
||||
float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
|
||||
|
||||
// 计算边的方向
|
||||
var direction = new GridPoint2D(nx - cx, ny - cy);
|
||||
var currentPos = new GridPoint2D(cx, cy);
|
||||
|
||||
// 计算沿该方向能够直走的距离
|
||||
int straightDistance = CalculateStraightDistance(currentPos, direction, gridMap, vehicleHeight);
|
||||
|
||||
// 直走距离越长，速度加成越高（最多+30 km/h）
|
||||
float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
|
||||
float finalSpeed = baseSpeedValue + bonusSpeed;
|
||||
|
||||
return finalSpeed;
|
||||
};
|
||||
}
|
||||
break;
|
||||
|
||||
case PathStrategy.SafestCenter:
|
||||
LogManager.Info($"[3D图构建] 使用安全优先策略");
|
||||
// 安全优先：计算安全距离图，基于到障碍物的距离调整速度
|
||||
{
|
||||
var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
|
||||
LogManager.Info($"[3D图构建] 安全距离图计算完成");
|
||||
|
||||
speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
|
||||
{
|
||||
// 基础速度（根据高度差）
|
||||
float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
|
||||
|
||||
// 获取两个节点的安全距离，取较大值（更安全的距离）
|
||||
int currentSafety = safetyDistanceMap[cx, cy];
|
||||
int neighborSafety = safetyDistanceMap[nx, ny];
|
||||
int maxSafety = Math.Max(currentSafety, neighborSafety);
|
||||
|
||||
// 根据安全距离计算速度系数
|
||||
float safetySpeed = CalculateSpeedBySafetyDistance(maxSafety);
|
||||
|
||||
// 返回基础速度和安全速度的加权组合
|
||||
// 高度差影响基础速度，安全距离影响额外速度
|
||||
return baseSpeedValue * 0.3f + safetySpeed * 0.7f;
|
||||
};
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
LogManager.Warning($"[3D图构建] 未知策略 {strategy}，使用默认最短路径");
|
||||
break;
|
||||
}
|
||||
|
||||
return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos, speedCalc);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -1329,11 +1415,15 @@ namespace NavisworksTransport.PathPlanning
|
||||
if (endCell.HasValue)
|
||||
{
|
||||
// 调试：检查门网格的Z坐标
|
||||
double endZ = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
|
||||
? endCell.Value.HeightLayers[0].Z
|
||||
: 0;
|
||||
|
||||
if (endCell.Value.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||||
{
|
||||
LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) WorldPosition.Z={endCell.Value.WorldPosition.Z:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
|
||||
LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].Z={endZ:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
|
||||
}
|
||||
endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endCell.Value.WorldPosition.Z);
|
||||
endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endZ);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@ -148,7 +148,9 @@ namespace NavisworksTransport.PathPlanning
|
||||
if (cell.IsWalkable)
|
||||
{
|
||||
walkableCount++;
|
||||
double cellZ = cell.WorldPosition.Z;
|
||||
double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0
|
||||
? cell.HeightLayers[0].Z
|
||||
: 0;
|
||||
|
||||
if (cellZ < minZ)
|
||||
minZ = cellZ;
|
||||
|
||||
@ -25,7 +25,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = channel,
|
||||
SpeedLimit = speedLimit,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
@ -49,7 +48,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.门,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = door,
|
||||
SpeedLimit = speedLimit,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
@ -71,7 +69,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.障碍物,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = obstacle,
|
||||
SpeedLimit = 0,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
@ -95,7 +92,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.电梯,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = elevator,
|
||||
SpeedLimit = speedLimit,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
@ -119,7 +115,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.楼梯,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = stairs,
|
||||
SpeedLimit = speedLimit,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
@ -140,7 +135,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown,
|
||||
IsInChannel = false,
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPosition,
|
||||
RelatedModelItem = null,
|
||||
SpeedLimit = 0,
|
||||
HeightLayers = new System.Collections.Generic.List<HeightLayer>(),
|
||||
|
||||
@ -164,8 +164,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型
|
||||
IsInChannel = false,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
ChannelType = ChannelType.Other,
|
||||
WorldPosition = worldPos
|
||||
ChannelType = ChannelType.Other
|
||||
};
|
||||
cell.Cost = cell.GetCost(); // 使用GetCost方法计算成本
|
||||
Cells[x, y] = cell;
|
||||
@ -200,16 +199,23 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将网格坐标转换为世界3D坐标（需要显式提供Z坐标）
|
||||
/// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标）
|
||||
/// </summary>
|
||||
/// <param name="gridPosition">网格坐标</param>
|
||||
/// <param name="z">Z坐标值</param>
|
||||
/// <returns>世界3D坐标点</returns>
|
||||
public Point3D GridToWorld3D(GridPoint2D gridPosition)
|
||||
{
|
||||
var world2D = GridToWorld2D(gridPosition);
|
||||
var cell = Cells[gridPosition.X, gridPosition.Y];
|
||||
return new Point3D(world2D.X, world2D.Y, cell.WorldPosition.Z);
|
||||
|
||||
// 使用最低层的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);
|
||||
}
|
||||
|
||||
|
||||
@ -259,7 +265,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道,
|
||||
HeightLayers = new List<HeightLayer>(),
|
||||
ChannelType = cellType == CategoryAttributeManager.LogisticsElementType.通道 ? ChannelType.Corridor : ChannelType.Other,
|
||||
WorldPosition = worldPos,
|
||||
SpeedLimit = speedLimit
|
||||
};
|
||||
|
||||
@ -693,10 +698,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// </summary>
|
||||
public ChannelType ChannelType { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 世界坐标位置 - 用于高度计算和空间查询
|
||||
/// </summary>
|
||||
public Point3D WorldPosition { get; set; }
|
||||
/// <summary>
|
||||
/// 限速（米/秒），0表示未设置限速
|
||||
/// </summary>
|
||||
|
||||
@ -520,7 +520,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
WorldPosition = point,
|
||||
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
|
||||
SpeedLimit = cell.SpeedLimit, // 保持原有限速
|
||||
HeightLayers = cell.HeightLayers, // 保持高度层
|
||||
@ -542,7 +541,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
CellType = CategoryAttributeManager.LogisticsElementType.通道,
|
||||
IsInChannel = true,
|
||||
ChannelType = ChannelType.Corridor,
|
||||
WorldPosition = point,
|
||||
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
|
||||
SpeedLimit = cell.SpeedLimit, // 保持原有限速
|
||||
HeightLayers = cell.HeightLayers, // 保持高度层
|
||||
@ -560,9 +558,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
// 非通道单元格：不应该通过垂直扫描改变其状态
|
||||
// 这种情况理论上不应该发生，因为只对通道单元格进行扫描
|
||||
LogManager.Warning($"[通道2.5D模式] 警告：非通道单元格 ({gridX},{gridY}) 收到高度数据，类型：{cell.CellType}, IsInChannel：{cell.IsInChannel}");
|
||||
|
||||
// 仍然更新世界位置，但保持原有状态
|
||||
cell.WorldPosition = point;
|
||||
|
||||
// 多层架构：高度区间在HeightLayer中设置
|
||||
// cell.PassableHeight = interval;
|
||||
gridMap.Cells[gridX, gridY] = cell;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user