Fix auto path obstacle geometry collection
This commit is contained in:
parent
fdb857af17
commit
1a9d0972f0
@ -61,6 +61,7 @@
|
||||
<Compile Include="UnitTests\Core\PathPersistenceTests.cs" />
|
||||
<Compile Include="UnitTests\Core\PathRouteCloneTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\AutoPathPlanningCoordinateSemanticsTests.cs" />
|
||||
<Compile Include="UnitTests\Integration\AutoPathGridGenerationAutomationTests.cs" />
|
||||
<Compile Include="UnitTests\Integration\NavisworksTestAutomationClient.cs" />
|
||||
<Compile Include="UnitTests\Integration\VirtualCollisionAutomationTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\HostCoordinateAdapterTests.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);
|
||||
|
||||
@ -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<bool>("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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -57,6 +57,15 @@ namespace NavisworksTransport.UnitTests.Integration
|
||||
return await PostJsonAsync(requestUri).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
public async Task<JObject> 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<JObject> GetJsonAsync(string requestUri)
|
||||
{
|
||||
using (HttpResponseMessage response = await _httpClient.GetAsync(requestUri).ConfigureAwait(false))
|
||||
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据宿主包围盒和当前宿主坐标系语义，计算自动路径规划的最优网格大小。
|
||||
/// 关键约束：
|
||||
/// - 仅修正“水平平面”的解释，不改变历史阈值与业务策略。
|
||||
/// - ZUp: 使用 Host XY 平面跨度。
|
||||
/// - YUp: 使用 Host XZ 平面跨度。
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取通道模型项用于高度计算
|
||||
/// </summary>
|
||||
@ -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++;
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 添加切点标记
|
||||
/// </summary>
|
||||
@ -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
|
||||
/// <param name="pointMarker">点标记</param>
|
||||
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);
|
||||
|
||||
@ -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<string, string> query)
|
||||
{
|
||||
PathPlanningManager pathManager = RequirePathManager();
|
||||
PathRoute route = ResolveVirtualCollisionRoute(pathManager, query, null);
|
||||
if (route.Points == null || route.Points.Count < 2)
|
||||
{
|
||||
throw new InvalidOperationException($"路径点不足，无法分析自动路径网格: {route.Name}");
|
||||
}
|
||||
|
||||
List<PathPoint> orderedPoints = route.Points.OrderBy(point => point.Index).ToList();
|
||||
PathPoint startPoint = orderedPoints.First();
|
||||
PathPoint endPoint = orderedPoints.Last();
|
||||
|
||||
BoundingBox3D bounds = InvokePathManagerPrivate<BoundingBox3D>(pathManager, "GetModelBounds");
|
||||
double gridSize = InvokePathManagerPrivate<double>(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<string, string> query)
|
||||
{
|
||||
PathPlanningManager pathManager = RequirePathManager();
|
||||
PathRoute route = ResolveVirtualCollisionRoute(pathManager, query, null);
|
||||
if (route.Points == null || route.Points.Count < 2)
|
||||
{
|
||||
throw new InvalidOperationException($"路径点不足，无法执行自动路径规划: {route.Name}");
|
||||
}
|
||||
|
||||
List<PathPoint> 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<BoundingBox3D>(pathManager, "GetModelBounds");
|
||||
double gridSize = InvokePathManagerPrivate<double>(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<ModelItem>)collectRelatedItemsMethod.Invoke(gridMapGenerator, new object[] { traversableItems });
|
||||
var irrelevantRelatedItems = (HashSet<ModelItem>)collectRelatedItemsMethod.Invoke(gridMapGenerator, new object[] { irrelevantItems });
|
||||
var geometryItems = ModelItemAnalysisHelper.GetAllVisibleGeometryItems();
|
||||
var itemCache = (Dictionary<ModelItem, ItemProperties>)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<object>();
|
||||
var filteredSampleItems = new List<object>();
|
||||
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<Triangle3D> 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<T>(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;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -147,6 +147,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
double minZ = double.MaxValue;
|
||||
double maxZ = double.MinValue;
|
||||
int walkableCount = 0;
|
||||
var elevationCounts = new Dictionary<double, int>();
|
||||
|
||||
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}]");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@ -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<HeightSample>()
|
||||
};
|
||||
@ -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
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -912,4 +1051,4 @@ namespace NavisworksTransport.PathPlanning
|
||||
Other
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -22,6 +22,11 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// 当前使用的坐标系
|
||||
/// </summary>
|
||||
private readonly ICoordinateSystem _coordinateSystem;
|
||||
|
||||
/// <summary>
|
||||
/// 当前网格采用的宿主坐标系类型。
|
||||
/// </summary>
|
||||
public CoordinateSystemType CoordinateSystemType => _coordinateSystem.Type;
|
||||
/// <summary>
|
||||
/// 网格宽度（单元格数量）
|
||||
/// </summary>
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 获取世界坐标范围在当前网格坐标语义下的高程区间。
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据世界坐标最小/最大点，计算其在当前网格坐标语义下覆盖的平面网格范围。
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取世界坐标范围在当前宿主坐标语义下的高程区间。
|
||||
/// 仅使用纯数值输入，便于单元测试验证多坐标系语义。
|
||||
/// </summary>
|
||||
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));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析边界最小点在当前宿主坐标语义下的高程值。
|
||||
/// 仅修正“高程轴”的解释，不改变原有边界层业务逻辑。
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据世界坐标最小/最大点与网格元数据，计算覆盖的平面网格范围。
|
||||
/// 仅使用纯数值输入，便于单元测试验证多坐标系语义。
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断包围盒在当前宿主平面语义下，是否与当前网格的平面范围真实相交。
|
||||
/// 这里只做诊断，不改变原有网格覆盖或裁剪逻辑。
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算包围盒在网格上覆盖的单元坐标
|
||||
/// 严格按照"网格坐标代表左下角"的设计原则进行边界处理
|
||||
@ -1475,36 +1623,88 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// <returns>覆盖的网格单元坐标列表</returns>
|
||||
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)>();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据门包围盒、限宽和当前宿主平面语义，计算门开口在网格上的覆盖范围。
|
||||
/// 关键约束：
|
||||
/// - 仅替换“哪两个轴组成水平平面”的解释，不改变历史限宽业务规则。
|
||||
/// - 门宽仍取平面上较大的那一维；限宽仍只裁剪门宽方向。
|
||||
/// </summary>
|
||||
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)
|
||||
{
|
||||
|
||||
@ -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<string, double> _heightSamples;
|
||||
private readonly double _sampleInterval;
|
||||
private readonly ChannelHeightDetector _heightDetector;
|
||||
private readonly ICoordinateSystem _coordinateSystem;
|
||||
|
||||
/// <summary>
|
||||
/// 构造函数
|
||||
@ -23,7 +25,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
_heightSamples = new Dictionary<string, double>();
|
||||
_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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -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}米";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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<string, ChannelSlopeInfo> _slopeCache;
|
||||
private readonly double _minSlopeAngle; // 最小坡度角度（弧度）
|
||||
private readonly ICoordinateSystem _coordinateSystem;
|
||||
|
||||
/// <summary>
|
||||
/// 构造函数
|
||||
@ -22,6 +24,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
_slopeCache = new Dictionary<string, ChannelSlopeInfo>();
|
||||
_minSlopeAngle = minSlopeAngle * Math.PI / 180.0; // 转换为弧度
|
||||
_coordinateSystem = CoordinateSystemManager.Instance.Current;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -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
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user