diff --git a/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs b/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs index 90ca32f..9280f70 100644 --- a/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs +++ b/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs @@ -63,11 +63,54 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4); } + [TestMethod] + public void CreateRotationFromPlanarBasePose_ShouldReturnBaseRotation_WhenYawUnchanged() + { + Quaternion baseRotation = Quaternion.Normalize( + Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.31f) * + Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.42f)); + + Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose( + baseRotation, + baseYawRadians: 1.25, + targetYawRadians: 1.25, + hostUp: Vector3.UnitY); + + AssertQuaternionEquivalent(resultRotation, baseRotation); + } + + [TestMethod] + public void CreateRotationFromPlanarBasePose_ShouldPreserveTiltAndApplyDeltaYaw() + { + Quaternion baseRotation = Quaternion.Normalize( + Quaternion.CreateFromAxisAngle(Vector3.UnitY, 0.20f) * + Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.35f) * + Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.40f)); + + Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose( + baseRotation, + baseYawRadians: 0.0, + targetYawRadians: System.Math.PI / 2.0, + hostUp: Vector3.UnitY); + + Quaternion expectedRotation = Quaternion.Normalize( + Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)(System.Math.PI / 2.0)) * + baseRotation); + + AssertQuaternionEquivalent(resultRotation, expectedRotation); + } + private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6) { Assert.AreEqual(x, actual.X, tolerance); Assert.AreEqual(y, actual.Y, tolerance); Assert.AreEqual(z, actual.Z, tolerance); } + + private static void AssertQuaternionEquivalent(Quaternion actual, Quaternion expected, double tolerance = 1e-6) + { + float dot = Quaternion.Dot(Quaternion.Normalize(actual), Quaternion.Normalize(expected)); + Assert.AreEqual(1.0, System.Math.Abs(dot), tolerance); + } } } diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 0780bb8..0f62d66 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -195,6 +195,9 @@ namespace NavisworksTransport.Core.Animation private Rotation3D _groundRealObjectBaseRotation = Rotation3D.Identity; private double _groundRealObjectBaseYaw = 0.0; private bool _hasGroundRealObjectBasePose = false; + private Rotation3D _hoistingRealObjectBaseRotation = Rotation3D.Identity; + private double _hoistingRealObjectBaseYaw = 0.0; + private bool _hasHoistingRealObjectBasePose = false; private Vector3D _groundRealObjectStartCompensation = new Vector3D(0, 0, 0); private bool _hasGroundRealObjectStartCompensation = false; private bool _suppressGroundRealObjectCompensation = false; @@ -844,16 +847,25 @@ namespace NavisworksTransport.Core.Animation if (IsRealObjectMode && _route.PathType == PathType.Ground) { _groundRealObjectBaseRotation = planarRotation; - _hasGroundRealObjectBasePose = TryResolveGroundRealObjectBaseYaw(out _groundRealObjectBaseYaw); + _hasGroundRealObjectBasePose = TryResolvePlanarRealObjectBaseYaw(PathType.Ground, out _groundRealObjectBaseYaw); LogManager.Info( $"[Ground真实物体基姿态] {animatedObject.DisplayName} BaseYaw={_groundRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " + $"已记录基姿态={_hasGroundRealObjectBasePose}, " + $"起点补偿=({_groundRealObjectStartCompensation.X:F3},{_groundRealObjectStartCompensation.Y:F3},{_groundRealObjectStartCompensation.Z:F3}), " + $"已启用补偿={_hasGroundRealObjectStartCompensation}"); } + else if (IsRealObjectMode && _route.PathType == PathType.Hoisting) + { + _hoistingRealObjectBaseRotation = planarRotation; + _hasHoistingRealObjectBasePose = TryResolvePlanarRealObjectBaseYaw(PathType.Hoisting, out _hoistingRealObjectBaseYaw); + LogManager.Debug( + $"[Hoisting真实物体基姿态] {(animatedObject ?? _animatedObject)?.DisplayName} BaseYaw={_hoistingRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " + + $"已记录基姿态={_hasHoistingRealObjectBasePose}, 姿态将复用于预览/生成/播放"); + } else { _hasGroundRealObjectBasePose = false; + _hasHoistingRealObjectBasePose = false; _groundRealObjectStartCompensation = new Vector3D(0, 0, 0); _hasGroundRealObjectStartCompensation = false; } @@ -978,6 +990,7 @@ namespace NavisworksTransport.Core.Animation UpdateObjectPosition(startPosition); SyncTrackedRotationToDisplayedPose(CurrentControlledObject ?? _animatedObject); _hasGroundRealObjectBasePose = false; + _hasHoistingRealObjectBasePose = false; _groundRealObjectStartCompensation = new Vector3D(0, 0, 0); _hasGroundRealObjectStartCompensation = false; @@ -3990,10 +4003,10 @@ namespace NavisworksTransport.Core.Animation $"来源={(hasActualGeometryRotation ? "实际几何姿态" : "跟踪姿态")}"); } - private bool TryResolveGroundRealObjectBaseYaw(out double yawRadians) + private bool TryResolvePlanarRealObjectBaseYaw(PathType pathType, out double yawRadians) { yawRadians = 0.0; - if (_route?.PathType != PathType.Ground || + if (_route?.PathType != pathType || !IsRealObjectMode || _pathPoints == null || _pathPoints.Count < 2) @@ -4008,7 +4021,7 @@ namespace NavisworksTransport.Core.Animation hostPoints.Add(new Vector3((float)_pathPoints[i].X, (float)_pathPoints[i].Y, (float)_pathPoints[i].Z)); } - return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(_route.PathType, hostPoints, hostType, out yawRadians); + return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(pathType, hostPoints, hostType, out yawRadians); } private bool TryCreateGroundRealObjectConstrainedRotation( @@ -4092,6 +4105,45 @@ namespace NavisworksTransport.Core.Animation return true; } + private bool TryCreateHoistingRealObjectConstrainedRotationFromHostForward( + Vector3 hostForward, + out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + if (!IsRealObjectMode || + _route?.PathType != PathType.Hoisting || + !_hasHoistingRealObjectBasePose) + { + return false; + } + + if (!PathTargetFrameResolver.TryCreatePlanarHostFrame( + hostForward, + CoordinateSystemManager.Instance.ResolvedType, + out var currentFrame)) + { + return false; + } + + if (!PathTargetFrameResolver.TryResolvePlanarHostYaw( + currentFrame.Forward, + CoordinateSystemManager.Instance.ResolvedType, + out double targetYawRadians)) + { + return false; + } + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + Quaternion baseQuaternion = Rotation3DToHostQuaternion(_hoistingRealObjectBaseRotation); + Quaternion targetQuaternion = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose( + baseQuaternion, + _hoistingRealObjectBaseYaw, + targetYawRadians, + Vector3.Normalize(adapter.HostUpVector3)); + rotation = adapter.FromHostQuaternionDirect(targetQuaternion); + return true; + } + private static Quaternion Rotation3DToHostQuaternion(Rotation3D rotation) { var linear = new Transform3D(rotation).Linear; @@ -4681,6 +4733,13 @@ namespace NavisworksTransport.Core.Animation { rotation = Rotation3D.Identity; + if (_route?.PathType == PathType.Hoisting && + _hasHoistingRealObjectBasePose && + TryCreateHoistingRealObjectConstrainedRotationFromHostForward(hostForward, out rotation)) + { + return true; + } + if (_route?.PathType == PathType.Hoisting && TryCreateHoistingRealObjectRotationFromActualPose(hostForward, out rotation)) { @@ -4711,7 +4770,7 @@ namespace NavisworksTransport.Core.Animation Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation); Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(hostComposedQuaternion); - LogManager.Info( + LogManager.Debug( $"[真实物体起点姿态] 选中参考轴=({solution.SelectedReferenceAxisLocal.X:F3},{solution.SelectedReferenceAxisLocal.Y:F3},{solution.SelectedReferenceAxisLocal.Z:F3}), " + $"投影前进=({solution.ProjectedForward.X:F3},{solution.ProjectedForward.Y:F3},{solution.ProjectedForward.Z:F3}), " + $"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " + @@ -4767,9 +4826,9 @@ namespace NavisworksTransport.Core.Animation _realObjectPlanarSelectedForwardAxis = selectedAxisDirection; _hasRealObjectPlanarSelectedForwardAxis = true; - LogManager.Info( + LogManager.Debug( $"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。"); - LogManager.Info( + 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°, " + @@ -4817,7 +4876,7 @@ namespace NavisworksTransport.Core.Animation { _realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX; _hasRealObjectPlanarSelectedForwardAxis = true; - LogManager.Info("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。"); + LogManager.Debug("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。"); } return true; @@ -5097,6 +5156,16 @@ namespace NavisworksTransport.Core.Animation _hasRealObjectPlanarSelectedForwardAxis = false; } + private void ResetPlanarRealObjectBasePoseCache() + { + _groundRealObjectBaseRotation = Rotation3D.Identity; + _groundRealObjectBaseYaw = 0.0; + _hasGroundRealObjectBasePose = false; + _hoistingRealObjectBaseRotation = Rotation3D.Identity; + _hoistingRealObjectBaseYaw = 0.0; + _hasHoistingRealObjectBasePose = false; + } + private LocalEulerRotationCorrection ResolveRealObjectLocalRotationCorrection() { return HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes( @@ -5114,6 +5183,7 @@ namespace NavisworksTransport.Core.Animation _objectStartPlacementMode = ObjectStartPlacementMode.AlignToPathPose; _hasRailPreservedPoseRotation = false; _objectRotationCorrection = rotationCorrection; + ResetPlanarRealObjectBasePoseCache(); // 如果动画已创建，更新物体到起点的朝向 if (_animatedObject != null && _pathPoints != null && _pathPoints.Count > 0) @@ -5145,6 +5215,7 @@ namespace NavisworksTransport.Core.Animation public void SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection rotationCorrection) { _objectRotationCorrection = rotationCorrection; + ResetPlanarRealObjectBasePoseCache(); LogManager.Debug($"[角度修正] 直接设置角度修正值: {_objectRotationCorrection}（不触发旋转）"); } diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs index 6da6be4..7dfc8b4 100644 --- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs +++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs @@ -1675,32 +1675,34 @@ namespace NavisworksTransport.UI.WPF.ViewModels _pathAnimationManager?.SetRealObjectDimensions(objectLength, objectWidth, objectHeight); LogManager.Debug($"[选择物体] 保存原始尺寸: 长度={_objectOriginalLength:F2}m, 宽度={_objectOriginalWidth:F2}m, 高度={_objectOriginalHeight:F2}m"); - // 3. 先注册到动画管理器，再设置 SelectedAnimatedObject。 - // SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()； - // 如果先移动、后 SetAnimatedObject，会把“已摆到起点后的当前姿态”再次当成参考姿态缓存。 - _pathAnimationManager?.SetAnimatedObject(newObject); - SelectedAnimatedObject = newObject; - LogManager.Info($"已选择移动物体: {SelectedAnimatedObject.DisplayName}"); - - // 选择实体物体意味着切换到实体模式，避免后续起点同步仍走虚拟物体分支。 + // 3. 选择实体物体意味着切换到实体模式，避免后续起点同步仍走虚拟物体分支。 if (UseVirtualObject) { UseVirtualObject = false; LogManager.Debug("[选择物体] 已切换到实体物体模式"); } - // 只有选择不同的物体时，才重置角度修正值 + // 4. 对“新物体”先清空角度修正，再触发 SelectedAnimatedObject 的起点落位。 + // SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()， + // 如果角度在后面才归零，就会先按旧角度把新选择的物体摆到起点。 if (!isSameObject) { - // 重置 ViewModel 中的角度修正值（会自动同步到 PathAnimationManager） + _pathAnimationManager?.SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection.Zero); ObjectRotationCorrection = LocalEulerRotationCorrection.Zero; LogManager.Debug("[选择物体] 已重置角度修正值为0（新物体）"); } else { - LogManager.Debug($"[选择物体] 保持当前角度修正值（同一物体）"); + LogManager.Debug("[选择物体] 保持当前角度修正值（同一物体）"); } + // 5. 先注册到动画管理器，再设置 SelectedAnimatedObject。 + // SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()； + // 如果先移动、后 SetAnimatedObject，会把“已摆到起点后的当前姿态”再次当成参考姿态缓存。 + _pathAnimationManager?.SetAnimatedObject(newObject); + SelectedAnimatedObject = newObject; + LogManager.Info($"已选择移动物体: {SelectedAnimatedObject.DisplayName}"); + } catch (Exception ex) { @@ -2121,6 +2123,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels // 1. 归位并清理 _pathAnimationManager.RestoreObjectToCADPosition(); _pathAnimationManager.ClearAnimationResults(); + _pathAnimationManager.SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection.Zero); + _pathAnimationManager.SetObjectStartPlacementMode(ObjectStartPlacementMode.AlignToPathPose); LogManager.Info("已清除PathAnimationManager中的动画数据并归位物体"); } diff --git a/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs b/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs index bd23473..c5ff9b4 100644 --- a/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs +++ b/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs @@ -5,6 +5,23 @@ namespace NavisworksTransport.Utils.CoordinateSystem { public static class HoistingRealObjectPoseHelper { + public static Quaternion CreateRotationFromPlanarBasePose( + Quaternion baseRotation, + double baseYawRadians, + double targetYawRadians, + Vector3 hostUp) + { + Vector3 normalizedHostUp = NormalizeSafe(hostUp); + if (normalizedHostUp.LengthSquared() < 1e-6f) + { + return Quaternion.Normalize(baseRotation); + } + + float deltaYawRadians = NormalizeRadians(targetYawRadians - baseYawRadians); + Quaternion deltaRotation = Quaternion.CreateFromAxisAngle(normalizedHostUp, deltaYawRadians); + return Quaternion.Normalize(deltaRotation * baseRotation); + } + public static bool TryCreateRotationFromActualPose( Quaternion actualRotation, Vector3 targetForward, @@ -131,5 +148,20 @@ namespace NavisworksTransport.Utils.CoordinateSystem return angle * sign; } + + private static float NormalizeRadians(double angleRadians) + { + while (angleRadians > Math.PI) + { + angleRadians -= 2.0 * Math.PI; + } + + while (angleRadians < -Math.PI) + { + angleRadians += 2.0 * Math.PI; + } + + return (float)angleRadians; + } } }