Use actual hoisting pose without fragment fallback

2026-04-02 00:15:58 +08:00 · 2026-04-02 00:15:58 +08:00 · 7ff510daf0
commit 7ff510daf0
parent 953306fdb1
2 changed files with 227 additions and 59 deletions
--- a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs
+++ b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs
@ -87,5 +87,59 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem

            Assert.IsFalse(shouldPreserve);
        }
+
+        [TestMethod]
+        public void ShouldAllowFragmentPlanarFallback_ShouldDisableHoisting()
+        {
+            Assert.IsFalse(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Hoisting));
+            Assert.IsTrue(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Ground));
+            Assert.IsTrue(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Rail));
+        }
+
+        [TestMethod]
+        public void ResolveCurrentRotationBaseline_ShouldPreferActualGeometryForHoisting()
+        {
+            Rotation3D trackedRotation = new Rotation3D(0.0, 0.0, 0.0, 1.0);
+            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);
+
+            Rotation3D resolved = PathAnimationManager.ResolveCurrentRotationBaseline(
+                PathType.Hoisting,
+                isRealObjectMode: true,
+                hasTrackedRotation: true,
+                trackedRotation: trackedRotation,
+                hasReferenceRotation: false,
+                referenceRotation: Rotation3D.Identity,
+                transformRotation: Rotation3D.Identity,
+                hasActualGeometryRotation: true,
+                actualGeometryRotation: actualGeometryRotation);
+
+            Assert.AreEqual(actualGeometryRotation.A, resolved.A, 1e-9);
+            Assert.AreEqual(actualGeometryRotation.B, resolved.B, 1e-9);
+            Assert.AreEqual(actualGeometryRotation.C, resolved.C, 1e-9);
+            Assert.AreEqual(actualGeometryRotation.D, resolved.D, 1e-9);
+        }
+
+        [TestMethod]
+        public void ResolveCurrentRotationBaseline_ShouldKeepTrackedRotationForGround()
+        {
+            Rotation3D trackedRotation = new Rotation3D(0.0, 0.0, 0.38268343, 0.92387953);
+            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);
+
+            Rotation3D resolved = PathAnimationManager.ResolveCurrentRotationBaseline(
+                PathType.Ground,
+                isRealObjectMode: true,
+                hasTrackedRotation: true,
+                trackedRotation: trackedRotation,
+                hasReferenceRotation: false,
+                referenceRotation: Rotation3D.Identity,
+                transformRotation: Rotation3D.Identity,
+                hasActualGeometryRotation: true,
+                actualGeometryRotation: actualGeometryRotation);
+
+            Assert.AreEqual(trackedRotation.A, resolved.A, 1e-9);
+            Assert.AreEqual(trackedRotation.B, resolved.B, 1e-9);
+            Assert.AreEqual(trackedRotation.C, resolved.C, 1e-9);
+            Assert.AreEqual(trackedRotation.D, resolved.D, 1e-9);
+        }
    }
 }
--- a/src/Core/Animation/PathAnimationManager.cs
+++ b/src/Core/Animation/PathAnimationManager.cs
@ -2774,23 +2774,25 @@ namespace NavisworksTransport.Core.Animation
                Rotation3D currentRotation;
                Rotation3D currentRotationForTransform;
                Rotation3D actualGeometryRotation = Rotation3D.Identity;
+                bool hasActualGeometryRotation = false;
                Rotation3D appliedTargetRotation = newRotation;
                Rotation3D targetRotationForTransform = newRotation;
                Point3D appliedTargetPosition = newPosition;
