From 1cf4fe59673ec91cb22b4273a6a7da0eb75a8f44 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Mon, 23 Mar 2026 12:25:00 +0800 Subject: [PATCH] Align terminal rail normals and object space rendering --- NavisworksTransport.UnitTests.csproj | 1 + TransportPlugin.csproj | 1 + ...semblyInstallationReferenceBuilderTests.cs | 55 +++++ .../CanonicalRailPoseBuilderTests.cs | 17 ++ .../ObjectSpaceOrientationHelperTests.cs | 40 ++++ src/Core/Animation/PathAnimationManager.cs | 2 + src/Core/PathPlanningModels.cs | 7 + src/Core/PathPointRenderPlugin.cs | 210 +++++------------- src/UI/WPF/ViewModels/PathEditingViewModel.cs | 80 ++++++- .../AssemblyInstallationReferenceBuilder.cs | 67 ++++++ .../CanonicalRailPoseBuilder.cs | 79 ++++++- .../ObjectSpaceOrientationHelper.cs | 92 ++++++++ src/Utils/RailPathPoseHelper.cs | 90 +++++++- 13 files changed, 568 insertions(+), 173 deletions(-) create mode 100644 UnitTests/CoordinateSystem/ObjectSpaceOrientationHelperTests.cs create mode 100644 src/Utils/CoordinateSystem/ObjectSpaceOrientationHelper.cs diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index 6ae959b..d79b769 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -56,6 +56,7 @@ + diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj index ce75558..af09c90 100644 --- a/TransportPlugin.csproj +++ b/TransportPlugin.csproj @@ -338,6 +338,7 @@ + diff --git a/UnitTests/CoordinateSystem/AssemblyInstallationReferenceBuilderTests.cs b/UnitTests/CoordinateSystem/AssemblyInstallationReferenceBuilderTests.cs index 55f7396..a058cdf 100644 --- a/UnitTests/CoordinateSystem/AssemblyInstallationReferenceBuilderTests.cs +++ b/UnitTests/CoordinateSystem/AssemblyInstallationReferenceBuilderTests.cs @@ -27,6 +27,61 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreEqual(0f, Vector3.Dot(result.PlaneNormal, result.PlaneSpanDirection), 1e-4f); } + [TestMethod] + public void BuildFromTwoPoints_ShouldCreatePlaneParallelToOpticalAxisAndContainBothPoints() + { + Vector3 axisBase = new Vector3(0f, 0f, 0f); + Vector3 axisDirection = Vector3.UnitX; + Vector3 pickPoint1 = new Vector3(2f, 3f, 4f); + Vector3 pickPoint2 = new Vector3(6f, 3f, 6f); + + AssemblyInstallationReferenceResult result = + AssemblyInstallationReferenceBuilder.Build(axisBase, axisDirection, pickPoint1, pickPoint2); + + Assert.AreEqual(3f, result.OffsetDistance, 1e-4f); + Assert.AreEqual(4f, result.AnchorPoint.X, 1e-4f); + Assert.AreEqual(3f, result.AnchorPoint.Y, 1e-4f); + Assert.AreEqual(0f, result.AnchorPoint.Z, 1e-4f); + Assert.AreEqual(0f, Vector3.Dot(result.PlaneNormal, result.OpticalAxisDirection), 1e-4f); + Assert.AreEqual(0f, Vector3.Dot(result.PlaneSpanDirection, result.OpticalAxisDirection), 1e-4f); + Assert.AreEqual(0f, Vector3.Dot(result.PlaneNormal, result.PlaneSpanDirection), 1e-4f); + Assert.AreEqual(0f, Vector3.Dot(pickPoint1 - result.AnchorPoint, result.PlaneNormal), 1e-4f); + Assert.AreEqual(0f, Vector3.Dot(pickPoint2 - result.AnchorPoint, result.PlaneNormal), 1e-4f); + } + + [TestMethod] + public void BuildFromTwoPoints_ShouldUseOpticalAxisProjectionAsInstallationCenterLine() + { + Vector3 axisBase = new Vector3(0f, 0f, 0f); + Vector3 axisDirection = Vector3.UnitX; + Vector3 pickPoint1 = new Vector3(2f, 3f, 4f); + Vector3 pickPoint2 = new Vector3(6f, 3f, 6f); + + AssemblyInstallationReferenceResult result = + AssemblyInstallationReferenceBuilder.Build(axisBase, axisDirection, pickPoint1, pickPoint2); + + Assert.AreEqual(0f, result.InstallLineBasePoint.X, 1e-4f); + Assert.AreEqual(3f, result.InstallLineBasePoint.Y, 1e-4f); + Assert.AreEqual(0f, result.InstallLineBasePoint.Z, 1e-4f); + Assert.AreEqual(4f, result.AnchorPoint.X, 1e-4f); + Assert.AreEqual(3f, result.AnchorPoint.Y, 1e-4f); + Assert.AreEqual(0f, result.AnchorPoint.Z, 1e-4f); + } + + [TestMethod] + public void BuildFromTwoPoints_WhenPlanePointsAreNearlyAxisAligned_ShouldThrow() + { + Vector3 axisBase = new Vector3(0f, 0f, 0f); + Vector3 axisDirection = Vector3.UnitX; + Vector3 pickPoint1 = new Vector3(2f, 3f, 4f); + Vector3 pickPoint2 = new Vector3(6f, 3.00001f, 4.00001f); + + InvalidOperationException ex = Assert.ThrowsException( + () => AssemblyInstallationReferenceBuilder.Build(axisBase, axisDirection, pickPoint1, pickPoint2)); + + StringAssert.Contains(ex.Message, "无法确定安装参考面"); + } + [TestMethod] public void Build_WhenPickPointTooCloseToOpticalAxis_ShouldThrow() { diff --git a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs index 0104b64..deef6f7 100644 --- a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs +++ b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs @@ -137,6 +137,23 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem AssertVector(mappedUp, 0, 0, 1); } + [TestMethod] + public void PreferredNormal_ShouldOverrideCanonicalUpForRailFrame() + { + bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame( + new Vector3(0, 0, 0), + new Vector3(1, 0, 0), + new Vector3(2, 0, 0), + Vector3.UnitZ, + Vector3.UnitY, + out RailLocalFrame frame); + + Assert.IsTrue(ok); + AssertVector(frame.Forward, 1, 0, 0); + AssertVector(frame.Normal, 0, 1, 0); + AssertVector(frame.Lateral, 0, 0, -1); + } + private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z) { switch (column) diff --git a/UnitTests/CoordinateSystem/ObjectSpaceOrientationHelperTests.cs b/UnitTests/CoordinateSystem/ObjectSpaceOrientationHelperTests.cs new file mode 100644 index 0000000..a15baf4 --- /dev/null +++ b/UnitTests/CoordinateSystem/ObjectSpaceOrientationHelperTests.cs @@ -0,0 +1,40 @@ +using System.Numerics; +using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Utils.CoordinateSystem; + +namespace NavisworksTransport.UnitTests.CoordinateSystem +{ + [TestClass] + public class ObjectSpaceOrientationHelperTests + { + [TestMethod] + public void CalculateAxes_ShouldHonorProvidedUpReference() + { + var segmentDirection = new Vector3(10f, 0f, 0f); + var upReference = new Vector3(0f, 1f, 0f); + + var (_, up) = ObjectSpaceOrientationHelper.CalculateAxes(segmentDirection, upReference); + + Assert.AreEqual(0.0, up.X, 1e-6); + Assert.AreEqual(1.0, System.Math.Abs(up.Y), 1e-6); + Assert.AreEqual(0.0, up.Z, 1e-6); + } + + [TestMethod] + public void CalculateAxes_ForVerticalSegment_ShouldUseHorizontalDirectionWithProvidedUpReference() + { + var segmentDirection = new Vector3(0f, 10f, 0f); + var upReference = new Vector3(0f, 1f, 0f); + var horizontalDirection = new Vector3(1f, 0f, 0f); + + var (right, up) = ObjectSpaceOrientationHelper.CalculateAxes(segmentDirection, upReference, horizontalDirection); + + Assert.AreEqual(0.0, right.X, 1e-6); + Assert.AreEqual(0.0, right.Y, 1e-6); + Assert.AreEqual(1.0, right.Z, 1e-6); + Assert.AreEqual(1.0, up.X, 1e-6); + Assert.AreEqual(0.0, up.Y, 1e-6); + Assert.AreEqual(0.0, up.Z, 1e-6); + } + } +} diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 6298b25..e6ef4fa 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -671,6 +671,7 @@ namespace NavisworksTransport.Core.Animation LogManager.Debug($"[移动到起点] Rail路径调整: 参考点=({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2}), 物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), 物体高度={objectHeight:F2}, 安装={_route.RailMountMode}, 对接={(PathRoute.IsTopPayloadAnchorForMountMode(_route.RailMountMode) ? "顶面对接" : "底面对接")}"); if (RailPathPoseHelper.TryCreateRailRotation( + _route, previousPoint, _pathPoints[0], nextPoint, @@ -1034,6 +1035,7 @@ namespace NavisworksTransport.Core.Animation if (_route.PathType == PathType.Rail && RailPathPoseHelper.TryCreateRailRotation( + _route, previousFramePoint, framePosition, nextFramePoint, diff --git a/src/Core/PathPlanningModels.cs b/src/Core/PathPlanningModels.cs index e3ba6bb..1a497b6 100644 --- a/src/Core/PathPlanningModels.cs +++ b/src/Core/PathPlanningModels.cs @@ -737,6 +737,12 @@ namespace NavisworksTransport /// public RailPathDefinitionMode RailPathDefinitionMode { get; set; } + /// + /// Rail 路径优先法向（宿主坐标）。 + /// 仅在存在显式安装面/业务法向时使用；为空时退回当前默认求解。 + /// + public Point3D RailPreferredNormal { get; set; } + // 数据库分析相关属性 /// /// 碰撞数量（从数据库加载） @@ -777,6 +783,7 @@ namespace NavisworksTransport IsCurved = false; RailMountMode = RailMountMode.UnderRail; RailPathDefinitionMode = RailPathDefinitionMode.LegacySuspensionPoint; + RailPreferredNormal = null; } /// diff --git a/src/Core/PathPointRenderPlugin.cs b/src/Core/PathPointRenderPlugin.cs index 36b7d80..13983d2 100644 --- a/src/Core/PathPointRenderPlugin.cs +++ b/src/Core/PathPointRenderPlugin.cs @@ -297,6 +297,11 @@ namespace NavisworksTransport /// public Vector3D HorizontalDirection { get; set; } + /// + /// 通行空间的优先 up 方向（例如 Rail 安装面法向） + /// + public Vector3D UpDirection { get; set; } + public override string ToString() { return $"LineMarker[{FromIndex}->{ToIndex}, 类型={SegmentType}, 起点=({StartPoint.X:F2},{StartPoint.Y:F2},{StartPoint.Z:F2}), 终点=({EndPoint.X:F2},{EndPoint.Y:F2},{EndPoint.Z:F2})]"; @@ -1658,7 +1663,8 @@ namespace NavisworksTransport var endPoint = edge.SampledPoints.Last(); // 计算长方体的轴向量（right和up向量） - var (right, up) = CalculateCuboidAxes(startPoint, endPoint); + var preferredUp = ResolveObjectSpaceUpDirection(visualization.PathRoute); + var (right, up) = CalculateCuboidAxes(startPoint, endPoint, preferredUp); // 根据路径类型调整起点和终点位置 // 关键修复：垂直偏移应该沿着长方体的轴线方向（up向量）平移 @@ -1710,7 +1716,8 @@ namespace NavisworksTransport SampledPoints = edge.SampledPoints, Height = height, // 根据模式设置高度 Width = width, // 根据模式设置宽度 - Opacity = lineOpacity // 根据模式设置透明度 + Opacity = lineOpacity, // 根据模式设置透明度 + UpDirection = preferredUp }; visualization.PathLineMarkers.Add(lineMarker); } @@ -1736,7 +1743,8 @@ namespace NavisworksTransport var endPoint = edge.Trajectory.Te; // 计算长方体的轴向量（right和up向量） - var (right, up) = CalculateCuboidAxes(startPoint, endPoint); + var preferredUp = ResolveObjectSpaceUpDirection(visualization.PathRoute); + var (right, up) = CalculateCuboidAxes(startPoint, endPoint, preferredUp); // 根据路径类型调整起点和终点位置 // 关键修复：垂直偏移应该沿着长方体的轴线方向（up向量）平移 @@ -1818,7 +1826,8 @@ namespace NavisworksTransport SampledPoints = adjustedSampledPoints, Height = height, // 根据模式设置高度 Width = width, // 根据模式设置宽度 - Opacity = arcLineOpacity // 根据模式设置透明度 + Opacity = arcLineOpacity, // 根据模式设置透明度 + UpDirection = preferredUp }; visualization.PathLineMarkers.Add(arcMarker); @@ -1838,73 +1847,13 @@ namespace NavisworksTransport /// 路径段终点 /// 水平段方向向量（用于垂直路径渲染时确定通行空间方向） /// 元组（right向量，up向量） - private (Vector3D right, Vector3D up) CalculateCuboidAxes(Point3D startPoint, Point3D endPoint, Vector3D horizontalDirection = null) + private (Vector3D right, Vector3D up) CalculateCuboidAxes(Point3D startPoint, Point3D endPoint, Vector3D upReference = null, Vector3D horizontalDirection = null) { - // 计算路径段的方向向量 - var segmentDirection = new Vector3D( - endPoint.X - startPoint.X, - endPoint.Y - startPoint.Y, - endPoint.Z - startPoint.Z - ); - var segmentLength = Math.Sqrt( - segmentDirection.X * segmentDirection.X + - segmentDirection.Y * segmentDirection.Y + - segmentDirection.Z * segmentDirection.Z - ); - - // 归一化方向向量 - var normalizedDirection = new Vector3D( - segmentDirection.X / segmentLength, - segmentDirection.Y / segmentLength, - segmentDirection.Z / segmentLength - ); - - // 使用宿主 up 方向构造水平 right 向量，避免把世界 Z 写死到 Y-up 项目里。 - var hostUp = GetHostUpVector(); - var right = Cross(normalizedDirection, hostUp); - - // 如果方向向量接近宿主 up（垂直段），则使用水平段方向重新构造 right。 - double parallelToUp = Math.Abs( - normalizedDirection.X * hostUp.X + - normalizedDirection.Y * hostUp.Y + - normalizedDirection.Z * hostUp.Z); - if (parallelToUp > 0.9) - { - if (horizontalDirection != null) - { - right = Cross(horizontalDirection, hostUp); - } - else - { - right = Math.Abs(hostUp.X) < 0.9 - ? Normalize(Cross(hostUp, new Vector3D(1, 0, 0))) - : Normalize(Cross(hostUp, new Vector3D(0, 1, 0))); - } - } - - // 归一化right向量 - var rightLength = Math.Sqrt(right.X * right.X + right.Y * right.Y + right.Z * right.Z); - if (rightLength > 0.001) - { - right = new Vector3D(right.X / rightLength, right.Y / rightLength, right.Z / rightLength); - } - - // 计算高度向量（垂直于方向向量和宽度向量）- 这是长方体的轴线方向 - // up = direction × right - var up = new Vector3D( - normalizedDirection.Y * right.Z - normalizedDirection.Z * right.Y, - normalizedDirection.Z * right.X - normalizedDirection.X * right.Z, - normalizedDirection.X * right.Y - normalizedDirection.Y * right.X - ); - - // 归一化up向量 - var upLength = Math.Sqrt(up.X * up.X + up.Y * up.Y + up.Z * up.Z); - if (upLength > 0.001) - { - up = new Vector3D(up.X / upLength, up.Y / upLength, up.Z / upLength); - } - - return (right, up); + return ObjectSpaceOrientationHelper.CalculateAxes( + startPoint, + endPoint, + upReference ?? GetHostUpVector(), + horizontalDirection); } /// @@ -2009,7 +1958,8 @@ namespace NavisworksTransport CalculatePassageSpaceParameters(visualization, isVerticalSegment, isTurn90Horizontal, isFirstOrLastSegment); // 计算长方体的轴向量（right和up向量） - var (right, up) = CalculateCuboidAxes(startPoint.Position, endPoint.Position, horizontalDirection); + var preferredUp = ResolveObjectSpaceUpDirection(visualization.PathRoute); + var (right, up) = CalculateCuboidAxes(startPoint.Position, endPoint.Position, preferredUp, horizontalDirection); // 根据路径类型调整起点和终点位置 // 关键修复：垂直偏移应该沿着长方体的轴线方向（up向量）平移，而不是简单地添加到Z坐标上 @@ -2099,7 +2049,8 @@ namespace NavisworksTransport Height = height, Width = width, Opacity = lineOpacity, - HorizontalDirection = horizontalDirection // 设置水平段方向向量 + HorizontalDirection = horizontalDirection, // 设置水平段方向向量 + UpDirection = preferredUp }; visualization.PathLineMarkers.Add(lineMarker); } @@ -2797,7 +2748,7 @@ namespace NavisworksTransport /// 宽度 /// 高度 /// 水平段方向向量（用于垂直路径渲染时确定通行空间方向） - private void RenderCuboidMarker(Graphics graphics, Point3D startPoint, Point3D endPoint, double width, double height, Vector3D horizontalDirection = null) + private void RenderCuboidMarker(Graphics graphics, Point3D startPoint, Point3D endPoint, double width, double height, Vector3D horizontalDirection = null, Vector3D upReference = null) { try { @@ -2817,47 +2768,11 @@ namespace NavisworksTransport // 归一化方向向量 direction = new Vector3D(direction.X / length, direction.Y / length, direction.Z / length); - var hostUp = GetHostUpVector(); - var right = Cross(direction, hostUp); - - double parallelToUp = Math.Abs( - direction.X * hostUp.X + - direction.Y * hostUp.Y + - direction.Z * hostUp.Z); - if (parallelToUp > 0.9) - { - if (horizontalDirection != null) - { - right = Cross(horizontalDirection, hostUp); - } - else - { - right = Math.Abs(hostUp.X) < 0.9 - ? Normalize(Cross(hostUp, new Vector3D(1, 0, 0))) - : Normalize(Cross(hostUp, new Vector3D(0, 1, 0))); - } - } - - // 归一化right向量 - var rightLength = Math.Sqrt(right.X * right.X + right.Y * right.Y + right.Z * right.Z); - if (rightLength > 0.001) - { - right = new Vector3D(right.X / rightLength, right.Y / rightLength, right.Z / rightLength); - } - - // 计算高度向量（垂直于方向向量和宽度向量） - var up = new Vector3D( - direction.Y * right.Z - direction.Z * right.Y, - direction.Z * right.X - direction.X * right.Z, - direction.X * right.Y - direction.Y * right.X - ); - - // 归一化up向量 - var upLength = Math.Sqrt(up.X * up.X + up.Y * up.Y + up.Z * up.Z); - if (upLength > 0.001) - { - up = new Vector3D(up.X / upLength, up.Y / upLength, up.Z / upLength); - } + var (right, up) = ObjectSpaceOrientationHelper.CalculateAxes( + startPoint, + endPoint, + upReference ?? GetHostUpVector(), + horizontalDirection); // 计算长方体的边界框和向量 var halfWidth = width / 2.0; @@ -2894,7 +2809,7 @@ namespace NavisworksTransport /// 高度 /// 指定长度（覆盖默认计算值） /// 水平段方向向量（用于垂直路径渲染时确定通行空间方向） - private void RenderCuboidMarkerWithLength(Graphics graphics, Point3D startPoint, Point3D endPoint, double width, double height, double length, Vector3D horizontalDirection = null) + private void RenderCuboidMarkerWithLength(Graphics graphics, Point3D startPoint, Point3D endPoint, double width, double height, double length, Vector3D horizontalDirection = null, Vector3D upReference = null) { try { @@ -2912,47 +2827,11 @@ namespace NavisworksTransport direction = new Vector3D(direction.X / directionLength, direction.Y / directionLength, direction.Z / directionLength); - var hostUp = GetHostUpVector(); - var right = Cross(direction, hostUp); - - double parallelToUp = Math.Abs( - direction.X * hostUp.X + - direction.Y * hostUp.Y + - direction.Z * hostUp.Z); - if (parallelToUp > 0.9) - { - if (horizontalDirection != null) - { - right = Cross(horizontalDirection, hostUp); - } - else - { - right = Math.Abs(hostUp.X) < 0.9 - ? Normalize(Cross(hostUp, new Vector3D(1, 0, 0))) - : Normalize(Cross(hostUp, new Vector3D(0, 1, 0))); - } - } - - // 归一化right向量 - var rightLength = Math.Sqrt(right.X * right.X + right.Y * right.Y + right.Z * right.Z); - if (rightLength > 0.001) - { - right = new Vector3D(right.X / rightLength, right.Y / rightLength, right.Z / rightLength); - } - - // 计算高度向量（垂直于方向向量和宽度向量） - var up = new Vector3D( - direction.Y * right.Z - direction.Z * right.Y, - direction.Z * right.X - direction.X * right.Z, - direction.X * right.Y - direction.Y * right.X - ); - - // 归一化up向量 - var upLength = Math.Sqrt(up.X * up.X + up.Y * up.Y + up.Z * up.Z); - if (upLength > 0.001) - { - up = new Vector3D(up.X / upLength, up.Y / upLength, up.Z / upLength); - } + var (right, up) = ObjectSpaceOrientationHelper.CalculateAxes( + startPoint, + endPoint, + upReference ?? GetHostUpVector(), + horizontalDirection); // 计算长方体的边界框和向量 var halfWidth = width / 2.0; @@ -3009,7 +2888,8 @@ namespace NavisworksTransport lineMarker.EndPoint, width, lineMarker.Height, - lineMarker.HorizontalDirection + lineMarker.HorizontalDirection, + lineMarker.UpDirection ); } } @@ -3052,11 +2932,25 @@ namespace NavisworksTransport width, lineMarker.Height, _passageAlongPath, - lineMarker.HorizontalDirection + lineMarker.HorizontalDirection, + lineMarker.UpDirection ); } } + private Vector3D ResolveObjectSpaceUpDirection(PathRoute route) + { + if (route?.PathType == NavisworksTransport.PathType.Rail && route.RailPreferredNormal != null) + { + return Normalize(new Vector3D( + route.RailPreferredNormal.X, + route.RailPreferredNormal.Y, + route.RailPreferredNormal.Z)); + } + + return GetHostUpVector(); + } + /// /// 计算采样点处的切线方向 /// diff --git a/src/UI/WPF/ViewModels/PathEditingViewModel.cs b/src/UI/WPF/ViewModels/PathEditingViewModel.cs index 1335e08..05e2cd6 100644 --- a/src/UI/WPF/ViewModels/PathEditingViewModel.cs +++ b/src/UI/WPF/ViewModels/PathEditingViewModel.cs @@ -154,6 +154,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels private Vector3 _assemblyInstallationPlaneNormal; private Vector3 _assemblyInstallationPlaneSpanDirection; private double _assemblyInstallationOffsetDistanceInMeters; + private readonly List _assemblyInstallationSeedPoints = new List(); private readonly List _assemblyEndFaceSeedPoints = new List(); private const string AssemblyAnchorMarkerPathId = "assembly_anchor_marker"; private const string AssemblyCenterGuideLinePathId = "assembly_center_guide_line"; @@ -1347,6 +1348,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels { await SafeExecuteAsync(() => { + ClearNonGridPathVisualizations("[直线装配] 捕获箱体"); + var document = NavisApplication.ActiveDocument; var selectedItem = document?.CurrentSelection?.SelectedItems?.FirstOrDefault(); if (selectedItem == null) @@ -1379,6 +1382,30 @@ namespace NavisworksTransport.UI.WPF.ViewModels }, "捕获终点箱体"); } + private void ClearNonGridPathVisualizations(string context) + { + if (PathPointRenderPlugin.Instance == null) + { + return; + } + + try + { + PathPointRenderPlugin.Instance.ClearPathsExcept( + "grid_visualization_all", + "grid_visualization_channel", + "grid_visualization_unknown", + "grid_visualization_obstacle", + "grid_visualization_door"); + LogManager.Info($"{context}：已清除现有路径可视化显示（保留网格可视化）"); + } + catch (Exception ex) + { + LogManager.Error($"{context}：清除现有路径可视化失败: {ex.Message}", ex); + throw; + } + } + private async Task ExecuteGenerateAssemblyReferenceRodAsync() { await SafeExecuteAsync(() => @@ -1546,7 +1573,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels throw new InvalidOperationException("ToolPlugin 初始化失败，请重试。"); } - UpdateMainStatus("请在终点箱体表面点击安装点，系统将按该点计算安装参考面和终点锚点。"); + _assemblyInstallationSeedPoints.Clear(); + UpdateMainStatus("请在终点箱体表面连续点击两个安装面点，系统将按两点和光轴计算安装参考面与安装点。"); LogManager.Info("[直线装配] 已进入安装点拾取模式"); }, "选择安装点"); } @@ -1640,7 +1668,21 @@ namespace NavisworksTransport.UI.WPF.ViewModels return; } - BuildAndRenderAssemblyInstallationReference(pickResult.Point); + _assemblyInstallationSeedPoints.Add(pickResult.Point); + RenderAssemblyInstallationPickPoints(_assemblyInstallationSeedPoints); + + int pickedCount = _assemblyInstallationSeedPoints.Count; + LogManager.Info($"[直线装配] 已记录安装面点 {pickedCount}: ({pickResult.Point.X:F3}, {pickResult.Point.Y:F3}, {pickResult.Point.Z:F3})"); + + if (pickedCount < 2) + { + UpdateMainStatus("已记录安装面点 1/2，请继续在同一安装面上点击第二个点。"); + return; + } + + BuildAndRenderAssemblyInstallationReference( + _assemblyInstallationSeedPoints[0], + _assemblyInstallationSeedPoints[1]); CleanupAssemblyInstallationSelection(); }, "处理安装点拾取"); } @@ -1665,6 +1707,14 @@ namespace NavisworksTransport.UI.WPF.ViewModels RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine }; + if (_hasAssemblyInstallationReference) + { + route.RailPreferredNormal = new Point3D( + _assemblyInstallationPlaneNormal.X, + _assemblyInstallationPlaneNormal.Y, + _assemblyInstallationPlaneNormal.Z); + } + route.AddPoint(new PathPoint(startPoint, "装配起点", PathPointType.StartPoint)); route.AddPoint(new PathPoint(endPoint, "装配终点", PathPointType.EndPoint)); @@ -1706,7 +1756,9 @@ namespace NavisworksTransport.UI.WPF.ViewModels GetAssemblyAnchorText()); OnPropertyChanged(nameof(PathRoutes)); - LogManager.Info($"[直线装配] 已生成并完成路径: {route.Name}，起点=({startPoint.X:F2}, {startPoint.Y:F2}, {startPoint.Z:F2})，终点=({endPoint.X:F2}, {endPoint.Y:F2}, {endPoint.Z:F2})"); + LogManager.Info( + $"[直线装配] 已生成并完成路径: {route.Name}，起点=({startPoint.X:F2}, {startPoint.Y:F2}, {startPoint.Z:F2})，终点=({endPoint.X:F2}, {endPoint.Y:F2}, {endPoint.Z:F2})" + + $"{(route.RailPreferredNormal != null ? $"，安装法向=({route.RailPreferredNormal.X:F3}, {route.RailPreferredNormal.Y:F3}, {route.RailPreferredNormal.Z:F3})" : string.Empty)}"); } private void HideAssemblyReferenceVisuals(bool resetStartPointText, string statusMessage, string logMessage) @@ -1882,7 +1934,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels : string.Format("{0}点=未选择", anchorText); string installationText = _hasAssemblyInstallationReference ? string.Format( - "选定安装点=({0:F2}, {1:F2}, {2:F2})，安装点=({3:F2}, {4:F2}, {5:F2})，偏距={6:F3}m", + "安装面点中点=({0:F2}, {1:F2}, {2:F2})，安装点=({3:F2}, {4:F2}, {5:F2})，偏距={6:F3}m", _assemblyInstallationPickPoint.X, _assemblyInstallationPickPoint.Y, _assemblyInstallationPickPoint.Z, @@ -2214,7 +2266,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels RenderStyleName.AssemblyGuideLine); } - private void BuildAndRenderAssemblyInstallationReference(Point3D pickPoint) + private void BuildAndRenderAssemblyInstallationReference(Point3D firstPickPoint, Point3D secondPickPoint) { Point3D opticalAxisReferencePoint = GetAssemblyOpticalAxisReferencePoint(); Point3D sphereCenterPoint = new Point3D( @@ -2230,10 +2282,11 @@ namespace NavisworksTransport.UI.WPF.ViewModels AssemblyInstallationReferenceResult result = AssemblyInstallationReferenceBuilder.Build( new Vector3((float)opticalAxisReferencePoint.X, (float)opticalAxisReferencePoint.Y, (float)opticalAxisReferencePoint.Z), opticalAxisDirection, - new Vector3((float)pickPoint.X, (float)pickPoint.Y, (float)pickPoint.Z)); + new Vector3((float)firstPickPoint.X, (float)firstPickPoint.Y, (float)firstPickPoint.Z), + new Vector3((float)secondPickPoint.X, (float)secondPickPoint.Y, (float)secondPickPoint.Z)); _hasAssemblyInstallationReference = true; - _assemblyInstallationPickPoint = pickPoint; + _assemblyInstallationPickPoint = AssemblyEndFaceAnalyzer.ToPoint3D((result.PickPoint + result.SecondaryPickPoint) * 0.5f); _assemblyInstallationBaseAnchorPoint = AssemblyEndFaceAnalyzer.ToPoint3D(result.AnchorPoint); _assemblyInstallationPlaneNormal = result.PlaneNormal; _assemblyInstallationPlaneSpanDirection = result.PlaneSpanDirection; @@ -2242,7 +2295,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels OnPropertyChanged(nameof(AssemblyAnchorVerticalOffsetInMeters)); UpdateAssemblyInstallationAnchorFromVerticalOffset(); - RenderAssemblyInstallationPickPoint(_assemblyInstallationPickPoint); + RenderAssemblyInstallationPickPoints(_assemblyInstallationSeedPoints); RenderAssemblyInstallationCenterLine(AssemblyEndFaceAnalyzer.ToPoint3D(result.InstallLineBasePoint), result.OpticalAxisDirection); RefreshAssemblyTerminalObjectInfo(); @@ -2250,14 +2303,15 @@ namespace NavisworksTransport.UI.WPF.ViewModels UpdateMainStatus( $"安装参考已计算：安装点=({_assemblyInstallationAnchorPoint.X:F2}, {_assemblyInstallationAnchorPoint.Y:F2}, {_assemblyInstallationAnchorPoint.Z:F2})，偏距={_assemblyInstallationOffsetDistanceInMeters:F3}m"); LogManager.Info( - $"[直线装配] 安装参考已计算: 选定安装点=({pickPoint.X:F3}, {pickPoint.Y:F3}, {pickPoint.Z:F3}), " + + $"[直线装配] 安装参考已计算: 安装面点1=({firstPickPoint.X:F3}, {firstPickPoint.Y:F3}, {firstPickPoint.Z:F3}), " + + $"安装面点2=({secondPickPoint.X:F3}, {secondPickPoint.Y:F3}, {secondPickPoint.Z:F3}), " + $"安装点=({_assemblyInstallationAnchorPoint.X:F3}, {_assemblyInstallationAnchorPoint.Y:F3}, {_assemblyInstallationAnchorPoint.Z:F3}), " + $"偏距={_assemblyInstallationOffsetDistanceInMeters:F3}m, 平面法向=({result.PlaneNormal.X:F4}, {result.PlaneNormal.Y:F4}, {result.PlaneNormal.Z:F4})"); OnPropertyChanged(nameof(CanGenerateAssemblyReferenceRod)); OnPropertyChanged(nameof(AssemblyInstallationPointButtonText)); } - private void RenderAssemblyInstallationPickPoint(Point3D pickPoint) + private void RenderAssemblyInstallationPickPoints(IReadOnlyList pickPoints) { var renderPlugin = PathPointRenderPlugin.Instance; if (renderPlugin == null) @@ -2271,7 +2325,10 @@ namespace NavisworksTransport.UI.WPF.ViewModels Id = AssemblyInstallationPickPointPathId, Description = "终端安装点" }; - AddVisualizationPoint(markerRoute, pickPoint, "安装点", PathPointType.WayPoint); + for (int i = 0; i < pickPoints.Count; i++) + { + AddVisualizationPoint(markerRoute, pickPoints[i], $"安装面点{i + 1}", PathPointType.WayPoint); + } renderPlugin.RenderPointOnly(markerRoute); } @@ -2462,6 +2519,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels _assemblyInstallationPlaneNormal = default(Vector3); _assemblyInstallationPlaneSpanDirection = default(Vector3); _assemblyInstallationOffsetDistanceInMeters = 0.0; + _assemblyInstallationSeedPoints.Clear(); _assemblyAnchorVerticalOffsetInMeters = DefaultAssemblyAnchorVerticalOffsetInMeters; OnPropertyChanged(nameof(AssemblyAnchorVerticalOffsetInMeters)); OnPropertyChanged(nameof(CanGenerateAssemblyReferenceRod)); diff --git a/src/Utils/Assembly/AssemblyInstallationReferenceBuilder.cs b/src/Utils/Assembly/AssemblyInstallationReferenceBuilder.cs index 2167c93..16f01eb 100644 --- a/src/Utils/Assembly/AssemblyInstallationReferenceBuilder.cs +++ b/src/Utils/Assembly/AssemblyInstallationReferenceBuilder.cs @@ -8,6 +8,7 @@ namespace NavisworksTransport.Utils.GeometryAnalysis public Vector3 OpticalAxisBasePoint { get; set; } public Vector3 OpticalAxisDirection { get; set; } public Vector3 PickPoint { get; set; } + public Vector3 SecondaryPickPoint { get; set; } public Vector3 PlaneNormal { get; set; } public Vector3 PlaneSpanDirection { get; set; } public float OffsetDistance { get; set; } @@ -57,6 +58,72 @@ namespace NavisworksTransport.Utils.GeometryAnalysis OpticalAxisBasePoint = opticalAxisBasePoint, OpticalAxisDirection = axisDir, PickPoint = pickPoint, + SecondaryPickPoint = pickPoint, + PlaneNormal = planeNormal, + PlaneSpanDirection = planeSpanDirection, + OffsetDistance = offsetDistance, + InstallLineBasePoint = installLineBasePoint, + AnchorPoint = anchorPoint + }; + } + + public static AssemblyInstallationReferenceResult Build( + Vector3 opticalAxisBasePoint, + Vector3 opticalAxisDirection, + Vector3 firstPickPoint, + Vector3 secondPickPoint) + { + float axisLength = opticalAxisDirection.Length(); + if (axisLength < 1e-6f) + { + throw new InvalidOperationException("光轴方向长度过小，无法计算安装参考。"); + } + + Vector3 axisDir = opticalAxisDirection / axisLength; + Vector3 pickSegment = secondPickPoint - firstPickPoint; + Vector3 axisAlignedComponent = axisDir * Vector3.Dot(pickSegment, axisDir); + Vector3 planeSpanDirection = pickSegment - axisAlignedComponent; + float spanLength = planeSpanDirection.Length(); + if (spanLength < MinimumOffsetDistance) + { + throw new InvalidOperationException("两次安装点拾取几乎与光轴平行，无法确定安装参考面，请重新选择。"); + } + + planeSpanDirection /= spanLength; + + Vector3 midpoint = (firstPickPoint + secondPickPoint) * 0.5f; + Vector3 planeNormal = Vector3.Cross(axisDir, planeSpanDirection); + float normalLength = planeNormal.Length(); + if (normalLength < 1e-6f) + { + throw new InvalidOperationException("安装参考面法向计算失败，请重新选择。"); + } + + planeNormal /= normalLength; + + Vector3 midpointOffset = midpoint - opticalAxisBasePoint; + float signedOffset = Vector3.Dot(midpointOffset, planeNormal); + if (Math.Abs(signedOffset) < MinimumOffsetDistance) + { + throw new InvalidOperationException("安装面中点距离光轴过小，无法确定安装参考面，请重新选择。"); + } + + if (signedOffset < 0f) + { + planeNormal = -planeNormal; + signedOffset = -signedOffset; + } + + float offsetDistance = signedOffset; + Vector3 installLineBasePoint = opticalAxisBasePoint + planeNormal * offsetDistance; + Vector3 anchorPoint = ProjectPointToLine(midpoint, installLineBasePoint, axisDir); + + return new AssemblyInstallationReferenceResult + { + OpticalAxisBasePoint = opticalAxisBasePoint, + OpticalAxisDirection = axisDir, + PickPoint = firstPickPoint, + SecondaryPickPoint = secondPickPoint, PlaneNormal = planeNormal, PlaneSpanDirection = planeSpanDirection, OffsetDistance = offsetDistance, diff --git a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs index e20e521..f1d5faa 100644 --- a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs +++ b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs @@ -17,6 +17,23 @@ namespace NavisworksTransport.Utils.CoordinateSystem Vector3 nextPoint, Vector3 canonicalUp, out RailLocalFrame frame) + { + return TryCreateLocalFrame( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + null, + out frame); + } + + public static bool TryCreateLocalFrame( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + Vector3? preferredNormal, + out RailLocalFrame frame) { frame = null; @@ -25,6 +42,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem currentPoint, nextPoint, canonicalUp, + preferredNormal, out var forward, out var lateral, out var up)) @@ -44,6 +62,27 @@ namespace NavisworksTransport.Utils.CoordinateSystem out Vector3 forward, out Vector3 lateral, out Vector3 up) + { + return TryCreateBasis( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + null, + out forward, + out lateral, + out up); + } + + public static bool TryCreateBasis( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + Vector3? preferredNormal, + out Vector3 forward, + out Vector3 lateral, + out Vector3 up) { forward = default; lateral = default; @@ -61,7 +100,13 @@ namespace NavisworksTransport.Utils.CoordinateSystem } forward = Vector3.Normalize(tangent); - Vector3 projectedUp = canonicalUp - Vector3.Dot(canonicalUp, forward) * forward; + Vector3 referenceUp = canonicalUp; + if (preferredNormal.HasValue && preferredNormal.Value.LengthSquared() >= TangentEpsilon) + { + referenceUp = Vector3.Normalize(preferredNormal.Value); + } + + Vector3 projectedUp = referenceUp - Vector3.Dot(referenceUp, forward) * forward; if (projectedUp.LengthSquared() < TangentEpsilon) { projectedUp = Math.Abs(forward.Z) < 0.9f @@ -95,6 +140,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem nextPoint, canonicalUp, convention, + null, 0.0, out rotation); } @@ -107,6 +153,27 @@ namespace NavisworksTransport.Utils.CoordinateSystem ModelAxisConvention convention, double localUpRotationDegrees, out Quaternion rotation) + { + return TryCreateQuaternion( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + convention, + null, + localUpRotationDegrees, + out rotation); + } + + public static bool TryCreateQuaternion( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + ModelAxisConvention convention, + Vector3? preferredNormal, + double localUpRotationDegrees, + out Quaternion rotation) { rotation = Quaternion.Identity; @@ -115,7 +182,15 @@ namespace NavisworksTransport.Utils.CoordinateSystem throw new ArgumentNullException(nameof(convention)); } - if (!TryCreateBasis(previousPoint, currentPoint, nextPoint, canonicalUp, out var forward, out _, out var up)) + if (!TryCreateBasis( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + preferredNormal, + out var forward, + out _, + out var up)) { return false; } diff --git a/src/Utils/CoordinateSystem/ObjectSpaceOrientationHelper.cs b/src/Utils/CoordinateSystem/ObjectSpaceOrientationHelper.cs new file mode 100644 index 0000000..65d9e2d --- /dev/null +++ b/src/Utils/CoordinateSystem/ObjectSpaceOrientationHelper.cs @@ -0,0 +1,92 @@ +using System; +using System.Numerics; +using Autodesk.Navisworks.Api; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + public static class ObjectSpaceOrientationHelper + { + public static (Vector3D right, Vector3D up) CalculateAxes( + Point3D startPoint, + Point3D endPoint, + Vector3D upReference, + Vector3D horizontalDirection = null) + { + var segmentDirection = new Vector3( + (float)(endPoint.X - startPoint.X), + (float)(endPoint.Y - startPoint.Y), + (float)(endPoint.Z - startPoint.Z)); + + var (right, up) = CalculateAxes( + segmentDirection, + ToNumerics(upReference), + horizontalDirection == null ? (Vector3?)null : ToNumerics(horizontalDirection)); + + return (ToNavisworks(right), ToNavisworks(up)); + } + + public static (Vector3 right, Vector3 up) CalculateAxes( + Vector3 segmentDirection, + Vector3 upReference, + Vector3? horizontalDirection = null) + { + double segmentLength = Math.Sqrt( + segmentDirection.X * segmentDirection.X + + segmentDirection.Y * segmentDirection.Y + + segmentDirection.Z * segmentDirection.Z); + + if (segmentLength < 1e-9) + { + throw new InvalidOperationException("路径段长度过小，无法计算通行空间方向。"); + } + + var normalizedDirection = Vector3.Normalize(segmentDirection); + var normalizedUpReference = Normalize(upReference); + var right = Cross(normalizedDirection, normalizedUpReference); + + double parallelToUp = Math.Abs(Vector3.Dot(normalizedDirection, normalizedUpReference)); + if (parallelToUp > 0.9) + { + if (horizontalDirection.HasValue) + { + right = Cross(horizontalDirection.Value, normalizedUpReference); + } + else + { + right = Math.Abs(normalizedUpReference.X) < 0.9f + ? Normalize(Cross(normalizedUpReference, new Vector3(1f, 0f, 0f))) + : Normalize(Cross(normalizedUpReference, new Vector3(0f, 1f, 0f))); + } + } + + right = Normalize(right); + var up = Normalize(Cross(normalizedDirection, right)); + return (right, up); + } + + private static Vector3 Cross(Vector3 a, Vector3 b) + { + return Vector3.Cross(a, b); + } + + private static Vector3 Normalize(Vector3 vector) + { + if (vector.LengthSquared() < 1e-9f) + { + return Vector3.Zero; + } + + return Vector3.Normalize(vector); + } + + private static Vector3 ToNumerics(Vector3D vector) + { + return new Vector3((float)vector.X, (float)vector.Y, (float)vector.Z); + } + + private static Vector3D ToNavisworks(Vector3 vector) + { + return new Vector3D(vector.X, vector.Y, vector.Z); + } + } +} diff --git a/src/Utils/RailPathPoseHelper.cs b/src/Utils/RailPathPoseHelper.cs index 2b74480..8fd306a 100644 --- a/src/Utils/RailPathPoseHelper.cs +++ b/src/Utils/RailPathPoseHelper.cs @@ -135,7 +135,7 @@ namespace NavisworksTransport.Utils var adapterForRail = CoordinateSystemManager.Instance.CreateHostAdapter(); var canonicalReferencePointForRail = adapterForRail.ToCanonicalPoint(referencePoint); - if (!TryCreateCanonicalLocalFrame(previousPoint, referencePoint, nextPoint, out RailLocalFrame frame)) + if (!TryCreateCanonicalLocalFrame(route, previousPoint, referencePoint, nextPoint, out RailLocalFrame frame)) { var canonicalCenterPointForRailFallback = new Point3D( canonicalReferencePointForRail.X, @@ -165,6 +165,22 @@ namespace NavisworksTransport.Utils out Matrix3 linearTransform) { return TryCreateRailLinearTransform( + null, + previousPoint, + currentPoint, + nextPoint, + out linearTransform); + } + + public static bool TryCreateRailLinearTransform( + PathRoute route, + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + out Matrix3 linearTransform) + { + return TryCreateRailLinearTransform( + route, previousPoint, currentPoint, nextPoint, @@ -177,6 +193,7 @@ namespace NavisworksTransport.Utils /// 通过模型局部轴约定显式指定本地哪个轴代表 forward/up。 /// public static bool TryCreateRailLinearTransform( + PathRoute route, Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, @@ -184,6 +201,7 @@ namespace NavisworksTransport.Utils out Matrix3 linearTransform) { return TryCreateRailLinearTransform( + route, previousPoint, currentPoint, nextPoint, @@ -193,6 +211,7 @@ namespace NavisworksTransport.Utils } public static bool TryCreateRailLinearTransform( + PathRoute route, Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, @@ -217,6 +236,7 @@ namespace NavisworksTransport.Utils canonicalCurrentPoint, canonicalNextPoint, HostCoordinateAdapter.CanonicalUpVector3, + ResolvePreferredNormal(route), out RailLocalFrame frame)) { return false; @@ -231,6 +251,7 @@ namespace NavisworksTransport.Utils canonicalNextPoint, HostCoordinateAdapter.CanonicalUpVector3, axisConvention, + ResolvePreferredNormal(route), localUpRotationDegrees, out var correctedRotation) ? correctedRotation @@ -270,6 +291,22 @@ namespace NavisworksTransport.Utils out Rotation3D rotation) { return TryCreateRailRotation( + null, + previousPoint, + currentPoint, + nextPoint, + out rotation); + } + + public static bool TryCreateRailRotation( + PathRoute route, + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + out Rotation3D rotation) + { + return TryCreateRailRotation( + route, previousPoint, currentPoint, nextPoint, @@ -285,6 +322,24 @@ namespace NavisworksTransport.Utils out Rotation3D rotation) { return TryCreateRailRotation( + null, + previousPoint, + currentPoint, + nextPoint, + axisConvention, + out rotation); + } + + public static bool TryCreateRailRotation( + PathRoute route, + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + ModelAxisConvention axisConvention, + out Rotation3D rotation) + { + return TryCreateRailRotation( + route, previousPoint, currentPoint, nextPoint, @@ -294,6 +349,7 @@ namespace NavisworksTransport.Utils } public static bool TryCreateRailRotation( + PathRoute route, Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, @@ -305,6 +361,7 @@ namespace NavisworksTransport.Utils LogRotationConstructorConventionOnce(); if (!TryCreateRailLinearTransform( + route, previousPoint, currentPoint, nextPoint, @@ -446,7 +503,7 @@ namespace NavisworksTransport.Utils private static Vector3D ResolveRailNormal(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint) { - if (TryCreateCanonicalLocalFrame(previousPoint, currentPoint, nextPoint, out RailLocalFrame frame)) + if (TryCreateCanonicalLocalFrame(null, previousPoint, currentPoint, nextPoint, out RailLocalFrame frame)) { return ToNavVector(frame.Normal); } @@ -479,6 +536,7 @@ namespace NavisworksTransport.Utils } private static bool TryCreateCanonicalLocalFrame( + PathRoute route, Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, @@ -490,9 +548,37 @@ namespace NavisworksTransport.Utils ToNumerics(adapter.ToCanonicalPoint(currentPoint)), ToNumerics(adapter.ToCanonicalPoint(nextPoint)), HostCoordinateAdapter.CanonicalUpVector3, + ResolvePreferredNormal(route), out frame); } + private static Vector3? ResolvePreferredNormal(PathRoute route) + { + if (route?.RailPreferredNormal == null) + { + return null; + } + + var preferredNormal = new Vector3( + (float)route.RailPreferredNormal.X, + (float)route.RailPreferredNormal.Y, + (float)route.RailPreferredNormal.Z); + + if (preferredNormal.LengthSquared() < 1e-9f) + { + return null; + } + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + Vector3 canonicalPreferredNormal = adapter.ToCanonicalVector3(preferredNormal); + if (canonicalPreferredNormal.LengthSquared() < 1e-9f) + { + return null; + } + + return Vector3.Normalize(canonicalPreferredNormal); + } + private static Vector3D Normalize(Vector3D vector) { double lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;