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

完善了网格地图的z坐标设置，改进了路径优化步骤，路径可以在台阶处保留2个路径点，更好贴合通道表面

Browse Source
This commit is contained in:
tian 2025-09-15 00:29:12 +08:00
parent 8946873e32
commit 810f874a50
7 changed files with 160 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/Core/Collision/ClashDetectiveIntegration.cs
View File

@ -626,6 +626,14 @@ namespace NavisworksTransport
LogManager.Info($"=== 统计分离完成 ===");
LogManager.Info("=== 动画碰撞测试（分组方案）完成 ===");
// 触发碰撞检测完成事件，通知生成报告
if (validCollisions.Count > 0)
{
LogManager.Info($"触发CollisionDetected事件，通知生成报告（{validCollisions.Count}个碰撞）");
var eventArgs = new CollisionDetectedEventArgs(validCollisions);
OnCollisionDetected(eventArgs);
}
}
}
catch (Exception ex)

36
src/PathPlanning/AutoPathFinder.cs
View File

@ -704,8 +704,17 @@ namespace NavisworksTransport.PathPlanning
var endCell = gridMap.GetCell(endGridPos);
if (endCell.HasValue)
{
// 调试：检查门网格的Z坐标
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})");
}
endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endCell.Value.WorldPosition.Z);
}
else
{
LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，Z坐标保持为0");
}
// 检查是否与上一个点重复（A*可能在转弯点有重复）
bool isDuplicate = worldPath.Count > 0 &&
@ -751,10 +760,27 @@ namespace NavisworksTransport.PathPlanning
{
var startPoint = optimizedPath[currentIndex];
var endPoint = optimizedPath[testIndex];
var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
Math.Pow(endPoint.Y - startPoint.Y, 2));
if (IsDirectPathClear(startPoint, endPoint, gridMap))
// 🔥 新增：在尝试连接远点之前，先检查是否会跳过高度变化点
bool canSkip = true;
const double heightTolerance = 1e-6; // 与PathOptimizer保持一致
for (int checkIdx = currentIndex + 1; checkIdx < testIndex; checkIdx++)
{
// 检查被跳过的点是否有高度变化
if (Math.Abs(optimizedPath[checkIdx].Z - optimizedPath[checkIdx-1].Z) > heightTolerance ||
(checkIdx + 1 < optimizedPath.Count &&
Math.Abs(optimizedPath[checkIdx+1].Z - optimizedPath[checkIdx].Z) > heightTolerance))
{
canSkip = false;
LogManager.Debug($"[斜线优化] 不能跳过点{checkIdx}，存在高度变化");
break;
}
}
if (canSkip && IsDirectPathClear(startPoint, endPoint, gridMap))
{
// 可以直线连接，更新最远索引
farthestIndex = testIndex;
@ -1367,9 +1393,9 @@ namespace NavisworksTransport.PathPlanning
if (validIntervals.Any())
{
// 选择最低的有效区间的底部高度
// 选择最低的有效区间的底部高度，转换为绝对坐标
var bestInterval = validIntervals.First();
return bestInterval.MinZ;
return cell.WorldPosition.Z + bestInterval.MinZ;
}
}

12
src/PathPlanning/ChannelBasedGridBuilder.cs
View File

@ -221,6 +221,9 @@ namespace NavisworksTransport.PathPlanning
var minGrid = gridMap.WorldToGrid(new Point3D(minX, minY, 0));
var maxGrid = gridMap.WorldToGrid(new Point3D(maxX, maxY, 0));
// 计算三角形的平均高度作为通道顶面高度
double triangleHeight = (triangle.Point1.Z + triangle.Point2.Z + triangle.Point3.Z) / 3.0;
// 遍历边界框内的所有网格点
for (int x = minGrid.X; x <= maxGrid.X; x++)
{
@ -234,7 +237,8 @@ namespace NavisworksTransport.PathPlanning
// 检查点是否在三角形内部
if (GeometryHelper.IsPointInTriangle2D(worldPos, triangle))
{
SetCellAsChannel(gridMap, gridPos, null);
// 传递实际的三角形高度
SetCellAsChannel(gridMap, gridPos, null, triangleHeight);
}
}
}
@ -248,10 +252,10 @@ namespace NavisworksTransport.PathPlanning
/// <param name="gridMap">网格地图</param>
/// <param name="gridPos">网格位置</param>
/// <param name="relatedItem">关联的模型物品</param>
private void SetCellAsChannel(GridMap gridMap, GridPoint2D gridPos, ModelItem relatedItem)
private void SetCellAsChannel(GridMap gridMap, GridPoint2D gridPos, ModelItem relatedItem, double channelHeight = 0.0)
{
// 使用GridMap提供的方法设置通道单元格
gridMap.SetCellAsChannel(gridPos, ChannelType.Corridor, relatedItem);
// 使用GridMap提供的方法设置通道单元格，传递实际高度
gridMap.SetCellAsChannel(gridPos, ChannelType.Corridor, relatedItem, channelHeight);
}

