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

坐标系适配（未完成）

Browse Source
This commit is contained in:
tian 2026-01-31 12:13:36 +08:00
parent 289fb2016b
commit 7671360454
8 changed files with 328 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
doc/working/coordinate-system-adaptation-design.md
View File

@ -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周

69
src/Core/MainPlugin.cs
View File

@ -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;
}
/// <summary>
/// 为当前文档初始化坐标系
/// 在文档加载完成后调用，确保可以正确检测 Document.UpVector
/// </summary>
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);
}
}
/// <summary>
/// 初始化各个管理器
/// </summary>
@ -560,6 +624,9 @@ namespace NavisworksTransport
LogManager.Info($"[文档管理] 开始为文档 '{activeDoc.FileName ?? "未保存"}' 初始化管理器...");
// 初始化坐标系（文档已加载，可以正确检测）
InitializeCoordinateSystemForDocument();
// 通知DocumentStateManager文档已就绪
DocumentStateManager.Instance.OnDocumentReady();

128
src/PathPlanning/ChannelBasedGridBuilder.cs
View File

@ -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;
/// <summary>
/// 通道几何类型
@ -40,13 +42,22 @@ namespace NavisworksTransport.PathPlanning
Complex
}
/// <summary>
/// 构造函数（自动使用当前坐标系）
/// </summary>
public ChannelBasedGridBuilder() : this(CoordinateSystemManager.Instance.Current)
{
}
/// <summary>
/// 构造函数
/// </summary>
public ChannelBasedGridBuilder()
/// <param name="coordinateSystem">坐标系</param>
public ChannelBasedGridBuilder(ICoordinateSystem coordinateSystem)
{
_heightDetector = new ChannelHeightDetector();
LogManager.Info("[通道网格构建器] 初始化完成");
_coordinateSystem = coordinateSystem ?? throw new ArgumentNullException(nameof(coordinateSystem));
_heightDetector = new ChannelHeightDetector(_coordinateSystem);
LogManager.Info($"[通道网格构建器] 初始化完成，坐标系: {_coordinateSystem.Type}");
}
/// <summary>
@ -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)
/// <summary>
/// 获取向上轴的分量（根据坐标系）
/// </summary>
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);
}
/// <summary>
/// 将三角形光栅化到网格
/// </summary>
@ -264,14 +276,18 @@ namespace NavisworksTransport.PathPlanning
/// <param name="channelSpeedLimit">预计算的通道限速</param>
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
}
}
}
/// <summary>
/// 检查点是否在三角形的水平投影内
/// </summary>
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);
}
/// <summary>
/// 计算点在三角形平面上的高度值（根据当前坐标系）
/// </summary>
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
/// <returns>格式化字符串</returns>
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}]";
}
}

59
src/PathPlanning/ChannelHeightDetector.cs
View File

@ -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
{
/// <summary>
/// 通道地面高度检测器
/// 负责检测路径点在通道中的精确地面高度
/// 支持动态坐标系（Z-Up 或 Y-Up）
/// </summary>
public class ChannelHeightDetector
{
/// <summary>
/// 当前使用的坐标系
/// </summary>
private readonly ICoordinateSystem _coordinateSystem;
private readonly Dictionary<string, ChannelHeightInfo> _heightCache;
private readonly double _raycastTolerance;
/// <summary>
/// 构造函数
/// 构造函数（自动使用当前坐标系）
/// </summary>
/// <param name="raycastTolerance">射线投射容差（米）</param>
public ChannelHeightDetector(double raycastTolerance = 0.1)
public ChannelHeightDetector(double raycastTolerance = 0.1)
: this(CoordinateSystemManager.Instance.Current, raycastTolerance)
{
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="coordinateSystem">坐标系</param>
/// <param name="raycastTolerance">射线投射容差（米）</param>
public ChannelHeightDetector(ICoordinateSystem coordinateSystem, double raycastTolerance = 0.1)
{
_coordinateSystem = coordinateSystem ?? throw new ArgumentNullException(nameof(coordinateSystem));
_heightCache = new Dictionary<string, ChannelHeightInfo>();
_raycastTolerance = raycastTolerance;
}
@ -690,30 +707,52 @@ namespace NavisworksTransport.PathPlanning
/// <summary>
/// 执行射线与三角形的交点检测
/// </summary>
/// <param name="position">射线起点的XY坐标</param>
/// <param name="position">射线起点的水平坐标</param>
/// <param name="triangles">三角形列表</param>
/// <returns>交点的Z坐标列表</returns>
/// <returns>交点的高度值列表</returns>
private List<double> PerformRayTriangleIntersection(Point3D position, List<Triangle3D> triangles)
{
var intersectionPoints = new List<double>();
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)

64
src/PathPlanning/GridMap.cs
View File

@ -2,15 +2,21 @@ using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.Navisworks.Api;
using NavisworksTransport.Utils.CoordinateSystem;
namespace NavisworksTransport.PathPlanning
{
/// <summary>
/// 网格地图数据结构
/// 用于将BIM模型转换为路径规划算法可使用的2D网格表示
/// 支持动态坐标系（Z-Up 或 Y-Up）
/// </summary>
public class GridMap
{
/// <summary>
/// 当前使用的坐标系
/// </summary>
private readonly ICoordinateSystem _coordinateSystem;
/// <summary>
/// 网格宽度（单元格数量）
/// </summary>
@ -78,22 +84,37 @@ namespace NavisworksTransport.PathPlanning
/// </summary>
public GridCell[,] Cells { get; set; }
/// <summary>
/// 构造函数（自动使用当前坐标系）
/// </summary>
/// <param name="bounds">世界坐标边界框</param>
/// <param name="cellSize">网格单元格大小</param>
public GridMap(BoundingBox3D bounds, double cellSize)
: this(bounds, cellSize, CoordinateSystemManager.Instance.Current)
{
}
/// <summary>
/// 构造函数
/// </summary>
/// <param name="bounds">世界坐标边界框</param>
/// <param name="cellSize">网格单元格大小</param>
public GridMap(BoundingBox3D bounds, double cellSize)
/// <param name="coordinateSystem">坐标系</param>
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}");
}
/// <summary>
@ -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
/// <returns>网格坐标（可能超出边界）</returns>
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));
}
/// <summary>
/// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算）
/// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及高度计算）
/// </summary>
/// <param name="gridPosition">网格坐标</param>
/// <returns>世界2D坐标点（网格左下角）</returns>
/// <returns>世界2D坐标点（网格左下角），高度设为0</returns>
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);
}
/// <summary>
/// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的Z坐标）
/// 将网格坐标转换为世界3D坐标（使用最低层HeightLayer的高度）
/// </summary>
/// <param name="gridPosition">网格坐标</param>
/// <returns>世界3D坐标点</returns>
@ -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);
}

5
src/Utils/CoordinateSystem/ICoordinateSystem.cs
View File

@ -17,6 +17,11 @@ namespace NavisworksTransport.Utils.CoordinateSystem
/// </summary>
Vector3D UpVector { get; }
/// <summary>
/// 获取向上轴的索引 (0=X, 1=Y, 2=Z)
/// </summary>
int UpAxisIndex { get; }
/// <summary>
/// 获取垂直扫描方向（用于障碍物检测，通常是 -UpVector）
/// </summary>

2
src/Utils/CoordinateSystem/YUpCoordinateSystem.cs
View File

@ -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)

2
src/Utils/CoordinateSystem/ZUpCoordinateSystem.cs
View File

@ -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)