From e9128282b5e9738207a6c3fa5dd66b63bc53f721 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Mon, 23 Mar 2026 00:11:49 +0800 Subject: [PATCH] Fix rail rotation correction semantics --- .../CanonicalRailPoseBuilderTests.cs | 82 +++++++++++++++++++ doc/working/current-engineering-state.md | 29 ++++++- src/Core/Animation/PathAnimationManager.cs | 2 + .../CanonicalRailPoseBuilder.cs | 79 +++++++++++++++++- src/Utils/RailPathPoseHelper.cs | 55 ++++++++++++- 5 files changed, 240 insertions(+), 7 deletions(-) diff --git a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs index 3e2fca1..0104b64 100644 --- a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs +++ b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs @@ -1,5 +1,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; using NavisworksTransport.Utils.CoordinateSystem; +using System; using System.Numerics; namespace NavisworksTransport.UnitTests.CoordinateSystem @@ -82,6 +83,60 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreEqual(0.0, Vector3.Dot(frame.Lateral, frame.Normal), 1e-6); } + [TestMethod] + public void ZeroLocalUpRotation_ShouldMatchCurrentRailBaseline() + { + var convention = ModelAxisConvention.CreateRailAssetConvention(); + + Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion( + new Vector3(0, 0, 0), + new Vector3(1, 0, 0), + new Vector3(2, 0, 0), + Vector3.UnitZ, + convention, + out Quaternion baselineRotation)); + + Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion( + new Vector3(0, 0, 0), + new Vector3(1, 0, 0), + new Vector3(2, 0, 0), + Vector3.UnitZ, + convention, + 0.0, + out Quaternion correctedRotation)); + + AssertQuaternionEquivalent(baselineRotation, correctedRotation); + } + + [TestMethod] + public void LocalUpRotation_ShouldAlignCorrectedLocalForwardWithRailTangent() + { + var convention = ModelAxisConvention.CreateRailAssetConvention(); + + Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion( + new Vector3(0, 0, 0), + new Vector3(1, 0, 0), + new Vector3(2, 0, 0), + Vector3.UnitZ, + convention, + 90.0, + out Quaternion rotation)); + + Quaternion localPreRotation = Quaternion.CreateFromAxisAngle( + Vector3.Normalize(convention.UpUnitVector), + (float)(90.0 * Math.PI / 180.0)); + + Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation)); + Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation)); + Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation); + + Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward); + Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp); + + AssertVector(mappedForward, 1, 0, 0); + AssertVector(mappedUp, 0, 0, 1); + } + private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z) { switch (column) @@ -106,5 +161,32 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem break; } } + + private static void AssertVector(Vector3 actual, double x, double y, double z) + { + Assert.AreEqual(x, actual.X, 1e-6); + Assert.AreEqual(y, actual.Y, 1e-6); + Assert.AreEqual(z, actual.Z, 1e-6); + } + + private static void AssertQuaternionEquivalent(Quaternion expected, Quaternion actual) + { + double dot = Math.Abs( + expected.X * actual.X + + expected.Y * actual.Y + + expected.Z * actual.Z + + expected.W * actual.W); + + Assert.AreEqual(1.0, dot, 1e-6); + } + + private static Vector3 TransformLocalVector(Matrix4x4 linear, Vector3 localVector) + { + Vector3 world = new Vector3( + linear.M11 * localVector.X + linear.M12 * localVector.Y + linear.M13 * localVector.Z, + linear.M21 * localVector.X + linear.M22 * localVector.Y + linear.M23 * localVector.Z, + linear.M31 * localVector.X + linear.M32 * localVector.Y + linear.M33 * localVector.Z); + return Vector3.Normalize(world); + } } } diff --git a/doc/working/current-engineering-state.md b/doc/working/current-engineering-state.md index 73e6ff1..9b0e59c 100644 --- a/doc/working/current-engineering-state.md +++ b/doc/working/current-engineering-state.md @@ -1,6 +1,6 @@ # 当前工程状态 -更新时间：2026-03-22 +更新时间：2026-03-23 ## 1. 当前稳定状态 @@ -136,8 +136,31 @@ - 那么 `YUp` 模型下聚焦就会跑到侧视图。 - 当前原则： - 聚焦输入/输出使用宿主坐标 - - 方向语义通过 `HostCoordinateAdapter` 明确适配 - - 不要在视点辅助逻辑里直接假设宿主一定是 `ZUp` +- 方向语义通过 `HostCoordinateAdapter` 明确适配 +- 不要在视点辅助逻辑里直接假设宿主一定是 `ZUp` + +### 4.7 Rail 角度修正 + +- `Rail` 的“调整角度”和 `Ground / Hoisting` 不是同一种实现语义。 +- `Ground / Hoisting` 当前语义： + - 在内部 `Canonical` 坐标系里，绕统一 `CanonicalUp` 做水平角度修正。 +- `Rail` 当前正确语义： + - 角度修正必须先作用在构件局部轴约定上，等价于“CAD 原位局部预旋转” + - 然后再进入 `Rail` 的切向/法向姿态求解 + - 不能在物体已经落到起点后，再在宿主空间对最终 pose 补一个世界轴旋转 +- 当前实现链路已经收束到： + - `CanonicalRailPoseBuilder` + - `RailPathPoseHelper` + - `PathAnimationManager` +- 当前规则： + - `Rail 0°` 时，必须严格保持当前稳定基线，不允许改变既有 `Rail` 姿态 + - `Rail` 有角度修正时，对齐到路径切向的是“修正后的 local forward” + - `Rail` 路径切向/法向框架本身不能因为角度修正被改坏 + - `Rail` 通行空间和真实物体姿态必须共享同一套角度语义，不能一个改局部 footprint、一个改世界 pose +- 调试优先级： + 1. 先验证 `0°` 基线是否保持不变 + 2. 再验证修正后 `forward` 是否仍沿 rail 切向 + 3. 最后再看 Navisworks 应用层是否正确按三步增量姿态落位 ## 5. 当前保留的日志策略 diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 2bccc0e..6298b25 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -675,6 +675,7 @@ namespace NavisworksTransport.Core.Animation _pathPoints[0], nextPoint, GetCurrentRailModelAxisConvention(), + _objectRotationCorrection, out var railRotation)) { var railLinearTransform = new Transform3D(railRotation).Linear; @@ -1037,6 +1038,7 @@ namespace NavisworksTransport.Core.Animation framePosition, nextFramePoint, GetCurrentRailModelAxisConvention(), + _objectRotationCorrection, out var railRotation)) { frame.Rotation = railRotation; diff --git a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs index ee77355..e20e521 100644 --- a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs +++ b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs @@ -88,6 +88,25 @@ namespace NavisworksTransport.Utils.CoordinateSystem Vector3 canonicalUp, ModelAxisConvention convention, out Quaternion rotation) + { + return TryCreateQuaternion( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + convention, + 0.0, + out rotation); + } + + public static bool TryCreateQuaternion( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + ModelAxisConvention convention, + double localUpRotationDegrees, + out Quaternion rotation) { rotation = Quaternion.Identity; @@ -101,9 +120,67 @@ namespace NavisworksTransport.Utils.CoordinateSystem return false; } - rotation = convention.CreateQuaternion(forward, up); + if (Math.Abs(localUpRotationDegrees) < 1e-9) + { + rotation = convention.CreateQuaternion(forward, up); + return true; + } + + float correctionRadians = (float)(localUpRotationDegrees * Math.PI / 180.0); + Quaternion localPreRotation = Quaternion.CreateFromAxisAngle( + Vector3.Normalize(convention.UpUnitVector), + correctionRadians); + + Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation)); + Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation)); + rotation = CreateQuaternionFromLocalAxes(correctedLocalForward, correctedLocalUp, forward, up); return true; } + private static Quaternion CreateQuaternionFromLocalAxes( + Vector3 localForward, + Vector3 localUp, + Vector3 worldForward, + Vector3 worldUp) + { + Vector3 normalizedLocalForward = Vector3.Normalize(localForward); + Vector3 normalizedLocalUp = Vector3.Normalize(localUp); + Vector3 normalizedWorldForward = Vector3.Normalize(worldForward); + Vector3 normalizedWorldUp = Vector3.Normalize(worldUp); + + Vector3 localRemaining = Vector3.Normalize(Vector3.Cross(normalizedLocalForward, normalizedLocalUp)); + Vector3 worldRemaining = Vector3.Normalize(Vector3.Cross(normalizedWorldForward, normalizedWorldUp)); + + Vector3 worldX = MapOriginalLocalAxis(Vector3.UnitX, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining); + Vector3 worldY = MapOriginalLocalAxis(Vector3.UnitY, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining); + Vector3 worldZ = MapOriginalLocalAxis(Vector3.UnitZ, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining); + + Matrix4x4 linear = new Matrix4x4( + worldX.X, worldY.X, worldZ.X, 0f, + worldX.Y, worldY.Y, worldZ.Y, 0f, + worldX.Z, worldY.Z, worldZ.Z, 0f, + 0f, 0f, 0f, 1f); + return Quaternion.CreateFromRotationMatrix(linear); + } + + private static Vector3 MapOriginalLocalAxis( + Vector3 originalAxis, + Vector3 localForward, + Vector3 localUp, + Vector3 localRemaining, + Vector3 worldForward, + Vector3 worldUp, + Vector3 worldRemaining) + { + float forwardWeight = Vector3.Dot(originalAxis, localForward); + float upWeight = Vector3.Dot(originalAxis, localUp); + float remainingWeight = Vector3.Dot(originalAxis, localRemaining); + + return Vector3.Normalize( + forwardWeight * worldForward + + upWeight * worldUp + + remainingWeight * worldRemaining); + } + } } diff --git a/src/Utils/RailPathPoseHelper.cs b/src/Utils/RailPathPoseHelper.cs index 82b507c..2b74480 100644 --- a/src/Utils/RailPathPoseHelper.cs +++ b/src/Utils/RailPathPoseHelper.cs @@ -182,6 +182,23 @@ namespace NavisworksTransport.Utils Point3D nextPoint, ModelAxisConvention axisConvention, out Matrix3 linearTransform) + { + return TryCreateRailLinearTransform( + previousPoint, + currentPoint, + nextPoint, + axisConvention, + 0.0, + out linearTransform); + } + + public static bool TryCreateRailLinearTransform( + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + ModelAxisConvention axisConvention, + double localUpRotationDegrees, + out Matrix3 linearTransform) { linearTransform = null; @@ -208,7 +225,16 @@ namespace NavisworksTransport.Utils var canonicalForward = frame.Forward; var canonicalLateral = frame.Lateral; var canonicalUp = frame.Normal; - Quaternion canonicalRotation = axisConvention.CreateQuaternion(canonicalForward, canonicalUp); + Quaternion canonicalRotation = CanonicalRailPoseBuilder.TryCreateQuaternion( + canonicalPreviousPoint, + canonicalCurrentPoint, + canonicalNextPoint, + HostCoordinateAdapter.CanonicalUpVector3, + axisConvention, + localUpRotationDegrees, + out var correctedRotation) + ? correctedRotation + : axisConvention.CreateQuaternion(canonicalForward, canonicalUp); Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation); var hostXAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M11, canonicalLinear.M21, canonicalLinear.M31)))); @@ -229,7 +255,7 @@ namespace NavisworksTransport.Utils $"Canonical侧向=({canonicalLateral.X:F4},{canonicalLateral.Y:F4},{canonicalLateral.Z:F4}), " + $"Canonical法向=({canonicalUp.X:F4},{canonicalUp.Y:F4},{canonicalUp.Z:F4}), " + $"Host法向=({hostZAxis.X:F4},{hostZAxis.Y:F4},{hostZAxis.Z:F4}), " + - $"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}"); + $"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}, 本地预旋转={localUpRotationDegrees:F1}°"); return true; } @@ -257,11 +283,34 @@ namespace NavisworksTransport.Utils Point3D nextPoint, ModelAxisConvention axisConvention, out Rotation3D rotation) + { + return TryCreateRailRotation( + previousPoint, + currentPoint, + nextPoint, + axisConvention, + 0.0, + out rotation); + } + + public static bool TryCreateRailRotation( + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + ModelAxisConvention axisConvention, + double localUpRotationDegrees, + out Rotation3D rotation) { rotation = Rotation3D.Identity; LogRotationConstructorConventionOnce(); - if (!TryCreateRailLinearTransform(previousPoint, currentPoint, nextPoint, axisConvention, out var linearTransform)) + if (!TryCreateRailLinearTransform( + previousPoint, + currentPoint, + nextPoint, + axisConvention, + localUpRotationDegrees, + out var linearTransform)) { return false; }