diff --git a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs index e8bb9a6..05e0d81 100644 --- a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs +++ b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs @@ -132,6 +132,24 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.IsFalse(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Rail, true)); } + [TestMethod] + public void ResolveAnimationFramePlaybackYawRadians_ShouldPreferPlanarHostYawWithHostCorrection() + { + var frame = new AnimationFrame + { + YawRadians = 0.25, + PlanarHostYawRadians = 0.75, + HasCustomRotation = true + }; + + double resolved = PathAnimationManager.ResolveAnimationFramePlaybackYawRadians( + frame, + new LocalEulerRotationCorrection(10.0, 20.0, 30.0), + CoordinateSystemType.YUp); + + Assert.AreEqual(0.75 + 20.0 * System.Math.PI / 180.0, resolved, 1e-9); + } + [TestMethod] public void ApplyRecordedPlanarRealObjectFramePose_ShouldPreserveFullRotation_ForGroundCollisionLogCase() { diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 1eef1cd..10dc9a1 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -759,7 +759,7 @@ namespace NavisworksTransport.Core.Animation $"Y=({railLinearTransform.Get(0, 1):F4},{railLinearTransform.Get(1, 1):F4},{railLinearTransform.Get(2, 1):F4}), " + $"Z=({railLinearTransform.Get(0, 2):F4},{railLinearTransform.Get(1, 2):F4},{railLinearTransform.Get(2, 2):F4})"); - UpdateObjectPosition(startPosition, railRotation); + ApplyFullPoseUpdate(startPosition, railRotation); LogManager.Info("[移动到起点] Rail路径已应用完整三维旋转姿态"); return true; } @@ -821,7 +821,7 @@ namespace NavisworksTransport.Core.Animation else if (_route.PathType != PathType.Rail && TryCreatePlanarPathRotationAtStart(out var planarRotation)) { - UpdateObjectPosition(startPosition, planarRotation); + ApplyFullPoseUpdate(startPosition, planarRotation); LogManager.Info("[移动到起点] 地面/吊装路径已应用完整三维姿态"); var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject); @@ -975,7 +975,7 @@ namespace NavisworksTransport.Core.Animation startPosition = ApplyGroundPathObjectLiftOffset(startPosition); } - UpdateObjectPosition(startPosition); + ApplyLegacyYawPoseUpdate(startPosition); SyncTrackedRotationToDisplayedPose(CurrentControlledObject ?? _animatedObject); _hasGroundRealObjectBasePose = false; @@ -1879,7 +1879,7 @@ namespace NavisworksTransport.Core.Animation _pausedProgress = 0.0; // 重置暂停进度 // 初始化动画状态时不再强行覆盖 _currentYaw 为第一帧的 yaw - // 这样在 UpdateObjectPosition(firstFrame.Position, firstFrame.YawRadians) 时 + // 这样在 ApplyLegacyYawPoseUpdate(firstFrame.Position, firstFrame.YawRadians) 时 // deltaYaw = firstFrame.YawRadians - 物体初始Yaw，从而产生旋转增量让物体转向 if (_animationFrames != null && _animationFrames.Count > 0) @@ -2685,7 +2685,7 @@ namespace NavisworksTransport.Core.Animation /// /// 更新对象位置和朝向（支持绕物体中心旋转） /// - private void UpdateObjectPosition(Point3D newPosition, double newYaw = double.NaN) + private void ApplyLegacyYawPoseUpdate(Point3D newPosition, double newYaw = double.NaN) { try { @@ -2741,7 +2741,7 @@ namespace NavisworksTransport.Core.Animation { // 纯平移 incrementalTransform = Transform3D.CreateTranslation(deltaPos); - LogManager.Debug($"[UpdateObjectPosition] 纯平移: ({deltaPos.X:F2},{deltaPos.Y:F2},{deltaPos.Z:F2})"); + LogManager.Debug($"[LegacyYawPose] 纯平移: ({deltaPos.X:F2},{deltaPos.Y:F2},{deltaPos.Z:F2})"); } // 应用增量变换（false = 增量模式） @@ -2760,7 +2760,7 @@ namespace NavisworksTransport.Core.Animation /// 使用完整三维姿态更新对象位置。 /// 当前先用于 Rail 路径，避免影响现有地面/吊装路径的 yaw 流程。 /// - private void UpdateObjectPosition(Point3D newPosition, Rotation3D newRotation) + private void ApplyFullPoseUpdate(Point3D newPosition, Rotation3D newRotation) { try { @@ -3054,19 +3054,13 @@ namespace NavisworksTransport.Core.Animation $"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})"); } - if (_savedObjectHasCustomRotation) - { - UpdateObjectPosition(_savedObjectPosition, _savedObjectRotation); - } - else - { - UpdateObjectPosition(_savedObjectPosition, _savedObjectYaw); - } - - _trackedPosition = _savedObjectPosition; - _currentYaw = _savedObjectYaw; - _trackedRotation = _savedObjectRotation; - _hasTrackedRotation = _savedObjectHasCustomRotation; + ApplyResolvedObjectPose( + _savedObjectPosition, + _savedObjectYaw, + _savedObjectRotation, + _savedObjectHasCustomRotation, + frameIndex: -1, + planarHostYawRadians: _savedObjectYaw); LogHostActualPoseAxes(obj, "[PAM恢复后宿主姿态]", false); @@ -3132,14 +3126,13 @@ namespace NavisworksTransport.Core.Animation LogHostActualPoseAxes(obj, "[动画姿态复用前宿主姿态]", false); - if (hasCustomRotation) - { - UpdateObjectPosition(targetPosition, targetRotation); - } - else - { - UpdateObjectPosition(targetPosition, targetYawRadians); - } + ApplyResolvedObjectPose( + targetPosition, + targetYawRadians, + targetRotation, + hasCustomRotation, + frameIndex: -1, + planarHostYawRadians: targetYawRadians); LogHostActualPoseAxes(obj, "[动画姿态复用后宿主姿态]", false); @@ -3251,16 +3244,9 @@ namespace NavisworksTransport.Core.Animation _currentFrameIndex = frameIndex; - // 更新对象位置和朝向 + // 更新对象位置和朝向，复用动画主播放与恢复使用的同一入口。 var frameData = _animationFrames[_currentFrameIndex]; - if (frameData.HasCustomRotation) - { - UpdateObjectPosition(frameData.Position, frameData.Rotation); - } - else - { - UpdateObjectPosition(frameData.Position, ResolveAnimationFramePlaybackYawRadians(frameData)); - } + ApplyAnimationFramePose(frameData, _currentFrameIndex); // 更新碰撞高亮 UpdateCollisionHighlightFromFrame(); @@ -5740,27 +5726,57 @@ namespace NavisworksTransport.Core.Animation return; } + ApplyResolvedObjectPose( + frameData.Position, + ResolveAnimationFramePlaybackYawRadians(frameData), + frameData.Rotation, + frameData.HasCustomRotation, + frameIndex, + frameData.PlanarHostYawRadians); + } + + private void ApplyResolvedObjectPose( + Point3D targetPosition, + double resolvedYawRadians, + Rotation3D targetRotation, + bool hasCustomRotation, + int frameIndex, + double? planarHostYawRadians = null) + { if (ShouldUsePlanarRealObjectPureIncrementFrames(_route?.PathType ?? PathType.Ground, IsRealObjectMode) && - frameData.PlanarHostYawRadians.HasValue) + planarHostYawRadians.HasValue) { - ApplyPlanarAnimationFrame(frameData.Position, frameData.PlanarHostYawRadians.Value); + ApplyPlanarAnimationFrame(targetPosition, planarHostYawRadians.Value); + return; } - else if (frameData.HasCustomRotation) + + if (hasCustomRotation) { - UpdateObjectPosition(frameData.Position, frameData.Rotation); + ApplyFullPoseUpdate(targetPosition, targetRotation); + return; } - else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) + + if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) { throw new InvalidOperationException( $"平面路径播放帧 {frameIndex} 缺少完整姿态，禁止退回旧 yaw 链路。"); } - else - { - UpdateObjectPosition(frameData.Position, ResolveAnimationFramePlaybackYawRadians(frameData)); - } + + ApplyLegacyYawPoseUpdate(targetPosition, resolvedYawRadians); } private double ResolveAnimationFramePlaybackYawRadians(AnimationFrame frameData) + { + return ResolveAnimationFramePlaybackYawRadians( + frameData, + _objectRotationCorrection, + CoordinateSystemManager.Instance.ResolvedType); + } + + internal static double ResolveAnimationFramePlaybackYawRadians( + AnimationFrame frameData, + LocalEulerRotationCorrection rotationCorrection, + CoordinateSystemType hostType) { if (frameData == null) { @@ -5772,14 +5788,14 @@ namespace NavisworksTransport.Core.Animation return NormalizeRadians( frameData.PlanarHostYawRadians.Value + ResolvePlanarHostUpCorrectionRadians( - _objectRotationCorrection, - CoordinateSystemManager.Instance.ResolvedType)); + rotationCorrection, + hostType)); } double resolvedYaw = frameData.YawRadians; - if (!frameData.HasCustomRotation && !_objectRotationCorrection.IsZero) + if (!frameData.HasCustomRotation && !rotationCorrection.IsZero) { - resolvedYaw += _objectRotationCorrection.ZDegrees * Math.PI / 180.0; + resolvedYaw += rotationCorrection.ZDegrees * Math.PI / 180.0; } return NormalizeRadians(resolvedYaw); diff --git a/src/Core/Collision/ClashDetectiveIntegration.cs b/src/Core/Collision/ClashDetectiveIntegration.cs index ac693d4..ebf343a 100644 --- a/src/Core/Collision/ClashDetectiveIntegration.cs +++ b/src/Core/Collision/ClashDetectiveIntegration.cs @@ -804,7 +804,7 @@ namespace NavisworksTransport if (candidate.AnimatedObjectHasTrackedRotation && targetRotation != null) { LogManager.Info( - $"[ClashDetective验证] 三维候选恢复: " + + $"[ClashDetective验证] 已跟踪姿态候选恢复: " + $"对象={testAnimatedObject.DisplayName}, " + $"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3})"); @@ -821,7 +821,7 @@ namespace NavisworksTransport else { throw new InvalidOperationException( - $"ClashDetective三维候选恢复失败：对象 {testAnimatedObject.DisplayName} 不受当前动画管理器控制。"); + $"ClashDetective已跟踪姿态候选恢复失败：对象 {testAnimatedObject.DisplayName} 不受当前动画管理器控制。"); } } else