From 3892ddababe7d5dabf68f152cebac601ea64dd88 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Thu, 9 Apr 2026 21:09:43 +0800 Subject: [PATCH] Stabilize ground incremental rotation and extent tests --- NavisworksTransport.UnitTests.csproj | 1 + .../RealObjectProjectedExtentResolverTests.cs | 79 ++++++ .../RotatedObjectExtentHelperTests.cs | 232 ++++++++++++++++++ src/Core/Animation/PathAnimationManager.cs | 49 +--- .../RealObjectProjectedExtentResolver.cs | 44 ++++ .../RotatedObjectExtentHelper.cs | 18 ++ 6 files changed, 383 insertions(+), 40 deletions(-) diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index f26da39..701304c 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -73,6 +73,7 @@ + diff --git a/UnitTests/CoordinateSystem/RealObjectProjectedExtentResolverTests.cs b/UnitTests/CoordinateSystem/RealObjectProjectedExtentResolverTests.cs index 87fa470..595a088 100644 --- a/UnitTests/CoordinateSystem/RealObjectProjectedExtentResolverTests.cs +++ b/UnitTests/CoordinateSystem/RealObjectProjectedExtentResolverTests.cs @@ -51,6 +51,85 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreNotEqual(4.0, extents.upExtent, 1e-3, "双轴旋转后,Ground 的法线尺寸不应停留在旧高度。"); } + [TestMethod] + public void Ground_DisplayedHostAxesProjection_ShouldKeepXAxisLengthAndSwapXZAsAxesRotate() + { + Vector3 targetForward = Vector3.UnitX; + Vector3 targetUp = Vector3.UnitY; + + var noCorrection = RealObjectProjectedExtentResolver.CalculateProjectedDisplayedExtents( + displayedAxisXSize: 6.0, + displayedAxisYSize: 2.0, + displayedAxisZSize: 4.0, + displayedHostAxisX: Vector3.UnitX, + displayedHostAxisY: Vector3.UnitZ, + displayedHostAxisZ: -Vector3.UnitY, + targetForward, + targetUp); + + Assert.AreEqual(6.0, noCorrection.forwardExtent, 1e-5); + Assert.AreEqual(2.0, noCorrection.sideExtent, 1e-5); + Assert.AreEqual(4.0, noCorrection.upExtent, 1e-5); + + var xRotated = RealObjectProjectedExtentResolver.CalculateProjectedDisplayedExtents( + displayedAxisXSize: 6.0, + displayedAxisYSize: 2.0, + displayedAxisZSize: 4.0, + displayedHostAxisX: Vector3.UnitX, + displayedHostAxisY: Vector3.UnitY, + displayedHostAxisZ: Vector3.UnitZ, + targetForward, + targetUp); + + Assert.AreEqual(6.0, xRotated.forwardExtent, 1e-5); + Assert.AreEqual(4.0, xRotated.sideExtent, 1e-5); + Assert.AreEqual(2.0, xRotated.upExtent, 1e-5); + + var zRotated = RealObjectProjectedExtentResolver.CalculateProjectedDisplayedExtents( + displayedAxisXSize: 6.0, + displayedAxisYSize: 2.0, + displayedAxisZSize: 4.0, + displayedHostAxisX: -Vector3.UnitZ, + displayedHostAxisY: Vector3.UnitX, + displayedHostAxisZ: -Vector3.UnitY, + targetForward, + targetUp); + + Assert.AreEqual(2.0, zRotated.forwardExtent, 1e-5); + Assert.AreEqual(6.0, zRotated.sideExtent, 1e-5); + Assert.AreEqual(4.0, zRotated.upExtent, 1e-5); + } + + [TestMethod] + public void Ground_DisplayedHostAxesProjection_ShouldIgnoreVerticalComponentInForward() + { + Vector3 targetUp = Vector3.UnitY; + Vector3 planarForward = Vector3.Normalize(new Vector3(1f, 0f, 1f)); + Vector3 slopedForward = Vector3.Normalize(new Vector3(1f, 2f, 1f)); + + var planar = RealObjectProjectedExtentResolver.CalculateProjectedDisplayedExtents( + displayedAxisXSize: 6.0, + displayedAxisYSize: 2.0, + displayedAxisZSize: 4.0, + displayedHostAxisX: Vector3.UnitX, + displayedHostAxisY: Vector3.UnitZ, + displayedHostAxisZ: -Vector3.UnitY, + planarForward, + targetUp); + + var sloped = RealObjectProjectedExtentResolver.CalculateProjectedDisplayedExtents( + displayedAxisXSize: 6.0, + displayedAxisYSize: 2.0, + displayedAxisZSize: 4.0, + displayedHostAxisX: Vector3.UnitX, + displayedHostAxisY: Vector3.UnitZ, + displayedHostAxisZ: -Vector3.UnitY, + slopedForward, + targetUp); + + Assert.AreNotEqual(planar.forwardExtent, sloped.forwardExtent, 1e-3, "原始带竖直分量的路径方向会污染 Ground 尺寸投影。"); + } + private static double ProjectExtent( Vector3 localSize, Vector3 rotatedLocalX, diff --git a/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs b/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs index e6785ae..ca93b9b 100644 --- a/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs +++ b/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs @@ -1,5 +1,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; using NavisworksTransport.Utils.CoordinateSystem; +using System; using System.Numerics; namespace NavisworksTransport.UnitTests.CoordinateSystem @@ -86,5 +87,236 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreEqual(6.0, result.sideExtent, 1e-6); Assert.AreEqual(2.0, result.upExtent, 1e-6); } + + [TestMethod] + public void Ground_YUp_HostY90_ShouldSwapForwardAndSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 90.0, 0.0)); + + Assert.AreEqual(4.0, result.forwardExtent, 1e-6); + Assert.AreEqual(6.0, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostY45_ShouldBlendForwardAndSide_AndKeepUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 45.0, 0.0)); + + double expectedPlanar = 5.0 * Math.Sqrt(2.0); + Assert.AreEqual(expectedPlanar, result.forwardExtent, 1e-6); + Assert.AreEqual(expectedPlanar, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostY135_ShouldBlendForwardAndSide_AndKeepUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 135.0, 0.0)); + + double expectedPlanar = 5.0 * Math.Sqrt(2.0); + Assert.AreEqual(expectedPlanar, result.forwardExtent, 1e-6); + Assert.AreEqual(expectedPlanar, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostY180_ShouldKeepSemanticExtents() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 180.0, 0.0)); + + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostY270_ShouldSwapForwardAndSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 270.0, 0.0)); + + Assert.AreEqual(4.0, result.forwardExtent, 1e-6); + Assert.AreEqual(6.0, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostX90_ShouldKeepForward_AndSwapSideWithUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(90.0, 0.0, 0.0)); + + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(2.0, result.sideExtent, 1e-6); + Assert.AreEqual(4.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostX45_ShouldKeepForward_AndBlendSideWithUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(45.0, 0.0, 0.0)); + + double expectedBlend = 3.0 * Math.Sqrt(2.0); + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.sideExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostX135_ShouldKeepForward_AndBlendSideWithUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(135.0, 0.0, 0.0)); + + double expectedBlend = 3.0 * Math.Sqrt(2.0); + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.sideExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostX180_ShouldKeepSemanticExtents() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(180.0, 0.0, 0.0)); + + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostX270_ShouldKeepForward_AndSwapSideWithUp() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(270.0, 0.0, 0.0)); + + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(2.0, result.sideExtent, 1e-6); + Assert.AreEqual(4.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostZ90_ShouldPromoteUpToForward_AndKeepSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 0.0, 90.0)); + + Assert.AreEqual(2.0, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(6.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostZ45_ShouldBlendForwardWithUp_AndKeepSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 0.0, 45.0)); + + double expectedBlend = 4.0 * Math.Sqrt(2.0); + Assert.AreEqual(expectedBlend, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostZ135_ShouldBlendForwardWithUp_AndKeepSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 0.0, 135.0)); + + double expectedBlend = 4.0 * Math.Sqrt(2.0); + Assert.AreEqual(expectedBlend, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(expectedBlend, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostZ180_ShouldKeepSemanticExtents() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 0.0, 180.0)); + + Assert.AreEqual(6.0, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(2.0, result.upExtent, 1e-6); + } + + [TestMethod] + public void Ground_YUp_HostZ270_ShouldPromoteUpToForward_AndKeepSide() + { + var result = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + CoordinateSystemType.YUp, + forwardSize: 6.0, + sideSize: 4.0, + upSize: 2.0, + hostCorrection: new LocalEulerRotationCorrection(0.0, 0.0, 270.0)); + + Assert.AreEqual(2.0, result.forwardExtent, 1e-6); + Assert.AreEqual(4.0, result.sideExtent, 1e-6); + Assert.AreEqual(6.0, result.upExtent, 1e-6); + } } } diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 65cc77d..fc3a74c 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -4465,30 +4465,11 @@ namespace NavisworksTransport.Core.Animation ApplyGroundNonUpAxisCorrectionsAtTrackedPosition(_trackedPosition, adapter.HostType); - Rotation3D currentRotation = Rotation3D.Identity; - if (!ModelItemTransformHelper.TryGetCurrentGeometryRotation(CurrentControlledObject, out currentRotation)) - { - if (_hasTrackedRotation) - { - currentRotation = _trackedRotation; - } - else - { - LogManager.Error("[Ground纯增量起点] 无法读取当前显示旋转。"); - return false; - } - } - - if (!TryResolveDisplayedPlanarYawFromRotation(currentRotation, adapter.HostType, out double currentYawRadians)) - { - LogManager.Error("[Ground纯增量起点] 无法从当前显示状态解析宿主平面角。"); - return false; - } - double upAxisCorrectionRadians = ResolveGroundHostUpCorrectionRadians( _objectRotationCorrection, adapter.HostType); targetYawRadians = NormalizeRadians(targetYawRadians + upAxisCorrectionRadians); + double currentYawRadians = _currentYaw; double deltaYawRadians = NormalizeRadians(targetYawRadians - currentYawRadians); LogManager.Info( $"[Ground纯增量起点] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, " + @@ -4510,11 +4491,6 @@ namespace NavisworksTransport.Core.Animation _trackedRotation = appliedRotation; _hasTrackedRotation = true; } - else - { - _trackedRotation = currentRotation; - _hasTrackedRotation = true; - } _currentYaw = targetYawRadians; LogHostActualPoseAxes(CurrentControlledObject, "[Ground纯增量起点应用后宿主姿态]", false); @@ -4775,25 +4751,18 @@ namespace NavisworksTransport.Core.Animation return false; } - if (_route?.PathType == PathType.Ground && - TryCreateRealObjectPlanarRotationFromHostForward(hostForward, out var groundRotation)) + if (_route?.PathType == PathType.Ground) { - LocalAxisDirection groundSemanticUpAxis = - adapter.HostType == CoordinateSystemType.YUp - ? LocalAxisDirection.PositiveY - : LocalAxisDirection.PositiveZ; - convention = new ModelAxisConvention(LocalAxisDirection.PositiveX, groundSemanticUpAxis); - - Quaternion groundFinalHostQuaternion = Rotation3DToHostQuaternion(groundRotation); - extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents( - convention, + convention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType); + extents = RotatedObjectExtentHelper.CalculateGroundSemanticExtents( + adapter.HostType, _realObjectLength, _realObjectWidth, _realObjectHeight, - groundFinalHostQuaternion, - groundFinalHostQuaternion, - targetFrame.Forward, - targetFrame.Up); + _objectRotationCorrection); + LogManager.Debug( + $"[Ground通行空间尺寸] 使用Ground宿主语义尺寸: " + + $"Forward={extents.forwardExtent:F3}, Side={extents.sideExtent:F3}, Up={extents.upExtent:F3}"); return true; } diff --git a/src/Utils/CoordinateSystem/RealObjectProjectedExtentResolver.cs b/src/Utils/CoordinateSystem/RealObjectProjectedExtentResolver.cs index 1805346..c07af02 100644 --- a/src/Utils/CoordinateSystem/RealObjectProjectedExtentResolver.cs +++ b/src/Utils/CoordinateSystem/RealObjectProjectedExtentResolver.cs @@ -39,6 +39,50 @@ namespace NavisworksTransport.Utils.CoordinateSystem ProjectExtent(localSize, rotatedLocalX, rotatedLocalY, rotatedLocalZ, normalizedTargetUp)); } + public static (double forwardExtent, double sideExtent, double upExtent) CalculateProjectedDisplayedExtents( + double displayedAxisXSize, + double displayedAxisYSize, + double displayedAxisZSize, + Vector3 displayedHostAxisX, + Vector3 displayedHostAxisY, + Vector3 displayedHostAxisZ, + Vector3 targetForward, + Vector3 targetUp) + { + Vector3 displayedSize = new Vector3( + (float)displayedAxisXSize, + (float)displayedAxisYSize, + (float)displayedAxisZSize); + + Vector3 normalizedDisplayedHostAxisX = Vector3.Normalize(displayedHostAxisX); + Vector3 normalizedDisplayedHostAxisY = Vector3.Normalize(displayedHostAxisY); + Vector3 normalizedDisplayedHostAxisZ = Vector3.Normalize(displayedHostAxisZ); + + Vector3 normalizedTargetForward = Vector3.Normalize(targetForward); + Vector3 normalizedTargetUp = Vector3.Normalize(targetUp); + Vector3 normalizedTargetSide = Vector3.Normalize(Vector3.Cross(normalizedTargetForward, normalizedTargetUp)); + + return ( + ProjectExtent( + displayedSize, + normalizedDisplayedHostAxisX, + normalizedDisplayedHostAxisY, + normalizedDisplayedHostAxisZ, + normalizedTargetForward), + ProjectExtent( + displayedSize, + normalizedDisplayedHostAxisX, + normalizedDisplayedHostAxisY, + normalizedDisplayedHostAxisZ, + normalizedTargetSide), + ProjectExtent( + displayedSize, + normalizedDisplayedHostAxisX, + normalizedDisplayedHostAxisY, + normalizedDisplayedHostAxisZ, + normalizedTargetUp)); + } + private static double ProjectExtent( Vector3 localSize, Vector3 rotatedLocalX, diff --git a/src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs b/src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs index 09d58b3..7637e54 100644 --- a/src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs +++ b/src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs @@ -8,6 +8,24 @@ namespace NavisworksTransport.Utils.CoordinateSystem /// public static class RotatedObjectExtentHelper { + public static (double forwardExtent, double sideExtent, double upExtent) CalculateGroundSemanticExtents( + CoordinateSystemType hostType, + double forwardSize, + double sideSize, + double upSize, + LocalEulerRotationCorrection hostCorrection) + { + var convention = ModelAxisConvention.CreateDefaultForHost(hostType); + var adapter = new HostCoordinateAdapter(hostType); + Quaternion correctionQuaternion = adapter.CreateHostRotationCorrection(hostCorrection); + return CalculateProjectedSemanticExtents( + convention, + forwardSize, + sideSize, + upSize, + correctionQuaternion); + } + public static (double forwardExtent, double sideExtent, double upExtent) CalculateProjectedSemanticExtents( ModelAxisConvention convention, double forwardSize,