78
src/PathPlanning/GridMap.cs
View File

@ -153,8 +153,10 @@ namespace NavisworksTransport.PathPlanning
{
for (int y = 0; y < Height; y++)
{
// 只计算2D世界坐标，Z坐标将在有规划点信息后单独设置
var worldPos = GridToWorld2D(new GridPoint2D(x, y)); // 直接使用返回的Point3D，Z=0
// 计算2D世界坐标，并设置合理的初始Z坐标（使用地面高度而不是0）
var worldPos2D = GridToWorld2D(new GridPoint2D(x, y));
// 使用边界框最小Z值（地面高度）作为Unknown网格的初始高度
var worldPos = new Point3D(worldPos2D.X, worldPos2D.Y, Bounds.Min.Z);
var cell = new GridCell
{
@ -209,68 +211,7 @@ namespace NavisworksTransport.PathPlanning
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>
/// 检查网格坐标是否在有效范围内
@ -342,13 +283,18 @@ namespace NavisworksTransport.PathPlanning
/// <param name="gridPosition">网格坐标</param>
/// <param name="channelType">通道类型</param>
/// <param name="relatedItem">关联的模型物品</param>
public void SetCellAsChannel(GridPoint2D gridPosition, ChannelType channelType = ChannelType.Corridor, ModelItem relatedItem = null)
public void SetCellAsChannel(GridPoint2D gridPosition, ChannelType channelType = ChannelType.Corridor, ModelItem relatedItem = null, double channelHeight = 0.0)
{
if (!IsValidGridPosition(gridPosition))
return;
var existingZ = Cells[gridPosition.X, gridPosition.Y].WorldPosition.Z;
var worldPos = GridToWorld3D(gridPosition, existingZ);
// 使用传入的通道高度，如果没有提供则使用现有的Z坐标或边界最大Z
double actualZ = channelHeight > 0 ? channelHeight :
(Cells[gridPosition.X, gridPosition.Y].WorldPosition.Z > 0 ?
Cells[gridPosition.X, gridPosition.Y].WorldPosition.Z :
Bounds.Max.Z);
var worldPos = GridToWorld3D(gridPosition, actualZ);
var cell = new GridCell
{
IsWalkable = true,

34
src/PathPlanning/GridMapGenerator.cs
View File

@ -174,8 +174,7 @@ namespace NavisworksTransport.PathPlanning
channelCoverage.GridMap.PlanningEndPoint = planningEndPoint;
channelCoverage.GridMap.HasPlanningPoints = true;
// 9. 更新所有网格单元格的Z坐标（现在有了规划点信息）
channelCoverage.GridMap.UpdateCellWorldPositionZ();
// 注意：网格的Z坐标已在通道生成时设置为实际通道高度，无需重新计算
}
// 🔥 关键修复：将通道数据设置到GridMap中，供后续PathPlanningManager使用
@ -270,8 +269,7 @@ namespace NavisworksTransport.PathPlanning
channelCoverage.GridMap.PlanningEndPoint = planningEndPoint;
channelCoverage.GridMap.HasPlanningPoints = true;
// 5. 更新所有网格单元格的Z坐标（现在有了规划点信息）
channelCoverage.GridMap.UpdateCellWorldPositionZ();
// 注意：网格的Z坐标已在通道生成时设置为实际通道高度，无需重新计算
}
// 将通道数据设置到GridMap中，供后续PathPlanningManager使用
@ -358,12 +356,34 @@ namespace NavisworksTransport.PathPlanning
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 限高: {doorHeight:F2} 模型单位");
}
// 设置开口区域的网格为可通行的门类型，使用正确的高度
// 获取门底部的Z坐标
double doorBottomZ = bbox.Min.Z;
// 设置开口区域的网格为可通行的门类型
foreach (var (x, y) in coveredCells)
{
var gridPos = new GridPoint2D(x, y);
// 使用正确的门高度设置网格
gridMap.SetCell(gridPos, true, doorHeight, CategoryAttributeManager.LogisticsElementType.门);
// 设置门网格，cost传0让SetCell自动调用GetCost()计算（门的默认cost是1.2）
gridMap.SetCell(gridPos, true, 0, CategoryAttributeManager.LogisticsElementType.门);
// 由于GridCell是结构体，需要重新获取、修改、再设置回去
var cell = gridMap.Cells[x, y];
// 更新门网格的Z坐标为门底部位置
var worldPos = gridMap.GridToWorld3D(gridPos, doorBottomZ);
cell.WorldPosition = worldPos;
// 将门的高度范围存储在PassableHeights中供后续使用
if (doorHeight > 0)
{
cell.PassableHeights = new List<HeightInterval>
{
new HeightInterval(0, doorHeight)
};
}
// 将修改后的结构体设置回数组
gridMap.Cells[x, y] = cell;
}
processedCount++;

71
src/PathPlanning/PathOptimizer.cs
View File

@ -174,24 +174,36 @@ namespace NavisworksTransport.PathPlanning
initialDy == 0 ? 0 : Math.Sign(initialDy)
);
// 简化路径：只保留转弯点（使用归一化方向）
// 简化路径：只保留转弯点和高度变化点（使用归一化方向）
for (int i = 2; i < dedupedGridPath.Count; i++)
{
// 计算当前线段的位移
int dx = dedupedGridPath[i].X - dedupedGridPath[i-1].X;
int dy = dedupedGridPath[i].Y - dedupedGridPath[i-1].Y;
// 归一化为单位方向向量 (-1, 0, 1)
var currentDirection = new GridPoint2D(
dx == 0 ? 0 : Math.Sign(dx),
dy == 0 ? 0 : Math.Sign(dy)
);
// 只有真正的方向改变时才保留转弯点
if (!currentDirection.Equals(prevDirection))
// 检查高度变化
bool hasHeightChange = HasHeightChange(
dedupedPoints[i-2],
dedupedPoints[i-1],
dedupedPoints[i]
);
// 只有真正的方向改变或高度变化时才保留转折点
if (!currentDirection.Equals(prevDirection) || hasHeightChange)
{
simplified.Add(dedupedGridPath[i - 1]);
prevDirection = currentDirection;
if (hasHeightChange)
{
LogManager.Debug($"[网格路径优化] 保留高度变化点 {i-1}");
}
}
}
@ -257,6 +269,51 @@ namespace NavisworksTransport.PathPlanning
return simplified;
}
/// <summary>
/// 检查两个路径点之间是否有显著的高度变化
/// 用于路径优化时判断是否需要保留中间点
/// </summary>
/// <param name="point1">第一个路径点</param>
/// <param name="point2">第二个路径点</param>
/// <returns>如果有高度变化返回true，需要保留中间点</returns>
private bool HasHeightChange(PathPoint point1, PathPoint point2)
{
const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
return Math.Abs(point1.Position.Z - point2.Position.Z) > heightTolerance;
}
/// <summary>
/// 检查三个点之间是否有高度变化（用于判断是否可以跳过中间点）
/// </summary>
/// <param name="p1">第一个点的3D坐标</param>
/// <param name="p2">中间点的3D坐标</param>
/// <param name="p3">第三个点的3D坐标</param>
/// <returns>如果有高度变化返回true，不能跳过中间点</returns>
private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3)
{
const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
// 检查任意两点间是否有高度变化
if (Math.Abs(p1.Z - p2.Z) > heightTolerance ||
Math.Abs(p2.Z - p3.Z) > heightTolerance)
{
return true;
}
return false;
}
/// <summary>
/// 检查三个点之间是否有高度变化（PathPoint版本）
/// </summary>
/// <param name="point1">第一个路径点</param>
/// <param name="point2">中间路径点</param>
/// <param name="point3">第三个路径点</param>
/// <returns>如果有高度变化返回true，不能跳过中间点</returns>
private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3)
{
return HasHeightChange(point1.Position, point2.Position, point3.Position);
}
/// <summary>
/// 检查三点是否真正共线（只有当三点都在同一条直线上且方向一致时才返回true）
/// 🔥 关键修复：严格检查两个线段的方向是否一致，防止在转折点处错误合并
@ -269,8 +326,8 @@ namespace NavisworksTransport.PathPlanning
{
const double tolerance = 0.1; // 容差值，单位：模型单位
// 🔥 修复：首先检查Z坐标，确保是2D路径
if (Math.Abs(p1.Z - p2.Z) > tolerance || Math.Abs(p2.Z - p3.Z) > tolerance)
// 🔥 修复：使用统一的高度检查函数
if (HasHeightChange(p1, p2, p3))
{
LogManager.Debug($"[共线检测] ❌ 拒绝Z方向变化：({p1.X:F3},{p1.Y:F3},{p1.Z:F3}) -> ({p2.X:F3},{p2.Y:F3},{p2.Z:F3}) -> ({p3.X:F3},{p3.Y:F3},{p3.Z:F3})");
return false;

13
src/UI/WPF/ViewModels/AnimationControlViewModel.cs
View File

@ -430,8 +430,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels
// 初始化命令
InitializeCommands();
// 订阅碰撞检测事件
_clashIntegration.CollisionDetected += OnCollisionDetected;
// 移除重复的碰撞检测事件订阅，只在带参构造函数中保留
// _clashIntegration.CollisionDetected += OnCollisionDetected;
LogManager.Info("AnimationControlViewModel初始化完成（含缓存管理）");
}
@ -1039,11 +1039,18 @@ namespace NavisworksTransport.UI.WPF.ViewModels
await _uiStateManager.ExecuteUIUpdateAsync(() =>
{
HasCollisionResults = e.Results.Count > 0;
var summary = e.Results.Count > 0
var summary = e.Results.Count > 0
? $"发现 {e.Results.Count} 个碰撞点"
: "未发现碰撞";
UpdateMainStatus(summary);
});
// 自动生成并显示碰撞报告
if (HasCollisionResults)
{
LogManager.Info("碰撞检测完成，自动生成报告");
await ExecuteViewCollisionReportAsync();
}
}
/// <summary>