From 6c74ea13199699fc11d2b706514616d713ea09f0 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Tue, 24 Mar 2026 21:59:53 +0800
Subject: [PATCH] Fix host-axis rotation correction for real objects
---
.../HostCoordinateAdapterTests.cs | 129 +++++++++++++++++-
.../ViewModels/AnimationControlViewModel.cs | 2 -
.../CoordinateSystem/HostCoordinateAdapter.cs | 44 +++++-
.../LocalEulerRotationCorrection.cs | 20 +++
4 files changed, 184 insertions(+), 11 deletions(-)
diff --git a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
index 12d773a..c206309 100644
--- a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
+++ b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
@@ -169,6 +169,28 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
AssertVector(rotatedUp, 0.0, 1.0, 0.0);
}
+ [TestMethod]
+ public void YUp_HostRotationCorrection_X90_ShouldRotateHostZIntoHostY()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(90.0, 0.0, 0.0));
+
+ Vector3 rotatedHostZ = Vector3.Transform(Vector3.UnitZ, hostCorrection);
+ AssertVector(rotatedHostZ, 0.0, -1.0, 0.0);
+ }
+
+ [TestMethod]
+ public void YUp_HostRotationCorrection_Z90_ShouldRotateHostXIntoNegativeHostY()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
+
+ Vector3 rotatedHostX = Vector3.Transform(Vector3.UnitX, hostCorrection);
+ AssertVector(rotatedHostX, 0.0, 1.0, 0.0);
+ }
+
[TestMethod]
public void ComposeHostQuaternion_ShouldApplyHostCorrectionAfterBaseline()
{
@@ -183,7 +205,7 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(0.0, linear.M11, 1e-6);
Assert.AreEqual(0.0, linear.M21, 1e-6);
- Assert.AreEqual(1.0, linear.M31, 1e-6);
+ Assert.AreEqual(-1.0, linear.M31, 1e-6);
Assert.AreEqual(0.0, linear.M12, 1e-6);
Assert.AreEqual(1.0, linear.M22, 1e-6);
@@ -214,10 +236,10 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
Assert.AreEqual(0.0, linear.M11, 1e-6);
- Assert.AreEqual(-1.0, linear.M21, 1e-6);
+ Assert.AreEqual(1.0, linear.M21, 1e-6);
Assert.AreEqual(0.0, linear.M31, 1e-6);
- Assert.AreEqual(1.0, linear.M12, 1e-6);
+ Assert.AreEqual(-1.0, linear.M12, 1e-6);
Assert.AreEqual(0.0, linear.M22, 1e-6);
Assert.AreEqual(0.0, linear.M32, 1e-6);
@@ -226,6 +248,63 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(1.0, linear.M33, 1e-6);
}
+ [TestMethod]
+ public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostXAxis()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
+ Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
+ Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
+ Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
+
+ var correction = new LocalEulerRotationCorrection(90.0, 0.0, 0.0);
+ Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
+ Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
+
+ AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
+ AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
+ AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
+ }
+
+ [TestMethod]
+ public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostYAxis()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
+ Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
+ Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
+ Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
+
+ var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
+ Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
+ Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
+
+ AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
+ AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
+ AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
+ }
+
+ [TestMethod]
+ public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostZAxis()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
+ Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
+ Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
+ Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
+
+ var correction = new LocalEulerRotationCorrection(0.0, 0.0, 90.0);
+ Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
+ Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
+
+ AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
+ AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
+ AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
+ }
+
private static void AssertPoint(Vector3 point, double x, double y, double z)
{
Assert.AreEqual(x, point.X, 1e-9);
@@ -233,11 +312,47 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(z, point.Z, 1e-9);
}
- private static void AssertVector(Vector3 vector, double x, double y, double z)
+ private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6)
{
- Assert.AreEqual(x, vector.X, 1e-9);
- Assert.AreEqual(y, vector.Y, 1e-9);
- Assert.AreEqual(z, vector.Z, 1e-9);
+ Assert.AreEqual(x, vector.X, tolerance);
+ Assert.AreEqual(y, vector.Y, tolerance);
+ Assert.AreEqual(z, vector.Z, tolerance);
+ }
+
+ private static Quaternion CreateQuaternionFromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
+ {
+ Matrix4x4 basis = new Matrix4x4(
+ xAxis.X, yAxis.X, zAxis.X, 0f,
+ xAxis.Y, yAxis.Y, zAxis.Y, 0f,
+ xAxis.Z, yAxis.Z, zAxis.Z, 0f,
+ 0f, 0f, 0f, 1f);
+ return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(basis));
+ }
+
+ private static void AssertAxis(Matrix4x4 linear, int axisIndex, Vector3 expectedAxis, double tolerance = 1e-4)
+ {
+ Vector3 normalizedExpected = Vector3.Normalize(expectedAxis);
+ switch (axisIndex)
+ {
+ case 1:
+ Assert.AreEqual(normalizedExpected.X, linear.M11, tolerance);
+ Assert.AreEqual(normalizedExpected.Y, linear.M21, tolerance);
+ Assert.AreEqual(normalizedExpected.Z, linear.M31, tolerance);
+ break;
+ case 2:
+ Assert.AreEqual(normalizedExpected.X, linear.M12, tolerance);
+ Assert.AreEqual(normalizedExpected.Y, linear.M22, tolerance);
+ Assert.AreEqual(normalizedExpected.Z, linear.M32, tolerance);
+ break;
+ case 3:
+ Assert.AreEqual(normalizedExpected.X, linear.M13, tolerance);
+ Assert.AreEqual(normalizedExpected.Y, linear.M23, tolerance);
+ Assert.AreEqual(normalizedExpected.Z, linear.M33, tolerance);
+ break;
+ default:
+ Assert.Fail($"未知轴索引: {axisIndex}");
+ break;
+ }
}
}
}
diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
index ac82b2c..635d6b2 100644
--- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
+++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
@@ -1737,8 +1737,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
LogManager.Debug($"[选择物体] 保持当前角度修正值（同一物体）");
}
- // 显式同步一次到当前路径起点，避免依赖 setter 分支导致真实物体停留在 CAD 原位。
- MoveAnimatedObjectToPathStart();
}
catch (Exception ex)
{
diff --git a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
index b780a22..ef2bf29 100644
--- a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
+++ b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
@@ -186,6 +186,15 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return new Rotation3D(hostQuaternion.X, hostQuaternion.Y, hostQuaternion.Z, hostQuaternion.W);
}
+ ///
+ /// 将“宿主坐标系语义下”的四元数直接包装成 Navisworks 的 Rotation3D。
+ ///
+ /// 项目硬约束：
+ /// - Rotation3D(x, y, z, w) 的参数顺序固定是 (x, y, z, w)
+ /// - Rotation3D.A/B/C/D 分别对应 x/y/z/w
+ ///
+ /// 这里不做任何轴交换、符号翻转或重新解释，只负责把已经确定好的宿主四元数写入 Navisworks。
+ ///
public Rotation3D FromHostQuaternion(Quaternion hostRotation)
{
Quaternion normalizedHostRotation = Quaternion.Normalize(hostRotation);
@@ -196,6 +205,17 @@ namespace NavisworksTransport.Utils.CoordinateSystem
normalizedHostRotation.W);
}
+ ///
+ /// 根据宿主坐标系的世界轴，构造一个“宿主 X/Y/Z 角度修正”四元数。
+ ///
+ /// 关键约束：
+ /// - correction.XDegrees 永远表示绕宿主世界 X 轴旋转
+ /// - correction.YDegrees 永远表示绕宿主世界 Y 轴旋转
+ /// - correction.ZDegrees 永远表示绕宿主世界 Z 轴旋转
+ /// - 角度应用顺序固定为 X -> Y -> Z，对应四元数组合 qz * qy * qx
+ ///
+ /// 本方法只负责生成“宿主世界轴修正”本身，不负责和某个基姿态做组合。
+ ///
public Quaternion CreateHostRotationCorrection(LocalEulerRotationCorrection correction)
{
if (correction.IsZero)
@@ -209,15 +229,35 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return Quaternion.Normalize(qz * qy * qx);
}
+ ///
+ /// 将宿主世界轴角度修正叠加到一个已经位于宿主坐标系中的基姿态上。
+ ///
+ /// 参数语义：
+ /// - baselineQuaternion: 已经求好的宿主基姿态
+ /// - correction: 仍按宿主世界 X/Y/Z 解释的角度修正
+ ///
+ /// 注意：
+ /// - 这里的组合顺序直接决定“修正是绕宿主世界轴”还是“绕基姿态当前轴”
+ /// - 后续若修改乘法顺序，必须同步更新真实场景测试，不能靠肉眼猜
+ /// - 本方法只用于宿主坐标系；不要把资产坐标系或内部坐标系的四元数传进来混用
+ ///
public Quaternion ComposeHostQuaternion(Quaternion baselineQuaternion, LocalEulerRotationCorrection correction)
{
Quaternion correctionQuaternion = CreateHostRotationCorrection(correction);
- return Quaternion.Normalize(correctionQuaternion * baselineQuaternion);
+ Quaternion inverseCorrectionQuaternion = Quaternion.Conjugate(correctionQuaternion);
+ return Quaternion.Normalize(baselineQuaternion * inverseCorrectionQuaternion);
}
///
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
- /// 约定：先绕宿主 X，再绕宿主 Y，最后绕宿主 Z。
+ ///
+ /// 关键约束：
+ /// - 输入角度仍然按宿主世界 X/Y/Z 解释
+ /// - 输出四元数位于内部坐标系
+ /// - 角度应用顺序固定为 X -> Y -> Z，对应四元数组合 qz * qy * qx
+ ///
+ /// 该方法只做“宿主世界轴 -> 内部坐标系轴”的映射，
+ /// 不负责决定真实物体、虚拟物体或路径姿态链应该如何消费这个修正。
///
public Quaternion CreateCanonicalRotationCorrection(LocalEulerRotationCorrection correction)
{
diff --git a/src/Utils/CoordinateSystem/LocalEulerRotationCorrection.cs b/src/Utils/CoordinateSystem/LocalEulerRotationCorrection.cs
index 61f8e27..6f9d3a9 100644
--- a/src/Utils/CoordinateSystem/LocalEulerRotationCorrection.cs
+++ b/src/Utils/CoordinateSystem/LocalEulerRotationCorrection.cs
@@ -7,6 +7,15 @@ namespace NavisworksTransport.Utils.CoordinateSystem
///
/// 三轴旋转修正角（单位：度）。
/// 该结构本身只保存 X/Y/Z 三个角度值，不直接绑定宿主/内部/资产语义。
+ ///
+ /// 关键约束：
+ /// - 构造参数顺序固定为 (xDegrees, yDegrees, zDegrees)
+ /// - 属性语义固定为 XDegrees / YDegrees / ZDegrees
+ /// - 本结构只表示“沿某三根给定轴分别旋转多少度”，不负责决定这些轴属于宿主坐标系、内部坐标系还是资产坐标系
+ ///
+ /// 因此，调用方必须先明确：
+ /// 1. 当前三根轴分别是什么
+ /// 2. 这些角度是按哪种坐标语义解释
///
public struct LocalEulerRotationCorrection : IEquatable
{
@@ -28,6 +37,17 @@ namespace NavisworksTransport.Utils.CoordinateSystem
Math.Abs(YDegrees) < 1e-9 &&
Math.Abs(ZDegrees) < 1e-9;
+ ///
+ /// 基于调用方传入的三根轴，按 X -> Y -> Z 的角度顺序构造旋转四元数。
+ ///
+ /// 注意：
+ /// - 参数顺序固定为 (axisX, axisY, axisZ)
+ /// - 角度来源固定为 (XDegrees, YDegrees, ZDegrees)
+ /// - 组合顺序固定为 qz * qy * qx
+ ///
+ /// 这里的 qz * qy * qx 只表示“先应用 X，再应用 Y，再应用 Z”这组三轴修正，
+ /// 不自动代表宿主世界轴或物体当前姿态轴。轴语义完全由调用方决定。
+ ///
public Quaternion CreateQuaternion(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
{
Quaternion qx = Quaternion.CreateFromAxisAngle(Vector3.Normalize(axisX), DegreesToRadians(XDegrees));