-                if (_hasTrackedRotation)
-                {
-                    currentRotation = _trackedRotation;
-                }
-                else if (IsRealObjectMode && _hasRealObjectReferenceRotation)
-                {
-                    currentRotation = CoordinateSystemManager.Instance
-                        .CreateHostAdapter()
-                        .FromHostQuaternionDirect(_realObjectReferenceRotation);
-                }
-                else
-                {
-                    currentRotation = controlledObject.Transform.Factor().Rotation;
-                }
+                Rotation3D referenceRotation = IsRealObjectMode && _hasRealObjectReferenceRotation
+                    ? CoordinateSystemManager.Instance.CreateHostAdapter().FromHostQuaternionDirect(_realObjectReferenceRotation)
+                    : Rotation3D.Identity;
+                Rotation3D transformRotation = controlledObject.Transform.Factor().Rotation;
+                PathType currentPathType = _route?.PathType ?? PathType.Ground;
+                currentRotation = ResolveCurrentRotationBaseline(
+                    currentPathType,
+                    IsRealObjectMode,
+                    _hasTrackedRotation,
+                    _trackedRotation,
+                    _hasRealObjectReferenceRotation,
+                    referenceRotation,
+                    transformRotation,
+                    hasActualGeometryRotation: false,
+                    actualGeometryRotation: Rotation3D.Identity);

                try
                {
@ -2805,6 +2807,17 @@ namespace NavisworksTransport.Core.Animation
                         _route?.PathType == PathType.Rail) &&
                        ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out actualGeometryRotation))
                    {
+                        hasActualGeometryRotation = true;
+                        currentRotation = ResolveCurrentRotationBaseline(
+                            currentPathType,
+                            IsRealObjectMode,
+                            _hasTrackedRotation,
+                            _trackedRotation,
+                            _hasRealObjectReferenceRotation,
+                            referenceRotation,
+                            transformRotation,
+                            hasActualGeometryRotation,
+                            actualGeometryRotation);
                        var trackedLinear = new Transform3D(currentRotation).Linear;
                        var actualLinear = new Transform3D(actualGeometryRotation).Linear;
                        string poseTag = _route?.PathType == PathType.Rail
@ -3987,6 +4000,42 @@ namespace NavisworksTransport.Core.Animation
            return pathType == PathType.Rail || pathType == PathType.Hoisting;
        }

+        internal static bool ShouldAllowFragmentPlanarFallback(PathType pathType)
+        {
+            return pathType != PathType.Hoisting;
+        }
+
+        internal static Rotation3D ResolveCurrentRotationBaseline(
+            PathType pathType,
+            bool isRealObjectMode,
+            bool hasTrackedRotation,
+            Rotation3D trackedRotation,
+            bool hasReferenceRotation,
+            Rotation3D referenceRotation,
+            Rotation3D transformRotation,
+            bool hasActualGeometryRotation,
+            Rotation3D actualGeometryRotation)
+        {
+            if (isRealObjectMode &&
+                pathType == PathType.Hoisting &&
+                hasActualGeometryRotation)
+            {
+                return actualGeometryRotation;
+            }
+
+            if (hasTrackedRotation)
+            {
+                return trackedRotation;
+            }
+
+            if (isRealObjectMode && hasReferenceRotation)
+            {
+                return referenceRotation;
+            }
+
+            return transformRotation;
+        }
+
        private void SyncTrackedRotationToDisplayedPose(ModelItem sourceObject)
        {
            if (sourceObject == null)
@ -4159,6 +4208,71 @@ namespace NavisworksTransport.Core.Animation
            return true;
        }

