Stabilize ground incremental rotation and extent tests

NavisworksTransport.UnitTests.csproj

@@ -73,6 +73,7 @@
     <Compile Include="UnitTests\CoordinateSystem\RealObjectReferencePoseResolverTests.cs" />
     <Compile Include="UnitTests\CoordinateSystem\RealObjectRailAxisConventionResolverTests.cs" />
     <Compile Include="UnitTests\CoordinateSystem\RealObjectRailExtentResolverTests.cs" />
+    <Compile Include="UnitTests\CoordinateSystem\RotatedObjectExtentHelperTests.cs" />
     <Compile Include="UnitTests\CoordinateSystem\PathTargetFrameResolverTests.cs" />
     <Compile Include="UnitTests\CoordinateSystem\AssemblyEndFaceAnalyzerTests.cs" />
     <Compile Include="UnitTests\CoordinateSystem\AssemblyInstallationReferenceBuilderTests.cs" />

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,

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);
+        }
     }
 }

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 &&
+            if (_route?.PathType == PathType.Ground)
-                TryCreateRealObjectPlanarRotationFromHostForward(hostForward, out var groundRotation))
             {
-                LocalAxisDirection groundSemanticUpAxis =
+                convention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
-                    adapter.HostType == CoordinateSystemType.YUp
+                extents = RotatedObjectExtentHelper.CalculateGroundSemanticExtents(
-                        ? LocalAxisDirection.PositiveY
+                    adapter.HostType,
-                        : LocalAxisDirection.PositiveZ;
-                convention = new ModelAxisConvention(LocalAxisDirection.PositiveX, groundSemanticUpAxis);
-
-                Quaternion groundFinalHostQuaternion = Rotation3DToHostQuaternion(groundRotation);
-                extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents(
-                    convention,
                     _realObjectLength,
                     _realObjectWidth,
                     _realObjectHeight,
-                    groundFinalHostQuaternion,
+                    _objectRotationCorrection);
-                    groundFinalHostQuaternion,
+                LogManager.Debug(
-                    targetFrame.Forward,
+                    $"[Ground通行空间尺寸] 使用Ground宿主语义尺寸: " +
-                    targetFrame.Up);
+                    $"Forward={extents.forwardExtent:F3}, Side={extents.sideExtent:F3}, Up={extents.upExtent:F3}");
                 return true;
             }
 

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,

src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs

@@ -8,6 +8,24 @@ namespace NavisworksTransport.Utils.CoordinateSystem
     /// </summary>
     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,