diff --git a/UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs b/UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs index 27284da..5d2ac9a 100644 --- a/UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs +++ b/UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs @@ -14,22 +14,28 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f); Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f); Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f)); + Vector3 desiredUp = Vector3.UnitY; bool ok = RealObjectRailAxisConventionResolver.TryResolve( referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveY, desiredForward, CoordinateSystemType.YUp, out ModelAxisConvention convention, out LocalAxisDirection selectedForwardAxis, - out Vector3 selectedForwardWorldAxis); + out Vector3 selectedForwardWorldAxis, + out LocalAxisDirection selectedUpAxis, + out Vector3 selectedUpWorldAxis); Assert.IsTrue(ok); Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis); Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis); Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis); + Assert.AreEqual(LocalAxisDirection.PositiveY, selectedUpAxis); AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0); + AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0); } [TestMethod] @@ -39,21 +45,27 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f); Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f); Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.9f, 0.3f, 0.0f)); + Vector3 desiredUp = Vector3.UnitZ; bool ok = RealObjectRailAxisConventionResolver.TryResolve( referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveZ, desiredForward, CoordinateSystemType.ZUp, out ModelAxisConvention convention, out LocalAxisDirection selectedForwardAxis, - out _); + out _, + out LocalAxisDirection selectedUpAxis, + out Vector3 selectedUpWorldAxis); Assert.IsTrue(ok); Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis); Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis); Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis); + AssertVector(selectedUpWorldAxis, 0.0, 0.0, 1.0); } [TestMethod] @@ -63,15 +75,19 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f); Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f); Vector3 desiredForward = Vector3.Normalize(new Vector3(0.0f, 1.0f, 0.01f)); + Vector3 desiredUp = Vector3.UnitY; bool ok = RealObjectRailAxisConventionResolver.TryResolve( referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveY, desiredForward, CoordinateSystemType.YUp, out _, out LocalAxisDirection selectedForwardAxis, + out _, + out _, out _); Assert.IsTrue(ok); @@ -79,6 +95,67 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreNotEqual(LocalAxisDirection.NegativeY, selectedForwardAxis); } + [TestMethod] + public void YUp_WhenRemainingZAxisBestMatchesDesiredUp_ShouldChooseZAsUp() + { + Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f); + Vector3 referenceAxisY = new Vector3(0.0f, 0.0f, 1.0f); + Vector3 referenceAxisZ = new Vector3(0.0f, 1.0f, 0.0f); + Vector3 desiredForward = new Vector3(-1.0f, 0.0f, 0.0f); + Vector3 desiredUp = new Vector3(0.0f, 1.0f, 0.0f); + + bool ok = RealObjectRailAxisConventionResolver.TryResolve( + referenceAxisX, + referenceAxisY, + referenceAxisZ, + LocalAxisDirection.PositiveZ, + desiredForward, + CoordinateSystemType.YUp, + out ModelAxisConvention convention, + out LocalAxisDirection selectedForwardAxis, + out Vector3 selectedForwardWorldAxis, + out LocalAxisDirection selectedUpAxis, + out Vector3 selectedUpWorldAxis); + + Assert.IsTrue(ok); + Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis); + Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis); + AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0); + AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0); + } + + [TestMethod] + public void YUp_ShouldUseMappedHostUpLocalAxis_AndSelectForwardFromRemainingAxes() + { + Vector3 referenceAxisX = new Vector3(0.0f, 0.0f, 1.0f); + Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f); + Vector3 referenceAxisZ = new Vector3(-1.0f, 0.0f, 0.0f); + Vector3 desiredForward = Vector3.Normalize(new Vector3(0.99f, -0.14f, -0.02f)); + + bool ok = RealObjectRailAxisConventionResolver.TryResolve( + referenceAxisX, + referenceAxisY, + referenceAxisZ, + LocalAxisDirection.PositiveZ, + desiredForward, + CoordinateSystemType.YUp, + out ModelAxisConvention convention, + out LocalAxisDirection selectedForwardAxis, + out Vector3 selectedForwardWorldAxis, + out LocalAxisDirection selectedUpAxis, + out Vector3 selectedUpWorldAxis); + + Assert.IsTrue(ok); + Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis); + Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis); + Assert.AreEqual(LocalAxisDirection.NegativeY, selectedForwardAxis); + Assert.AreEqual(LocalAxisDirection.NegativeY, convention.ForwardAxis); + AssertVector(selectedUpWorldAxis, -1.0, 0.0, 0.0); + AssertVector(selectedForwardWorldAxis, 0.0, -1.0, 0.0); + } + private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6) { Assert.AreEqual(x, actual.X, tolerance); diff --git a/UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs b/UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs index 7e1c0bb..35f6dc7 100644 --- a/UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs +++ b/UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs @@ -21,11 +21,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveY, desiredForward, CoordinateSystemType.YUp, - forwardSize: 6.0, - sideSize: 2.0, - upSize: 4.0, + 6.0, + 2.0, + 4.0, baseline, baseline, out ModelAxisConvention convention, @@ -56,11 +57,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveY, desiredForward, CoordinateSystemType.YUp, - forwardSize: 6.0, - sideSize: 2.0, - upSize: 4.0, + 6.0, + 2.0, + 4.0, baseline, final, out _, @@ -95,11 +97,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem referenceAxisX, referenceAxisY, referenceAxisZ, + LocalAxisDirection.PositiveY, desiredForward, CoordinateSystemType.YUp, - forwardSize: 6.0, - sideSize: 2.0, - upSize: 4.0, + 6.0, + 2.0, + 4.0, baseline, final, out ModelAxisConvention convention, diff --git a/src/Utils/CoordinateSystem/RealObjectRailAxisConventionResolver.cs b/src/Utils/CoordinateSystem/RealObjectRailAxisConventionResolver.cs index 6bf4f7c..0e9848c 100644 --- a/src/Utils/CoordinateSystem/RealObjectRailAxisConventionResolver.cs +++ b/src/Utils/CoordinateSystem/RealObjectRailAxisConventionResolver.cs @@ -22,15 +22,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem Vector3 referenceAxisX, Vector3 referenceAxisY, Vector3 referenceAxisZ, + LocalAxisDirection hostUpLocalAxis, Vector3 desiredForward, CoordinateSystemType hostType, out ModelAxisConvention convention, out LocalAxisDirection selectedForwardAxis, - out Vector3 selectedForwardWorldAxis) + out Vector3 selectedForwardWorldAxis, + out LocalAxisDirection selectedUpAxis, + out Vector3 selectedUpWorldAxis) { convention = null; selectedForwardAxis = LocalAxisDirection.PositiveX; selectedForwardWorldAxis = Vector3.Zero; + selectedUpAxis = hostType == CoordinateSystemType.YUp + ? LocalAxisDirection.PositiveY + : LocalAxisDirection.PositiveZ; + selectedUpWorldAxis = Vector3.Zero; if (!TryNormalize(desiredForward, out Vector3 normalizedForward)) { @@ -44,29 +51,31 @@ namespace NavisworksTransport.Utils.CoordinateSystem return false; } - LocalAxisDirection semanticUpAxis = - hostType == CoordinateSystemType.YUp - ? LocalAxisDirection.PositiveY - : LocalAxisDirection.PositiveZ; - - Candidate best = Candidate.Invalid; - foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, semanticUpAxis)) - { - float score = Vector3.Dot(candidate.WorldAxis, normalizedForward); - if (best.IsInvalid || score > best.Score) - { - best = new Candidate(candidate.Axis, candidate.WorldAxis, score); - } - } - - if (best.IsInvalid) + if (!TryResolveWorldAxisForDirection(hostUpLocalAxis, axisX, axisY, axisZ, out var fixedUpWorldAxis)) { return false; } - convention = new ModelAxisConvention(best.Axis, semanticUpAxis); - selectedForwardAxis = best.Axis; - selectedForwardWorldAxis = best.WorldAxis; + Candidate bestForward = Candidate.Invalid; + foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, hostUpLocalAxis)) + { + float score = Vector3.Dot(candidate.WorldAxis, normalizedForward); + if (bestForward.IsInvalid || score > bestForward.Score) + { + bestForward = new Candidate(candidate.Axis, candidate.WorldAxis, score); + } + } + + if (bestForward.IsInvalid) + { + return false; + } + + convention = new ModelAxisConvention(bestForward.Axis, hostUpLocalAxis); + selectedForwardAxis = bestForward.Axis; + selectedForwardWorldAxis = bestForward.WorldAxis; + selectedUpAxis = hostUpLocalAxis; + selectedUpWorldAxis = fixedUpWorldAxis; return true; } @@ -74,28 +83,78 @@ namespace NavisworksTransport.Utils.CoordinateSystem Vector3 axisX, Vector3 axisY, Vector3 axisZ, - LocalAxisDirection semanticUpAxis) + LocalAxisDirection selectedUpAxis) { - switch (semanticUpAxis) - { - case LocalAxisDirection.PositiveY: - return new[] - { - new Candidate(LocalAxisDirection.PositiveX, axisX, 0f), - new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f), - new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f), - new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f) - }; + int upAxisIndex = GetAxisFamilyIndex(selectedUpAxis); + return Array.FindAll( + EnumerateAxisCandidates(axisX, axisY, axisZ), + candidate => GetAxisFamilyIndex(candidate.Axis) != upAxisIndex); + } + private static Candidate[] EnumerateAxisCandidates( + Vector3 axisX, + Vector3 axisY, + Vector3 axisZ) + { + return new[] + { + new Candidate(LocalAxisDirection.PositiveX, axisX, 0f), + new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f), + new Candidate(LocalAxisDirection.PositiveY, axisY, 0f), + new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f), + new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f), + new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f) + }; + } + + private static bool TryResolveWorldAxisForDirection( + LocalAxisDirection axis, + Vector3 axisX, + Vector3 axisY, + Vector3 axisZ, + out Vector3 worldAxis) + { + switch (axis) + { + case LocalAxisDirection.PositiveX: + worldAxis = axisX; + return true; + case LocalAxisDirection.NegativeX: + worldAxis = -axisX; + return true; + case LocalAxisDirection.PositiveY: + worldAxis = axisY; + return true; + case LocalAxisDirection.NegativeY: + worldAxis = -axisY; + return true; case LocalAxisDirection.PositiveZ: + worldAxis = axisZ; + return true; + case LocalAxisDirection.NegativeZ: + worldAxis = -axisZ; + return true; default: - return new[] - { - new Candidate(LocalAxisDirection.PositiveX, axisX, 0f), - new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f), - new Candidate(LocalAxisDirection.PositiveY, axisY, 0f), - new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f) - }; + worldAxis = Vector3.Zero; + return false; + } + } + + private static int GetAxisFamilyIndex(LocalAxisDirection axis) + { + switch (axis) + { + case LocalAxisDirection.PositiveX: + case LocalAxisDirection.NegativeX: + return 0; + case LocalAxisDirection.PositiveY: + case LocalAxisDirection.NegativeY: + return 1; + case LocalAxisDirection.PositiveZ: + case LocalAxisDirection.NegativeZ: + return 2; + default: + throw new ArgumentOutOfRangeException(nameof(axis), axis, null); } } diff --git a/src/Utils/CoordinateSystem/RealObjectRailExtentResolver.cs b/src/Utils/CoordinateSystem/RealObjectRailExtentResolver.cs index 0a3767a..f332d4c 100644 --- a/src/Utils/CoordinateSystem/RealObjectRailExtentResolver.cs +++ b/src/Utils/CoordinateSystem/RealObjectRailExtentResolver.cs @@ -20,6 +20,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem Vector3 referenceAxisX, Vector3 referenceAxisY, Vector3 referenceAxisZ, + LocalAxisDirection hostUpLocalAxis, Vector3 desiredForward, CoordinateSystemType hostType, double forwardSize, @@ -37,10 +38,13 @@ namespace NavisworksTransport.Utils.CoordinateSystem referenceAxisX, referenceAxisY, referenceAxisZ, + hostUpLocalAxis, desiredForward, hostType, out convention, out _, + out _, + out _, out _)) { return false; diff --git a/src/Utils/ModelItemTransformHelper.cs b/src/Utils/ModelItemTransformHelper.cs index dd2f95a..1791ba0 100644 --- a/src/Utils/ModelItemTransformHelper.cs +++ b/src/Utils/ModelItemTransformHelper.cs @@ -564,6 +564,8 @@ namespace NavisworksTransport.Utils $"Z=({deltaLinear.Get(0, 2):F4},{deltaLinear.Get(1, 2):F4},{deltaLinear.Get(2, 2):F4}), " + $"旋后当前点=({rotatedCurrentPosition.X:F3},{rotatedCurrentPosition.Y:F3},{rotatedCurrentPosition.Z:F3})"); + LogGeometryLevelTransforms(item, "[模型增量姿态应用前][GeometryAPI]"); + // 用显式三步法应用三维增量位姿： // 1. 把当前锚点移到原点 // 2. 绕原点旋转到目标姿态 @@ -619,6 +621,62 @@ namespace NavisworksTransport.Utils } } + public static bool TryGetCurrentOverrideRotation(ModelItem item, out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + if (item == null) + { + return false; + } + + try + { + ModelGeometry geometry = item.FindFirstGeometry() ?? item.Geometry; + if (geometry?.PermanentOverrideTransform == null) + { + return true; + } + + rotation = geometry.PermanentOverrideTransform.Factor().Rotation; + return true; + } + catch (Exception ex) + { + LogManager.Warning($"[当前覆盖姿态] 读取 PermanentOverrideTransform 失败: {ex.Message}"); + return false; + } + } + + public static bool TryResolveOverrideRotationForFinalTarget( + ModelItem item, + Rotation3D finalTargetRotation, + out Rotation3D overrideRotation) + { + overrideRotation = Rotation3D.Identity; + if (item == null) + { + return false; + } + + try + { + ModelGeometry geometry = item.FindFirstGeometry() ?? item.Geometry; + Transform3D originalTransform = geometry?.OriginalTransform ?? item.Transform; + Rotation3D originalRotation = originalTransform.Factor().Rotation; + var originalInverse = originalRotation.Invert(); + var overrideTransform = Transform3D.Multiply( + new Transform3D(originalInverse), + new Transform3D(finalTargetRotation)); + overrideRotation = overrideTransform.Factor().Rotation; + return true; + } + catch (Exception ex) + { + LogManager.Warning($"[覆盖姿态换算] 计算 override 姿态失败: {ex.Message}"); + return false; + } + } + public static void LogCurrentGeometryTransformsForDebug(ModelItem item, string prefix) { LogGeometryLevelTransforms(item, prefix);