+        private bool TryResolveHoistingActualPose(
+            Vector3 hostForward,
+            out Rotation3D rotation,
+            out Quaternion baselineQuaternion,
+            out LocalAxisDirection selectedAxisDirection,
+            out Vector3 selectedAxisLocal,
+            out Vector3 projectedForward)
+        {
+            rotation = Rotation3D.Identity;
+            baselineQuaternion = Quaternion.Identity;
+            selectedAxisDirection = LocalAxisDirection.PositiveX;
+            selectedAxisLocal = Vector3.UnitX;
+            projectedForward = Vector3.Zero;
+
+            if (_animatedObject == null ||
+                !ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out var actualRotation3D))
+            {
+                return false;
+            }
+
+            var actualRotation = new Quaternion(
+                (float)actualRotation3D.A,
+                (float)actualRotation3D.B,
+                (float)actualRotation3D.C,
+                (float)actualRotation3D.D);
+
+            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
+            Vector3 hostUp = adapter.HostType == CoordinateSystemType.YUp ? Vector3.UnitY : Vector3.UnitZ;
+            if (!HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
+                    actualRotation,
+                    hostForward,
+                    hostUp,
+                    out baselineQuaternion,
+                    out selectedAxisDirection,
+                    out selectedAxisLocal,
+                    out projectedForward))
+            {
+                return false;
+            }
+
+            rotation = adapter.FromHostQuaternionDirect(
+                adapter.ComposeHostQuaternion(baselineQuaternion, _objectRotationCorrection));
+
+            Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(baselineQuaternion);
+            Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(new Quaternion(
+                (float)rotation.A,
+                (float)rotation.B,
+                (float)rotation.C,
+                (float)rotation.D));
+
+            LogManager.Debug(
+                $"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。");
+            LogManager.Debug(
+                $"[真实物体起点姿态] 选中参考轴=({selectedAxisLocal.X:F3},{selectedAxisLocal.Y:F3},{selectedAxisLocal.Z:F3}), " +
+                $"投影前进=({projectedForward.X:F3},{projectedForward.Y:F3},{projectedForward.Z:F3}), " +
+                $"宿主修正={_objectRotationCorrection}, 本地修正=X=0.0°,Y=0.0°,Z=0.0°, " +
+                $"BaselineX=({baselineLinear.M11:F4},{baselineLinear.M21:F4},{baselineLinear.M31:F4}), " +
+                $"BaselineY=({baselineLinear.M12:F4},{baselineLinear.M22:F4},{baselineLinear.M32:F4}), " +
+                $"BaselineZ=({baselineLinear.M13:F4},{baselineLinear.M23:F4},{baselineLinear.M33:F4}), " +
+                $"HostComposeX=({hostComposedLinear.M11:F4},{hostComposedLinear.M21:F4},{hostComposedLinear.M31:F4}), " +
+                $"HostComposeY=({hostComposedLinear.M12:F4},{hostComposedLinear.M22:F4},{hostComposedLinear.M32:F4}), " +
+                $"HostComposeZ=({hostComposedLinear.M13:F4},{hostComposedLinear.M23:F4},{hostComposedLinear.M33:F4})");
+            return true;
+        }
+
        private static Quaternion Rotation3DToHostQuaternion(Rotation3D rotation)
        {
            var linear = new Transform3D(rotation).Linear;
@ -4357,8 +4471,9 @@ namespace NavisworksTransport.Core.Animation
            convention = null;
            extents = (0.0, 0.0, 0.0);

+            bool requiresReferenceRotation = _route?.PathType != PathType.Hoisting;
            if (!IsRealObjectMode ||
-                !_hasRealObjectReferenceRotation ||
+                (requiresReferenceRotation && !_hasRealObjectReferenceRotation) ||
                _realObjectLength <= 0.0 ||
                _realObjectWidth <= 0.0 ||
                _realObjectHeight <= 0.0)
@ -4386,8 +4501,9 @@ namespace NavisworksTransport.Core.Animation
            convention = null;
            extents = (0.0, 0.0, 0.0);

+            bool requiresReferenceRotation = _route?.PathType != PathType.Hoisting;
            if (!IsRealObjectMode ||
-                !_hasRealObjectReferenceRotation ||
+                (requiresReferenceRotation && !_hasRealObjectReferenceRotation) ||
                _realObjectLength <= 0.0 ||
                _realObjectWidth <= 0.0 ||
                _realObjectHeight <= 0.0)
@ -4432,6 +4548,38 @@ namespace NavisworksTransport.Core.Animation
                return true;
            }

