diff --git a/doc/working/coordinate-system-adaptation-design.md b/doc/working/coordinate-system-adaptation-design.md index 7365df2..2a9ad7f 100644 --- a/doc/working/coordinate-system-adaptation-design.md +++ b/doc/working/coordinate-system-adaptation-design.md @@ -474,13 +474,58 @@ public Point3D GridToWorld3D(GridPoint2D gridPosition) ## 实施路线图 -### 阶段1：核心适配（P0）- 1-2周 +### 阶段1：核心适配（P0）- 1-2周 ✅ 已完成 -1. 创建坐标系抽象层（ICoordinateSystem + 实现类） -2. 修改 GridMap 和 GridMapGenerator -3. 修改 ChannelHeightDetector 的垂直扫描 -4. 添加配置支持 -5. 基础测试验证 +1. ✅ 创建坐标系抽象层（ICoordinateSystem + 实现类） + - `src/Utils/CoordinateSystem/ICoordinateSystem.cs` + - `src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs` + - `src/Utils/CoordinateSystem/YUpCoordinateSystem.cs` + - `src/Utils/CoordinateSystem/CoordinateSystemType.cs` + - `src/Utils/CoordinateSystem/CoordinateSystemManager.cs` + +2. ✅ 修改 GridMap 和 GridMapGenerator + - `GridMap.cs` 已使用 `ICoordinateSystem` 进行坐标转换 + - `GridMapGenerator.cs` 已适配坐标系 + +3. ✅ 修改 ChannelHeightDetector 的垂直扫描 + - 已使用 `VerticalScanDirection` 替代硬编码方向 + - 已使用 `GetElevation`/`SetElevation` 替代直接 Z 访问 + +4. ✅ 修改 ChannelBasedGridBuilder + - 已适配坐标系进行三角形光栅化 + - 已使用坐标系进行法向量向上判断 + +5. ✅ 添加配置支持 + - `default_config.toml` 已添加 `[coordinate_system]` 配置节 + - `SystemConfig.cs` 已添加 `CoordinateSystemConfig` 类 + - `ConfigManager.cs` 已添加坐标系配置解析 + - `MainPlugin.cs` 已添加坐标系初始化调用 + +6. ✅ 基础测试验证 + - 构建成功，无编译错误 + - 所有坐标系相关代码已正确集成 + +### 阶段1 完成总结 + +**已完成的核心适配工作：** + +| 组件 | 文件 | 适配内容 | +|------|------|----------| +| 坐标系抽象层 | `ICoordinateSystem.cs` | 定义坐标系接口，包含高度/水平坐标转换、扫描方向等 | +| Z-Up 坐标系 | `ZUpCoordinateSystem.cs` | X=Right, Y=Back, Z=Up 实现 | +| Y-Up 坐标系 | `YUpCoordinateSystem.cs` | X=Right, Y=Up, Z=Front 实现 | +| 坐标系管理器 | `CoordinateSystemManager.cs` | 单例模式，支持自动检测和手动配置 | +| 网格地图 | `GridMap.cs` | 使用坐标系进行世界/网格坐标转换 | +| 网格生成器 | `GridMapGenerator.cs` | 适配坐标系进行边界计算 | +| 通道网格构建 | `ChannelBasedGridBuilder.cs` | 三角形光栅化适配坐标系 | +| 高度检测器 | `ChannelHeightDetector.cs` | 垂直扫描使用坐标系扫描方向 | +| 配置系统 | `SystemConfig.cs`, `ConfigManager.cs` | 添加坐标系配置节 | +| 主插件 | `MainPlugin.cs` | 初始化坐标系管理器 | + +**向后兼容性：** +- 默认使用 Z-Up 坐标系，确保现有用户不受影响 +- 配置文件默认为 `AutoDetect`，自动检测文档坐标系 +- 所有修改对现有功能透明 ### 阶段2：完整适配（P1）- 1周 diff --git a/src/Core/MainPlugin.cs b/src/Core/MainPlugin.cs index 820de55..3a3dbf5 100644 --- a/src/Core/MainPlugin.cs +++ b/src/Core/MainPlugin.cs @@ -5,7 +5,9 @@ using System.Windows.Forms; using System.Windows.Forms.Integration; using NavisworksTransport.Core; using NavisworksTransport.Core.Animation; +using NavisworksTransport.Core.Config; using NavisworksTransport.Utils; +using NavisworksTransport.Utils.CoordinateSystem; using NavisApplication = Autodesk.Navisworks.Api.Application; namespace NavisworksTransport @@ -337,8 +339,11 @@ namespace NavisworksTransport //显式初始化配置管理器，确保配置加载完成 try { - var config = NavisworksTransport.Core.Config.ConfigManager.Instance.Current; + var config = ConfigManager.Instance.Current; LogManager.Info($"[配置管理] 配置已加载"); + + // 注意：坐标系初始化推迟到 InitializeManagers() 中进行 + // 因为此时文档可能尚未加载，无法正确检测坐标系 } catch (Exception ex) { @@ -544,6 +549,65 @@ namespace NavisworksTransport _hasModels = currentHasModels; } + /// + /// 为当前文档初始化坐标系 + /// 在文档加载完成后调用，确保可以正确检测 Document.UpVector + /// + private void InitializeCoordinateSystemForDocument() + { + try + { + var config = ConfigManager.Instance.Current; + if (config?.CoordinateSystem == null) + { + LogManager.Warning("[坐标系] 配置中缺少坐标系设置，使用默认自动检测"); + CoordinateSystemManager.Instance.Initialize(CoordinateSystemType.AutoDetect); + return; + } + + // 如果已经初始化且不是 AutoDetect 模式，则跳过 + // 这样用户手动设置的坐标系不会在文档切换时被覆盖 + if (CoordinateSystemManager.Instance.Current != null && + CoordinateSystemManager.Instance.ConfiguredType != CoordinateSystemType.AutoDetect) + { + LogManager.Debug("[坐标系] 使用手动指定的坐标系，跳过自动检测"); + return; + } + + // 解析坐标系类型配置 + CoordinateSystemType coordType; + var typeString = config.CoordinateSystem.Type?.Trim() ?? "AutoDetect"; + + switch (typeString.ToLowerInvariant()) + { + case "zup": + case "z-up": + coordType = CoordinateSystemType.ZUp; + break; + case "yup": + case "y-up": + coordType = CoordinateSystemType.YUp; + break; + case "autodetect": + case "auto": + default: + coordType = CoordinateSystemType.AutoDetect; + break; + } + + // 初始化坐标系 + CoordinateSystemManager.Instance.Initialize(coordType); + + var currentCS = CoordinateSystemManager.Instance.Current; + LogManager.Info($"[坐标系] 初始化完成 - {currentCS}"); + } + catch (Exception ex) + { + LogManager.Error($"[坐标系] 初始化失败: {ex.Message}，使用默认 Z-Up"); + CoordinateSystemManager.Instance.Initialize(CoordinateSystemType.ZUp); + } + } + /// /// 初始化各个管理器 /// @@ -560,6 +624,9 @@ namespace NavisworksTransport LogManager.Info($"[文档管理] 开始为文档 '{activeDoc.FileName ?? "未保存"}' 初始化管理器..."); + // 初始化坐标系（文档已加载，可以正确检测） + InitializeCoordinateSystemForDocument(); + // 通知DocumentStateManager文档已就绪 DocumentStateManager.Instance.OnDocumentReady(); diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs index 093375b..e29bd99 100644 --- a/src/PathPlanning/ChannelBasedGridBuilder.cs +++ b/src/PathPlanning/ChannelBasedGridBuilder.cs @@ -3,6 +3,7 @@ using System.Collections.Generic; using System.Linq; using Autodesk.Navisworks.Api; using NavisworksTransport.Utils; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { @@ -18,6 +19,7 @@ namespace NavisworksTransport.PathPlanning public class ChannelBasedGridBuilder { private ChannelHeightDetector _heightDetector; + private readonly ICoordinateSystem _coordinateSystem; /// /// 通道几何类型 @@ -40,13 +42,22 @@ namespace NavisworksTransport.PathPlanning Complex } + /// + /// 构造函数（自动使用当前坐标系） + /// + public ChannelBasedGridBuilder() : this(CoordinateSystemManager.Instance.Current) + { + } + /// /// 构造函数 /// - public ChannelBasedGridBuilder() + /// 坐标系 + public ChannelBasedGridBuilder(ICoordinateSystem coordinateSystem) { - _heightDetector = new ChannelHeightDetector(); - LogManager.Info("[通道网格构建器] 初始化完成"); + _coordinateSystem = coordinateSystem ?? throw new ArgumentNullException(nameof(coordinateSystem)); + _heightDetector = new ChannelHeightDetector(_coordinateSystem); + LogManager.Info($"[通道网格构建器] 初始化完成，坐标系: {_coordinateSystem.Type}"); } /// @@ -200,8 +211,8 @@ namespace NavisworksTransport.PathPlanning { var normal = ComputeTriangleNormal(triangle); - // 根据法向量决定处理方式 - if (normal.Z > 0) // 朝上的面 + // 根据法向量决定处理方式（使用坐标系判断向上） + if (GetUpComponent(normal) > 0) // 朝上的面 { RasterizeTriangleToGrid(gridMap, triangle, channel, channelSpeedLimit); processedTriangles++; @@ -244,17 +255,18 @@ namespace NavisworksTransport.PathPlanning return normal; } - - private double GetHeightAtPoint(Triangle3D triangle, double x, double y) + + /// + /// 获取向上轴的分量（根据坐标系） + /// + private double GetUpComponent(Point3D vector) { - var normal = ComputeTriangleNormal(triangle); - - // 使用平面方程计算点(x,y)在三角形平面上的Z值 - double height = triangle.Point1.Z - (normal.X * (x - triangle.Point1.X) + normal.Y * (y - triangle.Point1.Y)) / normal.Z; - return height; + return _coordinateSystem.GetElevation(vector); } + + /// /// 将三角形光栅化到网格 /// @@ -264,14 +276,18 @@ namespace NavisworksTransport.PathPlanning /// 预计算的通道限速 private void RasterizeTriangleToGrid(GridMap gridMap, Triangle3D triangle, ModelItem channel, double channelSpeedLimit) { - // 获取三角形的2D投影边界框 - var minX = Math.Min(triangle.Point1.X, Math.Min(triangle.Point2.X, triangle.Point3.X)); - var maxX = Math.Max(triangle.Point1.X, Math.Max(triangle.Point2.X, triangle.Point3.X)); - var minY = Math.Min(triangle.Point1.Y, Math.Min(triangle.Point2.Y, triangle.Point3.Y)); - var maxY = Math.Max(triangle.Point1.Y, Math.Max(triangle.Point2.Y, triangle.Point3.Y)); + // 获取三角形的2D投影边界框（使用水平坐标） + var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1); + var (p2h1, p2h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point2); + var (p3h1, p3h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point3); + + var minH1 = Math.Min(p1h1, Math.Min(p2h1, p3h1)); + var maxH1 = Math.Max(p1h1, Math.Max(p2h1, p3h1)); + var minH2 = Math.Min(p1h2, Math.Min(p2h2, p3h2)); + var maxH2 = Math.Max(p1h2, Math.Max(p2h2, p3h2)); - var minGrid = gridMap.WorldToGrid(new Point3D(minX, minY, 0)); - var maxGrid = gridMap.WorldToGrid(new Point3D(maxX, maxY, 0)); + var minGrid = gridMap.WorldToGrid(_coordinateSystem.CreatePoint(minH1, minH2, 0)); + var maxGrid = gridMap.WorldToGrid(_coordinateSystem.CreatePoint(maxH1, maxH2, 0)); // 遍历边界框内的所有网格点 for (int x = minGrid.X; x <= maxGrid.X; x++) @@ -283,16 +299,16 @@ namespace NavisworksTransport.PathPlanning { var worldPos = gridMap.GridToWorld3D(gridPos); - // 检查点是否在三角形内部 - if (GeometryHelper.IsPointInTriangle2D(worldPos, triangle)) + // 检查点是否在三角形内部（使用2D水平投影） + if (IsPointInTriangleHorizontal(worldPos, triangle)) { // 计算网格中心点在三角形平面上的精确高度 - double gridCenterHeight = GetHeightAtPoint(triangle, worldPos.X, worldPos.Y); + double gridCenterElevation = GetElevationAtPoint(triangle, worldPos); // 创建高度层（追加模式，不覆盖） var channelType = CategoryAttributeManager.GetLogisticsElementType(channel); var layer = new HeightLayer( - z: gridCenterHeight, + z: gridCenterElevation, passableHeight: new HeightInterval(0, 0), // 初始化，后续由SetChannelPassableHeights设置 sourceItem: channel, speedLimit: channelSpeedLimit, @@ -306,6 +322,68 @@ namespace NavisworksTransport.PathPlanning } } } + + /// + /// 检查点是否在三角形的水平投影内 + /// + private bool IsPointInTriangleHorizontal(Point3D point, Triangle3D triangle) + { + var (ph1, ph2) = _coordinateSystem.GetHorizontalCoords(point); + var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1); + var (p2h1, p2h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point2); + var (p3h1, p3h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point3); + + // 使用叉积法判断点是否在三角形内 + double Cross(double ax, double ay, double bx, double by) => ax * by - ay * bx; + + var d1 = Cross(ph1 - p1h1, ph2 - p1h2, p2h1 - p1h1, p2h2 - p1h2); + var d2 = Cross(ph1 - p2h1, ph2 - p2h2, p3h1 - p2h1, p3h2 - p2h2); + var d3 = Cross(ph1 - p3h1, ph2 - p3h2, p1h1 - p3h1, p1h2 - p3h2); + + bool hasNeg = (d1 < 0) || (d2 < 0) || (d3 < 0); + bool hasPos = (d1 > 0) || (d2 > 0) || (d3 > 0); + + return !(hasNeg && hasPos); + } + + /// + /// 计算点在三角形平面上的高度值（根据当前坐标系） + /// + private double GetElevationAtPoint(Triangle3D triangle, Point3D point) + { + var normal = ComputeTriangleNormal(triangle); + + // 获取向上轴的分量 + double upComponent = GetUpComponent(normal); + + // 如果向上分量接近0，说明是垂直面，返回第一个点的Z值 + if (Math.Abs(upComponent) < 0.0001) + { + return _coordinateSystem.GetElevation(triangle.Point1); + } + + // 获取各点的水平坐标 + var (p1h1, p1h2) = _coordinateSystem.GetHorizontalCoords(triangle.Point1); + var (ph1, ph2) = _coordinateSystem.GetHorizontalCoords(point); + + // 获取向上轴的索引来正确计算 + int upAxis = _coordinateSystem.UpAxisIndex; + + // 使用平面方程计算点的高度 + // n_x(x-x0) + n_y(y-y0) + n_z(z-z0) = 0 + // 解出高度值 + double elevation; + if (upAxis == 2) // Z-up + { + elevation = triangle.Point1.Z - (normal.X * (ph1 - p1h1) + normal.Y * (ph2 - p1h2)) / normal.Z; + } + else // Y-up + { + elevation = triangle.Point1.Y - (normal.X * (ph1 - p1h1) + normal.Z * (ph2 - p1h2)) / normal.Y; + } + + return elevation; + } @@ -332,7 +410,9 @@ namespace NavisworksTransport.PathPlanning /// 格式化字符串 private string FormatBounds(BoundingBox3D bounds) { - return $"[{bounds.Min.X:F2},{bounds.Min.Y:F2},{bounds.Min.Z:F2}] - [{bounds.Max.X:F2},{bounds.Max.Y:F2},{bounds.Max.Z:F2}]"; + var (min1, max1, min2, max2) = _coordinateSystem.GetHorizontalRange(bounds); + var (minElev, maxElev) = _coordinateSystem.GetHeightRange(bounds); + return $"[H1:{min1:F2}-{max1:F2}, H2:{min2:F2}-{max2:F2}, Elev:{minElev:F2}-{maxElev:F2}]"; } } diff --git a/src/PathPlanning/ChannelHeightDetector.cs b/src/PathPlanning/ChannelHeightDetector.cs index 44a79b6..1f901d7 100644 --- a/src/PathPlanning/ChannelHeightDetector.cs +++ b/src/PathPlanning/ChannelHeightDetector.cs @@ -3,24 +3,41 @@ using System.Collections.Generic; using System.Linq; using Autodesk.Navisworks.Api; using NavisworksTransport.Utils; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { /// /// 通道地面高度检测器 /// 负责检测路径点在通道中的精确地面高度 + /// 支持动态坐标系（Z-Up 或 Y-Up） /// public class ChannelHeightDetector { + /// + /// 当前使用的坐标系 + /// + private readonly ICoordinateSystem _coordinateSystem; private readonly Dictionary _heightCache; private readonly double _raycastTolerance; /// - /// 构造函数 + /// 构造函数（自动使用当前坐标系） /// /// 射线投射容差（米） - public ChannelHeightDetector(double raycastTolerance = 0.1) + public ChannelHeightDetector(double raycastTolerance = 0.1) + : this(CoordinateSystemManager.Instance.Current, raycastTolerance) { + } + + /// + /// 构造函数 + /// + /// 坐标系 + /// 射线投射容差（米） + public ChannelHeightDetector(ICoordinateSystem coordinateSystem, double raycastTolerance = 0.1) + { + _coordinateSystem = coordinateSystem ?? throw new ArgumentNullException(nameof(coordinateSystem)); _heightCache = new Dictionary(); _raycastTolerance = raycastTolerance; } @@ -690,30 +707,52 @@ namespace NavisworksTransport.PathPlanning /// /// 执行射线与三角形的交点检测 /// - /// 射线起点的XY坐标 + /// 射线起点的水平坐标 /// 三角形列表 - /// 交点的Z坐标列表 + /// 交点的高度值列表 private List PerformRayTriangleIntersection(Point3D position, List triangles) { var intersectionPoints = new List(); try { - // 创建垂直向下的射线（从通道上方合理高度开始） - var maxZ = triangles.Count > 0 ? triangles.Max(t => Math.Max(Math.Max(t.Point1.Z, t.Point2.Z), t.Point3.Z)) : 0; - var rayOrigin = new Point3D(position.X, position.Y, maxZ + 1000.0); // 从最高点上方1km开始 - var rayDirection = new Point3D(0, 0, -1); // 向下 + // 获取坐标系的垂直扫描方向 + var scanDirection = _coordinateSystem.VerticalScanDirection; + + // 计算三角形在垂直方向上的最大范围 + double maxHeight = 0; + if (triangles.Count > 0) + { + maxHeight = triangles.Max(t => + Math.Max( + _coordinateSystem.GetElevation(t.Point1), + Math.Max( + _coordinateSystem.GetElevation(t.Point2), + _coordinateSystem.GetElevation(t.Point3) + ) + ) + ); + } + + // 从最高点上方开始扫描 + var scanStartHeight = maxHeight + 1000.0; + var rayOrigin = _coordinateSystem.SetElevation(position, scanStartHeight); + + // 使用坐标系的垂直扫描方向（转换为 Point3D） + var rayDirection = new Point3D(scanDirection.X, scanDirection.Y, scanDirection.Z); int intersectionCount = 0; foreach (var triangle in triangles) { - if (GeometryHelper.RayTriangleIntersect(rayOrigin, rayDirection, triangle, out double intersectionZ)) + if (GeometryHelper.RayTriangleIntersect(rayOrigin, rayDirection, triangle, out double intersectionElevation)) { - intersectionPoints.Add(intersectionZ); + intersectionPoints.Add(intersectionElevation); intersectionCount++; } } + LogManager.Debug($"[射线-三角形] 找到 {intersectionCount} 个交点，扫描方向: ({rayDirection.X:F2}, {rayDirection.Y:F2}, {rayDirection.Z:F2})"); + return intersectionPoints; } catch (Exception ex) diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index 5738fc6..d84c31c 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -2,15 +2,21 @@ using System; using System.Collections.Generic; using System.Linq; using Autodesk.Navisworks.Api; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { /// /// 网格地图数据结构 /// 用于将BIM模型转换为路径规划算法可使用的2D网格表示 + /// 支持动态坐标系（Z-Up 或 Y-Up） /// public class GridMap { + /// + /// 当前使用的坐标系 + /// + private readonly ICoordinateSystem _coordinateSystem; /// /// 网格宽度（单元格数量） /// @@ -78,22 +84,37 @@ namespace NavisworksTransport.PathPlanning /// public GridCell[,] Cells { get; set; } + /// + /// 构造函数（自动使用当前坐标系） + /// + /// 世界坐标边界框 + /// 网格单元格大小 + public GridMap(BoundingBox3D bounds, double cellSize) + : this(bounds, cellSize, CoordinateSystemManager.Instance.Current) + { + } + /// /// 构造函数 /// /// 世界坐标边界框 /// 网格单元格大小 - public GridMap(BoundingBox3D bounds, double cellSize) + /// 坐标系 + public GridMap(BoundingBox3D bounds, double cellSize, ICoordinateSystem coordinateSystem) { if (cellSize <= 0) throw new ArgumentException("网格单元格大小必须大于0", nameof(cellSize)); + if (coordinateSystem == null) + throw new ArgumentNullException(nameof(coordinateSystem)); Bounds = bounds ?? throw new ArgumentNullException(nameof(bounds)); CellSize = cellSize; + _coordinateSystem = coordinateSystem; - // 计算网格尺寸 - double worldWidth = bounds.Max.X - bounds.Min.X; - double worldHeight = bounds.Max.Y - bounds.Min.Y; + // 使用坐标系获取水平范围 + var (min1, max1, min2, max2) = _coordinateSystem.GetHorizontalRange(bounds); + double worldWidth = max1 - min1; + double worldHeight = max2 - min2; Width = (int)Math.Ceiling(worldWidth / cellSize); Height = (int)Math.Ceiling(worldHeight / cellSize); @@ -107,6 +128,8 @@ namespace NavisworksTransport.PathPlanning // 初始化高度计算组件 InitializeHeightCalculation(); + + LogManager.Info($"[网格地图] 创建完成: {Width}x{Height}, 坐标系: {_coordinateSystem.Type}"); } /// @@ -116,7 +139,7 @@ namespace NavisworksTransport.PathPlanning { try { - HeightDetector = new ChannelHeightDetector(); + HeightDetector = new ChannelHeightDetector(_coordinateSystem); SlopeAnalyzer = new SlopeAnalyzer(); LogManager.Info("[网格地图] 高度计算组件初始化完成"); } @@ -156,27 +179,33 @@ namespace NavisworksTransport.PathPlanning /// 网格坐标（可能超出边界） public GridPoint2D WorldToGrid(Point3D worldPosition) { - double gridX = (worldPosition.X - Origin.X) / CellSize; - double gridY = (worldPosition.Y - Origin.Y) / CellSize; + // 使用坐标系获取水平坐标 + var (h1, h2) = _coordinateSystem.GetHorizontalCoords(worldPosition); + var (originH1, originH2) = _coordinateSystem.GetHorizontalCoords(Origin); + + double gridX = (h1 - originH1) / CellSize; + double gridY = (h2 - originH2) / CellSize; return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY)); } /// - /// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算） + /// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及高度计算） /// /// 网格坐标 - /// 世界2D坐标点（网格左下角） + /// 世界2D坐标点（网格左下角），高度设为0 public Point3D GridToWorld2D(GridPoint2D gridPosition) { - double worldX = Origin.X + gridPosition.X * CellSize; - double worldY = Origin.Y + gridPosition.Y * CellSize; + var (originH1, originH2) = _coordinateSystem.GetHorizontalCoords(Origin); + double worldH1 = originH1 + gridPosition.X * CellSize; + double worldH2 = originH2 + gridPosition.Y * CellSize; - return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换 + // 使用坐标系创建点，高度设为0 + return _coordinateSystem.CreatePoint(worldH1, worldH2, 0); } /// - /// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标） + /// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的高度） /// /// 网格坐标 /// 世界3D坐标点 @@ -185,14 +214,15 @@ namespace NavisworksTransport.PathPlanning var world2D = GridToWorld2D(gridPosition); var cell = Cells[gridPosition.X, gridPosition.Y]; - // 使用最低层的Z坐标（HeightLayers[0]），如果没有层则用0 - double z = 0; + // 获取高度（HeightLayer.Z 存储的是高度值，与坐标系无关） + double elevation = 0; if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) { - z = cell.HeightLayers[0].Z; + elevation = cell.HeightLayers[0].Z; } - return new Point3D(world2D.X, world2D.Y, z); + // 使用坐标系设置高度 + return _coordinateSystem.SetElevation(world2D, elevation); } diff --git a/src/Utils/CoordinateSystem/ICoordinateSystem.cs b/src/Utils/CoordinateSystem/ICoordinateSystem.cs index 0c0cbde..46d5576 100644 --- a/src/Utils/CoordinateSystem/ICoordinateSystem.cs +++ b/src/Utils/CoordinateSystem/ICoordinateSystem.cs @@ -17,6 +17,11 @@ namespace NavisworksTransport.Utils.CoordinateSystem /// Vector3D UpVector { get; } + /// + /// 获取向上轴的索引 (0=X, 1=Y, 2=Z) + /// + int UpAxisIndex { get; } + /// /// 获取垂直扫描方向（用于障碍物检测，通常是 -UpVector） /// diff --git a/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs b/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs index f4c06f6..dee8577 100644 --- a/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs +++ b/src/Utils/CoordinateSystem/YUpCoordinateSystem.cs @@ -12,6 +12,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem public Vector3D UpVector => new Vector3D(0, 1, 0); + public int UpAxisIndex => 1; // Y轴向上 + public Vector3D VerticalScanDirection => new Vector3D(0, -1, 0); public double GetElevation(Point3D point) diff --git a/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs b/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs index 8ae2dbf..d19ac32 100644 --- a/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs +++ b/src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs @@ -12,6 +12,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem public Vector3D UpVector => new Vector3D(0, 0, 1); + public int UpAxisIndex => 2; // Z轴向上 + public Vector3D VerticalScanDirection => new Vector3D(0, 0, -1); public double GetElevation(Point3D point)