From 1a9d0972f02bece8df1c1dab65b1a377e6fed653 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Tue, 14 Apr 2026 09:22:01 +0800 Subject: [PATCH] Fix auto path obstacle geometry collection --- NavisworksTransport.UnitTests.csproj | 1 + ...utoPathPlanningCoordinateSemanticsTests.cs | 243 ++++++++++ .../AutoPathGridGenerationAutomationTests.cs | 40 ++ .../NavisworksTestAutomationClient.cs | 9 + src/Core/PathPlanningManager.cs | 65 ++- src/Core/PathPointRenderPlugin.cs | 56 ++- .../Services/TestAutomationHttpService.cs | 436 ++++++++++++++++++ src/PathPlanning/AutoPathFinder.cs | 15 +- src/PathPlanning/ChannelBasedGridBuilder.cs | 41 ++ src/PathPlanning/ChannelHeightDetector.cs | 239 ++++++++-- src/PathPlanning/GridMap.cs | 5 + src/PathPlanning/GridMapGenerator.cs | 405 +++++++++++----- src/PathPlanning/OptimizedHeightCalculator.cs | 42 +- src/PathPlanning/SlopeAnalyzer.cs | 88 ++-- src/Utils/ModelItemAnalysisHelper.cs | 17 +- 15 files changed, 1438 insertions(+), 264 deletions(-) create mode 100644 UnitTests/Integration/AutoPathGridGenerationAutomationTests.cs diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index 8962550..ec90fa9 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -61,6 +61,7 @@ + diff --git a/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs b/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs index d6e1382..3f6495b 100644 --- a/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs +++ b/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs @@ -1,6 +1,11 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Core; +using NavisworksTransport.PathPlanning; using NavisworksTransport.Utils.CoordinateSystem; using System.Numerics; +using System.Reflection; +using System.Collections.Generic; +using Autodesk.Navisworks.Api; namespace NavisworksTransport.UnitTests.CoordinateSystem { @@ -104,6 +109,244 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreEqual(5, gridY); } + [TestMethod] + public void YUp_GridMapGenerator_GetObstacleElevationRange_ShouldUseHostYInsteadOfWorldZ() + { + var generator = new GridMapGenerator(); + + var method = typeof(GridMapGenerator).GetMethod( + "GetObstacleElevationRange", + BindingFlags.Instance | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 GetObstacleElevationRange 私有方法。"); + + var elevationRange = ((double min, double max))method.Invoke(generator, new object[] + { + -200.0, 10.0, -30.0, + -198.0, 30.0, -10.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(10.0, elevationRange.min, 1e-6, "YUp 下障碍物最小高程应来自宿主 Y。"); + Assert.AreEqual(30.0, elevationRange.max, 1e-6, "YUp 下障碍物最大高程应来自宿主 Y。"); + } + + [TestMethod] + public void YUp_GridMapGenerator_CalculateBoundingBoxGridCoverage_ShouldProjectToHostXZPlane() + { + var generator = new GridMapGenerator(); + + var method = typeof(GridMapGenerator).GetMethod( + "CalculateGridCoverageFromWorldExtents", + BindingFlags.Instance | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(int), typeof(int), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 CalculateGridCoverageFromWorldExtents 私有方法。"); + + var coveredCells = (List<(int x, int y)>)method.Invoke(generator, new object[] + { + -200.0, 10.0, -30.0, + -198.0, 30.0, -10.0, + -210.0, 14.83, -80.0, + 200, 200, 1.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(63, coveredCells.Count, "YUp 下障碍物包围盒应按宿主 XZ 平面覆盖 3x21 个网格。"); + CollectionAssert.Contains(coveredCells, (10, 50)); + CollectionAssert.Contains(coveredCells, (12, 70)); + } + + [TestMethod] + public void YUp_GridMapGenerator_CalculateDoorOpeningCoverage_ShouldProjectToHostXZPlane() + { + var generator = new GridMapGenerator(); + + var method = typeof(GridMapGenerator).GetMethod( + "CalculateDoorOpeningCoverageFromWorldExtents", + BindingFlags.Instance | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(int), typeof(int), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 CalculateDoorOpeningCoverageFromWorldExtents 私有方法。"); + + var coveredCells = (List<(int x, int y)>)method.Invoke(generator, new object[] + { + 10.0, 14.0, 20.0, + 12.0, 16.0, 30.0, + 4.0, + 0.0, 0.0, 0.0, + 100, 100, 1.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(15, coveredCells.Count, "YUp 下门开口应按宿主 XZ 平面覆盖 3x5 个网格。"); + CollectionAssert.Contains(coveredCells, (10, 23)); + CollectionAssert.Contains(coveredCells, (12, 27)); + } + + [TestMethod] + public void YUp_PathPlanningManager_CalculateOptimalGridSize_ShouldUseHostXZHorizontalRange() + { + var method = typeof(PathPlanningManager).GetMethod( + "CalculateOptimalGridSizeFromBounds", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 CalculateOptimalGridSizeFromBounds 私有方法。"); + + var gridSize = (double)method.Invoke(null, new object[] + { + -10.0, 100.0, -300.0, + 90.0, 120.0, 400.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(2.0, gridSize, 1e-6, "YUp 下网格大小应基于宿主 XZ 平面最大跨度 700 计算。"); + } + + [TestMethod] + public void YUp_ChannelHeightDetector_GetBoundsElevationRange_ShouldUseHostY() + { + var method = typeof(ChannelHeightDetector).GetMethod( + "GetBoundsElevationRange", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 GetBoundsElevationRange 私有方法。"); + + var elevationRange = ((double min, double max))method.Invoke(null, new object[] + { + -10.0, 14.83, -30.0, + 20.0, 18.50, 40.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(14.83, elevationRange.min, 1e-6); + Assert.AreEqual(18.50, elevationRange.max, 1e-6); + } + + [TestMethod] + public void YUp_ChannelHeightDetector_CreateHeightProfileSample_ShouldUseHostXZPlaneAndHostYElevation() + { + var method = typeof(ChannelHeightDetector).GetMethod( + "CreateHeightProfileSamplePoint3", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType), + typeof(double) + }, + null); + + Assert.IsNotNull(method, "未找到 CreateHeightProfileSamplePoint3 私有方法。"); + + var samplePoint = (Vector3)method.Invoke(null, new object[] + { + -10.0, 14.83, -30.0, + 20.0, 18.50, 40.0, + CoordinateSystemType.YUp, + 0.5 + }); + + Assert.AreEqual(5.0, samplePoint.X, 1e-6, "YUp 下采样点第一水平轴应沿宿主 X 插值。"); + Assert.AreEqual(14.83, samplePoint.Y, 1e-6, "YUp 下采样点高程应落在宿主底面 Y。"); + Assert.AreEqual(5.0, samplePoint.Z, 1e-6, "YUp 下采样点第二水平轴应沿宿主 Z 插值。"); + } + + [TestMethod] + public void YUp_ChannelHeightDetector_GetPickedSurfaceElevation_ShouldUseHostYInsteadOfWorldZ() + { + var method = typeof(ChannelHeightDetector).GetMethod( + "GetPickedSurfaceElevation", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 GetPickedSurfaceElevation 私有方法。"); + + var elevation = (double)method.Invoke(null, new object[] + { + 11.0, 22.0, 33.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(22.0, elevation, 1e-6, "YUp 下拾取表面点的高程应来自宿主 Y。"); + } + + [TestMethod] + public void YUp_GridMapGenerator_ResolveBoundsMinElevation_ShouldUseHostYInsteadOfWorldZ() + { + var method = typeof(GridMapGenerator).GetMethod( + "ResolveBoundsMinElevation", + BindingFlags.Static | BindingFlags.NonPublic, + null, + new[] + { + typeof(double), typeof(double), typeof(double), + typeof(CoordinateSystemType) + }, + null); + + Assert.IsNotNull(method, "未找到 ResolveBoundsMinElevation 私有方法。"); + + var elevation = (double)method.Invoke(null, new object[] + { + 0.0, 12.5, -100.0, + CoordinateSystemType.YUp + }); + + Assert.AreEqual(12.5, elevation, 1e-6, "YUp 下边界最小高程应来自宿主 Y，而不是 bounds.Min.Z。"); + } + private static void AssertPoint(Vector3 actual, double x, double y, double z) { Assert.AreEqual(x, actual.X, 1e-6); diff --git a/UnitTests/Integration/AutoPathGridGenerationAutomationTests.cs b/UnitTests/Integration/AutoPathGridGenerationAutomationTests.cs new file mode 100644 index 0000000..6fa673c --- /dev/null +++ b/UnitTests/Integration/AutoPathGridGenerationAutomationTests.cs @@ -0,0 +1,40 @@ +using System; +using System.Threading.Tasks; +using Microsoft.VisualStudio.TestTools.UnitTesting; +using Newtonsoft.Json.Linq; + +namespace NavisworksTransport.UnitTests.Integration +{ + [TestClass] + [TestCategory("NavisworksIntegration")] + [TestCategory("NavisworksIntegration.Ground")] + public class AutoPathGridGenerationAutomationTests + { + [TestMethod] + [Timeout(240000)] + public async Task GroundAutoPathGrid_DefaultRoute_ObstacleCellsShouldCoverAtLeastHalfOfGrid() + { + using (var client = new NavisworksTestAutomationClient()) + { + await client.EnsureServiceReadyAsync(TimeSpan.FromSeconds(90)).ConfigureAwait(false); + + JObject response = await client.AnalyzeAutoPathGridAsync("Ground").ConfigureAwait(false); + Assert.IsTrue(response.Value("ok"), "测试 HTTP 接口返回失败: " + (string)response["error"]); + + JObject data = (JObject)response["data"]; + Assert.IsNotNull(data, "缺少测试结果 data"); + + JObject gridStats = (JObject)data["gridStats"]; + Assert.IsNotNull(gridStats, "缺少 gridStats"); + + int totalCellCount = (int)gridStats["totalCellCount"]; + int obstacleCellCount = (int)gridStats["obstacleCellCount"]; + + Assert.IsTrue(totalCellCount > 0, "网格总数必须大于 0"); + Assert.IsTrue( + obstacleCellCount * 2 >= totalCellCount, + $"当前障碍物网格数量过少: obstacle={obstacleCellCount}, total={totalCellCount}"); + } + } + } +} diff --git a/UnitTests/Integration/NavisworksTestAutomationClient.cs b/UnitTests/Integration/NavisworksTestAutomationClient.cs index e5bfab5..68d1222 100644 --- a/UnitTests/Integration/NavisworksTestAutomationClient.cs +++ b/UnitTests/Integration/NavisworksTestAutomationClient.cs @@ -57,6 +57,15 @@ namespace NavisworksTransport.UnitTests.Integration return await PostJsonAsync(requestUri).ConfigureAwait(false); } + public async Task AnalyzeAutoPathGridAsync(string pathType) + { + string requestUri = string.Format( + "/api/test/analyze-auto-path-grid?pathType={0}", + Uri.EscapeDataString(pathType)); + + return await PostJsonAsync(requestUri).ConfigureAwait(false); + } + private async Task GetJsonAsync(string requestUri) { using (HttpResponseMessage response = await _httpClient.GetAsync(requestUri).ConfigureAwait(false)) diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index f325675..b93ac8c 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -1072,7 +1072,12 @@ namespace NavisworksTransport // 1. 获取模型边界 var bounds = GetModelBounds() ?? throw new Exception("无法获取模型边界，请确保模型已加载"); - LogManager.Info($"模型边界: Min({bounds.Min.X:F2}, {bounds.Min.Y:F2}), Max({bounds.Max.X:F2}, {bounds.Max.Y:F2})"); + var hostAdapter = new HostCoordinateAdapter(CoordinateSystemManager.Instance.ResolvedType); + var hostBoundsMin = new System.Numerics.Vector3((float)bounds.Min.X, (float)bounds.Min.Y, (float)bounds.Min.Z); + var hostBoundsMax = new System.Numerics.Vector3((float)bounds.Max.X, (float)bounds.Max.Y, (float)bounds.Max.Z); + var (boundsMinH1, boundsMaxH1, boundsMinH2, boundsMaxH2) = + HostPlanarGridHelper.GetHorizontalRange3(hostBoundsMin, hostBoundsMax, hostAdapter); + LogManager.Info($"模型边界: H1[{boundsMinH1:F2}, {boundsMaxH1:F2}], H2[{boundsMinH2:F2}, {boundsMaxH2:F2}]"); // 智能选择网格大小 if (gridSize <= 0) @@ -1096,7 +1101,7 @@ namespace NavisworksTransport // 获取当前文档 var document = Application.ActiveDocument; - LogManager.Info($"网格生成参数 - 边界: {bounds.Min.X:F2},{bounds.Min.Y:F2} -> {bounds.Max.X:F2},{bounds.Max.Y:F2}"); + LogManager.Info($"网格生成参数 - 边界: H1 {boundsMinH1:F2}->{boundsMaxH1:F2}, H2 {boundsMinH2:F2}->{boundsMaxH2:F2}"); LogManager.Info($"网格生成参数 - 网格大小: {gridSize}m, 物体半径: {objectRadius}m, 安全边距: {safetyMargin}m"); LogManager.Info($"网格生成参数 - 起点: ({startPoint.Position.X:F2}, {startPoint.Position.Y:F2}, {startPoint.Position.Z:F2})"); LogManager.Info($"网格生成参数 - 终点: ({endPoint.Position.X:F2}, {endPoint.Position.Y:F2}, {endPoint.Position.Z:F2})"); @@ -4717,14 +4722,10 @@ namespace NavisworksTransport { try { - var width = bounds.Max.X - bounds.Min.X; - var height = bounds.Max.Y - bounds.Min.Y; - var maxDimension = Math.Max(width, height); - - // 基于模型大小智能选择网格大小 - if (maxDimension > 1000) return 5.0; // 大型模型：5米 - if (maxDimension > 500) return 2.0; // 中型模型：2米 - return 0.5; // 小型模型：0.5米 + return CalculateOptimalGridSizeFromBounds( + bounds.Min.X, bounds.Min.Y, bounds.Min.Z, + bounds.Max.X, bounds.Max.Y, bounds.Max.Z, + CoordinateSystemManager.Instance.ResolvedType); } catch { @@ -4732,6 +4733,32 @@ namespace NavisworksTransport } } + /// + /// 根据宿主包围盒和当前宿主坐标系语义，计算自动路径规划的最优网格大小。 + /// 关键约束： + /// - 仅修正“水平平面”的解释，不改变历史阈值与业务策略。 + /// - ZUp: 使用 Host XY 平面跨度。 + /// - YUp: 使用 Host XZ 平面跨度。 + /// + private static double CalculateOptimalGridSizeFromBounds( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + var hostMin = new System.Numerics.Vector3((float)minX, (float)minY, (float)minZ); + var hostMax = new System.Numerics.Vector3((float)maxX, (float)maxY, (float)maxZ); + var (min1, max1, min2, max2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + + var width = max1 - min1; + var height = max2 - min2; + var maxDimension = Math.Max(width, height); + + if (maxDimension > 1000) return 5.0; + if (maxDimension > 500) return 2.0; + return 0.5; + } + /// /// 获取通道模型项用于高度计算 /// @@ -5198,11 +5225,14 @@ namespace NavisworksTransport { var cell = gridMap.Cells[x, y]; - // 计算网格单元格的XY中心位置（Z坐标在多层逻辑中单独处理） + // 计算网格单元格的宿主平面中心位置（高程在多层逻辑中单独处理） var gridPos = new NavisworksTransport.PathPlanning.GridPoint2D(x, y); - var gridCorner = gridMap.GridToWorld3D(gridPos); - double centerX = gridCorner.X + gridMap.CellSize / 2; - double centerY = gridCorner.Y + gridMap.CellSize / 2; + var gridCorner2D = gridMap.GridToWorld2D(gridPos); + var diagonalCorner2D = gridMap.GridToWorld2D(new NavisworksTransport.PathPlanning.GridPoint2D(x + 1, y + 1)); + var gridCenter2D = new Point3D( + (gridCorner2D.X + diagonalCorner2D.X) / 2.0, + (gridCorner2D.Y + diagonalCorner2D.Y) / 2.0, + (gridCorner2D.Z + diagonalCorner2D.Z) / 2.0); // 所有网格都基于高度层，统一处理 if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) @@ -5250,7 +5280,7 @@ namespace NavisworksTransport // 创建该层的可视化标记 if (cellType.HasValue) { - var gridCenter = new Point3D(centerX, centerY, layerZ); + var gridCenter = gridMap.SetWorldElevation(gridCenter2D, layerZ); var marker = new GridMarker(gridCenter, cellType.Value, x, y, layerZ); // 根据类型添加到对应列表 @@ -5278,8 +5308,9 @@ namespace NavisworksTransport // Unknown网格没有高度层，用于调试 if (_showUnknownGrid) { - var gridCenter = new Point3D(centerX, centerY, gridCorner.Z); - var marker = new GridMarker(gridCenter, GridVisualizationType.Unknown, x, y, gridCorner.Z); + double unknownElevation = gridMap.GetWorldElevation(gridCenter2D); + var gridCenter = gridMap.SetWorldElevation(gridCenter2D, unknownElevation); + var marker = new GridMarker(gridCenter, GridVisualizationType.Unknown, x, y, unknownElevation); gridVis.Unknown.Add(marker); unknownCells++; totalVisualized++; diff --git a/src/Core/PathPointRenderPlugin.cs b/src/Core/PathPointRenderPlugin.cs index b7b5f87..e4da8b3 100644 --- a/src/Core/PathPointRenderPlugin.cs +++ b/src/Core/PathPointRenderPlugin.cs @@ -2144,6 +2144,37 @@ namespace NavisworksTransport return new Vector3D(vector.X / length, vector.Y / length, vector.Z / length); } + private static Point3D ApplyVectorOffset(Point3D point, Vector3D direction, double distance) + { + var normalized = Normalize(direction); + return new Point3D( + point.X + normalized.X * distance, + point.Y + normalized.Y * distance, + point.Z + normalized.Z * distance); + } + + private static (Vector3D axis1, Vector3D axis2) GetHostPlanarAxes(Vector3D hostUp) + { + var normalizedUp = Normalize(hostUp); + var reference = Math.Abs(normalizedUp.X) > 0.9 + ? new Vector3D(0, 1, 0) + : new Vector3D(1, 0, 0); + + double projection = reference.X * normalizedUp.X + reference.Y * normalizedUp.Y + reference.Z * normalizedUp.Z; + var axis1 = Normalize(new Vector3D( + reference.X - projection * normalizedUp.X, + reference.Y - projection * normalizedUp.Y, + reference.Z - projection * normalizedUp.Z)); + + if (axis1.X == 0 && axis1.Y == 0 && axis1.Z == 0) + { + axis1 = new Vector3D(0, 0, 1); + } + + var axis2 = Normalize(Cross(normalizedUp, axis1)); + return (axis1, axis2); + } + /// /// 添加切点标记 /// @@ -3086,15 +3117,18 @@ namespace NavisworksTransport // 计算正方形的边长（直径） double sideLength = pointMarker.Radius * 2; - // 定义正方形的两个轴向量（沿X、Y轴） - var xVector = new Vector3D(sideLength, 0, 0); - var yVector = new Vector3D(0, sideLength, 0); + // 网格矩形必须落在宿主水平平面内，不能写死世界 XY。 + var hostUp = GetHostUpVector(); + var (planarAxis1, planarAxis2) = GetHostPlanarAxes(hostUp); + var xVector = new Vector3D(planarAxis1.X * sideLength, planarAxis1.Y * sideLength, planarAxis1.Z * sideLength); + var yVector = new Vector3D(planarAxis2.X * sideLength, planarAxis2.Y * sideLength, planarAxis2.Z * sideLength); - // 计算正方形原点（中心点减去各轴向量的一半）,略微抬高以避免与模型重叠 + // 沿宿主 up 略微抬高，避免与模型重叠。 + var liftedCenter = ApplyVectorOffset(pointMarker.Center, hostUp, 0.01); var origin = new Point3D( - pointMarker.Center.X - pointMarker.Radius, - pointMarker.Center.Y - pointMarker.Radius, - pointMarker.Center.Z + 0.01 + liftedCenter.X - planarAxis1.X * pointMarker.Radius - planarAxis2.X * pointMarker.Radius, + liftedCenter.Y - planarAxis1.Y * pointMarker.Radius - planarAxis2.Y * pointMarker.Radius, + liftedCenter.Z - planarAxis1.Z * pointMarker.Radius - planarAxis2.Z * pointMarker.Radius ); // 使用Rectangle API渲染正方形 @@ -3108,12 +3142,8 @@ namespace NavisworksTransport /// 点标记 private void RenderCircleMarker(Graphics graphics, CircleMarker pointMarker) { - // 调整中心点位置，略微抬高以避免与模型重叠 - Point3D adjustCenterPoint = new Point3D( - pointMarker.Center.X, - pointMarker.Center.Y, - pointMarker.Center.Z + 0.01 - ); + // 调整中心点位置，沿宿主 up 略微抬高以避免与模型重叠 + Point3D adjustCenterPoint = ApplyVectorOffset(pointMarker.Center, pointMarker.Normal, 0.01); // 使用Circle API渲染圆形 graphics.Circle(adjustCenterPoint, pointMarker.Normal, pointMarker.Radius, true); diff --git a/src/Core/Services/TestAutomationHttpService.cs b/src/Core/Services/TestAutomationHttpService.cs index ece2ce5..1aa6f6a 100644 --- a/src/Core/Services/TestAutomationHttpService.cs +++ b/src/Core/Services/TestAutomationHttpService.cs @@ -11,6 +11,8 @@ using System.Threading.Tasks; using Autodesk.Navisworks.Api; using NavisworksTransport.Commands; using NavisworksTransport.Core.Animation; +using NavisworksTransport.Core.Config; +using NavisworksTransport.PathPlanning; using NavisworksTransport.UI.WPF.ViewModels; using NavisworksTransport.Utils; using Newtonsoft.Json; @@ -297,6 +299,22 @@ namespace NavisworksTransport.Core.Services return; } + if (string.Equals(method, "POST", StringComparison.OrdinalIgnoreCase) && + string.Equals(request.Path, "/api/test/analyze-auto-path-grid", StringComparison.OrdinalIgnoreCase)) + { + object payload = InvokeOnUiThread(() => AnalyzeAutoPathGridPayload(request.Query)); + await WriteJsonResponseAsync(writer, 200, BuildEnvelope(true, payload)).ConfigureAwait(false); + return; + } + + if (string.Equals(method, "POST", StringComparison.OrdinalIgnoreCase) && + string.Equals(request.Path, "/api/test/run-auto-path", StringComparison.OrdinalIgnoreCase)) + { + object payload = InvokeOnUiThread(() => RunAutoPathPayload(request.Query)); + await WriteJsonResponseAsync(writer, 200, BuildEnvelope(true, payload)).ConfigureAwait(false); + return; + } + if (!string.Equals(method, "GET", StringComparison.OrdinalIgnoreCase) && !string.Equals(method, "POST", StringComparison.OrdinalIgnoreCase)) { @@ -714,6 +732,150 @@ namespace NavisworksTransport.Core.Services route.Name.StartsWith(DefaultAutoTestRoutePrefix, StringComparison.OrdinalIgnoreCase)); } + private static object AnalyzeAutoPathGridPayload(Dictionary query) + { + PathPlanningManager pathManager = RequirePathManager(); + PathRoute route = ResolveVirtualCollisionRoute(pathManager, query, null); + if (route.Points == null || route.Points.Count < 2) + { + throw new InvalidOperationException($"路径点不足，无法分析自动路径网格: {route.Name}"); + } + + List orderedPoints = route.Points.OrderBy(point => point.Index).ToList(); + PathPoint startPoint = orderedPoints.First(); + PathPoint endPoint = orderedPoints.Last(); + + BoundingBox3D bounds = InvokePathManagerPrivate(pathManager, "GetModelBounds"); + double gridSize = InvokePathManagerPrivate(pathManager, "CalculateOptimalGridSize", bounds); + + var config = ConfigManager.Instance.Current?.PathEditing + ?? throw new InvalidOperationException("缺少 PathEditing 配置，无法分析自动路径网格"); + + double objectLengthInMeters = config.ObjectLengthMeters; + double objectWidthInMeters = config.ObjectWidthMeters; + double objectHeightInMeters = config.ObjectHeightMeters; + double safetyMarginInMeters = config.SafetyMarginMeters; + double objectRadiusInMeters = Math.Max(objectLengthInMeters, objectWidthInMeters) / 2.0; + + var gridMapGenerator = new GridMapGenerator(); + GridMap gridMap = gridMapGenerator.GenerateFromBIM( + bounds, + gridSize, + objectRadiusInMeters, + safetyMarginInMeters, + startPoint.Position, + endPoint.Position, + objectHeightInMeters); + + return new + { + requestedPathType = route.PathType.ToString(), + route = new + { + id = route.Id, + name = route.Name, + pathType = route.PathType.ToString(), + pointCount = route.Points.Count, + startPoint = SerializePathPoint(startPoint), + endPoint = SerializePathPoint(endPoint) + }, + planningParameters = new + { + gridSizeInMeters = gridSize, + objectLengthInMeters, + objectWidthInMeters, + objectHeightInMeters, + objectRadiusInMeters, + safetyMarginInMeters + }, + bounds = new + { + min = SerializePoint3D(bounds.Min), + max = SerializePoint3D(bounds.Max) + }, + gridStats = SerializeGridStats(gridMap), + obstacleDiagnostics = BuildObstacleDiagnostics(gridMapGenerator, gridMap, objectHeightInMeters, safetyMarginInMeters) + }; + } + + private static object RunAutoPathPayload(Dictionary query) + { + PathPlanningManager pathManager = RequirePathManager(); + PathRoute route = ResolveVirtualCollisionRoute(pathManager, query, null); + if (route.Points == null || route.Points.Count < 2) + { + throw new InvalidOperationException($"路径点不足，无法执行自动路径规划: {route.Name}"); + } + + List orderedPoints = route.Points.OrderBy(point => point.Index).ToList(); + PathPoint startPoint = orderedPoints.First(); + PathPoint endPoint = orderedPoints.Last(); + + var config = ConfigManager.Instance.Current?.PathEditing + ?? throw new InvalidOperationException("缺少 PathEditing 配置，无法执行自动路径规划"); + + BoundingBox3D bounds = InvokePathManagerPrivate(pathManager, "GetModelBounds"); + double gridSize = InvokePathManagerPrivate(pathManager, "CalculateOptimalGridSize", bounds); + double objectHeightInMeters = config.ObjectHeightMeters; + double objectLengthInMeters = config.ObjectLengthMeters; + double objectWidthInMeters = config.ObjectWidthMeters; + double safetyMarginInMeters = config.SafetyMarginMeters; + double objectRadiusInMeters = Math.Max(objectLengthInMeters, objectWidthInMeters) / 2.0; + PathStrategy strategy = PathStrategy.Shortest; + + string strategyText = GetOptionalQueryValue(query, "strategy"); + if (!string.IsNullOrWhiteSpace(strategyText) && + Enum.TryParse(strategyText, true, out PathStrategy parsedStrategy)) + { + strategy = parsedStrategy; + } + + PathRoute autoRoute = pathManager + .AutoPlanPath( + startPoint, + endPoint, + objectRadiusInMeters, + safetyMarginInMeters, + gridSize, + objectHeightInMeters, + strategy) + .GetAwaiter() + .GetResult(); + + if (autoRoute == null) + { + throw new InvalidOperationException("AutoPlanPath 返回空结果"); + } + + return new + { + sourceRoute = new + { + id = route.Id, + name = route.Name, + pathType = route.PathType.ToString() + }, + generatedRoute = new + { + id = autoRoute.Id, + name = autoRoute.Name, + pathType = autoRoute.PathType.ToString(), + pointCount = autoRoute.Points?.Count ?? 0, + length = autoRoute.TotalLength + }, + planningParameters = new + { + gridSizeInMeters = gridSize, + objectHeightInMeters, + objectLengthInMeters, + objectWidthInMeters, + objectRadiusInMeters, + safetyMarginInMeters, + strategy = strategy.ToString() + } + }; + } + private static PathPlanningManager RequirePathManager() { PathPlanningManager pathManager = PathPlanningManager.GetActivePathManager(); @@ -867,6 +1029,280 @@ namespace NavisworksTransport.Core.Services }; } + private static object SerializePathPoint(PathPoint point) + { + if (point == null) + { + return null; + } + + return new + { + id = point.Id, + name = point.Name, + type = point.Type.ToString(), + index = point.Index, + position = SerializePoint3D(point.Position) + }; + } + + private static object SerializeGridStats(GridMap gridMap) + { + if (gridMap == null) + { + return null; + } + + int walkableCellCount = 0; + int nonWalkableCellCount = 0; + int obstacleCellCount = 0; + int holeCellCount = 0; + int boundaryLayerCount = 0; + + for (int x = 0; x < gridMap.Width; x++) + { + for (int y = 0; y < gridMap.Height; y++) + { + GridCell cell = gridMap.Cells[x, y]; + bool isWalkable = cell.HasAnyWalkableLayer(); + if (isWalkable) + { + walkableCellCount++; + } + else + { + nonWalkableCellCount++; + } + + if (string.Equals(cell.CellType, "障碍物", StringComparison.Ordinal)) + { + obstacleCellCount++; + } + + if (string.Equals(cell.CellType, "空洞", StringComparison.Ordinal)) + { + holeCellCount++; + } + + if (cell.HeightLayers != null) + { + boundaryLayerCount += cell.HeightLayers.Count(layer => layer.IsBoundary); + } + } + } + + int totalCellCount = gridMap.Width * gridMap.Height; + + return new + { + width = gridMap.Width, + height = gridMap.Height, + totalCellCount, + walkableCellCount, + nonWalkableCellCount, + obstacleCellCount, + holeCellCount, + boundaryLayerCount, + obstacleRatio = totalCellCount == 0 ? 0.0 : (double)obstacleCellCount / totalCellCount, + nonWalkableRatio = totalCellCount == 0 ? 0.0 : (double)nonWalkableCellCount / totalCellCount + }; + } + + private static object BuildObstacleDiagnostics(GridMapGenerator gridMapGenerator, GridMap gridMap, double objectHeightInMeters, double safetyMarginInMeters) + { + if (gridMapGenerator == null || gridMap == null) + { + return null; + } + + double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); + double scanHeightInModelUnits = (objectHeightInMeters + safetyMarginInMeters) * metersToModelUnits; + + var traversableItems = CategoryAttributeManager.GetAllTraversableLogisticsItems().ToList(); + var irrelevantItems = CategoryAttributeManager.GetLogisticsItemsByType("无关项"); + + var collectRelatedItemsMethod = typeof(GridMapGenerator).GetMethod( + "CollectRelatedItems", + System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic); + var postProcessGeometryItemsMethod = typeof(GridMapGenerator).GetMethod( + "PostProcessGeometryItems", + System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic); + var getObstacleElevationRangeMethod = typeof(GridMapGenerator).GetMethod( + "GetObstacleElevationRange", + System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic, + null, + new[] { typeof(Point3D), typeof(Point3D), typeof(GridMap) }, + null); + var calculateBoundingBoxGridCoverageMethod = typeof(GridMapGenerator).GetMethod( + "CalculateBoundingBoxGridCoverage", + System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic); + + var traversableRelatedItems = (HashSet)collectRelatedItemsMethod.Invoke(gridMapGenerator, new object[] { traversableItems }); + var irrelevantRelatedItems = (HashSet)collectRelatedItemsMethod.Invoke(gridMapGenerator, new object[] { irrelevantItems }); + var geometryItems = ModelItemAnalysisHelper.GetAllVisibleGeometryItems(); + var itemCache = (Dictionary)postProcessGeometryItemsMethod.Invoke( + gridMapGenerator, + new object[] { geometryItems, traversableRelatedItems, irrelevantRelatedItems }); + + var validItems = itemCache + .Where(kvp => kvp.Value.HasGeometry && + !kvp.Value.IsContainer && + !kvp.Value.IsChannelItem && + !kvp.Value.IsChildOfChannel && + kvp.Value.BoundingBox != null && + kvp.Value.IsInScanHeightRange) + .ToList(); + + double walkableMinElevation = double.MaxValue; + double walkableMaxElevation = double.MinValue; + int walkableGridCount = 0; + for (int x = 0; x < gridMap.Width; x++) + { + for (int y = 0; y < gridMap.Height; y++) + { + GridCell cell = gridMap.Cells[x, y]; + if (!cell.HasAnyWalkableLayer() || cell.HeightLayers == null || cell.HeightLayers.Count == 0) + { + continue; + } + + walkableGridCount++; + double cellElevation = cell.HeightLayers[0].Z; + walkableMinElevation = Math.Min(walkableMinElevation, cellElevation); + walkableMaxElevation = Math.Max(walkableMaxElevation, cellElevation); + } + } + + if (walkableGridCount == 0) + { + walkableMinElevation = 0.0; + walkableMaxElevation = 0.0; + } + + double scanMin = walkableMinElevation; + double scanMax = walkableMaxElevation + scanHeightInModelUnits; + + var keptItems = new List(); + var filteredSampleItems = new List(); + int filteredByHeightCount = 0; + + foreach (var kvp in validItems) + { + BoundingBox3D bbox = kvp.Value.BoundingBox; + var elevationRange = ((double min, double max))getObstacleElevationRangeMethod.Invoke( + gridMapGenerator, + new object[] { bbox.Min, bbox.Max, gridMap }); + + bool isInRange = !(elevationRange.max <= scanMin || elevationRange.min > scanMax); + if (!isInRange) + { + filteredByHeightCount++; + if (filteredSampleItems.Count < 10) + { + filteredSampleItems.Add(new + { + displayName = kvp.Key.DisplayName, + bboxMin = SerializePoint3D(bbox.Min), + bboxMax = SerializePoint3D(bbox.Max), + minElevation = elevationRange.min, + maxElevation = elevationRange.max + }); + } + + continue; + } + + if (keptItems.Count < 30) + { + var coveredCells = (List<(int x, int y)>)calculateBoundingBoxGridCoverageMethod.Invoke( + gridMapGenerator, + new object[] { bbox, gridMap }); + + object triangleBounds = keptItems.Count < 8 + ? SerializeTriangleBounds(kvp.Key) + : null; + + keptItems.Add(new + { + displayName = kvp.Key.DisplayName, + bboxMin = SerializePoint3D(bbox.Min), + bboxMax = SerializePoint3D(bbox.Max), + triangleBounds, + minElevation = elevationRange.min, + maxElevation = elevationRange.max, + coveredCellCount = coveredCells.Count + }); + } + } + + return new + { + geometryItemCount = geometryItems.Count, + traversableItemCount = traversableItems.Count, + traversableRelatedItemCount = traversableRelatedItems.Count, + irrelevantItemCount = irrelevantItems.Count, + irrelevantRelatedItemCount = irrelevantRelatedItems.Count, + postProcessedItemCount = itemCache.Count, + validItemCount = validItems.Count, + walkableGridCount, + walkableMinElevation, + walkableMaxElevation, + scanHeightInModelUnits, + scanMin, + scanMax, + filteredByHeightCount, + keptItemSample = keptItems, + filteredByHeightSample = filteredSampleItems + }; + } + + private static object SerializeTriangleBounds(ModelItem item) + { + if (item == null) + { + return null; + } + + List triangles = GeometryHelper.ExtractTriangles(new[] { item }); + if (triangles == null || triangles.Count == 0) + { + return null; + } + + double minX = triangles.Min(t => Math.Min(t.Point1.X, Math.Min(t.Point2.X, t.Point3.X))); + double minY = triangles.Min(t => Math.Min(t.Point1.Y, Math.Min(t.Point2.Y, t.Point3.Y))); + double minZ = triangles.Min(t => Math.Min(t.Point1.Z, Math.Min(t.Point2.Z, t.Point3.Z))); + double maxX = triangles.Max(t => Math.Max(t.Point1.X, Math.Max(t.Point2.X, t.Point3.X))); + double maxY = triangles.Max(t => Math.Max(t.Point1.Y, Math.Max(t.Point2.Y, t.Point3.Y))); + double maxZ = triangles.Max(t => Math.Max(t.Point1.Z, Math.Max(t.Point2.Z, t.Point3.Z))); + + return new + { + min = SerializePoint3D(new Point3D(minX, minY, minZ)), + max = SerializePoint3D(new Point3D(maxX, maxY, maxZ)) + }; + } + + private static T InvokePathManagerPrivate(PathPlanningManager pathManager, string methodName, params object[] args) + { + var method = typeof(PathPlanningManager).GetMethod( + methodName, + System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic); + + if (method == null) + { + throw new MissingMethodException(typeof(PathPlanningManager).FullName, methodName); + } + + object result = method.Invoke(pathManager, args); + if (result == null) + { + return default(T); + } + + return (T)result; + } + private static object SerializeRotation3D(Rotation3D rotation) { var linear = new Transform3D(rotation).Linear; diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index addaedd..ec1c023 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -1354,7 +1354,7 @@ namespace NavisworksTransport.PathPlanning { foreach (var layer in cell.HeightLayers) { - if (layer.PassableHeight.GetSpan() >= objectHeight) + if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight) return true; } return false; @@ -1379,7 +1379,7 @@ namespace NavisworksTransport.PathPlanning foreach (var layer in cell.HeightLayers) { - if (layer.PassableHeight.GetSpan() >= objectHeight) + if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight) return true; } @@ -1403,6 +1403,9 @@ namespace NavisworksTransport.PathPlanning foreach (var layer in layers) { + if (!layer.IsWalkable) + continue; + // 必须满足物体高度 if (layer.PassableHeight.GetSpan() < objectHeight) continue; @@ -2306,7 +2309,7 @@ namespace NavisworksTransport.PathPlanning if (layers.Count == 1) { var layer = layers[0]; - if (layer.PassableHeight.GetSpan() >= objectHeight) + if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight) return layer; return null; } @@ -2342,6 +2345,9 @@ namespace NavisworksTransport.PathPlanning foreach (var layer in layers) { + if (!layer.IsWalkable) + continue; + if (layer.PassableHeight.GetSpan() < objectHeight) continue; @@ -2582,9 +2588,6 @@ namespace NavisworksTransport.PathPlanning // 更新路径结果 pathResult.PathPoints = optimizedPoints; - // 🔥 步骤2：在现有优化基础上应用专门的斜线优化 - // 2D场景：高度差为0，不影响原有逻辑 - // 3D场景：通过高度差检查保护楼梯路径 var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap); if (diagonalOptimizedPoints.Count != optimizedPoints.Count) diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs index 4c2a29c..baee609 100644 --- a/src/PathPlanning/ChannelBasedGridBuilder.cs +++ b/src/PathPlanning/ChannelBasedGridBuilder.cs @@ -147,6 +147,7 @@ namespace NavisworksTransport.PathPlanning double minZ = double.MaxValue; double maxZ = double.MinValue; int walkableCount = 0; + var elevationCounts = new Dictionary(); LogManager.Info("[通道Z值计算] 开始计算可通行网格Z值范围"); @@ -168,6 +169,16 @@ namespace NavisworksTransport.PathPlanning minZ = cellZ; if (cellZ > maxZ) maxZ = cellZ; + + double roundedElevation = Math.Round(cellZ, 3); + if (elevationCounts.ContainsKey(roundedElevation)) + { + elevationCounts[roundedElevation]++; + } + else + { + elevationCounts[roundedElevation] = 1; + } } } } @@ -181,6 +192,17 @@ namespace NavisworksTransport.PathPlanning } LogManager.Info($"[通道Z值计算] 计算完成 - 可通行网格: {walkableCount}个, Z范围: [{minZ:F2}, {maxZ:F2}], 高度差: {maxZ - minZ:F2}"); + if (elevationCounts.Count > 0) + { + string distributionSummary = string.Join( + ", ", + elevationCounts + .OrderByDescending(kvp => kvp.Value) + .ThenBy(kvp => kvp.Key) + .Take(8) + .Select(kvp => $"{kvp.Key:F3}:{kvp.Value}")); + LogManager.Info($"[通道Z值计算] 可通行格高程分布样本(Top8): {distributionSummary}"); + } return (minZ, maxZ, walkableCount); } @@ -207,6 +229,9 @@ namespace NavisworksTransport.PathPlanning return; } + double upwardTriangleMinElevation = double.MaxValue; + double upwardTriangleMaxElevation = double.MinValue; + // 统一投影处理 int processedTriangles = 0; foreach (var triangle in triangles) @@ -216,6 +241,17 @@ namespace NavisworksTransport.PathPlanning // 根据法向量决定处理方式（使用坐标系判断向上） if (GetUpComponent(normal) > 0) // 朝上的面 { + upwardTriangleMinElevation = Math.Min( + upwardTriangleMinElevation, + Math.Min( + _coordinateSystem.GetElevation(triangle.Point1), + Math.Min(_coordinateSystem.GetElevation(triangle.Point2), _coordinateSystem.GetElevation(triangle.Point3)))); + upwardTriangleMaxElevation = Math.Max( + upwardTriangleMaxElevation, + Math.Max( + _coordinateSystem.GetElevation(triangle.Point1), + Math.Max(_coordinateSystem.GetElevation(triangle.Point2), _coordinateSystem.GetElevation(triangle.Point3)))); + RasterizeTriangleToGrid(gridMap, triangle, channel, channelSpeedLimit); processedTriangles++; } @@ -223,6 +259,11 @@ namespace NavisworksTransport.PathPlanning } LogManager.Info($"[通道网格构建器] 投影完成：{processedTriangles}/{triangles.Count} 个三角形"); + if (processedTriangles > 0) + { + LogManager.Info( + $"[通道网格构建器] 朝上三角高程范围: [{upwardTriangleMinElevation:F2}, {upwardTriangleMaxElevation:F2}]"); + } } /// diff --git a/src/PathPlanning/ChannelHeightDetector.cs b/src/PathPlanning/ChannelHeightDetector.cs index 16ff639..c9dbf68 100644 --- a/src/PathPlanning/ChannelHeightDetector.cs +++ b/src/PathPlanning/ChannelHeightDetector.cs @@ -65,8 +65,9 @@ namespace NavisworksTransport.PathPlanning var containingChannel = FindContainingChannel(position, channelItems); if (containingChannel == null) { - LogManager.Info($"[高度检测] ❌ 位置 ({position.X:F2}, {position.Y:F2}) 未找到包含的通道，使用原始Z坐标: {position.Z:F2}"); - return position.Z; + double fallbackElevation = _coordinateSystem.GetElevation(position); + LogManager.Info($"[高度检测] ❌ 位置 ({position.X:F2}, {position.Y:F2}, {position.Z:F2}) 未找到包含的通道，使用当前宿主高程: {fallbackElevation:F2}"); + return fallbackElevation; } //LogManager.Info($"[高度检测] ✅ 找到包含通道: {containingChannel.DisplayName}"); @@ -101,7 +102,7 @@ namespace NavisworksTransport.PathPlanning catch (Exception ex) { LogManager.Error($"[高度检测] 检测高度时发生错误: {ex.Message}"); - return position.Z; // 发生错误时使用原始Z坐标 + return _coordinateSystem.GetElevation(position); } } @@ -127,8 +128,9 @@ namespace NavisworksTransport.PathPlanning var containingChannel = FindContainingChannel(position, channelItems); if (containingChannel == null) { - LogManager.Info($"[高度检测] ❌ 位置 ({position.X:F2}, {position.Y:F2}) 未找到包含的通道，使用原始Z坐标: {position.Z:F2}"); - return position.Z; + double fallbackElevation = _coordinateSystem.GetElevation(position); + LogManager.Info($"[高度检测] ❌ 位置 ({position.X:F2}, {position.Y:F2}, {position.Z:F2}) 未找到包含的通道，使用当前宿主高程: {fallbackElevation:F2}"); + return fallbackElevation; } //LogManager.Info($"[高度检测] ✅ 找到包含通道: {containingChannel.DisplayName}"); @@ -159,19 +161,20 @@ namespace NavisworksTransport.PathPlanning var bounds = containingChannel.BoundingBox(); if (bounds != null) { - var fallbackTopHeight = bounds.Max.Z; + var (_, fallbackTopHeight) = GetBoundsElevationRange(bounds, _coordinateSystem); LogManager.Info($"[高度检测] 使用边界框顶面高度: {fallbackTopHeight:F2}"); return fallbackTopHeight; } // 最后的备选方案 - LogManager.Warning($"[高度检测] ⚠️ 无法获取通道顶面高度，使用原始Z坐标: {position.Z:F2}"); - return position.Z; + double finalFallbackElevation = _coordinateSystem.GetElevation(position); + LogManager.Warning($"[高度检测] ⚠️ 无法获取通道顶面高度，使用当前宿主高程: {finalFallbackElevation:F2}"); + return finalFallbackElevation; } catch (Exception ex) { LogManager.Error($"[高度检测] 检测顶面高度时发生错误: {ex.Message}"); - return position.Z; // 发生错误时使用原始Z坐标 + return _coordinateSystem.GetElevation(position); } } @@ -198,11 +201,13 @@ namespace NavisworksTransport.PathPlanning return null; } + var (floorHeight, ceilingHeight) = GetBoundsElevationRange(bounds, _coordinateSystem); + // 创建高度信息对象 var heightInfo = new ChannelHeightInfo { - FloorHeight = bounds.Min.Z, - CeilingHeight = bounds.Max.Z, + FloorHeight = floorHeight, + CeilingHeight = ceilingHeight, Type = ChannelType.Other, // 不再依赖类型判断 HeightProfile = new List() }; @@ -255,11 +260,7 @@ namespace NavisworksTransport.PathPlanning return false; var bounds = channel.Geometry.BoundingBox; - - // 检查XY平面是否在边界内（Z方向放宽检查） - return position.X >= bounds.Min.X && position.X <= bounds.Max.X && - position.Y >= bounds.Min.Y && position.Y <= bounds.Max.Y && - position.Z >= bounds.Min.Z - 2.0 && position.Z <= bounds.Max.Z + 2.0; // Z方向容差2米 + return IsPositionWithinBounds(position, bounds, _coordinateSystem, 2.0); } catch { @@ -283,14 +284,14 @@ namespace NavisworksTransport.PathPlanning for (int i = 0; i < sampleCount; i++) { double ratio = (double)i / (sampleCount - 1); - var sampleX = bounds.Min.X + (bounds.Max.X - bounds.Min.X) * ratio; - var sampleY = bounds.Min.Y + (bounds.Max.Y - bounds.Min.Y) * ratio; + var samplePosition = CreateHeightProfileSamplePoint(bounds, _coordinateSystem, ratio); + var (floorHeight, ceilingHeight) = GetBoundsElevationRange(bounds, _coordinateSystem); var sample = new HeightSample { - Position = new Point3D(sampleX, sampleY, bounds.Min.Z), - FloorHeight = bounds.Min.Z, - CeilingHeight = bounds.Max.Z + Position = samplePosition, + FloorHeight = floorHeight, + CeilingHeight = ceilingHeight }; heightInfo.HeightProfile.Add(sample); @@ -346,7 +347,7 @@ namespace NavisworksTransport.PathPlanning // 找到最近的两个采样点进行插值 var closestSamples = heightProfile - .OrderBy(s => Math.Sqrt(Math.Pow(s.Position.X - position.X, 2) + Math.Pow(s.Position.Y - position.Y, 2))) + .OrderBy(s => CalculatePlanarDistance(s.Position, position, _coordinateSystem)) .Take(2) .ToList(); @@ -359,8 +360,8 @@ namespace NavisworksTransport.PathPlanning var sample1 = closestSamples[0]; var sample2 = closestSamples[1]; - var distance1 = Math.Sqrt(Math.Pow(sample1.Position.X - position.X, 2) + Math.Pow(sample1.Position.Y - position.Y, 2)); - var distance2 = Math.Sqrt(Math.Pow(sample2.Position.X - position.X, 2) + Math.Pow(sample2.Position.Y - position.Y, 2)); + var distance1 = CalculatePlanarDistance(sample1.Position, position, _coordinateSystem); + var distance2 = CalculatePlanarDistance(sample2.Position, position, _coordinateSystem); var totalDistance = distance1 + distance2; if (totalDistance < 0.001) return sample1.CeilingHeight; @@ -386,29 +387,29 @@ namespace NavisworksTransport.PathPlanning { var bbox = channel.Geometry.BoundingBox; - // 检查位置是否在通道的X,Y范围内 - if (position.X >= bbox.Min.X && position.X <= bbox.Max.X && - position.Y >= bbox.Min.Y && position.Y <= bbox.Max.Y) + if (IsPositionWithinBounds(position, bbox, _coordinateSystem, 0.0)) { // 位置在通道范围内，尝试分析表面高度 - var surfaceHeight = AnalyzeSurfaceHeightAtPosition(position, channel, bbox.Min.Z, bbox.Max.Z); + var (floorHeight, ceilingHeight) = GetBoundsElevationRange(bbox, _coordinateSystem); + var surfaceHeight = AnalyzeSurfaceHeightAtPosition(position, channel, floorHeight, ceilingHeight); LogManager.Debug($"[射线投射] 表面高度分析结果: {surfaceHeight:F2}"); return surfaceHeight; } else { - LogManager.Warning($"[射线投射] 位置({position.X:F2}, {position.Y:F2})不在通道范围内"); - return position.Z; + LogManager.Warning($"[射线投射] 位置({position.X:F2}, {position.Y:F2}, {position.Z:F2})不在通道范围内"); + return _coordinateSystem.GetElevation(position); } } - LogManager.Warning($"[射线投射] 通道几何信息无效，使用原始Z坐标: {position.Z:F2}"); - return position.Z; + double fallbackElevation = _coordinateSystem.GetElevation(position); + LogManager.Warning($"[射线投射] 通道几何信息无效，使用当前宿主高程: {fallbackElevation:F2}"); + return fallbackElevation; } catch (Exception ex) { LogManager.Error($"[射线投射] 射线投射计算失败: {ex.Message}"); - return position.Z; + return _coordinateSystem.GetElevation(position); } } @@ -489,7 +490,11 @@ namespace NavisworksTransport.PathPlanning // 验证拾取到的模型是否为目标通道（或其子项） if (pickResult.ModelItem.Equals(channel) || ModelItemAnalysisHelper.IsChildOf(pickResult.ModelItem, channel)) { - height = pickResult.Point.Z; + height = GetPickedSurfaceElevation( + pickResult.Point.X, + pickResult.Point.Y, + pickResult.Point.Z, + _coordinateSystem.Type); LogManager.Info($"[几何分析] ✅ 使用投影法获得精确表面高度: {height:F2}，通道: {channel.DisplayName}"); return true; } @@ -556,9 +561,14 @@ namespace NavisworksTransport.PathPlanning /// private Point3D CalculateRelativePositionInChannel(Point3D position, BoundingBox3D bbox) { - var relativeX = (position.X - bbox.Min.X) / (bbox.Max.X - bbox.Min.X); - var relativeY = (position.Y - bbox.Min.Y) / (bbox.Max.Y - bbox.Min.Y); - var relativeZ = 0.0; // Z方向待计算 + var (positionH1, positionH2) = _coordinateSystem.GetHorizontalCoords(position); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bbox); + var (minElevation, maxElevation) = GetBoundsElevationRange(bbox, _coordinateSystem); + + var relativeX = Math.Abs(maxH1 - minH1) < 1e-9 ? 0.0 : (positionH1 - minH1) / (maxH1 - minH1); + var relativeY = Math.Abs(maxH2 - minH2) < 1e-9 ? 0.0 : (positionH2 - minH2) / (maxH2 - minH2); + var currentElevation = _coordinateSystem.GetElevation(position); + var relativeZ = Math.Abs(maxElevation - minElevation) < 1e-9 ? 0.0 : (currentElevation - minElevation) / (maxElevation - minElevation); return new Point3D(relativeX, relativeY, relativeZ); } @@ -583,8 +593,9 @@ namespace NavisworksTransport.PathPlanning private string GenerateCacheKey(ModelItem channel, Point3D position) { // 使用通道的实例ID和位置的网格坐标作为缓存键 - var gridX = (int)(position.X / 1.0); // 1米网格 - var gridY = (int)(position.Y / 1.0); + var (h1, h2) = _coordinateSystem.GetHorizontalCoords(position); + var gridX = (int)(h1 / 1.0); + var gridY = (int)(h2 / 1.0); return $"{channel.InstanceGuid}_{gridX}_{gridY}"; } @@ -632,7 +643,7 @@ namespace NavisworksTransport.PathPlanning foreach (var testHeight in testHeights) { - var testPoint = new Point3D(position.X, position.Y, testHeight); + var testPoint = _coordinateSystem.SetElevation(position, testHeight); // 将3D点投影到屏幕 if (TryProjectWorldToScreen(testPoint, activeView, out GridPoint2D screenPoint)) @@ -645,8 +656,9 @@ namespace NavisworksTransport.PathPlanning var pickResult = activeView.PickItemFromPoint(screenPoint.X, screenPoint.Y); if (pickResult != null && (pickResult.ModelItem.Equals(channel) || ModelItemAnalysisHelper.IsChildOf(pickResult.ModelItem, channel))) { - LogManager.Info($"[射线投射] ✅ 成功检测到表面点: Z={pickResult.Point.Z:F2}"); - return pickResult.Point.Z; + double detectedElevation = _coordinateSystem.GetElevation(pickResult.Point); + LogManager.Info($"[射线投射] ✅ 成功检测到表面点高程: {detectedElevation:F2}"); + return detectedElevation; } } } @@ -790,11 +802,11 @@ namespace NavisworksTransport.PathPlanning var sampleRadius = 50.0; // 50单位半径内采样 var samplePoints = new[] { - new Point3D(position.X, position.Y, 0), // 中心点 - new Point3D(position.X + sampleRadius, position.Y, 0), // 右 - new Point3D(position.X - sampleRadius, position.Y, 0), // 左 - new Point3D(position.X, position.Y + sampleRadius, 0), // 上 - new Point3D(position.X, position.Y - sampleRadius, 0), // 下 + CreatePlanarOffsetPoint(position, _coordinateSystem, 0.0, 0.0), + CreatePlanarOffsetPoint(position, _coordinateSystem, sampleRadius, 0.0), + CreatePlanarOffsetPoint(position, _coordinateSystem, -sampleRadius, 0.0), + CreatePlanarOffsetPoint(position, _coordinateSystem, 0.0, sampleRadius), + CreatePlanarOffsetPoint(position, _coordinateSystem, 0.0, -sampleRadius), }; foreach (var samplePoint in samplePoints) @@ -807,8 +819,9 @@ namespace NavisworksTransport.PathPlanning var pickResult = activeView.PickItemFromPoint((int)screenPoint.X, (int)screenPoint.Y); if (pickResult != null && (pickResult.ModelItem.Equals(channel) || ModelItemAnalysisHelper.IsChildOf(pickResult.ModelItem, channel))) { - validHeights.Add(pickResult.Point.Z); - LogManager.Debug($"[多点采样] 采样点({samplePoint.X:F1}, {samplePoint.Y:F1})高度: {pickResult.Point.Z:F2}"); + double detectedElevation = _coordinateSystem.GetElevation(pickResult.Point); + validHeights.Add(detectedElevation); + LogManager.Debug($"[多点采样] 采样点({samplePoint.X:F1}, {samplePoint.Y:F1}, {samplePoint.Z:F1})高程: {detectedElevation:F2}"); } } } @@ -832,6 +845,132 @@ namespace NavisworksTransport.PathPlanning return null; } } + + private static (double min, double max) GetBoundsElevationRange(BoundingBox3D bounds, ICoordinateSystem coordinateSystem) + { + if (bounds == null) + { + throw new ArgumentNullException(nameof(bounds)); + } + + if (coordinateSystem == null) + { + throw new ArgumentNullException(nameof(coordinateSystem)); + } + + return GetBoundsElevationRange( + bounds.Min.X, bounds.Min.Y, bounds.Min.Z, + bounds.Max.X, bounds.Max.Y, bounds.Max.Z, + coordinateSystem.Type); + } + + private static Point3D CreateHeightProfileSamplePoint(BoundingBox3D bounds, ICoordinateSystem coordinateSystem, double ratio) + { + if (bounds == null) + { + throw new ArgumentNullException(nameof(bounds)); + } + + if (coordinateSystem == null) + { + throw new ArgumentNullException(nameof(coordinateSystem)); + } + + return CreateHeightProfileSamplePoint( + bounds.Min.X, bounds.Min.Y, bounds.Min.Z, + bounds.Max.X, bounds.Max.Y, bounds.Max.Z, + coordinateSystem.Type, + ratio); + } + + private static bool IsPositionWithinBounds(Point3D position, BoundingBox3D bounds, ICoordinateSystem coordinateSystem, double elevationTolerance) + { + if (position == null || bounds == null || coordinateSystem == null) + { + return false; + } + + var (positionH1, positionH2) = coordinateSystem.GetHorizontalCoords(position); + var (minH1, maxH1, minH2, maxH2) = coordinateSystem.GetHorizontalRange(bounds); + var (minElevation, maxElevation) = coordinateSystem.GetHeightRange(bounds); + var elevation = coordinateSystem.GetElevation(position); + + return positionH1 >= minH1 && positionH1 <= maxH1 && + positionH2 >= minH2 && positionH2 <= maxH2 && + elevation >= minElevation - elevationTolerance && + elevation <= maxElevation + elevationTolerance; + } + + private static double CalculatePlanarDistance(Point3D pointA, Point3D pointB, ICoordinateSystem coordinateSystem) + { + var (aH1, aH2) = coordinateSystem.GetHorizontalCoords(pointA); + var (bH1, bH2) = coordinateSystem.GetHorizontalCoords(pointB); + return Math.Sqrt(Math.Pow(aH1 - bH1, 2) + Math.Pow(aH2 - bH2, 2)); + } + + private static Point3D CreatePlanarOffsetPoint(Point3D position, ICoordinateSystem coordinateSystem, double deltaH1, double deltaH2) + { + var (h1, h2) = coordinateSystem.GetHorizontalCoords(position); + var elevation = coordinateSystem.GetElevation(position); + return coordinateSystem.CreatePoint(h1 + deltaH1, h2 + deltaH2, elevation); + } + + private static (double min, double max) GetBoundsElevationRange( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + double elevation1 = HostPlanarGridHelper.GetElevation3( + new System.Numerics.Vector3((float)minX, (float)minY, (float)minZ), + adapter); + double elevation2 = HostPlanarGridHelper.GetElevation3( + new System.Numerics.Vector3((float)maxX, (float)maxY, (float)maxZ), + adapter); + return (Math.Min(elevation1, elevation2), Math.Max(elevation1, elevation2)); + } + + private static Point3D CreateHeightProfileSamplePoint( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + CoordinateSystemType coordinateSystemType, + double ratio) + { + var samplePoint = CreateHeightProfileSamplePoint3( + minX, minY, minZ, + maxX, maxY, maxZ, + coordinateSystemType, + ratio); + return new Point3D(samplePoint.X, samplePoint.Y, samplePoint.Z); + } + + private static System.Numerics.Vector3 CreateHeightProfileSamplePoint3( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + CoordinateSystemType coordinateSystemType, + double ratio) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + var hostMin = new System.Numerics.Vector3((float)minX, (float)minY, (float)minZ); + var hostMax = new System.Numerics.Vector3((float)maxX, (float)maxY, (float)maxZ); + var (minH1, maxH1, minH2, maxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + var (minElevation, _) = GetBoundsElevationRange(minX, minY, minZ, maxX, maxY, maxZ, coordinateSystemType); + double sampleH1 = minH1 + (maxH1 - minH1) * ratio; + double sampleH2 = minH2 + (maxH2 - minH2) * ratio; + return HostPlanarGridHelper.CreateHostPoint3(sampleH1, sampleH2, minElevation, adapter); + } + + private static double GetPickedSurfaceElevation( + double pointX, + double pointY, + double pointZ, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + return HostPlanarGridHelper.GetElevation3( + new System.Numerics.Vector3((float)pointX, (float)pointY, (float)pointZ), + adapter); + } } /// @@ -912,4 +1051,4 @@ namespace NavisworksTransport.PathPlanning Other } -} \ No newline at end of file +} diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index 3d1bc85..fbebbaa 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -22,6 +22,11 @@ namespace NavisworksTransport.PathPlanning /// 当前使用的坐标系 /// private readonly ICoordinateSystem _coordinateSystem; + + /// + /// 当前网格采用的宿主坐标系类型。 + /// + public CoordinateSystemType CoordinateSystemType => _coordinateSystem.Type; /// /// 网格宽度（单元格数量） /// diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index 934c945..a92165a 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -1,9 +1,11 @@ using System; using System.Collections.Generic; using System.Linq; +using System.Numerics; using Autodesk.Navisworks.Api; using NavisworksTransport.Utils; using NavisworksTransport.Core.Config; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { @@ -279,8 +281,13 @@ namespace NavisworksTransport.PathPlanning string heightLimitStr = CategoryAttributeManager.GetLogisticsPropertyValue(doorItem, CategoryAttributeManager.LogisticsProperties.HEIGHT_LIMIT); double configuredHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高"); + var (doorBottomElevation, doorTopElevation) = GetObstacleElevationRange( + bbox.Min.X, bbox.Min.Y, bbox.Min.Z, + bbox.Max.X, bbox.Max.Y, bbox.Max.Z, + gridMap.CoordinateSystemType); + // 计算门的实际物理高度 - double actualDoorHeight = bbox.Max.Z - bbox.Min.Z; + double actualDoorHeight = doorTopElevation - doorBottomElevation; double doorHeight; if (configuredHeight <= 0) @@ -300,9 +307,6 @@ namespace NavisworksTransport.PathPlanning LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 实际高度: {actualDoorHeight:F2}, 限高:{configuredHeightInModelUnits:F2}, 有效高度: {doorHeight:F2}"); } - // 获取门底部的Z坐标 - double doorBottomZ = bbox.Min.Z; - // 获取门的限速 double doorSpeedLimit = CategoryAttributeManager.GetSpeedLimit(doorItem); LogManager.Info($"[门网格限速] 门物品 '{doorItem.DisplayName}' 限速: {doorSpeedLimit:F2}m/s"); @@ -314,7 +318,7 @@ namespace NavisworksTransport.PathPlanning // 计算门网格的精确世界坐标 var world2D = gridMap.GridToWorld2D(gridPos); - var preciseWorldPosition = new Point3D(world2D.X, world2D.Y, doorBottomZ); + var preciseWorldPosition = gridMap.SetWorldElevation(world2D, doorBottomElevation); // 使用GridCellBuilder创建完整配置的门GridCell var cell = GridCellBuilder.Door(preciseWorldPosition, doorItem, doorSpeedLimit); @@ -331,15 +335,15 @@ namespace NavisworksTransport.PathPlanning var doorLayer = new HeightLayer { Type = "门", - Z = doorBottomZ, - PassableHeight = new HeightInterval(doorBottomZ, doorBottomZ + doorHeight), + Z = doorBottomElevation, + PassableHeight = new HeightInterval(doorBottomElevation, doorBottomElevation + doorHeight), IsWalkable = true, // 关键：门是可通行的 SpeedLimit = doorSpeedLimit, SourceItem = doorItem, IsBoundary = false }; gridMap.AddHeightLayer(gridPos, doorLayer); - LogManager.Info($"[门高度层添加] 网格({gridPos.X},{gridPos.Y}) 高度范围: [{doorBottomZ:F2}, {doorBottomZ + doorHeight:F2}], 可通行高度: {doorHeight:F2}"); + LogManager.Info($"[门高度层添加] 网格({gridPos.X},{gridPos.Y}) 高度范围: [{doorBottomElevation:F2}, {doorBottomElevation + doorHeight:F2}], 可通行高度: {doorHeight:F2}"); } processedCount++; @@ -538,7 +542,13 @@ namespace NavisworksTransport.PathPlanning else { // 邻居无层（Unknown）：使用网格底部Z值 - neighborZ = gridMap.Bounds.Min.Z; + neighborZ = gridMap.Bounds != null + ? ResolveBoundsMinElevation( + gridMap.Bounds.Min.X, + gridMap.Bounds.Min.Y, + gridMap.Bounds.Min.Z, + gridMap.CoordinateSystemType) + : 0.0; } // 计算层间高度差 @@ -624,9 +634,9 @@ namespace NavisworksTransport.PathPlanning // 重要修复：使用通道顶面作为垂直扫描的起点，而不是底面 // 从通道构建器的日志可知通道总边界MaxZ = 38.8，这是正确的扫描起点 - double channelTopZ = GetChannelTopZ(worldPos, gridMap); - - points.Add(new Point3D(worldPos.X, worldPos.Y, channelTopZ)); + double channelTopElevation = GetChannelTopZ(worldPos, gridMap); + + points.Add(gridMap.SetWorldElevation(worldPos, channelTopElevation)); } } } @@ -644,28 +654,25 @@ namespace NavisworksTransport.PathPlanning { try { - // 从通道构建器的日志信息可知： - // 通道总边界: [-242.7,-62.3,35.4] - [10.8,70.9,38.8] - // 通道顶面Z坐标为38.8，这应该作为垂直扫描的起点 - - // 方法1: 优先使用网格边界的MaxZ（通道顶面） - double channelTopZ = gridMap.Bounds.Max.Z; - - // 方法2: 备选方案 - 如果网格边界不可用，使用固定的通道高度 - if (channelTopZ <= gridMap.Bounds.Min.Z) + if (gridMap.Bounds != null) { - // 基于日志观察到的通道Z范围 [35.4, 38.8]，使用顶面 - channelTopZ = worldPos.Z + 3.4; // 假设通道高度为3.4米 - LogManager.Debug($"[通道顶面计算] 使用备选方案: 底面{worldPos.Z:F2} + 3.4m = 顶面{channelTopZ:F2}"); + var (minElevation, maxElevation) = + GetObstacleElevationRange(gridMap.Bounds.Min, gridMap.Bounds.Max, gridMap); + + if (maxElevation > minElevation) + { + return maxElevation; + } } - - // LogManager.Debug($"[通道顶面计算] 世界位置({worldPos.X:F2}, {worldPos.Y:F2}) -> 通道顶面Z={channelTopZ:F2}"); // 调试完成，日志已删除 - return channelTopZ; + + double fallbackTopElevation = gridMap.GetWorldElevation(worldPos) + 3.4; + LogManager.Debug($"[通道顶面计算] 使用备选方案: 底面高程{gridMap.GetWorldElevation(worldPos):F2} + 3.4m = 顶面高程{fallbackTopElevation:F2}"); + return fallbackTopElevation; } catch (Exception ex) { - LogManager.Warning($"[通道顶面计算] 计算失败: {ex.Message}，使用世界位置Z坐标"); - return worldPos.Z; + LogManager.Warning($"[通道顶面计算] 计算失败: {ex.Message}，使用世界位置高程"); + return gridMap.GetWorldElevation(worldPos); } } @@ -686,7 +693,7 @@ namespace NavisworksTransport.PathPlanning var interval = kvp.Value; // 将世界坐标转换为网格坐标 - var gridPos = gridMap.WorldToGrid(new Point3D(point.X, point.Y, 0)); + var gridPos = gridMap.WorldToGrid(point); int gridX = gridPos.X; int gridY = gridPos.Y; @@ -1306,7 +1313,10 @@ namespace NavisworksTransport.PathPlanning var filterElapsed = (DateTime.Now - filterStart).TotalMilliseconds; LogManager.Info($"[高性能障碍物处理] 阶段3完成: 从缓存中筛选出 {validItems.Count} 个有效项，耗时: {filterElapsed:F1}ms"); - + + int horizontalOverlapCount = validItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap)); + LogManager.Info($"[高性能障碍物处理] 阶段3.5: 与当前网格平面范围相交的有效几何体: {horizontalOverlapCount}/{validItems.Count}"); + // 获取可通行网格的实际高度范围信息 var walkableMinZ = channelCoverage.WalkableMinZ; var walkableMaxZ = channelCoverage.WalkableMaxZ; @@ -1325,10 +1335,12 @@ namespace NavisworksTransport.PathPlanning { // 精确高度检查：使用几何体中心点对应的网格高度 var bbox = kvp.Value.BoundingBox; - var centerX = (bbox.Min.X + bbox.Max.X) / 2; - var centerY = (bbox.Min.Y + bbox.Max.Y) / 2; + var centerPoint = new Point3D( + (bbox.Min.X + bbox.Max.X) / 2, + (bbox.Min.Y + bbox.Max.Y) / 2, + (bbox.Min.Z + bbox.Max.Z) / 2); - var centerGridPos = gridMap.WorldToGrid(new Point3D(centerX, centerY, 0)); + var centerGridPos = gridMap.WorldToGrid(centerPoint); if (!gridMap.IsValidGridPosition(centerGridPos)) { return false; @@ -1342,7 +1354,9 @@ namespace NavisworksTransport.PathPlanning var scanMax = walkableMaxZ + scanHeightInModelUnits; // 只有与该网格高度范围重叠的几何体才进入处理 - bool isInRange = !(bbox.Max.Z <= scanMin || bbox.Min.Z > scanMax); + var (obstacleMinElevation, obstacleMaxElevation) = + GetObstacleElevationRange(bbox.Min, bbox.Max, gridMap); + bool isInRange = !(obstacleMaxElevation <= scanMin || obstacleMinElevation > scanMax); return isInRange; }) @@ -1366,9 +1380,12 @@ namespace NavisworksTransport.PathPlanning var gridCalcElapsed = (DateTime.Now - gridCalcStart).TotalMilliseconds; var totalCheckedItems = validItems.Count; var filteredItems = totalCheckedItems - heightCheckedItems.Count; + int overlappingItemsRetained = heightCheckedItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap)); + int overlappingItemsFiltered = validItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap)) - overlappingItemsRetained; LogManager.Info($"[高性能障碍物处理] 阶段4完成: 生成 {gridUpdates.Count} 个网格更新操作，耗时: {gridCalcElapsed:F1}ms"); LogManager.Info($"[精确高度检查] 检查了 {totalCheckedItems} 个几何体，过滤掉 {filteredItems} 个高度范围外的几何体，保留 {heightCheckedItems.Count} 个"); + LogManager.Info($"[精确高度检查] 其中与当前网格平面范围相交的有效几何体: 过滤 {overlappingItemsFiltered} 个, 保留 {overlappingItemsRetained} 个"); // 阶段5：网格状态更新（单线程写入，3D高度检查） LogManager.Info("[高性能障碍物处理] 阶段5: 单线程网格状态更新（3D高度检查）"); @@ -1409,8 +1426,7 @@ namespace NavisworksTransport.PathPlanning double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan(); // 障碍物的高度范围 - double obstacleMinZ = bbox.Min.Z; - double obstacleMaxZ = bbox.Max.Z; + var (obstacleMinZ, obstacleMaxZ) = GetObstacleElevationRange(bbox.Min, bbox.Max, gridMap); // 检查是否重叠 bool overlaps = !(obstacleMaxZ <= layerMinZ || obstacleMinZ > layerMaxZ); @@ -1466,6 +1482,138 @@ namespace NavisworksTransport.PathPlanning } + /// + /// 获取世界坐标范围在当前网格坐标语义下的高程区间。 + /// + private (double min, double max) GetObstacleElevationRange(Point3D minPoint, Point3D maxPoint, GridMap gridMap) + { + return GetObstacleElevationRange( + minPoint.X, minPoint.Y, minPoint.Z, + maxPoint.X, maxPoint.Y, maxPoint.Z, + gridMap.CoordinateSystemType); + } + + /// + /// 根据世界坐标最小/最大点，计算其在当前网格坐标语义下覆盖的平面网格范围。 + /// + private List<(int x, int y)> CalculateGridCoverageFromWorldExtents(Point3D minPoint, Point3D maxPoint, GridMap gridMap) + { + return CalculateGridCoverageFromWorldExtents( + minPoint.X, minPoint.Y, minPoint.Z, + maxPoint.X, maxPoint.Y, maxPoint.Z, + gridMap.Origin.X, gridMap.Origin.Y, gridMap.Origin.Z, + gridMap.Width, gridMap.Height, gridMap.CellSize, + gridMap.CoordinateSystemType); + } + + /// + /// 获取世界坐标范围在当前宿主坐标语义下的高程区间。 + /// 仅使用纯数值输入，便于单元测试验证多坐标系语义。 + /// + private (double min, double max) GetObstacleElevationRange( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + double elevation1 = HostPlanarGridHelper.GetElevation3(new Vector3((float)minX, (float)minY, (float)minZ), adapter); + double elevation2 = HostPlanarGridHelper.GetElevation3(new Vector3((float)maxX, (float)maxY, (float)maxZ), adapter); + return (Math.Min(elevation1, elevation2), Math.Max(elevation1, elevation2)); + } + + /// + /// 解析边界最小点在当前宿主坐标语义下的高程值。 + /// 仅修正“高程轴”的解释，不改变原有边界层业务逻辑。 + /// + private static double ResolveBoundsMinElevation( + double minX, double minY, double minZ, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + return HostPlanarGridHelper.GetElevation3(new Vector3((float)minX, (float)minY, (float)minZ), adapter); + } + + /// + /// 根据世界坐标最小/最大点与网格元数据，计算覆盖的平面网格范围。 + /// 仅使用纯数值输入，便于单元测试验证多坐标系语义。 + /// + private List<(int x, int y)> CalculateGridCoverageFromWorldExtents( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + double originX, double originY, double originZ, + int width, int height, double cellSize, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + var minPoint = new Vector3((float)minX, (float)minY, (float)minZ); + var maxPoint = new Vector3((float)maxX, (float)maxY, (float)maxZ); + var originPoint = new Vector3((float)originX, (float)originY, (float)originZ); + + var (minH1, minH2) = HostPlanarGridHelper.GetHorizontalCoords3(minPoint, adapter); + var (maxH1, maxH2) = HostPlanarGridHelper.GetHorizontalCoords3(maxPoint, adapter); + var (originH1, originH2) = HostPlanarGridHelper.GetHorizontalCoords3(originPoint, adapter); + + int minGridX = (int)Math.Round((minH1 - originH1) / cellSize); + int minGridY = (int)Math.Round((minH2 - originH2) / cellSize); + int maxGridX = (int)Math.Round((maxH1 - originH1) / cellSize); + int maxGridY = (int)Math.Round((maxH2 - originH2) / cellSize); + + int clampedMinX = Math.Max(0, Math.Min(minGridX, maxGridX)); + int clampedMinY = Math.Max(0, Math.Min(minGridY, maxGridY)); + int clampedMaxX = Math.Min(width - 1, Math.Max(minGridX, maxGridX)); + int clampedMaxY = Math.Min(height - 1, Math.Max(minGridY, maxGridY)); + + if (clampedMaxX < clampedMinX) clampedMaxX = clampedMinX; + if (clampedMaxY < clampedMinY) clampedMaxY = clampedMinY; + + var coveredCells = new List<(int x, int y)>(); + for (int x = clampedMinX; x <= clampedMaxX; x++) + { + for (int y = clampedMinY; y <= clampedMaxY; y++) + { + coveredCells.Add((x, y)); + } + } + + return coveredCells; + } + + /// + /// 判断包围盒在当前宿主平面语义下，是否与当前网格的平面范围真实相交。 + /// 这里只做诊断，不改变原有网格覆盖或裁剪逻辑。 + /// + private bool IntersectsGridPlanarExtents(BoundingBox3D bbox, GridMap gridMap) + { + if (bbox == null || gridMap == null || gridMap.Bounds == null) + { + return false; + } + + var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType); + var hostMin = new Vector3((float)bbox.Min.X, (float)bbox.Min.Y, (float)bbox.Min.Z); + var hostMax = new Vector3((float)bbox.Max.X, (float)bbox.Max.Y, (float)bbox.Max.Z); + var gridHostMin = new Vector3((float)gridMap.Bounds.Min.X, (float)gridMap.Bounds.Min.Y, (float)gridMap.Bounds.Min.Z); + var gridHostMax = new Vector3((float)gridMap.Bounds.Max.X, (float)gridMap.Bounds.Max.Y, (float)gridMap.Bounds.Max.Z); + + var (bboxMinH1, bboxMaxH1, bboxMinH2, bboxMaxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + var (gridMinH1, gridMaxH1, gridMinH2, gridMaxH2) = HostPlanarGridHelper.GetHorizontalRange3(gridHostMin, gridHostMax, adapter); + + bool overlapH1 = !(bboxMaxH1 < gridMinH1 || bboxMinH1 > gridMaxH1); + bool overlapH2 = !(bboxMaxH2 < gridMinH2 || bboxMinH2 > gridMaxH2); + return overlapH1 && overlapH2; + } + + private static (double minH1, double maxH1, double minH2, double maxH2) GetPlanarRange( + Point3D minPoint, + Point3D maxPoint, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + var hostMin = new Vector3((float)minPoint.X, (float)minPoint.Y, (float)minPoint.Z); + var hostMax = new Vector3((float)maxPoint.X, (float)maxPoint.Y, (float)maxPoint.Z); + return HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + } + /// /// 计算包围盒在网格上覆盖的单元坐标 /// 严格按照"网格坐标代表左下角"的设计原则进行边界处理 @@ -1475,36 +1623,88 @@ namespace NavisworksTransport.PathPlanning /// 覆盖的网格单元坐标列表 private List<(int x, int y)> CalculateBoundingBoxGridCoverage(BoundingBox3D bbox, GridMap gridMap) { - var coveredCells = new List<(int x, int y)>(); - try { - // 直接使用WorldToGrid进行坐标转换 - var minGridPos = gridMap.WorldToGrid(new Point3D(bbox.Min.X, bbox.Min.Y, 0)); - var maxGridPos = gridMap.WorldToGrid(new Point3D(bbox.Max.X, bbox.Max.Y, 0)); - - // 确保坐标在有效范围内 - int minX = Math.Max(0, minGridPos.X); - int minY = Math.Max(0, minGridPos.Y); - int maxX = Math.Min(gridMap.Width - 1, maxGridPos.X); - int maxY = Math.Min(gridMap.Height - 1, maxGridPos.Y); - - // 确保至少覆盖一个网格（防止空包围盒） - if (maxX < minX) maxX = minX; - if (maxY < minY) maxY = minY; - - // 遍历覆盖的矩形区域 - for (int x = minX; x <= maxX; x++) - { - for (int y = minY; y <= maxY; y++) - { - coveredCells.Add((x, y)); - } - } + return CalculateGridCoverageFromWorldExtents(bbox.Min, bbox.Max, gridMap); } catch (Exception ex) { LogManager.Debug($"[包围盒覆盖计算] 计算失败: {ex.Message}"); + return new List<(int x, int y)>(); + } + } + + /// + /// 根据门包围盒、限宽和当前宿主平面语义，计算门开口在网格上的覆盖范围。 + /// 关键约束： + /// - 仅替换“哪两个轴组成水平平面”的解释，不改变历史限宽业务规则。 + /// - 门宽仍取平面上较大的那一维；限宽仍只裁剪门宽方向。 + /// + private List<(int x, int y)> CalculateDoorOpeningCoverageFromWorldExtents( + double minX, double minY, double minZ, + double maxX, double maxY, double maxZ, + double limitWidthInModelUnits, + double originX, double originY, double originZ, + int width, int height, double cellSize, + CoordinateSystemType coordinateSystemType) + { + var adapter = new HostCoordinateAdapter(coordinateSystemType); + var hostMin = new Vector3((float)minX, (float)minY, (float)minZ); + var hostMax = new Vector3((float)maxX, (float)maxY, (float)maxZ); + var hostOrigin = new Vector3((float)originX, (float)originY, (float)originZ); + var (minH1, maxH1, minH2, maxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + var (originH1, originH2) = HostPlanarGridHelper.GetHorizontalCoords3(hostOrigin, adapter); + + double planarWidth = maxH1 - minH1; + double planarDepth = maxH2 - minH2; + bool widthAlongH1 = planarWidth > planarDepth; + double doorWidth = Math.Max(planarWidth, planarDepth); + double effectiveWidth = Math.Min(limitWidthInModelUnits, doorWidth); + + double openingMinH1; + double openingMaxH1; + double openingMinH2; + double openingMaxH2; + + if (widthAlongH1) + { + double centerH1 = (minH1 + maxH1) / 2.0; + double halfWidth = effectiveWidth / 2.0; + openingMinH1 = centerH1 - halfWidth; + openingMaxH1 = centerH1 + halfWidth; + openingMinH2 = minH2; + openingMaxH2 = maxH2; + } + else + { + double centerH2 = (minH2 + maxH2) / 2.0; + double halfWidth = effectiveWidth / 2.0; + openingMinH1 = minH1; + openingMaxH1 = maxH1; + openingMinH2 = centerH2 - halfWidth; + openingMaxH2 = centerH2 + halfWidth; + } + + int minGridX = (int)Math.Round((openingMinH1 - originH1) / cellSize); + int minGridY = (int)Math.Round((openingMinH2 - originH2) / cellSize); + int maxGridX = (int)Math.Round((openingMaxH1 - originH1) / cellSize); + int maxGridY = (int)Math.Round((openingMaxH2 - originH2) / cellSize); + + int clampedMinX = Math.Max(0, Math.Min(minGridX, maxGridX)); + int clampedMinY = Math.Max(0, Math.Min(minGridY, maxGridY)); + int clampedMaxX = Math.Min(width - 1, Math.Max(minGridX, maxGridX)); + int clampedMaxY = Math.Min(height - 1, Math.Max(minGridY, maxGridY)); + + if (clampedMaxX < clampedMinX) clampedMaxX = clampedMinX; + if (clampedMaxY < clampedMinY) clampedMaxY = clampedMinY; + + var coveredCells = new List<(int x, int y)>(); + for (int x = clampedMinX; x <= clampedMaxX; x++) + { + for (int y = clampedMinY; y <= clampedMaxY; y++) + { + coveredCells.Add((x, y)); + } } return coveredCells; @@ -1545,74 +1745,35 @@ namespace NavisworksTransport.PathPlanning double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); double limitWidthInModelUnits = limitWidthMeters * metersToModelUnits; - // 4. 分析门的方向：哪个数大，哪个就是门的宽度 - double width = bbox.Max.X - bbox.Min.X; // X方向宽度 - double depth = bbox.Max.Y - bbox.Min.Y; // Y方向深度 - double doorWidth = Math.Max(width, depth); // 门宽（较大的那个方向） - bool isDoorWidthInXDirection = width > depth; // 门宽是否在X方向 - - LogManager.Debug($"【门开口计算】{doorItem.DisplayName} - 尺寸: {width:F2}x{depth:F2}, 门宽: {doorWidth:F2}, 门宽方向: {(isDoorWidthInXDirection ? "X" : "Y")}, 限宽: {limitWidthMeters}m"); - - // 5. 限宽约束：限宽不能超过门宽 - double effectiveWidth = Math.Min(limitWidthInModelUnits, doorWidth); - + var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType); + var hostMin = new Vector3((float)bbox.Min.X, (float)bbox.Min.Y, (float)bbox.Min.Z); + var hostMax = new Vector3((float)bbox.Max.X, (float)bbox.Max.Y, (float)bbox.Max.Z); + var (minH1, maxH1, minH2, maxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter); + double planarWidth = maxH1 - minH1; + double planarDepth = maxH2 - minH2; + double doorWidth = Math.Max(planarWidth, planarDepth); + + coveredCells = CalculateDoorOpeningCoverageFromWorldExtents( + bbox.Min.X, bbox.Min.Y, bbox.Min.Z, + bbox.Max.X, bbox.Max.Y, bbox.Max.Z, + limitWidthInModelUnits, + gridMap.Origin.X, gridMap.Origin.Y, gridMap.Origin.Z, + gridMap.Width, gridMap.Height, gridMap.CellSize, + gridMap.CoordinateSystemType); + if (limitWidthInModelUnits > doorWidth) { LogManager.Info($"【门开口计算】{doorItem.DisplayName} 限宽({limitWidthMeters}m)超过门宽({doorWidth / metersToModelUnits:F2}m)，按门宽处理"); } - - // 6. 直接计算开口区域坐标范围 - double minX, maxX, minY, maxY; - - if (isDoorWidthInXDirection) - { - // 门宽在X方向：在X方向应用限宽，Y方向保持完整 - double centerX = (bbox.Min.X + bbox.Max.X) / 2; - double halfWidth = effectiveWidth / 2; - minX = centerX - halfWidth; - maxX = centerX + halfWidth; - minY = bbox.Min.Y; - maxY = bbox.Max.Y; - } - else - { - // 门宽在Y方向：在Y方向应用限宽，X方向保持完整 - double centerY = (bbox.Min.Y + bbox.Max.Y) / 2; - double halfWidth = effectiveWidth / 2; - minX = bbox.Min.X; - maxX = bbox.Max.X; - minY = centerY - halfWidth; - maxY = centerY + halfWidth; - } - - LogManager.Debug($"【门开口计算】{doorItem.DisplayName} 开口区域坐标: [{minX:F2},{minY:F2}]-[{maxX:F2},{maxY:F2}]"); - - // 7. 将开口区域坐标转换为网格坐标（严格按照左下角原则） - var minGridPos = gridMap.WorldToGrid(new Point3D(minX, minY, 0)); - - var maxGridPos = gridMap.WorldToGrid(new Point3D(maxX, maxY, 0)); - - // 确保坐标在有效范围内 - int gridMinX = Math.Max(0, minGridPos.X); - int gridMinY = Math.Max(0, minGridPos.Y); - int gridMaxX = Math.Min(gridMap.Width - 1, maxGridPos.X); - int gridMaxY = Math.Min(gridMap.Height - 1, maxGridPos.Y); - - // 确保至少有1个网格可通行 - if (gridMaxX < gridMinX) gridMaxX = gridMinX; - if (gridMaxY < gridMinY) gridMaxY = gridMinY; - - // 8. 生成覆盖的网格单元列表 - for (int x = gridMinX; x <= gridMaxX; x++) + if (coveredCells.Any()) { - for (int y = gridMinY; y <= gridMaxY; y++) - { - coveredCells.Add((x, y)); - } + int gridMinX = coveredCells.Min(cell => cell.x); + int gridMinY = coveredCells.Min(cell => cell.y); + int gridMaxX = coveredCells.Max(cell => cell.x); + int gridMaxY = coveredCells.Max(cell => cell.y); + LogManager.Info($"【门开口计算】{doorItem.DisplayName} 成功处理 - 门宽: {doorWidth / metersToModelUnits:F2}m, 限宽: {limitWidthMeters}m, 开口覆盖: {coveredCells.Count}个网格 (范围: [{gridMinX},{gridMinY}]-[{gridMaxX},{gridMaxY}])"); } - - LogManager.Info($"【门开口计算】{doorItem.DisplayName} 成功处理 - 门宽: {doorWidth / metersToModelUnits:F2}m, 限宽: {limitWidthMeters}m, 开口覆盖: {coveredCells.Count}个网格 (范围: [{gridMinX},{gridMinY}]-[{gridMaxX},{gridMaxY}])"); } catch (Exception ex) { diff --git a/src/PathPlanning/OptimizedHeightCalculator.cs b/src/PathPlanning/OptimizedHeightCalculator.cs index 7683e85..87e2af2 100644 --- a/src/PathPlanning/OptimizedHeightCalculator.cs +++ b/src/PathPlanning/OptimizedHeightCalculator.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Linq; using Autodesk.Navisworks.Api; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { @@ -14,6 +15,7 @@ namespace NavisworksTransport.PathPlanning private readonly Dictionary _heightSamples; private readonly double _sampleInterval; private readonly ChannelHeightDetector _heightDetector; + private readonly ICoordinateSystem _coordinateSystem; /// /// 构造函数 @@ -23,7 +25,8 @@ namespace NavisworksTransport.PathPlanning { _heightSamples = new Dictionary(); _sampleInterval = sampleInterval; - _heightDetector = new ChannelHeightDetector(); + _coordinateSystem = CoordinateSystemManager.Instance.Current; + _heightDetector = new ChannelHeightDetector(_coordinateSystem); LogManager.Info($"[优化高度计算] 初始化完成，采样间隔: {sampleInterval}米"); } @@ -50,8 +53,9 @@ namespace NavisworksTransport.PathPlanning // 计算采样网格 double sampleIntervalModel = _sampleInterval * 39.37; // 转换为模型单位 - int samplesX = (int)Math.Ceiling((bounds.Max.X - bounds.Min.X) / sampleIntervalModel); - int samplesY = (int)Math.Ceiling((bounds.Max.Y - bounds.Min.Y) / sampleIntervalModel); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + int samplesX = (int)Math.Ceiling((maxH1 - minH1) / sampleIntervalModel); + int samplesY = (int)Math.Ceiling((maxH2 - minH2) / sampleIntervalModel); LogManager.Info($"[优化高度计算] 采样网格: {samplesX}x{samplesY} = {samplesX * samplesY}个采样点"); @@ -63,13 +67,13 @@ namespace NavisworksTransport.PathPlanning { for (int y = 0; y < samplesY; y++) { - double worldX = bounds.Min.X + x * sampleIntervalModel; - double worldY = bounds.Min.Y + y * sampleIntervalModel; + double worldH1 = minH1 + x * sampleIntervalModel; + double worldH2 = minH2 + y * sampleIntervalModel; - var sampleKey = GenerateSampleKey(worldX, worldY); + var sampleKey = GenerateSampleKey(worldH1, worldH2); // 只对通道区域进行精确计算 - var position = new Point3D(worldX, worldY, 0); + var position = _coordinateSystem.CreatePoint(worldH1, worldH2, 0); var containingChannel = FindNearestChannel(position, channels); if (containingChannel != null) @@ -156,7 +160,8 @@ namespace NavisworksTransport.PathPlanning // 对于大多数建筑元素，表面接近顶面 double surfaceRatio = DetermineChannelSurfaceRatio(channel); - double height = bbox.Min.Z + (bbox.Max.Z - bbox.Min.Z) * surfaceRatio; + var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bbox); + double height = minElevation + (maxElevation - minElevation) * surfaceRatio; return height; } @@ -166,7 +171,7 @@ namespace NavisworksTransport.PathPlanning LogManager.Debug($"[优化高度计算] 几何高度计算失败: {ex.Message}"); } - return position.Z; + return _coordinateSystem.GetElevation(position); } /// @@ -187,8 +192,9 @@ namespace NavisworksTransport.PathPlanning var pickResult = view.PickItemFromPoint((int)screenPoint.X, (int)screenPoint.Y); if (pickResult?.Point != null && pickResult.ModelItem == channel) { - LogManager.Debug($"[优化高度计算] View API精确检测: {worldPosition} -> {pickResult.Point.Z:F3}"); - return pickResult.Point.Z; + double detectedElevation = _coordinateSystem.GetElevation(pickResult.Point); + LogManager.Debug($"[优化高度计算] View API精确检测: {worldPosition} -> {detectedElevation:F3}"); + return detectedElevation; } } } @@ -245,13 +251,15 @@ namespace NavisworksTransport.PathPlanning var bbox = channel.Geometry.BoundingBox; // 检查位置是否在通道边界内（带容差） - if (position.X >= bbox.Min.X - 50 && position.X <= bbox.Max.X + 50 && - position.Y >= bbox.Min.Y - 50 && position.Y <= bbox.Max.Y + 50) + var (positionH1, positionH2) = _coordinateSystem.GetHorizontalCoords(position); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bbox); + if (positionH1 >= minH1 - 50 && positionH1 <= maxH1 + 50 && + positionH2 >= minH2 - 50 && positionH2 <= maxH2 + 50) { // 计算到边界中心的距离 - double centerX = (bbox.Min.X + bbox.Max.X) / 2; - double centerY = (bbox.Min.Y + bbox.Max.Y) / 2; - double distance = Math.Sqrt(Math.Pow(position.X - centerX, 2) + Math.Pow(position.Y - centerY, 2)); + double centerH1 = (minH1 + maxH1) / 2; + double centerH2 = (minH2 + maxH2) / 2; + double distance = Math.Sqrt(Math.Pow(positionH1 - centerH1, 2) + Math.Pow(positionH2 - centerH2, 2)); if (distance < minDistance) { @@ -368,4 +376,4 @@ namespace NavisworksTransport.PathPlanning return $"采样点数: {_heightSamples.Count}, 采样间隔: {_sampleInterval}米"; } } -} \ No newline at end of file +} diff --git a/src/PathPlanning/SlopeAnalyzer.cs b/src/PathPlanning/SlopeAnalyzer.cs index afd9454..09faeb2 100644 --- a/src/PathPlanning/SlopeAnalyzer.cs +++ b/src/PathPlanning/SlopeAnalyzer.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Linq; using Autodesk.Navisworks.Api; +using NavisworksTransport.Utils.CoordinateSystem; namespace NavisworksTransport.PathPlanning { @@ -13,6 +14,7 @@ namespace NavisworksTransport.PathPlanning { private readonly Dictionary _slopeCache; private readonly double _minSlopeAngle; // 最小坡度角度（弧度） + private readonly ICoordinateSystem _coordinateSystem; /// /// 构造函数 @@ -22,6 +24,7 @@ namespace NavisworksTransport.PathPlanning { _slopeCache = new Dictionary(); _minSlopeAngle = minSlopeAngle * Math.PI / 180.0; // 转换为弧度 + _coordinateSystem = CoordinateSystemManager.Instance.Current; } /// @@ -157,8 +160,10 @@ namespace NavisworksTransport.PathPlanning // 根据几何特征判断 var bounds = channel.Geometry.BoundingBox; - var heightDiff = bounds.Max.Z - bounds.Min.Z; - var horizontalLength = Math.Max(bounds.Max.X - bounds.Min.X, bounds.Max.Y - bounds.Min.Y); + var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + var heightDiff = maxElevation - minElevation; + var horizontalLength = Math.Max(maxH1 - minH1, maxH2 - minH2); if (heightDiff > 0.1 && horizontalLength > 0.1) // 有明显高度差 { @@ -185,25 +190,26 @@ namespace NavisworksTransport.PathPlanning private void AddBoundaryPoints(ChannelSlopeInfo slopeInfo, BoundingBox3D bounds) { // 添加8个边界框顶点 - slopeInfo.SlopePoints.Add(new Point3D(bounds.Min.X, bounds.Min.Y, bounds.Min.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Max.X, bounds.Min.Y, bounds.Min.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Min.X, bounds.Max.Y, bounds.Min.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Max.X, bounds.Max.Y, bounds.Min.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Min.X, bounds.Min.Y, bounds.Max.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Max.X, bounds.Min.Y, bounds.Max.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Min.X, bounds.Max.Y, bounds.Max.Z)); - slopeInfo.SlopePoints.Add(new Point3D(bounds.Max.X, bounds.Max.Y, bounds.Max.Z)); + var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, minH2, minElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, minH2, minElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, maxH2, minElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, maxH2, minElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, minH2, maxElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, minH2, maxElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, maxH2, maxElevation)); + slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, maxH2, maxElevation)); // 添加中心线上的采样点 int sampleCount = 5; for (int i = 0; i < sampleCount; i++) { double ratio = (double)i / (sampleCount - 1); - var samplePoint = new Point3D( - bounds.Min.X + (bounds.Max.X - bounds.Min.X) * ratio, - bounds.Min.Y + (bounds.Max.Y - bounds.Min.Y) * ratio, - bounds.Min.Z + (bounds.Max.Z - bounds.Min.Z) * ratio - ); + var samplePoint = _coordinateSystem.CreatePoint( + minH1 + (maxH1 - minH1) * ratio, + minH2 + (maxH2 - minH2) * ratio, + minElevation + (maxElevation - minElevation) * ratio); slopeInfo.SlopePoints.Add(samplePoint); } } @@ -216,26 +222,29 @@ namespace NavisworksTransport.PathPlanning private void CalculateSlopeAngleAndDirection(ChannelSlopeInfo slopeInfo, BoundingBox3D bounds) { // 计算主要方向向量 - var directionX = bounds.Max.X - bounds.Min.X; - var directionY = bounds.Max.Y - bounds.Min.Y; - var directionZ = bounds.Max.Z - bounds.Min.Z; + var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + var directionH1 = maxH1 - minH1; + var directionH2 = maxH2 - minH2; + var directionElevation = maxElevation - minElevation; // 选择主要的水平方向 - var horizontalLength = Math.Max(Math.Abs(directionX), Math.Abs(directionY)); + var horizontalLength = Math.Max(Math.Abs(directionH1), Math.Abs(directionH2)); if (horizontalLength > 0.001) { // 计算坡度角度 - slopeInfo.SlopeAngle = Math.Atan(Math.Abs(directionZ) / horizontalLength); + slopeInfo.SlopeAngle = Math.Atan(Math.Abs(directionElevation) / horizontalLength); // 计算坡度方向向量 - var length = Math.Sqrt(directionX * directionX + directionY * directionY + directionZ * directionZ); + var directionPoint = _coordinateSystem.CreatePoint(directionH1, directionH2, directionElevation); + var length = Math.Sqrt(directionPoint.X * directionPoint.X + directionPoint.Y * directionPoint.Y + directionPoint.Z * directionPoint.Z); if (length > 0.001) { slopeInfo.SlopeDirection = new Vector3D( - directionX / length, - directionY / length, - directionZ / length + directionPoint.X / length, + directionPoint.Y / length, + directionPoint.Z / length ); } else @@ -246,7 +255,8 @@ namespace NavisworksTransport.PathPlanning else { slopeInfo.SlopeAngle = 0; - slopeInfo.SlopeDirection = new Vector3D(0, 0, 1); + var up = _coordinateSystem.UpVector; + slopeInfo.SlopeDirection = new Vector3D(up.X, up.Y, up.Z); } } @@ -279,8 +289,10 @@ namespace NavisworksTransport.PathPlanning try { var bounds = channel.Geometry.BoundingBox; - var totalHeight = bounds.Max.Z - bounds.Min.Z; - var totalLength = Math.Max(bounds.Max.X - bounds.Min.X, bounds.Max.Y - bounds.Min.Y); + var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + var totalHeight = maxElevation - minElevation; + var totalLength = Math.Max(maxH1 - minH1, maxH2 - minH2); // 估算楼梯级数（假设每级高度15-20cm） var estimatedSteps = (int)(totalHeight / 0.175); // 平均17.5cm每级 @@ -310,8 +322,9 @@ namespace NavisworksTransport.PathPlanning var bounds = channel.Geometry.BoundingBox; // 检查是否为弯曲坡道（通过长宽比判断） - var width = Math.Min(bounds.Max.X - bounds.Min.X, bounds.Max.Y - bounds.Min.Y); - var length = Math.Max(bounds.Max.X - bounds.Min.X, bounds.Max.Y - bounds.Min.Y); + var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds); + var width = Math.Min(maxH1 - minH1, maxH2 - minH2); + var length = Math.Max(maxH1 - minH1, maxH2 - minH2); if (width > 0 && length / width > 3.0) // 长宽比大于3:1，可能是弯曲坡道 { @@ -352,18 +365,21 @@ namespace NavisworksTransport.PathPlanning } // 找到楼梯的起点和终点 - var startPoint = slopeInfo.SlopePoints.OrderBy(p => p.Z).First(); - var endPoint = slopeInfo.SlopePoints.OrderBy(p => p.Z).Last(); + var startPoint = slopeInfo.SlopePoints.OrderBy(p => _coordinateSystem.GetElevation(p)).First(); + var endPoint = slopeInfo.SlopePoints.OrderBy(p => _coordinateSystem.GetElevation(p)).Last(); // 计算位置在楼梯长度方向上的投影 + var (startH1, startH2) = _coordinateSystem.GetHorizontalCoords(startPoint); + var (endH1, endH2) = _coordinateSystem.GetHorizontalCoords(endPoint); + var (positionH1, positionH2) = _coordinateSystem.GetHorizontalCoords(position); var totalDistance = Math.Sqrt( - Math.Pow(endPoint.X - startPoint.X, 2) + - Math.Pow(endPoint.Y - startPoint.Y, 2) + Math.Pow(endH1 - startH1, 2) + + Math.Pow(endH2 - startH2, 2) ); var currentDistance = Math.Sqrt( - Math.Pow(position.X - startPoint.X, 2) + - Math.Pow(position.Y - startPoint.Y, 2) + Math.Pow(positionH1 - startH1, 2) + + Math.Pow(positionH2 - startH2, 2) ); if (totalDistance <= 0.001) @@ -522,4 +538,4 @@ namespace NavisworksTransport.PathPlanning Other } - } \ No newline at end of file + } diff --git a/src/Utils/ModelItemAnalysisHelper.cs b/src/Utils/ModelItemAnalysisHelper.cs index d1aed30..0316da6 100644 --- a/src/Utils/ModelItemAnalysisHelper.cs +++ b/src/Utils/ModelItemAnalysisHelper.cs @@ -425,6 +425,8 @@ namespace NavisworksTransport.Utils if (isComposite || hasGeometry) { + bool addedCurrentItem = false; + // 复合对象或独立实体节点：检查几何体类型 var geometry = item.Geometry; @@ -440,6 +442,7 @@ namespace NavisworksTransport.Utils else mixedCount++; visibleItems.Add(item); // 只保留包含三角形的几何体 + addedCurrentItem = true; } else if (primitiveTypes == PrimitiveTypes.Lines) { @@ -465,6 +468,7 @@ namespace NavisworksTransport.Utils { mixedCount++; visibleItems.Add(item); // 其他混合类型保留 + addedCurrentItem = true; } } else @@ -473,8 +477,15 @@ namespace NavisworksTransport.Utils // 不添加：无几何体数据 } - // 复合对象：不继续遍历子节点（Clash Detective API 会自动处理） - // 独立实体节点：也没有子节点 + // 只有当前节点已经可直接作为几何候选时，才截断子树。 + // 如果当前节点本身没有可用三角几何，则继续向下遍历，避免把真实几何子节点整棵截断。 + if (!addedCurrentItem) + { + foreach (var child in item.Children) + { + stack.Push(child); + } + } } else { @@ -536,4 +547,4 @@ namespace NavisworksTransport.Utils } #endregion -} \ No newline at end of file +}