Stabilize ground incremental rotation and extent tests

2026-04-09 21:09:43 +08:00 · 2026-04-09 21:09:43 +08:00 · 3892ddabab
commit 3892ddabab
parent 405f721811
6 changed files with 383 additions and 40 deletions
--- a/NavisworksTransport.UnitTests.csproj
+++ b/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" />
--- 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,
--- 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);
+        }
    }
 }
--- 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;
            }

--- 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,
--- a/src/Utils/CoordinateSystem/RotatedObjectExtentHelper.cs
+++ b/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,