Use actual hoisting pose baseline for real objects

2026-03-29 23:36:43 +08:00 · 2026-03-29 23:36:43 +08:00 · 9093374399
commit 9093374399
parent 766b5af887
7 changed files with 320 additions and 13 deletions
--- a/NavisworksTransport.UnitTests.csproj
+++ b/NavisworksTransport.UnitTests.csproj
@ -65,6 +65,7 @@
    <Compile Include="UnitTests\CoordinateSystem\CanonicalTrackedPositionResolverTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\FragmentRepresentativePoseHelperTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\HoistingCoordinateHelperTests.cs" />
+    <Compile Include="UnitTests\CoordinateSystem\HoistingRealObjectPoseHelperTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\ModelAxisConventionTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\ProjectReferenceFrameTests.cs" />
    <Compile Include="UnitTests\CoordinateSystem\RealObjectPlanarPoseSolverTests.cs" />
--- a/TransportPlugin.csproj
+++ b/TransportPlugin.csproj
@ -345,6 +345,7 @@
    <Compile Include="src\Utils\CoordinateSystem\FragmentRepresentativePoseHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\RealObjectReferencePose.cs" />
    <Compile Include="src\Utils\CoordinateSystem\RealObjectReferencePoseResolver.cs" />
+    <Compile Include="src\Utils\CoordinateSystem\HoistingRealObjectPoseHelper.cs" />
    <Compile Include="src\Utils\CoordinateSystem\PathTargetFrame.cs" />
    <Compile Include="src\Utils\CoordinateSystem\PathTargetFrameResolver.cs" />
    <Compile Include="src\Utils\CoordinateSystem\RealObjectPlanarPoseSolver.cs" />
--- a/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs
+++ b/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs
@ -0,0 +1,73 @@
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using NavisworksTransport.Utils.CoordinateSystem;
+using System.Numerics;
+
+namespace NavisworksTransport.UnitTests.CoordinateSystem
+{
+    [TestClass]
+    public class HoistingRealObjectPoseHelperTests
+    {
+        [TestMethod]
+        public void ShouldKeepActualUpAxis_WhenAligningHoistingForward()
+        {
+            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
+            Vector3 hostUp = Vector3.UnitY;
+            Vector3 targetForward = -Vector3.UnitX;
+
+            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
+                actualRotation,
+                targetForward,
+                hostUp,
+                out Quaternion resultRotation,
+                out LocalAxisDirection selectedDirection,
+                out Vector3 selectedAxisLocal,
+                out Vector3 projectedForward);
+
+            Assert.IsTrue(ok);
+            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
+            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
+            AssertVector(projectedForward, 1.0, 0.0, 0.0);
+
+            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
+            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);
+
+            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
+            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
+        }
+
+        [TestMethod]
+        public void ShouldChooseHorizontalAxisFromActualPose_ForHoisting()
+        {
+            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
+            Vector3 hostUp = Vector3.UnitY;
+            Vector3 targetForward = Vector3.UnitX;
+
+            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
+                actualRotation,
+                targetForward,
+                hostUp,
+                out Quaternion resultRotation,
+                out LocalAxisDirection selectedDirection,
+                out Vector3 selectedAxisLocal,
+                out Vector3 projectedForward);
+
+            Assert.IsTrue(ok);
+            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
+            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
+            AssertVector(projectedForward, 1.0, 0.0, 0.0);
+
+            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
+            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);
+
+            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
+            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
+        }
+
+        private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
+        {
+            Assert.AreEqual(x, actual.X, tolerance);
+            Assert.AreEqual(y, actual.Y, tolerance);
+            Assert.AreEqual(z, actual.Z, tolerance);
+        }
+    }
+}
--- a/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs
+++ b/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs
@ -118,6 +118,27 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
            Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostUpLocalAxis);
        }

