From fdb857af17d05532d8bc92230216ef1d4409f437 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Mon, 13 Apr 2026 10:31:14 +0800 Subject: [PATCH] Refactor auto path planning coordinate semantics --- NavisworksTransport.UnitTests.csproj | 1 + TransportPlugin.csproj | 1 + ...utoPathPlanningCoordinateSemanticsTests.cs | 114 ++++++ ...-path-planning-coordinate-refactor-plan.md | 338 ++++++++++++++++++ src/PathPlanning/AutoPathFinder.cs | 145 ++++---- src/PathPlanning/GridMap.cs | 86 ++++- src/PathPlanning/PathOptimizer.cs | 74 ++-- .../CoordinateSystem/HostPlanarGridHelper.cs | 99 +++++ .../CoordinateSystem/YUpCoordinateSystem.cs | 23 +- .../CoordinateSystem/ZUpCoordinateSystem.cs | 23 +- 10 files changed, 797 insertions(+), 107 deletions(-) create mode 100644 UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs create mode 100644 doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md create mode 100644 src/Utils/CoordinateSystem/HostPlanarGridHelper.cs diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index 113b76a..8962550 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -60,6 +60,7 @@ + diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj index 6cf663a..4b86f88 100644 --- a/TransportPlugin.csproj +++ b/TransportPlugin.csproj @@ -359,6 +359,7 @@ + diff --git a/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs b/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs new file mode 100644 index 0000000..d6e1382 --- /dev/null +++ b/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs @@ -0,0 +1,114 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Utils.CoordinateSystem; +using System.Numerics; + +namespace NavisworksTransport.UnitTests.CoordinateSystem +{ + [TestClass] + public class AutoPathPlanningCoordinateSemanticsTests + { + [TestMethod] + public void YUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostY() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + + Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 14.83, adapter); + + AssertPoint(point, 2.0, 14.83, 3.0); + } + + [TestMethod] + public void ZUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostZ() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp); + + Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 6.25, adapter); + + AssertPoint(point, 2.0, 3.0, 6.25); + } + + [TestMethod] + public void YUp_HostPlanarGridHelper_GetAndSetElevation_ShouldUseHostY() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var hostPoint = new Vector3(4.0f, 15.0f, 6.0f); + + double elevation = HostPlanarGridHelper.GetElevation3(hostPoint, adapter); + Vector3 adjusted = HostPlanarGridHelper.SetElevation3(hostPoint, 20.0, adapter); + + Assert.AreEqual(15.0, elevation, 1e-6); + AssertPoint(adjusted, 4.0, 20.0, 6.0); + } + + [TestMethod] + public void YUp_HostPlanarGridHelper_GetHorizontalCoords_ShouldProjectToHostXZPlane() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var hostPoint = new Vector3(8.0f, 14.83f, -5.35f); + + (double h1, double h2) = HostPlanarGridHelper.GetHorizontalCoords3(hostPoint, adapter); + + Assert.AreEqual(8.0, h1, 1e-6); + Assert.AreEqual(-5.35, h2, 1e-6); + } + + [TestMethod] + public void YUp_HostPlanarGridHelper_HorizontalRange_ShouldMatchHostXZPlane() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var hostMin = new Vector3(-10.0f, 2.0f, -30.0f); + var hostMax = new Vector3(40.0f, 12.0f, 5.0f); + + (double min1, double max1, double min2, double max2) = + HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + + Assert.AreEqual(-10.0, min1, 1e-6); + Assert.AreEqual(40.0, max1, 1e-6); + Assert.AreEqual(-30.0, min2, 1e-6); + Assert.AreEqual(5.0, max2, 1e-6); + } + + [TestMethod] + public void YUp_GridLikeWorldPointConstruction_ShouldWriteElevationToHostY() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + double originH1 = -210.0; + double originH2 = -10.0; + double cellSize = 1.0; + + Vector3 worldPoint = HostPlanarGridHelper.CreateHostPoint3( + originH1 + 4 * cellSize, + originH2 + 6 * cellSize, + 14.83, + adapter); + + Assert.AreEqual(-206.0, worldPoint.X, 1e-6); + Assert.AreEqual(14.83, worldPoint.Y, 1e-6); + Assert.AreEqual(-4.0, worldPoint.Z, 1e-6); + } + + [TestMethod] + public void YUp_GridLikeWorldToGrid_ShouldReadHostXZAsPlanarAxes() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var origin = new Vector3(-210.0f, 14.83f, -10.0f); + var endPoint = new Vector3(-169.20f, 14.83f, -5.35f); + const double cellSize = 1.0; + + (double pointH1, double pointH2) = HostPlanarGridHelper.GetHorizontalCoords3(endPoint, adapter); + (double originH1, double originH2) = HostPlanarGridHelper.GetHorizontalCoords3(origin, adapter); + int gridX = (int)System.Math.Round((pointH1 - originH1) / cellSize); + int gridY = (int)System.Math.Round((pointH2 - originH2) / cellSize); + + Assert.AreEqual(41, gridX); + Assert.AreEqual(5, gridY); + } + + private static void AssertPoint(Vector3 actual, double x, double y, double z) + { + Assert.AreEqual(x, actual.X, 1e-6); + Assert.AreEqual(y, actual.Y, 1e-6); + Assert.AreEqual(z, actual.Z, 1e-6); + } + } +} diff --git a/doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md b/doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md new file mode 100644 index 0000000..1e24334 --- /dev/null +++ b/doc/working/2026-04-13-auto-path-planning-coordinate-refactor-plan.md @@ -0,0 +1,338 @@ +# 自动路径规划坐标系重构实施方案 + +更新时间：2026-04-13 +状态：实施中 +目标：将 `Ground` 自动路径规划整条链路并入当前稳定的“宿主坐标系 -> Canonical Space -> 宿主坐标系”架构，消除旧 `XY` 平面 / `Z` 高度硬编码，不再针对单一 `YUp` 现象打补丁。 + +--- + +## 1. 背景与问题定义 + +当前项目的坐标系架构已经明确： + +- 宿主坐标系（Host Space） + - Navisworks 文档坐标系 + - UI 输入输出、鼠标拾取、日志、渲染交互都属于这一层 +- 内部坐标系（Canonical Space） + - 项目内部统一固定为 `ZUp` + - 几何、姿态、法向、路径计算优先在这一层完成 +- 业务语义层 + - 在 canonical 之上表达地面接触点、吊点、轨道法向、动画跟踪点等业务含义 + +但自动路径规划链路目前仍大量保留旧实现习惯： + +- 默认 `XY` 是水平平面 +- 默认 `Z` 是高度轴 +- 默认世界 `Z` 可以直接代表“向上” +- 路径起终点修正仍然是“保留 `XY`，替换 `Z`” + +这说明当前问题不是“某个 `YUp` 细节没处理”，而是： + +- 自动路径规划整条链没有接入当前稳定的坐标系架构 + +因此本方案的目标不是修一个 `YUp` bug，而是把自动路径规划整体收编到现有坐标系体系中。 + +--- + +## 2. 现象与直接证据 + +以 `YUp` 文档中的地面自动路径为例，日志显示： + +- 起点：`(-209.94, 14.83, -1.68)` +- 终点：`(-169.20, 14.83, -5.35)` + +对当前文档来说，宿主 `up` 是 `Y`，所以： + +- 地面高度应主要体现在 `Y` +- `Z` 应属于宿主水平面的一部分 + +但自动路径规划链中仍有如下旧假设： + +1. `PathPlanningManager` 按 `X/Y` 计算边界与网格尺寸 +- `src/Core/PathPlanningManager.cs` +- `GetModelBounds()` +- `CalculateOptimalGridSize(...)` + +2. `AutoPathFinder` 把图节点还原为： +- `new Point3D(worldPos.X, worldPos.Y, nodeInfo.Z)` + +3. `AutoPathFinder` 的起终点修正逻辑仍是： +- 保留原始 `X/Y` +- 只替换 `Z` + +这套实现天然绑定旧 `ZUp` 语义，无法正确支持当前多坐标系架构。 + +--- + +## 3. 重构目标 + +本次重构要达成的不是“YUp 也能跑”，而是以下更高层目标： + +1. 自动路径规划输入输出继续保持宿主坐标语义 +- 用户点击点、UI 日志、路径显示、数据库路径点仍然按宿主坐标解释 + +2. 自动路径规划内部计算统一切到 canonical +- 网格范围 +- 栅格映射 +- A* 路径求解 +- 高度层处理 +- 曲线化前的路径点 + +3. Ground 自动路径与通行空间、路径渲染、动画主链共享同一套 up / 高度语义 + +4. 禁止继续保留旧 `XY/Z` 写死逻辑作为 fallback + +--- + +## 4. 范围界定 + +### 4.1 本次重构范围 + +- `PathEditingViewModel` + - 自动路径起点/终点输入边界 +- `PathPlanningManager` + - 自动路径入口、边界、网格参数、路径落库前收口 +- `GridMapGenerator` + - BIM 到网格的平面/高度语义 +- `AutoPathFinder` + - 路径搜索、节点转世界坐标、首尾点修正 +- 与自动路径规划直接耦合的测试 + +### 4.2 暂不纳入本次范围 + +- 手动路径编辑主链 +- 吊装路径自动生成链 +- Rail 自动生成链 +- 动画姿态链 +- 碰撞恢复链 + +说明： + +- 本次先聚焦 `Ground` 自动路径规划 +- 但设计必须是通用的宿主/内部坐标边界，不允许做成“只为 `YUp` 单独补丁” + +--- + +## 5. 关键设计原则 + +### 5.1 边界清晰 + +- 宿主坐标只在输入输出边界存在 +- 内部计算统一使用 canonical +- 不允许在算法中间层直接假设世界 `Z` 是高度 + +### 5.2 不新增 fallback + +不允许： + +- canonical 转换失败时回退旧 `XY/Z` 逻辑 +- `YUp` 失败时偷偷走旧分支 +- 用“如果是 YUp 就特判”掩盖整体架构问题 + +### 5.3 优先复用现有坐标工具 + +优先复用： + +- `HostCoordinateAdapter` +- `CanonicalTrackedPositionResolver` +- 坐标系统一设计文档中已经稳定的术语和边界 + +### 5.4 先锁测试，再动实现 + +本次重构必须先补针对多坐标系的自动路径测试，再改实现。 + +--- + +## 6. 目标架构 + +目标链路应收敛为： + +```text +宿主点击点 / 宿主通道边界 / 宿主模型几何 + ↓ HostCoordinateAdapter +Canonical 起点 / 终点 / 边界 / 网格 / 高度层 + ↓ 自动路径规划算法（统一在 canonical 内） +Canonical 路径点 + ↓ HostCoordinateAdapter +宿主路径点 / 渲染 / UI / 数据库存储 +``` + +关键语义： + +- canonical 内部固定： + - `XY` 为水平平面 + - `Z` 为高度 +- 宿主坐标系的 `YUp/ZUp` 差异只由 `HostCoordinateAdapter` 负责吸收 + +--- + +## 7. 分阶段实施计划 + +### Phase 0：现状锁定与测试补齐 + +目标： + +- 先把当前正确/错误行为锁定，避免重构中语义漂移 + +任务： + +- [ ] 新增 `Ground` 自动路径在 `ZUp` 文档下的首尾点测试 +- [ ] 新增 `Ground` 自动路径在 `YUp` 文档下的首尾点测试 +- [ ] 新增“宿主点 -> 自动路径 -> 路径点输出”边界测试 +- [ ] 新增“路径点 / 路径线 / 通行空间”首尾地面一致性测试 + +验收标准： + +- 在没有改实现前，测试至少能明确暴露 `YUp` 下当前错误 +- `ZUp` 当前稳定行为被锁住，不允许回归 + +### Phase 1：输入边界 canonical 化 + +目标： + +- 自动路径规划入口不再把宿主点直接扔进旧 `XY/Z` 规划逻辑 + +任务： + +- [ ] 梳理 `PathEditingViewModel` 起点/终点点击数据的宿主语义 +- [ ] 在自动路径规划入口显式引入 `HostCoordinateAdapter` +- [ ] 将 `AutoPlanPath(...)` 的内部输入切换为 canonical 起点/终点 +- [ ] 明确日志里同时打印宿主输入点与 canonical 输入点 + +验收标准： + +- 自动路径规划入口不再直接依赖宿主 `YUp/ZUp` +- 起点终点进入算法前，坐标语义清晰可见 + +### Phase 2：网格与边界 canonical 化 + +目标： + +- 网格边界、障碍物、通道数据全部按 canonical 平面/高度工作 + +任务： + +- [ ] 重构 `GetModelBounds()` / `CalculateChannelsBounds(...)` +- [ ] 重构 `CalculateOptimalGridSize(...)`，不再把 `X/Y` 固定当平面 +- [ ] 重构 `GridMapGenerator.GenerateFromBIM(...)` 的输入语义 +- [ ] 明确网格二维平面坐标与 canonical 三维坐标的边界 + +验收标准： + +- 网格生成不再依赖宿主世界 `XY` +- `YUp` / `ZUp` 两类文档进入网格阶段后语义完全一致 + +### Phase 3：路径求解与输出 canonical 化 + +目标： + +- `AutoPathFinder` 内部只处理 canonical +- 路径输出统一经适配器返回宿主 + +任务： + +- [ ] 重构图节点到三维点的转换逻辑 +- [ ] 删除或重写“保留 XY、替换 Z”的旧首尾点修正 +- [ ] 重构路径输出与落库前的宿主还原逻辑 +- [ ] 日志里同时打印 canonical 路径首尾点与宿主路径首尾点 + +验收标准： + +- 路径首尾点在 `YUp/ZUp` 下都能正确贴回宿主地面 +- 不再出现“终点被压到地面下方”的旧 `ZUp` 语义残留 + +### Phase 4：联动校验与收尾 + +目标： + +- 自动路径规划与可视化、通行空间、动画链的基础语义重新对齐 + +任务： + +- [ ] 检查路径点显示是否仍使用宿主坐标 +- [ ] 检查路径线和通行空间首尾点是否与路径点一致 +- [ ] 清理旧 `XY/Z` 注释、旧日志、旧辅助方法 +- [ ] 更新 `current-engineering-state.md` 中自动路径规划状态说明 + +验收标准： + +- 自动路径规划的起点、终点、路径线、通行空间在多坐标系文档下语义一致 +- 旧 `XY/Z` 硬编码实现不再残留为正式执行链 + +--- + +## 8. 关键改动点清单 + +高风险文件： + +- `src/UI/WPF/ViewModels/PathEditingViewModel.cs` +- `src/Core/PathPlanningManager.cs` +- `src/PathPlanning/GridMapGenerator.cs` +- `src/PathPlanning/AutoPathFinder.cs` + +重点清理的旧逻辑： + +- [ ] “默认 `XY` 为地面平面”的边界计算 +- [ ] “默认 `Z` 为高度”的路径节点组装 +- [ ] “保留 `XY`，替换 `Z`”的起终点修正 +- [ ] 任何直接写死世界 `Z` 为 up 的自动路径规划实现 + +--- + +## 9. 测试与验证策略 + +### 9.1 单元/集成测试 + +至少补齐： + +- [ ] `ZUp Ground` 自动路径首尾点测试 +- [ ] `YUp Ground` 自动路径首尾点测试 +- [ ] 起点/终点保持宿主地面接触语义测试 +- [ ] 路径输出后首尾点与通行空间一致性测试 + +### 9.2 手工验证 + +在真实 Navisworks 场景中验证： + +- [ ] `ZUp` 模型中自动地面路径仍正常显示 +- [ ] `YUp` 模型中自动地面路径终点不再掉到地面下 +- [ ] 默认显示模式下能看到起点、终点和路径线 +- [ ] 打开通行空间后，首尾位置与路径点一致 + +--- + +## 10. 风险与注意事项 + +1. 自动路径规划链路历史较老，旧实现可能分散在多个辅助方法中 +- 不能只改主入口，必须顺着网格、A*、输出一路清理 + +2. 数据库存储仍然是宿主坐标语义 +- 不能把 canonical 点直接落库 + +3. 路径渲染和自动路径规划不能各自维护一套“高度轴”解释 +- 否则会再次出现“路径点对、通行空间错”或反过来的问题 + +4. 本次重构不是 `Ground` 的最后一步 +- 后续 `Hoisting/Rail` 若要做自动路径，也应直接复用这次收好的边界层 + +--- + +## 11. 进度记录 + +### 2026-04-13 + +- [x] 确认当前自动路径规划主链仍大量保留旧 `XY/Z` 假设 +- [x] 明确本次方向应为“并入现有坐标系架构”，而不是追加 `YUp` 特判 +- [x] 建立本实施方案文档 +- [x] 引入 `HostPlanarGridHelper`，明确“旧 ICoordinateSystem 水平/高程契约”与 `HostCoordinateAdapter` 的桥接边界 +- [x] `YUpCoordinateSystem` / `ZUpCoordinateSystem` 改为复用统一桥接 helper，而不是各自散落实现 +- [x] 为宿主平面/高程桥接补充纯数学单测，锁定： + - `YUp` 高程走宿主 `Y` + - `YUp` 水平面走宿主 `XZ` + - `ZUp` 仍保持宿主 `XY + Z` +- [x] `AutoPathFinder` 第一轮收口完成： + - 起终点映射改为通过 `gridMap.GetWorldElevation(...)` + - 图节点 / 路径点还原改为通过 `gridMap.CreateWorldPoint(...)` + - 高度层匹配与路径恢复改为通过 `gridMap.GetWorldElevation(...) / SetWorldElevation(...)` +- [ ] Phase 0 剩余：补“自动路径首尾点”更贴近业务的回归测试 +- [ ] Phase 2 尚未开始：`GridMapGenerator` 仍存在较多旧 `XY/Z` 硬编码 diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index 415c0dc..addaedd 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -495,8 +495,11 @@ namespace NavisworksTransport.PathPlanning var endGridPos = gridMap.WorldToGrid(end); // 找到起点和终点最近的3D节点 - var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z); - var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z); + double startElevation = gridMap.GetWorldElevation(start); + double endElevation = gridMap.GetWorldElevation(end); + + var startNode = FindClosest3DNode(graph3D, startGridPos, startElevation); + var endNode = FindClosest3DNode(graph3D, endGridPos, endElevation); // 起点和终点必须检查可达性 bool startOnObstacle = (startNode == null); @@ -524,8 +527,7 @@ namespace NavisworksTransport.PathPlanning foreach (var node in graph3D.AllNodes) { var info = graph3D.NodeInfo[node]; - var nodeWorldPos = gridMap.GridToWorld3D(new GridPoint2D(info.X, info.Y)); - nodeWorldPos = new Point3D(nodeWorldPos.X, nodeWorldPos.Y, info.Z); + var nodeWorldPos = gridMap.CreateWorldPoint(new GridPoint2D(info.X, info.Y), info.Z); double distance = Math.Sqrt( Math.Pow(nodeWorldPos.X - end.X, 2) + @@ -554,8 +556,8 @@ namespace NavisworksTransport.PathPlanning var startInfo = graph3D.NodeInfo[startNode]; var endInfo = graph3D.NodeInfo[endNode]; - LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},Z={start.Z:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},Z={startInfo.Z:F2})"); - LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},Z={end.Z:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},Z={endInfo.Z:F2})"); + LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},高程={startElevation:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},高程={startInfo.Z:F2})"); + LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},高程={endElevation:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},高程={endInfo.Z:F2})"); // 执行A* var pathfinder = new Roy_T.AStar.Paths.PathFinder(); @@ -699,7 +701,7 @@ namespace NavisworksTransport.PathPlanning path3D.Add(originalStart); int pointIndex = 0; - LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, Z={originalStart.Z:F2})"); + LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, {originalStart.Z:F2}), 高程={gridMap.GetWorldElevation(originalStart):F2}"); pointIndex++; // 添加第一条边的起点（如果与originalStart不同） @@ -710,14 +712,13 @@ namespace NavisworksTransport.PathPlanning if (firstNode != null && graph3D.NodeInfo.ContainsKey(firstNode)) { var firstInfo = graph3D.NodeInfo[firstNode]; - var firstPos = gridMap.GridToWorld2D(new GridPoint2D(firstInfo.X, firstInfo.Y)); - var firstPoint = new Point3D(firstPos.X, firstPos.Y, firstInfo.Z); + var firstPoint = gridMap.CreateWorldPoint(new GridPoint2D(firstInfo.X, firstInfo.Y), firstInfo.Z); // 如果与起点高度不同，添加这个节点（避免直接跳跃） - if (Math.Abs(firstPoint.Z - originalStart.Z) > 0.01) + if (Math.Abs(gridMap.GetWorldElevation(firstPoint) - gridMap.GetWorldElevation(originalStart)) > 0.01) { path3D.Add(firstPoint); - LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, Z={firstInfo.Z:F2}, 类型={firstInfo.CellType}"); + LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, 高程={firstInfo.Z:F2}, 类型={firstInfo.CellType}"); pointIndex++; } } @@ -731,10 +732,9 @@ namespace NavisworksTransport.PathPlanning continue; var nodeInfo = graph3D.NodeInfo[endNode]; - var worldPos = gridMap.GridToWorld2D(new GridPoint2D(nodeInfo.X, nodeInfo.Y)); - path3D.Add(new Point3D(worldPos.X, worldPos.Y, nodeInfo.Z)); + path3D.Add(gridMap.CreateWorldPoint(new GridPoint2D(nodeInfo.X, nodeInfo.Y), nodeInfo.Z)); - LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, Z={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}"); + LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, 高程={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}"); pointIndex++; } @@ -829,10 +829,10 @@ namespace NavisworksTransport.PathPlanning // 初始化激活层字典 var activeLayerZ = new Dictionary(); - double targetZ = endPos.Z; // 使用终点Z作为目标高度 + double targetZ = gridMap.GetWorldElevation(endPos); // 使用宿主坐标语义下的终点高程作为目标高度 double baseSpeed = 5.0; // km/h - LogManager.Info($"[A*转换-2.5D] 目标高度: {targetZ:F2}m，基础速度: {baseSpeed}km/h"); + LogManager.Info($"[A*转换-2.5D] 目标高程: {targetZ:F2}m，基础速度: {baseSpeed}km/h"); // 输出详细的网格统计信息 LogManager.Info($"[A*转换-2.5D] 输入网格统计信息:\n{gridMap.GetStatistics()}"); @@ -1083,7 +1083,7 @@ namespace NavisworksTransport.PathPlanning // 初始化激活高度层字典和目标高度 var activeLayerZ = new Dictionary(); - double targetZ = endPos.Z; + double targetZ = gridMap.GetWorldElevation(endPos); // 1. 创建网格基础结构 double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize); @@ -1564,19 +1564,19 @@ namespace NavisworksTransport.PathPlanning if (endCell.HasValue) { // 调试：检查门网格的Z坐标 - double endZ = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0 + double endElevation = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0 ? endCell.Value.HeightLayers[0].Z : 0; if (endCell.Value.CellType == "门") { - LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].Z={endZ:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})"); + LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].高程={endElevation:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})"); } - endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endZ); + endWorldPos = gridMap.SetWorldElevation(endWorldPos, endElevation); } else { - LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，Z坐标保持为0"); + LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，高程保持为默认值"); } // 检查是否与上一个点重复（A*可能在转弯点有重复） @@ -1632,8 +1632,8 @@ namespace NavisworksTransport.PathPlanning bool canSkip = false; double maxAllowedHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff; // 从配置文件读取高度差阈值（模型单位） - double startZ = startPoint.Z; - double endZ = endPoint.Z; + double startZ = gridMap.GetWorldElevation(startPoint); + double endZ = gridMap.GetWorldElevation(endPoint); double totalHeightDiff = Math.Abs(endZ - startZ); // 检查1：起终点高度差必须在阈值内 @@ -1645,7 +1645,7 @@ namespace NavisworksTransport.PathPlanning for (int midIndex = currentIndex + 1; midIndex < testIndex; midIndex++) { - double midZ = optimizedPath[midIndex].Z; + double midZ = gridMap.GetWorldElevation(optimizedPath[midIndex]); // 如果中间点高度与起点或终点不同，说明有高度变化，不能优化掉 if (Math.Abs(midZ - startZ) > heightTolerance || Math.Abs(midZ - endZ) > heightTolerance) @@ -1746,18 +1746,23 @@ namespace NavisworksTransport.PathPlanning { // 线性插值计算采样点 double t = samples > 0 ? (double)i / samples : 0; - var samplePoint = new Point3D( - start.X + t * (end.X - start.X), - start.Y + t * (end.Y - start.Y), - start.Z + t * (end.Z - start.Z) - ); + double interpolatedElevation = + gridMap.GetWorldElevation(start) + + t * (gridMap.GetWorldElevation(end) - gridMap.GetWorldElevation(start)); + + var samplePoint = gridMap.SetWorldElevation( + new Point3D( + start.X + t * (end.X - start.X), + start.Y + t * (end.Y - start.Y), + start.Z + t * (end.Z - start.Z)), + interpolatedElevation); // 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内 // 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层 var gridPos = gridMap.WorldToGrid(samplePoint); double tolerance = 0.1; // Z坐标容差（模型单位） - if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAt3DPoint(gridPos, samplePoint.Z, tolerance)) + if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAtElevation(gridPos, gridMap.GetWorldElevation(samplePoint), tolerance)) { // 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize; @@ -1766,7 +1771,7 @@ namespace NavisworksTransport.PathPlanning // 计算网格中心点世界坐标（基于左下角） var gridCenterX = gridMinX + 0.5 * gridMap.CellSize; var gridCenterY = gridMinY + 0.5 * gridMap.CellSize; - var gridCenterZ = samplePoint.Z; // Z保持不变 + var gridCenterZ = gridMap.GetWorldElevation(samplePoint); // 计算偏差 var deltaX = samplePoint.X - gridCenterX; @@ -2015,7 +2020,7 @@ namespace NavisworksTransport.PathPlanning /// 原始起点坐标 /// 原始终点坐标 /// 修正后的路径 - private List CorrectStartEndPoints(List path, Point3D originalStart, Point3D originalEnd) + private List CorrectStartEndPoints(List path, Point3D originalStart, Point3D originalEnd, GridMap gridMap) { if (path == null || path.Count == 0) return path; @@ -2025,21 +2030,23 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"[坐标修正] 开始修正路径起终点坐标"); var correctedPath = new List(path); - // 修正起点：保持原始XY坐标，使用路径的Z坐标 + // 修正起点：保持原始宿主水平位置，复用路径求得的高程 if (correctedPath.Count > 0) { - var originalStartWithZ = new Point3D(originalStart.X, originalStart.Y, correctedPath[0].Z); - LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({originalStartWithZ.X:F3}, {originalStartWithZ.Y:F3}, {originalStartWithZ.Z:F3})"); - correctedPath[0] = originalStartWithZ; + double startElevation = gridMap.GetWorldElevation(correctedPath[0]); + var correctedStart = gridMap.SetWorldElevation(originalStart, startElevation); + LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({correctedStart.X:F3}, {correctedStart.Y:F3}, {correctedStart.Z:F3}), 高程={startElevation:F3}"); + correctedPath[0] = correctedStart; } - // 修正终点：保持原始XY坐标，使用路径的Z坐标 + // 修正终点：保持原始宿主水平位置，复用路径求得的高程 if (correctedPath.Count > 1) { var lastIndex = correctedPath.Count - 1; - var originalEndWithZ = new Point3D(originalEnd.X, originalEnd.Y, correctedPath[lastIndex].Z); - LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({originalEndWithZ.X:F3}, {originalEndWithZ.Y:F3}, {originalEndWithZ.Z:F3})"); - correctedPath[lastIndex] = originalEndWithZ; + double endElevation = gridMap.GetWorldElevation(correctedPath[lastIndex]); + var correctedEnd = gridMap.SetWorldElevation(originalEnd, endElevation); + LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({correctedEnd.X:F3}, {correctedEnd.Y:F3}, {correctedEnd.Z:F3}), 高程={endElevation:F3}"); + correctedPath[lastIndex] = correctedEnd; } LogManager.Info($"[坐标修正] 路径起终点坐标修正完成"); @@ -2087,7 +2094,7 @@ namespace NavisworksTransport.PathPlanning if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) { // 查找包含指定Z坐标且满足物体高度的层 - var matchingLayer = gridMap.FindLayerContainingZ(gridPos, point.Z, tolerance: 0.5); + var matchingLayer = gridMap.FindLayerContainingElevation(gridPos, gridMap.GetWorldElevation(point), tolerance: 0.5); if (matchingLayer.HasValue) { @@ -2098,12 +2105,12 @@ namespace NavisworksTransport.PathPlanning } else { - LogManager.Warning($"[高度检查] ❌ 找到匹配Z={point.Z:F2}的层Z={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}"); + LogManager.Warning($"[高度检查] ❌ 找到匹配高程={gridMap.GetWorldElevation(point):F2}的层高程={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}"); } } else { - LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含Z={point.Z:F2}的层"); + LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含高程={gridMap.GetWorldElevation(point):F2}的层"); } return false; } @@ -2136,17 +2143,17 @@ namespace NavisworksTransport.PathPlanning var adjustedPath = new List(); - // 1. 使用用户指定的原始起点和终点Z坐标（而不是从转换后的路径中提取） - double startZ = originalStart.Z; - double endZ = originalEnd.Z; + // 1. 使用用户指定的原始起点和终点高程（而不是从转换后的路径中提取） + double startZ = gridMap.GetWorldElevation(originalStart); + double endZ = gridMap.GetWorldElevation(originalEnd); // 对比日志：显示原始坐标与路径坐标的差异 - var pathStartZ = pathWithGridCoords[0].point.Z; - var pathEndZ = pathWithGridCoords[pathWithGridCoords.Count - 1].point.Z; + var pathStartZ = gridMap.GetWorldElevation(pathWithGridCoords[0].point); + var pathEndZ = gridMap.GetWorldElevation(pathWithGridCoords[pathWithGridCoords.Count - 1].point); - LogManager.Info($"[智能选层] 原始起点Z={startZ:F3}, 原始终点Z={endZ:F3}"); - LogManager.Info($"[智能选层] 路径起点Z={pathStartZ:F3}, 路径终点Z={pathEndZ:F3}"); - LogManager.Info($"[智能选层] 使用原始Z坐标进行智能选层，高度变化={endZ - startZ:F3}"); + LogManager.Info($"[智能选层] 原始起点高程={startZ:F3}, 原始终点高程={endZ:F3}"); + LogManager.Info($"[智能选层] 路径起点高程={pathStartZ:F3}, 路径终点高程={pathEndZ:F3}"); + LogManager.Info($"[智能选层] 使用原始高程进行智能选层，高程变化={endZ - startZ:F3}"); // 2. 为每个路径点选择高度层 for (int i = 0; i < pathWithGridCoords.Count; i++) @@ -2175,8 +2182,8 @@ namespace NavisworksTransport.PathPlanning var selectedLayer = SelectBestLayer(cell.HeightLayers, startZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true); if (selectedLayer.HasValue) { - adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z)); - LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) Z={selectedLayer.Value.Z:F3}"); + adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z)); + LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}"); } else { @@ -2188,8 +2195,8 @@ namespace NavisworksTransport.PathPlanning var selectedLayer = SelectBestLayer(cell.HeightLayers, endZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true); if (selectedLayer.HasValue) { - adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z)); - LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) Z={selectedLayer.Value.Z:F3}"); + adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z)); + LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}"); } else { @@ -2199,12 +2206,12 @@ namespace NavisworksTransport.PathPlanning else { // 中间点：根据高度趋势选择最佳层 - var prevZ = adjustedPath[i - 1].Z; + var prevZ = gridMap.GetWorldElevation(adjustedPath[i - 1]); var selectedLayer = SelectBestLayer(cell.HeightLayers, prevZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, false); if (selectedLayer.HasValue) { - adjustedPath.Add(new Point3D(point.X, point.Y, selectedLayer.Value.Z)); + adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z)); } else { @@ -2243,36 +2250,34 @@ namespace NavisworksTransport.PathPlanning for (int i = 0; i < gridPath.Count; i++) { var gridPos = gridPath[i]; - var worldXY = gridMap.GridToWorld3D(gridPos); - - double z; + double elevation; // 起点：使用原始起点Z if (i == 0) { - z = originalStart.Z; - LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) Z={z:F2}m (原始起点)"); + elevation = gridMap.GetWorldElevation(originalStart); + LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始起点)"); } // 终点：使用原始终点Z else if (i == gridPath.Count - 1) { - z = originalEnd.Z; - LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) Z={z:F2}m (原始终点)"); + elevation = gridMap.GetWorldElevation(originalEnd); + LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始终点)"); } // 中间点：使用A*阶段记录的激活层 else if (activeLayerZ.TryGetValue(gridPos, out double activeZ)) { - z = activeZ; - LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) Z={z:F2}m (激活层)"); + elevation = activeZ; + LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (激活层)"); } else { // 降级：无激活层记录，使用前一点高度 - z = path3D[i - 1].Z; - LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点Z={z:F2}m"); + elevation = gridMap.GetWorldElevation(path3D[i - 1]); + LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点高程={elevation:F2}m"); } - path3D.Add(new Point3D(worldXY.X, worldXY.Y, z)); + path3D.Add(gridMap.CreateWorldPoint(gridPos, elevation)); } LogManager.Info($"[3D还原] 完成，生成了 {path3D.Count} 个3D路径点"); @@ -2627,7 +2632,7 @@ namespace NavisworksTransport.PathPlanning // 初始化激活高度层字典和目标高度 var activeLayerZ = new Dictionary(); - double targetZ = endPos.Z; + double targetZ = gridMap.GetWorldElevation(endPos); // 清空日志记录集合，确保每次执行都能看到映射关系 _loggedDistances.Clear(); diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index 458f317..3d1bc85 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -137,6 +137,36 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"[网格地图] 创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}"); } + /// + /// 轻量构造函数，主要用于单元测试与脱离 Navisworks 原生边界句柄的纯坐标语义验证。 + /// + public GridMap(Point3D origin, int width, int height, double cellSize, ICoordinateSystem coordinateSystem) + { + if (origin == null) + throw new ArgumentNullException(nameof(origin)); + if (width <= 0) + throw new ArgumentException("网格宽度必须大于0", nameof(width)); + if (height <= 0) + throw new ArgumentException("网格高度必须大于0", nameof(height)); + if (cellSize <= 0) + throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize)); + if (coordinateSystem == null) + throw new ArgumentNullException(nameof(coordinateSystem)); + + Origin = origin; + Width = width; + Height = height; + CellSize = cellSize; + _coordinateSystem = coordinateSystem; + Bounds = null; + + Cells = new GridCell[Width, Height]; + InitializeCells(); + InitializeHeightCalculation(); + + LogManager.Info($"[网格地图] 轻量创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}"); + } + /// /// 初始化高度计算组件 /// @@ -230,6 +260,44 @@ namespace NavisworksTransport.PathPlanning return _coordinateSystem.SetElevation(world2D, elevation); } + /// + /// 获取宿主坐标点在当前网格坐标语义下的高程值。 + /// 注意： + /// - 对 ZUp 文档，高程是 point.Z + /// - 对 YUp 文档，高程是 point.Y + /// + public double GetWorldElevation(Point3D worldPoint) + { + if (worldPoint == null) + { + throw new ArgumentNullException(nameof(worldPoint)); + } + + return _coordinateSystem.GetElevation(worldPoint); + } + + /// + /// 在当前网格坐标语义下，为一个宿主点设置高程。 + /// + public Point3D SetWorldElevation(Point3D worldPoint, double elevation) + { + if (worldPoint == null) + { + throw new ArgumentNullException(nameof(worldPoint)); + } + + return _coordinateSystem.SetElevation(worldPoint, elevation); + } + + /// + /// 根据网格坐标和指定高程创建宿主坐标点。 + /// + public Point3D CreateWorldPoint(GridPoint2D gridPosition, double elevation) + { + Point3D world2D = GridToWorld2D(gridPosition); + return _coordinateSystem.SetElevation(world2D, elevation); + } + /// @@ -344,7 +412,7 @@ namespace NavisworksTransport.PathPlanning /// Z坐标（米） /// 容差（米），默认0.1m /// 符合条件的高度层，如果没有则返回null - public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) + public HeightLayer? FindLayerContainingElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) { if (!IsValidGridPosition(gridPosition)) return null; @@ -359,7 +427,7 @@ namespace NavisworksTransport.PathPlanning foreach (var layer in cell.HeightLayers) { - double distance = Math.Abs(layer.Z - z); + double distance = Math.Abs(layer.Z - elevation); if (distance < minDistance && distance <= tolerance) { minDistance = distance; @@ -370,6 +438,11 @@ namespace NavisworksTransport.PathPlanning return bestLayer; } + public HeightLayer? FindLayerContainingZ(GridPoint2D gridPosition, double z, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) + { + return FindLayerContainingElevation(gridPosition, z, tolerance); + } + /// /// 获取指定网格位置的所有高度层 /// @@ -441,7 +514,7 @@ namespace NavisworksTransport.PathPlanning /// Z坐标（模型单位） /// Z坐标容差（模型单位），默认0.1 /// 是否可通行 - public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) + public bool IsPassableAtElevation(GridPoint2D gridPosition, double elevation, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) { if (!IsValidGridPosition(gridPosition)) return false; @@ -463,7 +536,7 @@ namespace NavisworksTransport.PathPlanning double layerMinZ = layer.Z; double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan(); - if (zCoord >= layerMinZ - tolerance && zCoord <= layerMaxZ + tolerance) + if (elevation >= layerMinZ - tolerance && elevation <= layerMaxZ + tolerance) { return true; } @@ -473,6 +546,11 @@ namespace NavisworksTransport.PathPlanning return false; } + public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = DEFAULT_LAYER_TOLERANCE_METERS) + { + return IsPassableAtElevation(gridPosition, zCoord, tolerance); + } + /// /// 获取所有通道类型的网格单元 /// diff --git a/src/PathPlanning/PathOptimizer.cs b/src/PathPlanning/PathOptimizer.cs index e5cd381..4df1a70 100644 --- a/src/PathPlanning/PathOptimizer.cs +++ b/src/PathPlanning/PathOptimizer.cs @@ -118,7 +118,7 @@ namespace NavisworksTransport.PathPlanning } else { - optimizedPoints = SimplifyCollinearPoints(optimizedPoints); + optimizedPoints = SimplifyCollinearPoints(optimizedPoints, gridMap); LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}"); } } @@ -170,7 +170,7 @@ namespace NavisworksTransport.PathPlanning // 3D场景：保留同网格不同高度的点 bool hasGridChange = i == 0 || !gridPath[i].Equals(gridPath[i-1]); bool hasSpeedChange = i > 0 && HasSpeedLimitChange(points[i-1], points[i]); - bool hasHeightChange = i > 0 && Math.Abs(points[i].Position.Z - points[i-1].Position.Z) > 1e-6; + bool hasHeightChange = i > 0 && HasHeightChange(points[i], points[i - 1], gridMap); bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange; @@ -213,7 +213,8 @@ namespace NavisworksTransport.PathPlanning bool hasHeightChange = HasHeightChange( dedupedPoints[i-2], dedupedPoints[i-1], - dedupedPoints[i] + dedupedPoints[i], + gridMap ); // 检查限速变化 @@ -264,7 +265,7 @@ namespace NavisworksTransport.PathPlanning /// /// 原始路径点列表 /// 简化后的路径点列表 - private List SimplifyCollinearPoints(List points) + private List SimplifyCollinearPoints(List points, GridMap gridMap = null) { if (points.Count <= 2) return points; @@ -281,7 +282,7 @@ namespace NavisworksTransport.PathPlanning var nextPoint = points[i + 1]; // 检查是否在同一直线上 - bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position); + bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position, gridMap); // 🔥 关键修复：检查限速变化，即使共线也不能移除限速边界点 bool hasSpeedLimitChange = HasSpeedLimitChange(prevPoint, currPoint, nextPoint); @@ -316,10 +317,12 @@ namespace NavisworksTransport.PathPlanning /// 第一个路径点 /// 第二个路径点 /// 如果有高度变化返回true，需要保留中间点 - private bool HasHeightChange(PathPoint point1, PathPoint point2) + private bool HasHeightChange(PathPoint point1, PathPoint point2, GridMap gridMap = null) { const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差 - return Math.Abs(point1.Position.Z - point2.Position.Z) > heightTolerance; + double elevation1 = GetElevation(point1.Position, gridMap); + double elevation2 = GetElevation(point2.Position, gridMap); + return Math.Abs(elevation1 - elevation2) > heightTolerance; } /// @@ -329,13 +332,17 @@ namespace NavisworksTransport.PathPlanning /// 中间点的3D坐标 /// 第三个点的3D坐标 /// 如果有高度变化返回true，不能跳过中间点 - private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3) + private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null) { const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差 // 检查任意两点间是否有高度变化 - if (Math.Abs(p1.Z - p2.Z) > heightTolerance || - Math.Abs(p2.Z - p3.Z) > heightTolerance) + double elevation1 = GetElevation(p1, gridMap); + double elevation2 = GetElevation(p2, gridMap); + double elevation3 = GetElevation(p3, gridMap); + + if (Math.Abs(elevation1 - elevation2) > heightTolerance || + Math.Abs(elevation2 - elevation3) > heightTolerance) { return true; } @@ -349,9 +356,9 @@ namespace NavisworksTransport.PathPlanning /// 中间路径点 /// 第三个路径点 /// 如果有高度变化返回true，不能跳过中间点 - private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3) + private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3, GridMap gridMap = null) { - return HasHeightChange(point1.Position, point2.Position, point3.Position); + return HasHeightChange(point1.Position, point2.Position, point3.Position, gridMap); } /// @@ -395,25 +402,41 @@ namespace NavisworksTransport.PathPlanning /// 第二个点（中间点） /// 第三个点 /// 如果三点在同一直线上且方向一致返回true - private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3) + private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null) { const double tolerance = COLLINEAR_TOLERANCE_METERS; // 容差值，单位：模型单位 // 🔥 修复：使用统一的高度检查函数 - if (HasHeightChange(p1, p2, p3)) + if (HasHeightChange(p1, p2, p3, gridMap)) { 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; } - // 🔥 关键修复：分别检查两个线段的方向 - // 线段1: p1 -> p2 - double dx12 = p2.X - p1.X; - double dy12 = p2.Y - p1.Y; - - // 线段2: p2 -> p3 - double dx23 = p3.X - p2.X; - double dy23 = p3.Y - p2.Y; + double dx12; + double dy12; + double dx23; + double dy23; + + if (gridMap != null) + { + var g1 = gridMap.WorldToGrid(p1); + var g2 = gridMap.WorldToGrid(p2); + var g3 = gridMap.WorldToGrid(p3); + + dx12 = g2.X - g1.X; + dy12 = g2.Y - g1.Y; + dx23 = g3.X - g2.X; + dy23 = g3.Y - g2.Y; + } + else + { + // 回退到历史 ZUp 语义，仅用于未提供网格地图的旧调用链。 + dx12 = p2.X - p1.X; + dy12 = p2.Y - p1.Y; + dx23 = p3.X - p2.X; + dy23 = p3.Y - p2.Y; + } // 🔥 修复：使用更严格的容差检查真正的重复点（坐标完全相同） const double duplicatePointTolerance = DUPLICATE_POINT_TOLERANCE_METERS; // 用很小的值检查真正的重复点 @@ -453,6 +476,11 @@ namespace NavisworksTransport.PathPlanning } } + private static double GetElevation(Point3D point, GridMap gridMap) + { + return gridMap?.GetWorldElevation(point) ?? point.Z; + } + /// /// 创建优化后的路径对象 /// @@ -521,4 +549,4 @@ namespace NavisworksTransport.PathPlanning #endregion } -} \ No newline at end of file +} diff --git a/src/Utils/CoordinateSystem/HostPlanarGridHelper.cs b/src/Utils/CoordinateSystem/HostPlanarGridHelper.cs new file mode 100644 index 0000000..e6dd2bc --- /dev/null +++ b/src/Utils/CoordinateSystem/HostPlanarGridHelper.cs @@ -0,0 +1,99 @@ +using System; +using System.Numerics; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// 自动路径规划 / 网格系统使用的宿主平面与高程语义辅助。 + /// + /// 关键约束： + /// - 对外继续维持 ICoordinateSystem 的历史契约： + /// - ZUp: 水平面 = Host XY，高程 = Host Z + /// - YUp: 水平面 = Host XZ，高程 = Host Y + /// - 对内通过 HostCoordinateAdapter 统一解释“哪根轴是宿主 up”。 + /// + /// 这样自动路径规划既不会继续把世界 Z 硬编码成高度轴， + /// 也不会把旧的水平坐标 h1/h2 语义误替换成 raw canonical XY。 + /// + public static class HostPlanarGridHelper + { + public static double GetElevation3(Vector3 hostPoint, HostCoordinateAdapter adapter) + { + if (adapter == null) + { + throw new ArgumentNullException(nameof(adapter)); + } + + return adapter.ToCanonicalPoint3(hostPoint).Z; + } + + public static Vector3 SetElevation3(Vector3 hostPoint, double elevation, HostCoordinateAdapter adapter) + { + if (adapter == null) + { + throw new ArgumentNullException(nameof(adapter)); + } + + Vector3 canonicalPoint = adapter.ToCanonicalPoint3(hostPoint); + Vector3 canonicalResult = new Vector3(canonicalPoint.X, canonicalPoint.Y, (float)elevation); + return adapter.FromCanonicalPoint3(canonicalResult); + } + + public static (double h1, double h2) GetHorizontalCoords3(Vector3 hostPoint, HostCoordinateAdapter adapter) + { + if (adapter == null) + { + throw new ArgumentNullException(nameof(adapter)); + } + + switch (adapter.HostType) + { + case CoordinateSystemType.ZUp: + return (hostPoint.X, hostPoint.Y); + case CoordinateSystemType.YUp: + return (hostPoint.X, hostPoint.Z); + default: + throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}"); + } + } + + public static Vector3 CreateHostPoint3(double h1, double h2, double elevation, HostCoordinateAdapter adapter) + { + if (adapter == null) + { + throw new ArgumentNullException(nameof(adapter)); + } + + switch (adapter.HostType) + { + case CoordinateSystemType.ZUp: + return new Vector3((float)h1, (float)h2, (float)elevation); + case CoordinateSystemType.YUp: + return new Vector3((float)h1, (float)elevation, (float)h2); + default: + throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}"); + } + } + + public static (double min1, double max1, double min2, double max2) GetHorizontalRange3( + Vector3 hostMin, + Vector3 hostMax, + HostCoordinateAdapter adapter) + { + if (adapter == null) + { + throw new ArgumentNullException(nameof(adapter)); + } + + switch (adapter.HostType) + { + case CoordinateSystemType.ZUp: + return (hostMin.X, hostMax.X, hostMin.Y, hostMax.Y); + case CoordinateSystemType.YUp: + return (hostMin.X, hostMax.X, hostMin.Z, hostMax.Z); + default: + throw new InvalidOperationException($"不支持的宿主坐标系: {adapter.HostType}"); + } + } + } +} diff --git a/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs b/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs index dee8577..c55362b 100644 --- a/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs +++ b/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs @@ -1,4 +1,5 @@ using Autodesk.Navisworks.Api; +using System.Numerics; namespace NavisworksTransport.Utils.CoordinateSystem { @@ -8,6 +9,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem /// public class YUpCoordinateSystem : ICoordinateSystem { + private static readonly HostCoordinateAdapter Adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + public CoordinateSystemType Type => CoordinateSystemType.YUp; public Vector3D UpVector => new Vector3D(0, 1, 0); @@ -18,22 +21,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem public double GetElevation(Point3D point) { - return point.Y; + return HostPlanarGridHelper.GetElevation3(ToVector3(point), Adapter); } public Point3D SetElevation(Point3D point, double elevation) { - return new Point3D(point.X, elevation, point.Z); + return ToPoint3D(HostPlanarGridHelper.SetElevation3(ToVector3(point), elevation, Adapter)); } public (double h1, double h2) GetHorizontalCoords(Point3D point) { - return (point.X, point.Z); + return HostPlanarGridHelper.GetHorizontalCoords3(ToVector3(point), Adapter); } public Point3D CreatePoint(double h1, double h2, double elevation) { - return new Point3D(h1, elevation, h2); + return ToPoint3D(HostPlanarGridHelper.CreateHostPoint3(h1, h2, elevation, Adapter)); } public (double min, double max) GetHeightRange(BoundingBox3D bounds) @@ -43,12 +46,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem public (double min1, double max1, double min2, double max2) GetHorizontalRange(BoundingBox3D bounds) { - return (bounds.Min.X, bounds.Max.X, bounds.Min.Z, bounds.Max.Z); + return HostPlanarGridHelper.GetHorizontalRange3(ToVector3(bounds.Min), ToVector3(bounds.Max), Adapter); } public override string ToString() { return "Y-Up 坐标系 (Y轴向上)"; } + + private static Vector3 ToVector3(Point3D point) + { + return new Vector3((float)point.X, (float)point.Y, (float)point.Z); + } + + private static Point3D ToPoint3D(Vector3 point) + { + return new Point3D(point.X, point.Y, point.Z); + } } } diff --git a/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs b/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs index d19ac32..c263633 100644 --- a/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs +++ b/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs @@ -1,4 +1,5 @@ using Autodesk.Navisworks.Api; +using System.Numerics; namespace NavisworksTransport.Utils.CoordinateSystem { @@ -8,6 +9,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem /// public class ZUpCoordinateSystem : ICoordinateSystem { + private static readonly HostCoordinateAdapter Adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp); + public CoordinateSystemType Type => CoordinateSystemType.ZUp; public Vector3D UpVector => new Vector3D(0, 0, 1); @@ -18,22 +21,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem public double GetElevation(Point3D point) { - return point.Z; + return HostPlanarGridHelper.GetElevation3(ToVector3(point), Adapter); } public Point3D SetElevation(Point3D point, double elevation) { - return new Point3D(point.X, point.Y, elevation); + return ToPoint3D(HostPlanarGridHelper.SetElevation3(ToVector3(point), elevation, Adapter)); } public (double h1, double h2) GetHorizontalCoords(Point3D point) { - return (point.X, point.Y); + return HostPlanarGridHelper.GetHorizontalCoords3(ToVector3(point), Adapter); } public Point3D CreatePoint(double h1, double h2, double elevation) { - return new Point3D(h1, h2, elevation); + return ToPoint3D(HostPlanarGridHelper.CreateHostPoint3(h1, h2, elevation, Adapter)); } public (double min, double max) GetHeightRange(BoundingBox3D bounds) @@ -43,12 +46,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem public (double min1, double max1, double min2, double max2) GetHorizontalRange(BoundingBox3D bounds) { - return (bounds.Min.X, bounds.Max.X, bounds.Min.Y, bounds.Max.Y); + return HostPlanarGridHelper.GetHorizontalRange3(ToVector3(bounds.Min), ToVector3(bounds.Max), Adapter); } public override string ToString() { return "Z-Up 坐标系 (Z轴向上)"; } + + private static Vector3 ToVector3(Point3D point) + { + return new Vector3((float)point.X, (float)point.Y, (float)point.Z); + } + + private static Point3D ToPoint3D(Vector3 point) + { + return new Point3D(point.X, point.Y, point.Z); + } } }