Refine rail real-object pose handling

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);

UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs

@@ -21,11 +21,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
                 referenceAxisX,
                 referenceAxisY,
                 referenceAxisZ,
+                LocalAxisDirection.PositiveY,
                 desiredForward,
                 CoordinateSystemType.YUp,
-                forwardSize: 6.0,
+                6.0,
-                sideSize: 2.0,
+                2.0,
-                upSize: 4.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,
+                6.0,
-                sideSize: 2.0,
+                2.0,
-                upSize: 4.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,
+                6.0,
-                sideSize: 2.0,
+                2.0,
-                upSize: 4.0,
+                4.0,
                 baseline,
                 final,
                 out ModelAxisConvention convention,

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 =
+            if (!TryResolveWorldAxisForDirection(hostUpLocalAxis, axisX, axisY, axisZ, out var fixedUpWorldAxis))
-                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)
             {
                 return false;
             }
 
-            convention = new ModelAxisConvention(best.Axis, semanticUpAxis);
+            Candidate bestForward = Candidate.Invalid;
-            selectedForwardAxis = best.Axis;
+            foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, hostUpLocalAxis))
-            selectedForwardWorldAxis = best.WorldAxis;
+            {
+                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)
+            int upAxisIndex = GetAxisFamilyIndex(selectedUpAxis);
-            {
+            return Array.FindAll(
-                case LocalAxisDirection.PositiveY:
+                EnumerateAxisCandidates(axisX, axisY, axisZ),
-                    return new[]
+                candidate => GetAxisFamilyIndex(candidate.Axis) != upAxisIndex);
-                    {
+        }
-                        new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
-                        new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
-                        new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f),
-                        new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f)
-                    };
 
+        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[]
+                    worldAxis = Vector3.Zero;
-                    {
+                    return false;
-                        new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
+            }
-                        new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
+        }
-                        new Candidate(LocalAxisDirection.PositiveY, axisY, 0f),
+
-                        new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f)
+        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);
             }
         }
 

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;

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);