+        [TestMethod]
+        public void YUp_WorldAxisMapping_ShouldRejectAmbiguousRawFragmentFrame()
+        {
+            var fragmentMatrices = new List<double[]>
+            {
+                CreateMatrix(
+                    axisX: Vector3.Normalize(new Vector3(0.80f, 0.60f, 0.0f)),
+                    axisY: new Vector3(0.0f, 0.0f, -1.0f),
+                    axisZ: Vector3.Normalize(new Vector3(-0.60f, 0.80f, 0.0f)))
+            };
+
+            bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
+                fragmentMatrices,
+                fragmentDefaultUpAxis: "Y",
+                hostUpAxis: "Y",
+                out RealObjectReferencePose pose);
+
+            Assert.IsFalse(ok);
+            Assert.IsNull(pose);
+        }
+
        private static double[] CreateMatrix(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
        {
            return new double[]
--- a/src/Core/Animation/PathAnimationManager.cs
+++ b/src/Core/Animation/PathAnimationManager.cs
@ -4609,6 +4609,12 @@ namespace NavisworksTransport.Core.Animation
        {
            rotation = Rotation3D.Identity;

+            if (_route?.PathType == PathType.Hoisting &&
+                TryCreateHoistingRealObjectRotationFromActualPose(hostForward, out rotation))
+            {
+                return true;
+            }
+
            if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
                hostForward,
                CoordinateSystemManager.Instance.CreateHostAdapter().HostType,
@ -4646,6 +4652,64 @@ namespace NavisworksTransport.Core.Animation
            return true;
        }

+        private bool TryCreateHoistingRealObjectRotationFromActualPose(Vector3 hostForward, out Rotation3D rotation)
+        {
+            rotation = Rotation3D.Identity;
+
+            if (_animatedObject == null ||
+                !ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out var actualRotation3D))
+            {
+                return false;
+            }
+
+            var actualRotation = new Quaternion(
+                (float)actualRotation3D.A,
+                (float)actualRotation3D.B,
+                (float)actualRotation3D.C,
+                (float)actualRotation3D.D);
+
+            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
+            Vector3 hostUp = adapter.HostType == CoordinateSystemType.YUp ? Vector3.UnitY : Vector3.UnitZ;
+            if (!HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
+                    actualRotation,
+                    hostForward,
+                    hostUp,
+                    out var baselineQuaternion,
+                    out var selectedAxisDirection,
+                    out var selectedAxisLocal,
+                    out var projectedForward))
+            {
+                return false;
+            }
+
+            rotation = adapter.FromHostQuaternionDirect(
+                adapter.ComposeHostQuaternion(baselineQuaternion, _objectRotationCorrection));
+
+            Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(baselineQuaternion);
+            Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(new Quaternion(
+                (float)rotation.A,
+                (float)rotation.B,
+                (float)rotation.C,
+                (float)rotation.D));
+
+            _realObjectPlanarSelectedForwardAxis = selectedAxisDirection;
+            _hasRealObjectPlanarSelectedForwardAxis = true;
+
+            LogManager.Info(
+                $"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。");
+            LogManager.Info(
+                $"[真实物体起点姿态] 选中参考轴=({selectedAxisLocal.X:F3},{selectedAxisLocal.Y:F3},{selectedAxisLocal.Z:F3}), " +
+                $"投影前进=({projectedForward.X:F3},{projectedForward.Y:F3},{projectedForward.Z:F3}), " +
+                $"宿主修正={_objectRotationCorrection}, 本地修正=X=0.0°,Y=0.0°,Z=0.0°, " +
+                $"BaselineX=({baselineLinear.M11:F4},{baselineLinear.M21:F4},{baselineLinear.M31:F4}), " +
+                $"BaselineY=({baselineLinear.M12:F4},{baselineLinear.M22:F4},{baselineLinear.M32:F4}), " +
+                $"BaselineZ=({baselineLinear.M13:F4},{baselineLinear.M23:F4},{baselineLinear.M33:F4}), " +
+                $"HostComposeX=({hostComposedLinear.M11:F4},{hostComposedLinear.M21:F4},{hostComposedLinear.M31:F4}), " +
+                $"HostComposeY=({hostComposedLinear.M12:F4},{hostComposedLinear.M22:F4},{hostComposedLinear.M32:F4}), " +
+                $"HostComposeZ=({hostComposedLinear.M13:F4},{hostComposedLinear.M23:F4},{hostComposedLinear.M33:F4})");
+            return true;
+        }
+
        private bool TryCreateRealObjectPlanarPoseSolution(
            Vector3 targetForward,
            Vector3 targetUp,
--- a/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs
+++ b/src/Utils/CoordinateSystem/HoistingRealObjectPoseHelper.cs
@ -0,0 +1,135 @@
+using System;
+using System.Numerics;
+
+namespace NavisworksTransport.Utils.CoordinateSystem
+{
+    public static class HoistingRealObjectPoseHelper
+    {
+        public static bool TryCreateRotationFromActualPose(
+            Quaternion actualRotation,
+            Vector3 targetForward,
+            Vector3 hostUp,
+            out Quaternion resultRotation,
+            out LocalAxisDirection selectedForwardAxisDirection,
+            out Vector3 selectedForwardAxisLocal,
+            out Vector3 projectedForward)
+        {
+            resultRotation = Quaternion.Identity;
+            selectedForwardAxisDirection = LocalAxisDirection.PositiveX;
+            selectedForwardAxisLocal = Vector3.UnitX;
+            projectedForward = Vector3.Zero;
+
+            if (actualRotation.LengthSquared() < 1e-6f)
+            {
+                return false;
+            }
+
+            Vector3 normalizedHostUp = NormalizeSafe(hostUp);
+            if (normalizedHostUp.LengthSquared() < 1e-6f)
+            {
+                return false;
+            }
+
+            Vector3 normalizedTargetForward = NormalizeOnPlane(targetForward, normalizedHostUp);
+            if (normalizedTargetForward.LengthSquared() < 1e-6f)
+            {
+                return false;
+            }
+
+            Quaternion normalizedActualRotation = Quaternion.Normalize(actualRotation);
+            Vector3[] localAxes =
+            {
+                Vector3.UnitX,
+                Vector3.UnitY,
+                Vector3.UnitZ
+            };
+            LocalAxisDirection[] directions =
+            {
+                LocalAxisDirection.PositiveX,
+                LocalAxisDirection.PositiveY,
+                LocalAxisDirection.PositiveZ
+            };
+
+            float bestScore = float.NegativeInfinity;
+            Vector3 bestProjectedAxis = Vector3.Zero;
+
+            for (int i = 0; i < localAxes.Length; i++)
+            {
+                Vector3 worldAxis = Vector3.Transform(localAxes[i], normalizedActualRotation);
+                float upAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(worldAxis), normalizedHostUp));
+                if (upAlignment > 0.9f)
+                {
+                    continue;
+                }
+
+                Vector3 projectedAxis = NormalizeOnPlane(worldAxis, normalizedHostUp);
+                if (projectedAxis.LengthSquared() < 1e-6f)
+                {
+                    continue;
+                }
+
+                float score = Math.Abs(Vector3.Dot(projectedAxis, normalizedTargetForward));
+                if (score > bestScore)
+                {
+                    bestScore = score;
+                    bestProjectedAxis = projectedAxis;
+                    selectedForwardAxisDirection = directions[i];
+                    selectedForwardAxisLocal = localAxes[i];
+                }
+            }
+
+            if (bestScore == float.NegativeInfinity)
+            {
+                return false;
+            }
+
+            Vector3 resolvedForward = Vector3.Dot(bestProjectedAxis, normalizedTargetForward) >= 0.0f
+                ? normalizedTargetForward
+                : -normalizedTargetForward;
+
+            projectedForward = resolvedForward;
+            float signedAngle = SignedAngleAroundAxis(bestProjectedAxis, resolvedForward, normalizedHostUp);
+            Quaternion deltaRotation = Quaternion.CreateFromAxisAngle(normalizedHostUp, signedAngle);
+            resultRotation = Quaternion.Normalize(deltaRotation * normalizedActualRotation);
+            return true;
+        }
+
+        private static Vector3 NormalizeOnPlane(Vector3 vector, Vector3 planeNormal)
+        {
+            Vector3 planar = vector - Vector3.Dot(vector, planeNormal) * planeNormal;
+            return NormalizeSafe(planar);
+        }
+
+        private static Vector3 NormalizeSafe(Vector3 vector)
+        {
+            if (vector.LengthSquared() < 1e-6f)
+            {
+                return Vector3.Zero;
+            }
+
+            return Vector3.Normalize(vector);
+        }
+
+        private static float SignedAngleAroundAxis(Vector3 from, Vector3 to, Vector3 axis)
+        {
+            float dot = Vector3.Dot(from, to);
+            if (dot > 1.0f)
+            {
+                dot = 1.0f;
+            }
+            else if (dot < -1.0f)
+            {
+                dot = -1.0f;
+            }
+
+            float angle = (float)Math.Acos(dot);
+            float sign = Math.Sign(Vector3.Dot(axis, Vector3.Cross(from, to)));
+            if (Math.Abs(sign) < 1e-6f)
+            {
+                sign = 1.0f;
+            }
+
+            return angle * sign;
+        }
+    }
+}
--- a/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs
+++ b/src/Utils/CoordinateSystem/RealObjectReferencePoseResolver.cs
@ -17,6 +17,10 @@ namespace NavisworksTransport.Utils.CoordinateSystem
    /// </summary>
    public static class RealObjectReferencePoseResolver
    {
+        private const float WorldAxisMinimumAlignment = 0.98f;
+        private const float SemanticAxisMinimumAlignment = 0.985f;
+        private const float MinimumBestToSecondGap = 0.02f;
+
        public static bool TryResolveFromModelItem(
            ModelItem sourceObject,
            Document doc,
@ -174,7 +178,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                    rawFrame.AxisX,
                    rawFrame.AxisY,
                    rawFrame.AxisZ,
-                    0.5f,
+                    WorldAxisMinimumAlignment,
                    out hostWorldXAxisLocalAxis))
            {
                return false;
@ -185,7 +189,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                    rawFrame.AxisX,
                    rawFrame.AxisY,
                    rawFrame.AxisZ,
-                    0.5f,
+                    WorldAxisMinimumAlignment,
                    out hostWorldYAxisLocalAxis))
            {
                return false;
@ -196,7 +200,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                    rawFrame.AxisX,
                    rawFrame.AxisY,
                    rawFrame.AxisZ,
-                    0.5f,
+                    WorldAxisMinimumAlignment,
                    out hostWorldZAxisLocalAxis))
            {
                return false;
@ -221,7 +225,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                rawFrame.AxisX,
                rawFrame.AxisY,
                rawFrame.AxisZ,
-                0.95f,
+                SemanticAxisMinimumAlignment,
                out hostSemanticXAxisLocalAxis))
            {
                return false;
@ -232,7 +236,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                rawFrame.AxisX,
                rawFrame.AxisY,
                rawFrame.AxisZ,
-                0.95f,
+                SemanticAxisMinimumAlignment,
                out hostSemanticYAxisLocalAxis))
            {
                return false;
@ -243,7 +247,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
                rawFrame.AxisX,
                rawFrame.AxisY,
                rawFrame.AxisZ,
-                0.95f,
+                SemanticAxisMinimumAlignment,
                out hostSemanticZAxisLocalAxis))
            {
                return false;
@ -262,15 +266,17 @@ namespace NavisworksTransport.Utils.CoordinateSystem
        {
            axisDirection = LocalAxisDirection.PositiveX;
            float bestAlignment = float.NegativeInfinity;
+            float secondBestAlignment = float.NegativeInfinity;

-            TryUpdateBestMatch(targetAxis, rawAxisX, LocalAxisDirection.PositiveX, ref bestAlignment, ref axisDirection);
-            TryUpdateBestMatch(targetAxis, -rawAxisX, LocalAxisDirection.NegativeX, ref bestAlignment, ref axisDirection);
-            TryUpdateBestMatch(targetAxis, rawAxisY, LocalAxisDirection.PositiveY, ref bestAlignment, ref axisDirection);
-            TryUpdateBestMatch(targetAxis, -rawAxisY, LocalAxisDirection.NegativeY, ref bestAlignment, ref axisDirection);
-            TryUpdateBestMatch(targetAxis, rawAxisZ, LocalAxisDirection.PositiveZ, ref bestAlignment, ref axisDirection);
-            TryUpdateBestMatch(targetAxis, -rawAxisZ, LocalAxisDirection.NegativeZ, ref bestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, rawAxisX, LocalAxisDirection.PositiveX, ref bestAlignment, ref secondBestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, -rawAxisX, LocalAxisDirection.NegativeX, ref bestAlignment, ref secondBestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, rawAxisY, LocalAxisDirection.PositiveY, ref bestAlignment, ref secondBestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, -rawAxisY, LocalAxisDirection.NegativeY, ref bestAlignment, ref secondBestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, rawAxisZ, LocalAxisDirection.PositiveZ, ref bestAlignment, ref secondBestAlignment, ref axisDirection);
+            TryUpdateBestMatch(targetAxis, -rawAxisZ, LocalAxisDirection.NegativeZ, ref bestAlignment, ref secondBestAlignment, ref axisDirection);

-            return bestAlignment >= minimumAlignment;
+            return bestAlignment >= minimumAlignment
+                && bestAlignment - secondBestAlignment >= MinimumBestToSecondGap;
        }

        private static void TryUpdateBestMatch(
@ -278,14 +284,20 @@ namespace NavisworksTransport.Utils.CoordinateSystem
            Vector3 candidateAxis,
            LocalAxisDirection candidateDirection,
            ref float bestAlignment,
+            ref float secondBestAlignment,
            ref LocalAxisDirection bestDirection)
        {
            float alignment = Vector3.Dot(Vector3.Normalize(targetAxis), Vector3.Normalize(candidateAxis));
            if (alignment > bestAlignment)
            {
+                secondBestAlignment = bestAlignment;
                bestAlignment = alignment;
                bestDirection = candidateDirection;
            }
+            else if (alignment > secondBestAlignment)
+            {
+                secondBestAlignment = alignment;
+            }
        }

        public static bool TryDetectDefaultUpAxis(