From 290df34dec544ba651cac3aa8b93fbe18252cad0 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Tue, 24 Mar 2026 21:04:24 +0800 Subject: [PATCH] Interpret fragment up for real object pose --- AGENTS.md | 29 +++ TransportPlugin.csproj | 1 + .../FragmentRepresentativePoseHelperTests.cs | 49 +++++ .../RealObjectPlanarPoseSolverTests.cs | 129 ++---------- doc/design/2026/NavisworksAPI使用方法.md | 9 + ...oordinate-system-canonical-space-design.md | 22 ++ src/Core/Animation/PathAnimationManager.cs | 196 ++++++++++++++---- .../ViewModels/SystemManagementViewModel.cs | 65 +----- .../FragmentDefaultUpContext.cs | 90 ++++++++ .../FragmentRepresentativePoseHelper.cs | 118 +++++++++++ 10 files changed, 494 insertions(+), 214 deletions(-) create mode 100644 src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs diff --git a/AGENTS.md b/AGENTS.md index d5ee793..9a438af 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -133,6 +133,35 @@ 禁止再使用“本地坐标系”这种含糊说法。 +### 4.1.1 Quaternion / Rotation3D 固定定义 + +这是项目级硬约束，不允许在任何修复中重新猜测、重新验证或临时改口： + +- Navisworks `Rotation3D(double, double, double, double)` 的参数顺序固定为： + - `x, y, z, w` +- `Rotation3D.A/B/C/D` 与 quaternion 分量的对应关系固定为： + - `A = x` + - `B = y` + - `C = z` + - `D = w` +- 当代码里已经拿到 quaternion `(qx, qy, qz, qw)` 时，唯一允许的构造方式是： + +```csharp +var rotation = new Rotation3D(qx, qy, qz, qw); +``` + +- 严禁再写成： + +```csharp +var rotation = new Rotation3D(qw, qx, qy, qz); // 错误 +``` + +- 以后遇到姿态问题，禁止再把故障归因到 “Navisworks 四元数分量顺序可能又不一样了”。 +- 如果问题涉及真实物体或 fragment 参考姿态的轴语义，优先检查： + - fragment 三轴的业务解释是否正确 + - 参考姿态是否应该用显式三轴而不是只传 quaternion + - 宿主坐标系 / 内部坐标系 / 资产坐标系 是否被混用 + ### 4.2 真实物体 vs 虚拟物体 这是当前架构里最容易被改坏的地方。 diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj index f515279..e2b3465 100644 --- a/TransportPlugin.csproj +++ b/TransportPlugin.csproj @@ -340,6 +340,7 @@ + diff --git a/UnitTests/CoordinateSystem/FragmentRepresentativePoseHelperTests.cs b/UnitTests/CoordinateSystem/FragmentRepresentativePoseHelperTests.cs index 6efb96f..751eba4 100644 --- a/UnitTests/CoordinateSystem/FragmentRepresentativePoseHelperTests.cs +++ b/UnitTests/CoordinateSystem/FragmentRepresentativePoseHelperTests.cs @@ -98,6 +98,55 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem AssertVector(Vector3.Transform(Vector3.UnitZ, rotation), axisZ, 1e-4); } + [TestMethod] + public void RepresentativeFrame_ShouldExposeSameAxes_AsRepresentativeRotation() + { + Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f); + Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f); + Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f); + + double[] fragmentMatrix = + { + axisX.X, axisX.Y, axisX.Z, 0.0, + axisY.X, axisY.Y, axisY.Z, 0.0, + axisZ.X, axisZ.Y, axisZ.Z, 0.0, + 0.0, 0.0, 0.0, 1.0 + }; + + bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeFrame( + new[] { fragmentMatrix }, + out FragmentRepresentativePoseHelper.RepresentativeFrame frame); + + Assert.IsTrue(ok); + AssertVector(frame.AxisX, axisX, 1e-4); + AssertVector(frame.AxisY, axisY, 1e-4); + AssertVector(frame.AxisZ, axisZ, 1e-4); + AssertVector(Vector3.Transform(Vector3.UnitX, frame.Rotation), axisX, 1e-4); + AssertVector(Vector3.Transform(Vector3.UnitY, frame.Rotation), axisY, 1e-4); + AssertVector(Vector3.Transform(Vector3.UnitZ, frame.Rotation), axisZ, 1e-4); + } + + [TestMethod] + public void TryInterpretRepresentativeFrame_YUpHost_WithFragmentDefaultUpZ_ShouldMapZToHostY() + { + var raw = new FragmentRepresentativePoseHelper.RepresentativeFrame( + new Vector3(1.0f, 0.0f, 0.0f), + new Vector3(0.0f, 0.0f, -1.0f), + new Vector3(0.0f, 1.0f, 0.0f), + Quaternion.Identity); + + bool ok = FragmentRepresentativePoseHelper.TryInterpretRepresentativeFrame( + raw, + "Z", + "Y", + out var interpreted); + + Assert.IsTrue(ok); + AssertVector(interpreted.AxisX, new Vector3(1.0f, 0.0f, 0.0f), 1e-4); + AssertVector(interpreted.AxisY, new Vector3(0.0f, 1.0f, 0.0f), 1e-4); + AssertVector(interpreted.AxisZ, new Vector3(0.0f, 0.0f, 1.0f), 1e-4); + } + private static double[] CreateFragmentMatrix(Quaternion rotation, Vector3 translation) { Vector3 axisX = Vector3.Transform(Vector3.UnitX, rotation); diff --git a/UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs b/UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs index a9679c0..a57f560 100644 --- a/UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs +++ b/UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs @@ -2,7 +2,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; using NavisworksTransport.Utils.CoordinateSystem; using System; using System.Numerics; -using Autodesk.Navisworks.Api; namespace NavisworksTransport.UnitTests.CoordinateSystem { @@ -12,9 +11,7 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem [TestMethod] public void ShouldChooseClosestCandidateAxis_FromRotatedReferencePose() { - Quaternion referenceRotation = Quaternion.Normalize( - Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0))); - + Quaternion referenceRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0)); Vector3 desiredForward = new Vector3(1.0f, 0.2f, 0.1f); Vector3 desiredUp = Vector3.UnitZ; @@ -26,48 +23,37 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.IsTrue(ok); AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0); - AssertVector(solution.ProjectedForward, 0.97590, 0.19518, 0.09759, 1e-4); - Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation); - AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4); + Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation)); + AssertVector(transformedSelectedAxis, desiredForward.X / desiredForward.Length(), desiredForward.Y / desiredForward.Length(), desiredForward.Z / desiredForward.Length(), 1e-4); } [TestMethod] public void ShouldPreferNegativeAxisWhenItMatchesForwardBest() { - Quaternion referenceRotation = Quaternion.Identity; - Vector3 desiredForward = new Vector3(-0.1f, -1.0f, 0.05f); - Vector3 desiredUp = Vector3.UnitZ; - bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose( - referenceRotation, - desiredForward, - desiredUp, + Quaternion.Identity, + new Vector3(-0.1f, -1.0f, 0.05f), + Vector3.UnitZ, out RealObjectPlanarPoseSolution solution); Assert.IsTrue(ok); AssertVector(solution.SelectedReferenceAxisLocal, 0.0, -1.0, 0.0); - - Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation); - AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4); } [TestMethod] - public void ShouldRotateWholeReferencePose_NotFlattenItToHostUpPlane() + public void ShouldRespectDesiredUpPlane_WhenDesiredForwardHasVerticalComponent() { - Quaternion referenceRotation = Quaternion.Identity; Vector3 desiredForward = Vector3.Normalize(new Vector3(1.0f, 1.0f, 1.0f)); bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose( - referenceRotation, + Quaternion.Identity, desiredForward, Vector3.UnitY, out RealObjectPlanarPoseSolution solution); Assert.IsTrue(ok); - AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0); - - Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation); + Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation)); AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4); } @@ -90,95 +76,16 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.IsTrue(ok); - Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation); - AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4); + Vector3 transformedX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, solution.BaselineRotation)); + Vector3 transformedY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, solution.BaselineRotation)); + Vector3 transformedZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation)); - Vector3 transformedReferenceUp = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation)); - Assert.AreEqual(1.0, transformedReferenceUp.Length(), 1e-4); - Assert.AreEqual(0.0, Vector3.Dot(transformedSelectedAxis, transformedReferenceUp), 1e-4); - } - - [TestMethod] - public void YUp_HorizontalRealObject_ToAnotherHorizontalDirection_ShouldKeepVerticalAxis() - { - Vector3 referenceX = new Vector3(1.0f, 0.0f, 0.0f); - Vector3 referenceY = new Vector3(0.0f, 0.0f, 1.0f); - Vector3 referenceZ = new Vector3(0.0f, -1.0f, 0.0f); - Quaternion referenceRotation = CreateQuaternionFromBasis(referenceX, referenceY, referenceZ); - - Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f)); - Vector3 desiredUp = Vector3.UnitY; - - bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose( - referenceRotation, - desiredForward, - desiredUp, - out RealObjectPlanarPoseSolution solution); - - Assert.IsTrue(ok); - AssertVector(solution.SelectedReferenceAxisLocal, -1.0, 0.0, 0.0); - - Vector3 transformedForward = Vector3.Normalize( - Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation)); - Vector3 transformedVertical = Vector3.Normalize( - Vector3.Transform(-Vector3.UnitZ, solution.BaselineRotation)); - - AssertVector(transformedForward, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4); - AssertVector(transformedVertical, 0.0, 1.0, 0.0, 1e-4); - } - - private static Quaternion CreateQuaternionFromBasis(Vector3 axisX, Vector3 axisY, Vector3 axisZ) - { - float m00 = axisX.X; - float m01 = axisY.X; - float m02 = axisZ.X; - float m10 = axisX.Y; - float m11 = axisY.Y; - float m12 = axisZ.Y; - float m20 = axisX.Z; - float m21 = axisY.Z; - float m22 = axisZ.Z; - - float trace = m00 + m11 + m22; - float qx; - float qy; - float qz; - float qw; - - if (trace > 0.0f) - { - float s = (float)Math.Sqrt(trace + 1.0f) * 2.0f; - qw = 0.25f * s; - qx = (m21 - m12) / s; - qy = (m02 - m20) / s; - qz = (m10 - m01) / s; - } - else if (m00 > m11 && m00 > m22) - { - float s = (float)Math.Sqrt(1.0f + m00 - m11 - m22) * 2.0f; - qw = (m21 - m12) / s; - qx = 0.25f * s; - qy = (m01 + m10) / s; - qz = (m02 + m20) / s; - } - else if (m11 > m22) - { - float s = (float)Math.Sqrt(1.0f + m11 - m00 - m22) * 2.0f; - qw = (m02 - m20) / s; - qx = (m01 + m10) / s; - qy = 0.25f * s; - qz = (m12 + m21) / s; - } - else - { - float s = (float)Math.Sqrt(1.0f + m22 - m00 - m11) * 2.0f; - qw = (m10 - m01) / s; - qx = (m02 + m20) / s; - qy = (m12 + m21) / s; - qz = 0.25f * s; - } - - return Quaternion.Normalize(new Quaternion(qx, qy, qz, qw)); + Assert.AreEqual(1.0, transformedX.Length(), 1e-4); + Assert.AreEqual(1.0, transformedY.Length(), 1e-4); + Assert.AreEqual(1.0, transformedZ.Length(), 1e-4); + Assert.AreEqual(0.0, Vector3.Dot(transformedX, transformedY), 1e-4); + Assert.AreEqual(0.0, Vector3.Dot(transformedY, transformedZ), 1e-4); + Assert.AreEqual(0.0, Vector3.Dot(transformedZ, transformedX), 1e-4); } private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6) diff --git a/doc/design/2026/NavisworksAPI使用方法.md b/doc/design/2026/NavisworksAPI使用方法.md index 39f03e4..e70b26d 100644 --- a/doc/design/2026/NavisworksAPI使用方法.md +++ b/doc/design/2026/NavisworksAPI使用方法.md @@ -779,6 +779,15 @@ var rotation = new Rotation3D(qw, qx, qy, qz); // ❌ 错误 这个结论已经在 Rail 三维姿态、参考杆三维定向的实际运行中验证通过。 +**项目级硬约束：** + +- 后续任何代码和文档中，都不允许再重新解释 `Rotation3D` 的四元数分量顺序 +- 遇到姿态问题时，不要再把问题归因到 `Rotation3D(x,y,z,w)` 是否可能变成了别的顺序 +- 如果真实物体或 fragment 参考姿态出现轴翻转/符号错误，应优先检查： + - fragment 三轴的业务语义解释 + - 宿主坐标系 / 内部坐标系 / 资产坐标系 的边界 + - 是否应该传递显式参考轴，而不是只传 quaternion + #### 11.7.2.2 三维参考杆/任意向量对齐的正确用法 适用场景： diff --git a/doc/design/2026/coordinate-system-canonical-space-design.md b/doc/design/2026/coordinate-system-canonical-space-design.md index 9c06e51..77acdb5 100644 --- a/doc/design/2026/coordinate-system-canonical-space-design.md +++ b/doc/design/2026/coordinate-system-canonical-space-design.md @@ -145,6 +145,28 @@ flowchart LR 后续凡是 UI 输入/输出，必须明确是**宿主坐标系**语义；凡是姿态或几何内部计算，必须明确是**内部坐标系**语义；凡是虚拟物体/单位圆柱体资源朝向，必须明确是**资产坐标系**语义。 - 业务锚点 +### 3.2.2 Quaternion / Rotation3D 解释约束 + +坐标系分层之外，还必须固定一条旋转解释规则： + +- Navisworks `Rotation3D(double, double, double, double)` 的参数顺序固定为 `x, y, z, w` +- `Rotation3D.A/B/C/D` 固定对应 quaternion 的 `x/y/z/w` + +这条规则在项目中视为硬约束，禁止后续代码再次用不同顺序解释四元数。 + +因此： + +- 内部坐标系里算出的 quaternion `(qx, qy, qz, qw)`，写回 Navisworks 时必须使用： + +```csharp +var rotation = new Rotation3D(qx, qy, qz, qw); +``` + +- 后续遇到姿态异常时，不应再次怀疑 `Rotation3D` 分量顺序；应优先检查： + - 宿主坐标系 / 内部坐标系 / 资产坐标系 是否混用 + - fragment 参考姿态是否被误当成业务语义姿态 + - 真实物体是否错误地只传 quaternion 而没有保留显式参考轴 + 这层不能偷用世界原点，也不能偷用宿主世界轴。 ### 3.3 坐标系定义必须包含的语义 diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 2d24ba7..fa76673 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -102,6 +102,7 @@ namespace NavisworksTransport.Core.Animation { private static PathAnimationManager _instance; private static readonly Dictionary _animationHashToRecordId = new Dictionary(); // 动画配置哈希到检测记录ID的映射 + private static readonly HashSet _realObjectFragmentUpMismatchHintsShown = new HashSet(StringComparer.OrdinalIgnoreCase); private ModelItem _animatedObject; private AnimatedObjectMode _animatedObjectMode = AnimatedObjectMode.None; private double _virtualObjectLength = 0; // 虚拟物体长度（模型单位） @@ -111,6 +112,9 @@ namespace NavisworksTransport.Core.Animation private double _realObjectWidth = 0; // 真实物体宽度（模型单位，固定物理尺寸） private double _realObjectHeight = 0; // 真实物体高度（模型单位，固定物理尺寸） private Quaternion _realObjectReferenceRotation = Quaternion.Identity; + private Vector3 _realObjectReferenceAxisX = Vector3.UnitX; + private Vector3 _realObjectReferenceAxisY = Vector3.UnitY; + private Vector3 _realObjectReferenceAxisZ = Vector3.UnitZ; private bool _hasRealObjectReferenceRotation = false; private List _pathPoints; private List _manualCollisionTargets = new List(); @@ -443,12 +447,10 @@ namespace NavisworksTransport.Core.Animation // 2. 重置内部跟踪变量（同步到原始状态） // 注意：ResetPermanentTransform 后物体的 Transform 属性会自动变回原始值 - _currentYaw = ModelItemTransformHelper.GetYawFromTransform(objectToRestore.Transform); - _trackedRotation = objectToRestore.Transform.Factor().Rotation; - _hasTrackedRotation = true; + SyncTrackedRotationToObjectReference(objectToRestore, isVirtual); - var originalBoundingBox = objectToRestore.BoundingBox(); - _trackedPosition = GetTrackedObjectPosition(objectToRestore); + var originalBoundingBox = objectToRestore.BoundingBox(); + _trackedPosition = GetTrackedObjectPosition(objectToRestore); string objectName = isVirtual ? "虚拟物体" : objectToRestore.DisplayName; LogManager.Info($"[归位] {objectName} 已彻底恢复到原始位置, yaw={_currentYaw:F3}"); @@ -486,10 +488,7 @@ namespace NavisworksTransport.Core.Animation _originalTransform = animatedObject.Transform; _originalCenter = animatedObject.BoundingBox().Center; _trackedPosition = GetTrackedObjectPosition(animatedObject); - _currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform); - _trackedRotation = _originalTransform.Factor().Rotation; - _hasTrackedRotation = true; - TryCaptureRealObjectReferenceRotation(animatedObject, out _); + SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject: false); } } @@ -651,16 +650,7 @@ namespace NavisworksTransport.Core.Animation _originalTransform = animatedObject.Transform; _originalCenter = animatedObject.BoundingBox().Center; _trackedPosition = GetTrackedObjectPosition(animatedObject); - - // 统一逻辑：从当前 Transform 中提取实际朝向（无论虚拟物体还是普通物体） - _currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform); - _trackedRotation = _originalTransform.Factor().Rotation; - _hasTrackedRotation = true; - - if (!isVirtualObject) - { - TryCaptureRealObjectReferenceRotation(animatedObject, out _); - } + SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject); LogManager.Info( $"[移动到起点] 更新动画对象内部状态: 对象={animatedObject.DisplayName}, 虚拟物体={IsVirtualObjectMode}"); @@ -2471,15 +2461,27 @@ namespace NavisworksTransport.Core.Animation { VirtualObjectManager.Instance.MoveVirtualObject(newPosition, newRotation); } - else if (_animatedObject != null) - { - Point3D currentPositionForTransform = _trackedPosition; - var currentRotation = _hasTrackedRotation - ? _trackedRotation - : _animatedObject.Transform.Factor().Rotation; - try - { - var actualHostPosition = GetTrackedObjectPosition(_animatedObject); + else if (_animatedObject != null) + { + Point3D currentPositionForTransform = _trackedPosition; + Rotation3D currentRotation; + if (_hasTrackedRotation) + { + currentRotation = _trackedRotation; + } + else if (IsRealObjectMode && _hasRealObjectReferenceRotation) + { + currentRotation = CoordinateSystemManager.Instance + .CreateHostAdapter() + .FromHostQuaternion(_realObjectReferenceRotation); + } + else + { + currentRotation = _animatedObject.Transform.Factor().Rotation; + } + try + { + var actualHostPosition = GetTrackedObjectPosition(_animatedObject); currentPositionForTransform = actualHostPosition; LogManager.Info( $"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})"); @@ -2493,11 +2495,11 @@ namespace NavisworksTransport.Core.Animation LogManager.Info( $"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " + $"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})"); - LogManager.Info( - $"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " + - $"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " + - $"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " + - $"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})"); + LogManager.Info( + $"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " + + $"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " + + $"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " + + $"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})"); LogManager.Info( $"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " + $"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " + @@ -3665,10 +3667,10 @@ namespace NavisworksTransport.Core.Animation var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); if (!RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose( - referenceRotation, - hostForward, - adapter.HostUpVector3, - out var solution)) + referenceRotation, + hostForward, + adapter.HostUpVector3, + out var solution)) { return false; } @@ -3676,13 +3678,76 @@ namespace NavisworksTransport.Core.Animation Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, _objectRotationCorrection); rotation = adapter.FromHostQuaternion(hostComposedQuaternion); + Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation); + Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(hostComposedQuaternion); + 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}), " + - $"基姿态已计算"); + $"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 void ShowRealObjectFragmentUpMismatchHintIfNeeded(Vector3 representativeYAxis, Vector3 representativeZAxis) + { + if (_route == null || (_route.PathType != PathType.Ground && _route.PathType != PathType.Hoisting)) + { + return; + } + + if (!IsRealObjectMode) + { + return; + } + + var doc = NavisApplication.ActiveDocument; + string hostUpAxis = FragmentDefaultUpContext.GetCurrentHostUpAxis(doc); + string configuredFragmentUpAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(doc); + Vector3 hostUpVector = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase) + ? Vector3.UnitY + : Vector3.UnitZ; + + float yAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeYAxis), hostUpVector)); + float zAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeZAxis), hostUpVector)); + string detectedFragmentUpAxis = zAlignment > yAlignment ? "Z" : "Y"; + + if (string.Equals(configuredFragmentUpAxis, detectedFragmentUpAxis, StringComparison.OrdinalIgnoreCase)) + { + return; + } + + string documentKey = FragmentDefaultUpContext.GetCurrentDocumentKey(doc); + if (string.IsNullOrWhiteSpace(documentKey)) + { + return; + } + + string hintKey = $"{documentKey}|{configuredFragmentUpAxis}|{detectedFragmentUpAxis}"; + lock (_realObjectFragmentUpMismatchHintsShown) + { + if (_realObjectFragmentUpMismatchHintsShown.Contains(hintKey)) + { + return; + } + + _realObjectFragmentUpMismatchHintsShown.Add(hintKey); + } + + string message = + $"请到系统管理的坐标系设置中，将 Fragment默认Up 调整为 {detectedFragmentUpAxis}。"; + LogManager.Info($"[fragment默认Up提示] 当前值={configuredFragmentUpAxis}, 检测值={detectedFragmentUpAxis}, 模型Up={hostUpAxis}。{message}"); + DialogHelper.ShowMessageBox( + message, + "Fragment默认Up提示", + MessageBoxButton.OK, + MessageBoxImage.Information); + } + private bool TryGetRealObjectReferenceRotation(out Quaternion referenceRotation) { referenceRotation = Quaternion.Identity; @@ -3760,22 +3825,38 @@ namespace NavisworksTransport.Core.Animation return false; } - if (!FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(fragmentMatrices, out referenceRotation)) + if (!FragmentRepresentativePoseHelper.TryGetRepresentativeFrame(fragmentMatrices, out var rawRepresentativeFrame)) { LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 代表姿态求解失败"); return false; } + var doc = NavisApplication.ActiveDocument; + string fragmentDefaultUpAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(doc); + string hostUpAxis = FragmentDefaultUpContext.GetCurrentHostUpAxis(doc); + if (!FragmentRepresentativePoseHelper.TryInterpretRepresentativeFrame( + rawRepresentativeFrame, + fragmentDefaultUpAxis, + hostUpAxis, + out var representativeFrame)) + { + LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 姿态解释失败"); + return false; + } + + referenceRotation = representativeFrame.Rotation; _realObjectReferenceRotation = referenceRotation; + _realObjectReferenceAxisX = representativeFrame.AxisX; + _realObjectReferenceAxisY = representativeFrame.AxisY; + _realObjectReferenceAxisZ = representativeFrame.AxisZ; _hasRealObjectReferenceRotation = true; - Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(referenceRotation); LogManager.Info( $"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " + - $"X=({linear.M11:F4},{linear.M21:F4},{linear.M31:F4}), " + - $"Y=({linear.M12:F4},{linear.M22:F4},{linear.M32:F4}), " + - $"Z=({linear.M13:F4},{linear.M23:F4},{linear.M33:F4}), " + - $"fragment数量={fragmentMatrices.Count}"); + $"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数量={fragmentMatrices.Count}, Fragment默认Up={fragmentDefaultUpAxis}, 模型Up={hostUpAxis}"); return true; } finally @@ -3816,9 +3897,36 @@ namespace NavisworksTransport.Core.Animation } } + private void SyncTrackedRotationToObjectReference(ModelItem sourceObject, bool isVirtualObject) + { + if (sourceObject == null) + { + _trackedRotation = Rotation3D.Identity; + _hasTrackedRotation = false; + _currentYaw = 0.0; + return; + } + + if (!isVirtualObject && TryCaptureRealObjectReferenceRotation(sourceObject, out Quaternion referenceRotation)) + { + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + _trackedRotation = adapter.FromHostQuaternion(referenceRotation); + _hasTrackedRotation = true; + _currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation); + return; + } + + _currentYaw = ModelItemTransformHelper.GetYawFromTransform(sourceObject.Transform); + _trackedRotation = sourceObject.Transform.Factor().Rotation; + _hasTrackedRotation = true; + } + private void ResetRealObjectReferenceRotation() { _realObjectReferenceRotation = Quaternion.Identity; + _realObjectReferenceAxisX = Vector3.UnitX; + _realObjectReferenceAxisY = Vector3.UnitY; + _realObjectReferenceAxisZ = Vector3.UnitZ; _hasRealObjectReferenceRotation = false; } diff --git a/src/UI/WPF/ViewModels/SystemManagementViewModel.cs b/src/UI/WPF/ViewModels/SystemManagementViewModel.cs index 432b116..7695501 100644 --- a/src/UI/WPF/ViewModels/SystemManagementViewModel.cs +++ b/src/UI/WPF/ViewModels/SystemManagementViewModel.cs @@ -27,7 +27,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels #region 私有字段 private readonly UIStateManager _uiStateManager; - private readonly Dictionary _documentFragmentDefaultUpAxes = new Dictionary(StringComparer.OrdinalIgnoreCase); private readonly HashSet _documentRotationHintShown = new HashSet(StringComparer.OrdinalIgnoreCase); // 日志管理字段 @@ -472,7 +471,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels private void RefreshFragmentDefaultUpAxisSelectionFromCurrentDocument() { - string resolvedAxis = GetCurrentDocumentFragmentDefaultUpAxis(); + string resolvedAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(); if (_selectedFragmentDefaultUpAxis != resolvedAxis) { SetProperty(ref _selectedFragmentDefaultUpAxis, resolvedAxis, nameof(SelectedFragmentDefaultUpAxis)); @@ -481,79 +480,27 @@ namespace NavisworksTransport.UI.WPF.ViewModels private string GetCurrentDocumentFragmentDefaultUpAxis() { - string documentKey = GetCurrentDocumentKey(); - if (!string.IsNullOrWhiteSpace(documentKey) && - _documentFragmentDefaultUpAxes.TryGetValue(documentKey, out string storedAxis) && - IsSupportedFragmentDefaultUpAxis(storedAxis)) - { - return storedAxis; - } - - return GetCurrentHostUpAxis(); + return FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(); } private void SaveCurrentDocumentFragmentDefaultUpAxis(string axisName) { - if (!IsSupportedFragmentDefaultUpAxis(axisName)) - { - return; - } - - string documentKey = GetCurrentDocumentKey(); - if (string.IsNullOrWhiteSpace(documentKey)) - { - return; - } - - _documentFragmentDefaultUpAxes[documentKey] = axisName; + FragmentDefaultUpContext.SetCurrentDocumentFragmentDefaultUpAxis(axisName); } private string GetCurrentDocumentKey() { - var doc = Autodesk.Navisworks.Api.Application.ActiveDocument; - if (doc == null || doc.IsClear) - { - return string.Empty; - } - - if (!string.IsNullOrWhiteSpace(doc.FileName)) - { - return doc.FileName; - } - - return doc.Title ?? string.Empty; + return FragmentDefaultUpContext.GetCurrentDocumentKey(); } private string GetCurrentHostUpAxis() { - var doc = Autodesk.Navisworks.Api.Application.ActiveDocument; - if (doc != null && !doc.IsClear) - { - UnitVector3D worldUp = doc.CurrentViewpoint.Value.WorldUpVector; - if (!worldUp.IsZero) - { - return Math.Abs(worldUp.Y) >= Math.Abs(worldUp.Z) ? "Y" : "Z"; - } - - Vector3D documentUp = doc.UpVector; - if (!documentUp.IsZero) - { - return Math.Abs(documentUp.Y) >= Math.Abs(documentUp.Z) ? "Y" : "Z"; - } - } - - if (string.Equals(_selectedCoordinateSystem, "YUp", StringComparison.OrdinalIgnoreCase)) - { - return "Y"; - } - - return "Z"; + return FragmentDefaultUpContext.GetCurrentHostUpAxis(); } private static bool IsSupportedFragmentDefaultUpAxis(string axisName) { - return string.Equals(axisName, "Y", StringComparison.OrdinalIgnoreCase) || - string.Equals(axisName, "Z", StringComparison.OrdinalIgnoreCase); + return FragmentDefaultUpContext.IsSupportedAxis(axisName); } private void ShowRootModelRotationHintIfNeeded() diff --git a/src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs b/src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs new file mode 100644 index 0000000..32ab907 --- /dev/null +++ b/src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs @@ -0,0 +1,90 @@ +using System; +using System.Collections.Concurrent; +using Autodesk.Navisworks.Api; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// 当前文档级的 fragment 默认 Up 运行时状态。 + /// 不写入系统配置，只在当前进程内按文档维持。 + /// + internal static class FragmentDefaultUpContext + { + private static readonly ConcurrentDictionary DocumentFragmentDefaultUpAxes = + new ConcurrentDictionary(StringComparer.OrdinalIgnoreCase); + + public static string GetCurrentDocumentKey(Document doc = null) + { + doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument; + if (doc == null || doc.IsClear) + { + return string.Empty; + } + + if (!string.IsNullOrWhiteSpace(doc.FileName)) + { + return doc.FileName; + } + + return doc.Title ?? string.Empty; + } + + public static string GetCurrentHostUpAxis(Document doc = null) + { + doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument; + if (doc != null && !doc.IsClear) + { + UnitVector3D worldUp = doc.CurrentViewpoint.Value.WorldUpVector; + if (!worldUp.IsZero) + { + return Math.Abs(worldUp.Y) >= Math.Abs(worldUp.Z) ? "Y" : "Z"; + } + + Vector3D documentUp = doc.UpVector; + if (!documentUp.IsZero) + { + return Math.Abs(documentUp.Y) >= Math.Abs(documentUp.Z) ? "Y" : "Z"; + } + } + + return "Z"; + } + + public static string GetCurrentDocumentFragmentDefaultUpAxis(Document doc = null) + { + doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument; + string documentKey = GetCurrentDocumentKey(doc); + if (!string.IsNullOrWhiteSpace(documentKey) && + DocumentFragmentDefaultUpAxes.TryGetValue(documentKey, out string storedAxis) && + IsSupportedAxis(storedAxis)) + { + return storedAxis; + } + + return GetCurrentHostUpAxis(doc); + } + + public static void SetCurrentDocumentFragmentDefaultUpAxis(string axisName, Document doc = null) + { + if (!IsSupportedAxis(axisName)) + { + return; + } + + doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument; + string documentKey = GetCurrentDocumentKey(doc); + if (string.IsNullOrWhiteSpace(documentKey)) + { + return; + } + + DocumentFragmentDefaultUpAxes[documentKey] = axisName; + } + + public static bool IsSupportedAxis(string axisName) + { + return string.Equals(axisName, "Y", StringComparison.OrdinalIgnoreCase) || + string.Equals(axisName, "Z", StringComparison.OrdinalIgnoreCase); + } + } +} diff --git a/src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs b/src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs index 98a9ee0..4982a2b 100644 --- a/src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs +++ b/src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs @@ -20,6 +20,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem { private const float AxisEpsilon = 1e-6f; + public readonly struct RepresentativeFrame + { + public Vector3 AxisX { get; } + public Vector3 AxisY { get; } + public Vector3 AxisZ { get; } + public Quaternion Rotation { get; } + + public RepresentativeFrame(Vector3 axisX, Vector3 axisY, Vector3 axisZ, Quaternion rotation) + { + AxisX = axisX; + AxisY = axisY; + AxisZ = axisZ; + Rotation = rotation; + } + } + /// /// 从 ModelItem 的 fragment 几何中提取代表旋转。 /// @@ -73,6 +89,108 @@ namespace NavisworksTransport.Utils.CoordinateSystem return TryGetRepresentativeRotation(fragmentRotations, out representativeRotation); } + public static bool TryGetRepresentativeFrame( + IEnumerable fragmentMatrices, + out RepresentativeFrame representativeFrame) + { + representativeFrame = default; + + if (fragmentMatrices == null) + { + return false; + } + + var rotations = new List(); + foreach (double[] fragmentMatrix in fragmentMatrices) + { + if (TryExtractRotationFromFragmentMatrix(fragmentMatrix, out Quaternion fragmentRotation)) + { + rotations.Add(fragmentRotation); + } + } + + if (!TryGetRepresentativeRotation(rotations, out Quaternion representativeRotation)) + { + return false; + } + + Vector3 axisX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, representativeRotation)); + Vector3 axisY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, representativeRotation)); + Vector3 axisZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, representativeRotation)); + representativeFrame = new RepresentativeFrame(axisX, axisY, axisZ, representativeRotation); + return true; + } + + public static bool TryInterpretRepresentativeFrame( + RepresentativeFrame rawFrame, + string fragmentDefaultUpAxis, + string hostUpAxis, + out RepresentativeFrame interpretedFrame) + { + interpretedFrame = default; + + bool hostIsYUp = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase); + bool fragmentIsYUp = string.Equals(fragmentDefaultUpAxis, "Y", StringComparison.OrdinalIgnoreCase); + + Vector3 semanticAxisX = rawFrame.AxisX; + Vector3 semanticAxisY; + Vector3 semanticAxisZ; + + if (hostIsYUp == fragmentIsYUp) + { + semanticAxisY = rawFrame.AxisY; + semanticAxisZ = rawFrame.AxisZ; + } + else if (hostIsYUp) + { + // fragment 按 ZUp 解释，宿主按 YUp 解释：X 保持，Y 取原始 Z，Z 由右手系重建。 + semanticAxisY = rawFrame.AxisZ; + semanticAxisZ = Vector3.Normalize(Vector3.Cross(semanticAxisX, semanticAxisY)); + } + else + { + // fragment 按 YUp 解释，宿主按 ZUp 解释：X 保持，Z 取原始 Y，Y 由右手系重建。 + semanticAxisZ = rawFrame.AxisY; + semanticAxisY = Vector3.Normalize(Vector3.Cross(semanticAxisZ, semanticAxisX)); + } + + if (!TryNormalize(semanticAxisX, out semanticAxisX) || + !TryNormalize(semanticAxisY, out semanticAxisY) || + !TryNormalize(semanticAxisZ, out semanticAxisZ)) + { + return false; + } + + // 用解释后的三轴重新正交化，保证右手系稳定。 + Vector3 axisX = semanticAxisX; + Vector3 axisY = semanticAxisY - Vector3.Dot(semanticAxisY, axisX) * axisX; + if (!TryNormalize(axisY, out axisY)) + { + return false; + } + + Vector3 axisZ = Vector3.Cross(axisX, axisY); + if (!TryNormalize(axisZ, out axisZ)) + { + return false; + } + + if (hostIsYUp && Vector3.Dot(axisY, semanticAxisY) < 0.0f) + { + axisY = -axisY; + axisZ = -axisZ; + } + else if (!hostIsYUp && Vector3.Dot(axisZ, semanticAxisZ) < 0.0f) + { + axisY = -axisY; + axisZ = -axisZ; + } + + Quaternion rotation = Quaternion.Normalize(CreateQuaternionFromBasis(axisX, axisY, axisZ)); + interpretedFrame = new RepresentativeFrame(axisX, axisY, axisZ, rotation); + return true; + } + /// /// 从 fragment 旋转集合中提取代表旋转。 ///