preserve planar collision pose rotations

UnitTests/CoordinateSystem/RailPreservedPoseTests.cs

@@ -2,6 +2,7 @@ using Autodesk.Navisworks.Api;
 using Microsoft.VisualStudio.TestTools.UnitTesting;
 using NavisworksTransport.Core.Animation;
 using NavisworksTransport.Utils.CoordinateSystem;
+using System.Numerics;
 
 namespace NavisworksTransport.UnitTests.CoordinateSystem
 {
@@ -131,6 +132,38 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
             Assert.IsFalse(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Rail, true));
         }
 
+        [TestMethod]
+        public void ApplyRecordedPlanarRealObjectFramePose_ShouldPreserveFullRotation_ForGroundCollisionLogCase()
+        {
+            var frame = new AnimationFrame();
+            double yawRadians = 115.85 * System.Math.PI / 180.0;
+
+            var hostLinear = new Matrix4x4(
+                -0.4360f, 0.0000f, -0.8999f, 0.0f,
+                -0.6364f, 0.7071f, 0.3083f, 0.0f,
+                 0.6364f, 0.7071f, -0.3083f, 0.0f,
+                 0.0f,    0.0f,    0.0f,    1.0f);
+            Quaternion hostQuaternion = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(hostLinear));
+            var rotation = new Rotation3D(hostQuaternion.X, hostQuaternion.Y, hostQuaternion.Z, hostQuaternion.W);
+
+            PathAnimationManager.ApplyRecordedPlanarRealObjectFramePose(frame, yawRadians, rotation);
+
+            Assert.IsTrue(frame.PlanarHostYawRadians.HasValue);
+            Assert.AreEqual(yawRadians, frame.PlanarHostYawRadians.Value, 1e-9);
+            Assert.IsTrue(frame.HasCustomRotation);
+
+            Matrix3 linear = new Transform3D(frame.Rotation).Linear;
+            Assert.AreEqual(-0.4360, linear.Get(0, 0), 5e-4);
+            Assert.AreEqual(-0.6364, linear.Get(1, 0), 5e-4);
+            Assert.AreEqual(0.6364, linear.Get(2, 0), 5e-4);
+            Assert.AreEqual(0.0000, linear.Get(0, 1), 5e-4);
+            Assert.AreEqual(0.7071, linear.Get(1, 1), 5e-4);
+            Assert.AreEqual(0.7071, linear.Get(2, 1), 5e-4);
+            Assert.AreEqual(-0.8999, linear.Get(0, 2), 5e-4);
+            Assert.AreEqual(0.3083, linear.Get(1, 2), 5e-4);
+            Assert.AreEqual(-0.3083, linear.Get(2, 2), 5e-4);
+        }
+
         [TestMethod]
         public void TryResolveGroundHostPlanarYawFromFramePoints_ShouldResolveYUpHostYaw()
         {

src/Core/Animation/PathAnimationManager.cs

@@ -1344,7 +1344,21 @@ namespace NavisworksTransport.Core.Animation
                                 $"{_route.PathType.GetDisplayName()}真实物体第 {i} 帧未能解析宿主平面目标角。");
                         }
 
-                        frame.PlanarHostYawRadians = planarHostYawRadians;
+                        if (!TryResolveRecordedPlanarRealObjectFrameRotation(
+                                _route.PathType,
+                                previousFramePoint,
+                                framePosition,
+                                nextFramePoint,
+                                out var recordedRotation))
+                        {
+                            throw new InvalidOperationException(
+                                $"{_route.PathType.GetDisplayName()}真实物体第 {i} 帧未能记录完整姿态。");
+                        }
+
+                        ApplyRecordedPlanarRealObjectFramePose(
+                            frame,
+                            planarHostYawRadians,
+                            recordedRotation);
                     }
                     else if (ShouldPreservePathRotationForFrames(
                             _route.PathType,
@@ -3960,6 +3974,26 @@ namespace NavisworksTransport.Core.Animation
                    (pathType == PathType.Ground || pathType == PathType.Hoisting);
         }
 
+        internal static void ApplyRecordedPlanarRealObjectFramePose(
+            AnimationFrame frame,
+            double planarHostYawRadians,
+            Rotation3D recordedRotation)
+        {
+            if (frame == null)
+            {
+                throw new ArgumentNullException(nameof(frame));
+            }
+
+            if (recordedRotation == null)
+            {
+                throw new ArgumentNullException(nameof(recordedRotation));
+            }
+
+            frame.PlanarHostYawRadians = planarHostYawRadians;
+            frame.Rotation = recordedRotation;
+            frame.HasCustomRotation = true;
+        }
+
         internal static bool TryResolveDisplayedPlanarYawFromRotation(
             Rotation3D rotation,
             CoordinateSystemType hostType,
@@ -4195,6 +4229,53 @@ namespace NavisworksTransport.Core.Animation
             return true;
         }
 
+        private bool TryResolveRecordedPlanarRealObjectFrameRotation(
+            PathType pathType,
+            Point3D previousFramePoint,
+            Point3D currentFramePoint,
+            Point3D nextFramePoint,
+            out Rotation3D rotation)
+        {
+            rotation = Rotation3D.Identity;
+
+            if (!IsRealObjectMode)
+            {
+                return false;
+            }
+
+            if (pathType == PathType.Ground)
+            {
+                return TryCreateGroundRealObjectConstrainedRotation(
+                    previousFramePoint,
+                    currentFramePoint,
+                    nextFramePoint,
+                    out rotation);
+            }
+
+            if (pathType == PathType.Hoisting)
+            {
+                Vector3 hostForward = new Vector3(
+                    (float)(nextFramePoint.X - previousFramePoint.X),
+                    (float)(nextFramePoint.Y - previousFramePoint.Y),
+                    (float)(nextFramePoint.Z - previousFramePoint.Z));
+
+                if (hostForward.LengthSquared() <= 1e-6f)
+                {
+                    return false;
+                }
+
+                return TryResolveHoistingActualPose(
+                    hostForward,
+                    out rotation,
+                    out _,
+                    out _,
+                    out _,
+                    out _);
+            }
+
+            return false;
+        }
+
         private bool TryResolveHoistingActualPose(
             Vector3 hostForward,
             out Rotation3D rotation,