diff --git a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs index afac4e6..72f9614 100644 --- 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); + } } } diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index ad1507f..6cdfe7d 100644 --- 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; }