+            if (_route?.PathType == PathType.Hoisting)
+            {
+                if (!TryResolveHoistingActualPose(
+                        hostForward,
+                        out var hoistingRotation,
+                        out var baselineQuaternion,
+                        out var selectedAxisDirection,
+                        out _,
+                        out _))
+                {
+                    return false;
+                }
+
+                LocalAxisDirection hoistingSemanticUpAxis =
+                    adapter.HostType == CoordinateSystemType.YUp
+                        ? LocalAxisDirection.PositiveY
+                        : LocalAxisDirection.PositiveZ;
+                convention = new ModelAxisConvention(selectedAxisDirection, hoistingSemanticUpAxis);
+
+                Quaternion hoistingFinalHostQuaternion = Rotation3DToHostQuaternion(hoistingRotation);
+                extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents(
+                    convention,
+                    _realObjectLength,
+                    _realObjectWidth,
+                    _realObjectHeight,
+                    baselineQuaternion,
+                    hoistingFinalHostQuaternion,
+                    targetFrame.Forward,
+                    targetFrame.Up);
+                return true;
+            }
+
            if (!TryCreateRealObjectPlanarPoseSolution(
                targetFrame.Forward,
                targetFrame.Up,
@ -4761,6 +4909,12 @@ namespace NavisworksTransport.Core.Animation
                return true;
            }

+            if (_route?.PathType == PathType.Hoisting)
+            {
+                LogManager.Error("[吊装真实物体姿态] 无法基于实际几何姿态生成起点姿态，已禁止回退到 fragment 参考姿态。");
+                return false;
+            }
+
            if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
                hostForward,
                CoordinateSystemManager.Instance.CreateHostAdapter().HostType,
@ -4800,27 +4954,10 @@ namespace NavisworksTransport.Core.Animation

        private bool TryCreateHoistingRealObjectRotationFromActualPose(Vector3 hostForward, out Rotation3D rotation)
        {
-            rotation = Rotation3D.Identity;
-
-            if (_animatedObject == null ||
-                !ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out var actualRotation3D))
-            {
-                return false;
-            }
-
-            var actualRotation = new Quaternion(
-                (float)actualRotation3D.A,
-                (float)actualRotation3D.B,
-                (float)actualRotation3D.C,
-                (float)actualRotation3D.D);
-
-            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
-            Vector3 hostUp = adapter.HostType == CoordinateSystemType.YUp ? Vector3.UnitY : Vector3.UnitZ;
-            if (!HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
-                    actualRotation,
+            if (!TryResolveHoistingActualPose(
                    hostForward,
-                    hostUp,
-                    out var baselineQuaternion,
+                    out rotation,
+                    out _,
                    out var selectedAxisDirection,
                    out var selectedAxisLocal,
                    out var projectedForward))
@ -4828,31 +4965,8 @@ namespace NavisworksTransport.Core.Animation
                return false;
            }

-            rotation = adapter.FromHostQuaternionDirect(
-                adapter.ComposeHostQuaternion(baselineQuaternion, _objectRotationCorrection));
-
-            Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(baselineQuaternion);
-            Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(new Quaternion(
-                (float)rotation.A,
-                (float)rotation.B,
-                (float)rotation.C,
-                (float)rotation.D));
-
            _realObjectPlanarSelectedForwardAxis = selectedAxisDirection;
            _hasRealObjectPlanarSelectedForwardAxis = true;
-
-            LogManager.Debug(
-                $"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。");
-            LogManager.Debug(
-                $"[真实物体起点姿态] 选中参考轴=({selectedAxisLocal.X:F3},{selectedAxisLocal.Y:F3},{selectedAxisLocal.Z:F3}), " +
-                $"投影前进=({projectedForward.X:F3},{projectedForward.Y:F3},{projectedForward.Z:F3}), " +
-                $"宿主修正={_objectRotationCorrection}, 本地修正=X=0.0°,Y=0.0°,Z=0.0°, " +
-                $"BaselineX=({baselineLinear.M11:F4},{baselineLinear.M21:F4},{baselineLinear.M31:F4}), " +
-                $"BaselineY=({baselineLinear.M12:F4},{baselineLinear.M22:F4},{baselineLinear.M32:F4}), " +
-                $"BaselineZ=({baselineLinear.M13:F4},{baselineLinear.M23:F4},{baselineLinear.M33:F4}), " +
-                $"HostComposeX=({hostComposedLinear.M11:F4},{hostComposedLinear.M21:F4},{hostComposedLinear.M31:F4}), " +
-                $"HostComposeY=({hostComposedLinear.M12:F4},{hostComposedLinear.M22:F4},{hostComposedLinear.M32:F4}), " +
-                $"HostComposeZ=({hostComposedLinear.M13:F4},{hostComposedLinear.M23:F4},{hostComposedLinear.M33:F4})");
            return true;
        }