From 7851e6affa6f1ef162ec44d3eb070d9dfd9988f2 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Sat, 28 Mar 2026 22:21:11 +0800 Subject: [PATCH] Map real-object angle adjustments from host axes --- .../HostCoordinateAdapterTests.cs | 28 ++++ .../RealObjectReferencePoseResolverTests.cs | 36 +++++ src/Core/Animation/PathAnimationManager.cs | 149 ++++++++++++++--- .../CoordinateSystem/HostCoordinateAdapter.cs | 57 +++++++ .../RealObjectReferencePose.cs | 30 ++++ .../RealObjectReferencePoseResolver.cs | 152 ++++++++++++++++++ 6 files changed, 432 insertions(+), 20 deletions(-) diff --git a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs index 3ea652f..a5d7931 100644 --- a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs +++ b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs @@ -362,6 +362,34 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection)); } + [TestMethod] + public void RemapHostSemanticCorrectionToLocalAxes_ShouldPermuteAnglesFromSemanticAxes() + { + LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes( + new LocalEulerRotationCorrection(10.0, 20.0, 30.0), + LocalAxisDirection.PositiveY, + LocalAxisDirection.PositiveZ, + LocalAxisDirection.PositiveX); + + Assert.AreEqual(30.0, mapped.XDegrees, 1e-9); + Assert.AreEqual(10.0, mapped.YDegrees, 1e-9); + Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9); + } + + [TestMethod] + public void RemapHostSemanticCorrectionToLocalAxes_ShouldApplySignForNegativeMappedAxes() + { + LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes( + new LocalEulerRotationCorrection(10.0, 20.0, 30.0), + LocalAxisDirection.NegativeY, + LocalAxisDirection.PositiveZ, + LocalAxisDirection.NegativeX); + + Assert.AreEqual(-30.0, mapped.XDegrees, 1e-9); + Assert.AreEqual(-10.0, mapped.YDegrees, 1e-9); + Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9); + } + private static void AssertPoint(Vector3 point, double x, double y, double z) { Assert.AreEqual(x, point.X, 1e-9); diff --git a/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs b/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs index a731e10..5d6d355 100644 --- a/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs +++ b/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs @@ -26,9 +26,45 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem out RealObjectReferencePose pose); Assert.IsTrue(ok); + AssertVector(pose.RawAxisX, 1.0, 0.0, 0.0); + AssertVector(pose.RawAxisY, 0.0, 1.0, 0.0); + AssertVector(pose.RawAxisZ, 0.0, 0.0, 1.0); AssertVector(pose.AxisX, 1.0, 0.0, 0.0); AssertVector(pose.AxisY, 0.0, 0.0, 1.0); AssertVector(pose.AxisZ, 0.0, -1.0, 0.0); + Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldXAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldYAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldZAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostUpLocalAxis); + } + + [TestMethod] + public void YUp_WorldAxisMapping_ShouldFollowRawRepresentativeFrame() + { + var fragmentMatrices = new List + { + CreateMatrix( + axisX: new Vector3(0.0f, 0.0f, 1.0f), + axisY: new Vector3(1.0f, 0.0f, 0.0f), + axisZ: new Vector3(0.0f, 1.0f, 0.0f)) + }; + + bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices( + fragmentMatrices, + fragmentDefaultUpAxis: "Z", + hostUpAxis: "Y", + out RealObjectReferencePose pose); + + Assert.IsTrue(ok); + Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldXAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldYAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldZAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis); + Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis); } [TestMethod] diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 4ed30db..2c87bc5 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -115,6 +115,13 @@ namespace NavisworksTransport.Core.Animation private Vector3 _realObjectReferenceAxisX = Vector3.UnitX; private Vector3 _realObjectReferenceAxisY = Vector3.UnitY; private Vector3 _realObjectReferenceAxisZ = Vector3.UnitZ; + private LocalAxisDirection _realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX; + private LocalAxisDirection _realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY; + private LocalAxisDirection _realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ; + private LocalAxisDirection _realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX; + private LocalAxisDirection _realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY; + private LocalAxisDirection _realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ; + private LocalAxisDirection _realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY; private bool _hasRealObjectReferenceRotation = false; private LocalAxisDirection _realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX; private bool _hasRealObjectPlanarSelectedForwardAxis = false; @@ -2737,7 +2744,11 @@ namespace NavisworksTransport.Core.Animation Point3D currentPositionForTransform = _trackedPosition; Rotation3D currentRotation; + Rotation3D currentRotationForTransform; + Rotation3D actualGeometryRotation = Rotation3D.Identity; + bool hasActualGeometryRotation = false; Rotation3D appliedTargetRotation = newRotation; + Rotation3D targetRotationForTransform = newRotation; Point3D appliedTargetPosition = newPosition; if (_hasTrackedRotation) { @@ -2762,18 +2773,24 @@ namespace NavisworksTransport.Core.Animation $"[动画姿态入口] {controlledObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})"); if (IsRealObjectMode && - (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) && - ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out var actualGeometryRotation)) + (_route?.PathType == PathType.Ground || + _route?.PathType == PathType.Hoisting || + _route?.PathType == PathType.Rail) && + ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out actualGeometryRotation)) { + hasActualGeometryRotation = true; var trackedLinear = new Transform3D(currentRotation).Linear; var actualLinear = new Transform3D(actualGeometryRotation).Linear; + string poseTag = _route?.PathType == PathType.Rail + ? "[Rail姿态基线诊断]" + : "[平面姿态基线诊断]"; LogManager.Debug( - $"[平面姿态基线诊断] {controlledObject.DisplayName} 跟踪旋转: " + + $"{poseTag} {controlledObject.DisplayName} 跟踪旋转: " + $"X=({trackedLinear.Get(0, 0):F4},{trackedLinear.Get(1, 0):F4},{trackedLinear.Get(2, 0):F4}), " + $"Y=({trackedLinear.Get(0, 1):F4},{trackedLinear.Get(1, 1):F4},{trackedLinear.Get(2, 1):F4}), " + $"Z=({trackedLinear.Get(0, 2):F4},{trackedLinear.Get(1, 2):F4},{trackedLinear.Get(2, 2):F4})"); LogManager.Debug( - $"[平面姿态基线诊断] {controlledObject.DisplayName} 实际几何旋转: " + + $"{poseTag} {controlledObject.DisplayName} 实际几何旋转: " + $"X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " + $"Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " + $"Z=({actualLinear.Get(0, 2):F4},{actualLinear.Get(1, 2):F4},{actualLinear.Get(2, 2):F4})"); @@ -2783,6 +2800,27 @@ namespace NavisworksTransport.Core.Animation { LogManager.Warning($"[动画姿态入口] 读取宿主实际状态失败: {ex.Message}"); } + currentRotationForTransform = currentRotation; + + if (isRailRealObject) + { + if (ModelItemTransformHelper.TryGetCurrentOverrideRotation(controlledObject, out var currentOverrideRotation)) + { + currentRotationForTransform = currentOverrideRotation; + } + else + { + currentRotationForTransform = Rotation3D.Identity; + } + + if (!ModelItemTransformHelper.TryResolveOverrideRotationForFinalTarget( + controlledObject, + appliedTargetRotation, + out targetRotationForTransform)) + { + targetRotationForTransform = appliedTargetRotation; + } + } if (IsRealObjectMode && _route?.PathType == PathType.Ground && @@ -2803,7 +2841,7 @@ namespace NavisworksTransport.Core.Animation $"应用目标点=({appliedTargetPosition.X:F3},{appliedTargetPosition.Y:F3},{appliedTargetPosition.Z:F3})"); } - var currentLinear = new Transform3D(currentRotation).Linear; + var currentLinear = new Transform3D(currentRotationForTransform).Linear; var targetLinear = new Transform3D(appliedTargetRotation).Linear; LogManager.Debug( $"[动画姿态入口] {controlledObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " + @@ -2818,12 +2856,23 @@ namespace NavisworksTransport.Core.Animation $"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " + $"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " + $"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})"); + + if (isRailRealObject) + { + var overrideLinear = new Transform3D(targetRotationForTransform).Linear; + LogManager.Info( + $"[Rail覆盖姿态] {controlledObject.DisplayName} Override目标: " + + $"X=({overrideLinear.Get(0, 0):F4},{overrideLinear.Get(1, 0):F4},{overrideLinear.Get(2, 0):F4}), " + + $"Y=({overrideLinear.Get(0, 1):F4},{overrideLinear.Get(1, 1):F4},{overrideLinear.Get(2, 1):F4}), " + + $"Z=({overrideLinear.Get(0, 2):F4},{overrideLinear.Get(1, 2):F4},{overrideLinear.Get(2, 2):F4})"); + } + ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation( controlledObject, currentPositionForTransform, - currentRotation, + currentRotationForTransform, appliedTargetPosition, - appliedTargetRotation); + targetRotationForTransform); _trackedPosition = appliedTargetPosition; _trackedRotation = appliedTargetRotation; @@ -4044,16 +4093,23 @@ namespace NavisworksTransport.Core.Animation _realObjectReferenceAxisX, _realObjectReferenceAxisY, _realObjectReferenceAxisZ, + _realObjectReferenceHostUpLocalAxis, targetFrame.Forward, adapter.HostType, out convention, out var selectedForwardAxis, - out var selectedForwardWorldAxis)) + out var selectedForwardWorldAxis, + out var selectedUpAxis, + out var selectedUpWorldAxis)) { LogManager.Debug( $"[Rail姿态修正] Host={adapter.HostType}, 虚拟物体={IsVirtualObjectMode}, " + $"真实物体参考姿态生效, Forward={convention.ForwardAxis}, Up={convention.UpAxis}, " + - $"选中Forward轴={selectedForwardAxis}, 选中Forward世界轴=({selectedForwardWorldAxis.X:F4},{selectedForwardWorldAxis.Y:F4},{selectedForwardWorldAxis.Z:F4})"); + $"选中Forward轴={selectedForwardAxis}, 选中Forward世界轴=({selectedForwardWorldAxis.X:F4},{selectedForwardWorldAxis.Y:F4},{selectedForwardWorldAxis.Z:F4}), " + + $"选中Up轴={selectedUpAxis}, 选中Up世界轴=({selectedUpWorldAxis.X:F4},{selectedUpWorldAxis.Y:F4},{selectedUpWorldAxis.Z:F4}), " + + $"宿主Up对应本地轴={_realObjectReferenceHostUpLocalAxis}, " + + $"目标Forward=({targetFrame.Forward.X:F4},{targetFrame.Forward.Y:F4},{targetFrame.Forward.Z:F4}), " + + $"目标Up=({targetFrame.Up.X:F4},{targetFrame.Up.Y:F4},{targetFrame.Up.Z:F4})"); return true; } } @@ -4065,6 +4121,25 @@ namespace NavisworksTransport.Core.Animation return true; } + private static Vector3 ResolveReferenceWorldAxisForLocalDirection( + LocalAxisDirection axis, + Vector3 referenceAxisX, + Vector3 referenceAxisY, + Vector3 referenceAxisZ) + { + switch (axis) + { + case LocalAxisDirection.PositiveX: return Vector3.Normalize(referenceAxisX); + case LocalAxisDirection.NegativeX: return Vector3.Normalize(-referenceAxisX); + case LocalAxisDirection.PositiveY: return Vector3.Normalize(referenceAxisY); + case LocalAxisDirection.NegativeY: return Vector3.Normalize(-referenceAxisY); + case LocalAxisDirection.PositiveZ: return Vector3.Normalize(referenceAxisZ); + case LocalAxisDirection.NegativeZ: return Vector3.Normalize(-referenceAxisZ); + default: + throw new ArgumentOutOfRangeException(nameof(axis), axis, null); + } + } + private bool TryCalculateCurrentRealObjectRailProjectedExtents( Point3D previousPoint, Point3D currentPoint, @@ -4112,14 +4187,16 @@ namespace NavisworksTransport.Core.Animation (float)baselineRotation.B, (float)baselineRotation.C, (float)baselineRotation.D); + LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection(); Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion( baselineHostQuaternion, - _objectRotationCorrection); + localCorrection); return RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents( _realObjectReferenceAxisX, _realObjectReferenceAxisY, _realObjectReferenceAxisZ, + _realObjectReferenceHostUpLocalAxis, targetFrame.Forward, adapter.HostType, _realObjectLength, @@ -4232,7 +4309,7 @@ namespace NavisworksTransport.Core.Animation Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion( solution.BaselineRotation, - _objectRotationCorrection); + ResolveRealObjectLocalRotationCorrection()); extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents( convention, _realObjectLength, @@ -4351,9 +4428,10 @@ namespace NavisworksTransport.Core.Animation (float)baselineRotation.B, (float)baselineRotation.C, (float)baselineRotation.D); + LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection(); Quaternion composedHostQuaternion = adapter.ComposeHostQuaternion( baselineHostQuaternion, - _objectRotationCorrection); + localCorrection); rotation = adapter.FromHostQuaternionDirect(composedHostQuaternion); Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(baselineHostQuaternion); @@ -4362,6 +4440,7 @@ namespace NavisworksTransport.Core.Animation $"[Rail真实物体角度修正] 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}), " + + $"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " + $"HostComposeX=({composedLinear.M11:F4},{composedLinear.M21:F4},{composedLinear.M31:F4}), " + $"HostComposeY=({composedLinear.M12:F4},{composedLinear.M22:F4},{composedLinear.M32:F4}), " + $"HostComposeZ=({composedLinear.M13:F4},{composedLinear.M23:F4},{composedLinear.M33:F4})"); @@ -4547,7 +4626,8 @@ namespace NavisworksTransport.Core.Animation } var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); - Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, _objectRotationCorrection); + LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection(); + Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, localCorrection); rotation = adapter.FromHostQuaternionDirect(hostComposedQuaternion); Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation); @@ -4556,6 +4636,7 @@ namespace NavisworksTransport.Core.Animation LogManager.Info( $"[真实物体起点姿态] 选中参考轴=({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}, " + $"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}), " + @@ -4732,17 +4813,29 @@ namespace NavisworksTransport.Core.Animation referenceRotation = referencePose.Rotation; _realObjectReferenceRotation = referenceRotation; - _realObjectReferenceAxisX = referencePose.AxisX; - _realObjectReferenceAxisY = referencePose.AxisY; - _realObjectReferenceAxisZ = referencePose.AxisZ; + _realObjectReferenceAxisX = referencePose.RawAxisX; + _realObjectReferenceAxisY = referencePose.RawAxisY; + _realObjectReferenceAxisZ = referencePose.RawAxisZ; + _realObjectReferenceHostWorldXAxisLocalAxis = referencePose.HostWorldXAxisLocalAxis; + _realObjectReferenceHostWorldYAxisLocalAxis = referencePose.HostWorldYAxisLocalAxis; + _realObjectReferenceHostWorldZAxisLocalAxis = referencePose.HostWorldZAxisLocalAxis; + _realObjectReferenceHostSemanticXAxisLocalAxis = referencePose.HostSemanticXAxisLocalAxis; + _realObjectReferenceHostSemanticYAxisLocalAxis = referencePose.HostSemanticYAxisLocalAxis; + _realObjectReferenceHostSemanticZAxisLocalAxis = referencePose.HostSemanticZAxisLocalAxis; + _realObjectReferenceHostUpLocalAxis = referencePose.HostUpLocalAxis; _hasRealObjectReferenceRotation = true; LogManager.Info( $"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " + - $"X=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " + - $"Y=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " + - $"Z=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " + - $"fragment数量={referencePose.FragmentCount}, Fragment默认Up={referencePose.FragmentDefaultUpAxis}, 模型Up={referencePose.HostUpAxis}"); + $"RawX=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " + + $"RawY=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " + + $"RawZ=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " + + $"宿主世界X对应本地轴={referencePose.HostWorldXAxisLocalAxis}, 宿主世界Y对应本地轴={referencePose.HostWorldYAxisLocalAxis}, 宿主世界Z对应本地轴={referencePose.HostWorldZAxisLocalAxis}, " + + $"宿主语义X=({referencePose.AxisX.X:F4},{referencePose.AxisX.Y:F4},{referencePose.AxisX.Z:F4}), " + + $"宿主语义Y=({referencePose.AxisY.X:F4},{referencePose.AxisY.Y:F4},{referencePose.AxisY.Z:F4}), " + + $"宿主语义Z=({referencePose.AxisZ.X:F4},{referencePose.AxisZ.Y:F4},{referencePose.AxisZ.Z:F4}), " + + $"宿主语义X对应本地轴={referencePose.HostSemanticXAxisLocalAxis}, 宿主语义Y对应本地轴={referencePose.HostSemanticYAxisLocalAxis}, 宿主语义Z对应本地轴={referencePose.HostSemanticZAxisLocalAxis}, " + + $"宿主Up对应本地轴={referencePose.HostUpLocalAxis}, fragment数量={referencePose.FragmentCount}, Fragment默认Up={referencePose.FragmentDefaultUpAxis}, 模型Up={referencePose.HostUpAxis}"); return true; } catch (Exception ex) @@ -4809,11 +4902,27 @@ namespace NavisworksTransport.Core.Animation _realObjectReferenceAxisX = Vector3.UnitX; _realObjectReferenceAxisY = Vector3.UnitY; _realObjectReferenceAxisZ = Vector3.UnitZ; + _realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX; + _realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY; + _realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ; + _realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX; + _realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY; + _realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ; + _realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY; _hasRealObjectReferenceRotation = false; _realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX; _hasRealObjectPlanarSelectedForwardAxis = false; } + private LocalEulerRotationCorrection ResolveRealObjectLocalRotationCorrection() + { + return HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes( + _objectRotationCorrection, + _realObjectReferenceHostWorldXAxisLocalAxis, + _realObjectReferenceHostWorldYAxisLocalAxis, + _realObjectReferenceHostWorldZAxisLocalAxis); + } + /// /// 设置物体绕宿主 X/Y/Z 轴的角度修正 /// diff --git a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs index f95f366..db9efd1 100644 --- a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs +++ b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs @@ -277,6 +277,31 @@ namespace NavisworksTransport.Utils.CoordinateSystem return Quaternion.Normalize(baselineQuaternion * inverseCorrectionQuaternion); } + /// + /// 将“宿主语义 X/Y/Z”的角度修正重映射为当前真实物体本地 X/Y/Z 的角度修正。 + /// + public static LocalEulerRotationCorrection RemapHostSemanticCorrectionToLocalAxes( + LocalEulerRotationCorrection hostCorrection, + LocalAxisDirection hostSemanticXAxisLocalAxis, + LocalAxisDirection hostSemanticYAxisLocalAxis, + LocalAxisDirection hostSemanticZAxisLocalAxis) + { + if (hostCorrection.IsZero) + { + return LocalEulerRotationCorrection.Zero; + } + + double localXDegrees = 0.0; + double localYDegrees = 0.0; + double localZDegrees = 0.0; + + ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticXAxisLocalAxis, hostCorrection.XDegrees); + ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticYAxisLocalAxis, hostCorrection.YDegrees); + ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticZAxisLocalAxis, hostCorrection.ZDegrees); + + return new LocalEulerRotationCorrection(localXDegrees, localYDegrees, localZDegrees); + } + /// /// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。 /// @@ -462,5 +487,37 @@ namespace NavisworksTransport.Utils.CoordinateSystem { return (float)(degrees * Math.PI / 180.0); } + + private static void ApplyMappedAngle( + ref double localXDegrees, + ref double localYDegrees, + ref double localZDegrees, + LocalAxisDirection mappedLocalAxis, + double hostDegrees) + { + switch (mappedLocalAxis) + { + case LocalAxisDirection.PositiveX: + localXDegrees += hostDegrees; + break; + case LocalAxisDirection.NegativeX: + localXDegrees -= hostDegrees; + break; + case LocalAxisDirection.PositiveY: + localYDegrees += hostDegrees; + break; + case LocalAxisDirection.NegativeY: + localYDegrees -= hostDegrees; + break; + case LocalAxisDirection.PositiveZ: + localZDegrees += hostDegrees; + break; + case LocalAxisDirection.NegativeZ: + localZDegrees -= hostDegrees; + break; + default: + throw new ArgumentOutOfRangeException(nameof(mappedLocalAxis), mappedLocalAxis, null); + } + } } } diff --git a/src/Utils/CoordinateSystem/RealObjectReferencePose.cs b/src/Utils/CoordinateSystem/RealObjectReferencePose.cs index 75c9346..0f47001 100644 --- a/src/Utils/CoordinateSystem/RealObjectReferencePose.cs +++ b/src/Utils/CoordinateSystem/RealObjectReferencePose.cs @@ -10,26 +10,56 @@ namespace NavisworksTransport.Utils.CoordinateSystem { public RealObjectReferencePose( Quaternion rotation, + Vector3 rawAxisX, + Vector3 rawAxisY, + Vector3 rawAxisZ, Vector3 axisX, Vector3 axisY, Vector3 axisZ, + LocalAxisDirection hostWorldXAxisLocalAxis, + LocalAxisDirection hostWorldYAxisLocalAxis, + LocalAxisDirection hostWorldZAxisLocalAxis, + LocalAxisDirection hostSemanticXAxisLocalAxis, + LocalAxisDirection hostSemanticYAxisLocalAxis, + LocalAxisDirection hostSemanticZAxisLocalAxis, + LocalAxisDirection hostUpLocalAxis, int fragmentCount, string fragmentDefaultUpAxis, string hostUpAxis) { Rotation = rotation; + RawAxisX = rawAxisX; + RawAxisY = rawAxisY; + RawAxisZ = rawAxisZ; AxisX = axisX; AxisY = axisY; AxisZ = axisZ; + HostWorldXAxisLocalAxis = hostWorldXAxisLocalAxis; + HostWorldYAxisLocalAxis = hostWorldYAxisLocalAxis; + HostWorldZAxisLocalAxis = hostWorldZAxisLocalAxis; + HostSemanticXAxisLocalAxis = hostSemanticXAxisLocalAxis; + HostSemanticYAxisLocalAxis = hostSemanticYAxisLocalAxis; + HostSemanticZAxisLocalAxis = hostSemanticZAxisLocalAxis; + HostUpLocalAxis = hostUpLocalAxis; FragmentCount = fragmentCount; FragmentDefaultUpAxis = fragmentDefaultUpAxis; HostUpAxis = hostUpAxis; } public Quaternion Rotation { get; } + public Vector3 RawAxisX { get; } + public Vector3 RawAxisY { get; } + public Vector3 RawAxisZ { get; } public Vector3 AxisX { get; } public Vector3 AxisY { get; } public Vector3 AxisZ { get; } + public LocalAxisDirection HostWorldXAxisLocalAxis { get; } + public LocalAxisDirection HostWorldYAxisLocalAxis { get; } + public LocalAxisDirection HostWorldZAxisLocalAxis { get; } + public LocalAxisDirection HostSemanticXAxisLocalAxis { get; } + public LocalAxisDirection HostSemanticYAxisLocalAxis { get; } + public LocalAxisDirection HostSemanticZAxisLocalAxis { get; } + public LocalAxisDirection HostUpLocalAxis { get; } public int FragmentCount { get; } public string FragmentDefaultUpAxis { get; } public string HostUpAxis { get; } diff --git a/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs b/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs index 01fea03..767daf4 100644 --- a/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs +++ b/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs @@ -117,17 +117,169 @@ namespace NavisworksTransport.Utils.CoordinateSystem return false; } + if (!TryResolveSemanticLocalAxes( + rawFrame, + interpretedFrame, + out var hostSemanticXAxisLocalAxis, + out var hostSemanticYAxisLocalAxis, + out var hostSemanticZAxisLocalAxis)) + { + return false; + } + + if (!TryResolveWorldLocalAxes( + rawFrame, + out var hostWorldXAxisLocalAxis, + out var hostWorldYAxisLocalAxis, + out var hostWorldZAxisLocalAxis)) + { + return false; + } + referencePose = new RealObjectReferencePose( interpretedFrame.Rotation, + rawFrame.AxisX, + rawFrame.AxisY, + rawFrame.AxisZ, interpretedFrame.AxisX, interpretedFrame.AxisY, interpretedFrame.AxisZ, + hostWorldXAxisLocalAxis, + hostWorldYAxisLocalAxis, + hostWorldZAxisLocalAxis, + hostSemanticXAxisLocalAxis, + hostSemanticYAxisLocalAxis, + hostSemanticZAxisLocalAxis, + string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase) + ? hostSemanticYAxisLocalAxis + : hostSemanticZAxisLocalAxis, fragmentMatrices.Count, fragmentDefaultUpAxis, hostUpAxis); return true; } + private static bool TryResolveWorldLocalAxes( + FragmentRepresentativePoseHelper.RepresentativeFrame rawFrame, + out LocalAxisDirection hostWorldXAxisLocalAxis, + out LocalAxisDirection hostWorldYAxisLocalAxis, + out LocalAxisDirection hostWorldZAxisLocalAxis) + { + hostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX; + hostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY; + hostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ; + + if (!TryMatchRawAxisDirection(Vector3.UnitX, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldXAxisLocalAxis)) + { + return false; + } + + if (!TryMatchRawAxisDirection(Vector3.UnitY, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldYAxisLocalAxis)) + { + return false; + } + + if (!TryMatchRawAxisDirection(Vector3.UnitZ, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldZAxisLocalAxis)) + { + return false; + } + + return true; + } + + private static bool TryResolveSemanticLocalAxes( + FragmentRepresentativePoseHelper.RepresentativeFrame rawFrame, + FragmentRepresentativePoseHelper.RepresentativeFrame interpretedFrame, + out LocalAxisDirection hostSemanticXAxisLocalAxis, + out LocalAxisDirection hostSemanticYAxisLocalAxis, + out LocalAxisDirection hostSemanticZAxisLocalAxis) + { + hostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX; + hostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY; + hostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ; + + if (!TryMatchRawAxisDirection( + interpretedFrame.AxisX, + rawFrame.AxisX, + rawFrame.AxisY, + rawFrame.AxisZ, + out hostSemanticXAxisLocalAxis)) + { + return false; + } + + if (!TryMatchRawAxisDirection( + interpretedFrame.AxisY, + rawFrame.AxisX, + rawFrame.AxisY, + rawFrame.AxisZ, + out hostSemanticYAxisLocalAxis)) + { + return false; + } + + if (!TryMatchRawAxisDirection( + interpretedFrame.AxisZ, + rawFrame.AxisX, + rawFrame.AxisY, + rawFrame.AxisZ, + out hostSemanticZAxisLocalAxis)) + { + return false; + } + + return true; + } + + private static bool TryMatchRawAxisDirection( + Vector3 targetAxis, + Vector3 rawAxisX, + Vector3 rawAxisY, + Vector3 rawAxisZ, + out LocalAxisDirection axisDirection) + { + axisDirection = LocalAxisDirection.PositiveX; + const float tolerance = 1e-4f; + + if (Vector3.Distance(targetAxis, rawAxisX) < tolerance) + { + axisDirection = LocalAxisDirection.PositiveX; + return true; + } + + if (Vector3.Distance(targetAxis, -rawAxisX) < tolerance) + { + axisDirection = LocalAxisDirection.NegativeX; + return true; + } + + if (Vector3.Distance(targetAxis, rawAxisY) < tolerance) + { + axisDirection = LocalAxisDirection.PositiveY; + return true; + } + + if (Vector3.Distance(targetAxis, -rawAxisY) < tolerance) + { + axisDirection = LocalAxisDirection.NegativeY; + return true; + } + + if (Vector3.Distance(targetAxis, rawAxisZ) < tolerance) + { + axisDirection = LocalAxisDirection.PositiveZ; + return true; + } + + if (Vector3.Distance(targetAxis, -rawAxisZ) < tolerance) + { + axisDirection = LocalAxisDirection.NegativeZ; + return true; + } + + return false; + } + public static bool TryDetectDefaultUpAxis( IReadOnlyCollection fragmentMatrices, string hostUpAxis,