From eaf24420b7166a2f7e6f2ab28ed229239902c15e Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Mon, 23 Mar 2026 22:46:38 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E7=9C=9F=E5=AE=9E=E7=89=A9?=
=?UTF-8?q?=E4=BD=93=E7=9A=84=E6=97=8B=E8=BD=AC=E5=92=8C=E9=80=9A=E8=A1=8C?=
=?UTF-8?q?=E7=A9=BA=E9=97=B4=E4=B8=8D=E9=80=82=E9=85=8D=E7=9A=84=E9=97=AE?=
=?UTF-8?q?=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../CanonicalPlanarPoseBuilderTests.cs | 31 +++++
.../HostCoordinateAdapterTests.cs | 68 +++++++++++
.../RotatedObjectExtentHelperTests.cs | 90 ++++++++++++++
src/Core/Animation/PathAnimationManager.cs | 114 ++++++++++++++++--
.../ViewModels/AnimationControlViewModel.cs | 6 +-
.../CanonicalPlanarPoseBuilder.cs | 31 +++++
.../CoordinateSystem/HostCoordinateAdapter.cs | 28 +++++
src/Utils/ModelItemTransformHelper.cs | 12 +-
8 files changed, 359 insertions(+), 21 deletions(-)
create mode 100644 UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs
diff --git a/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs b/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs
index 89b058c..d56e57d 100644
--- a/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs
+++ b/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs
@@ -102,6 +102,37 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
AssertVector(mappedUp, 0, 0, 1);
}
+ [TestMethod]
+ public void WorldCorrection_ShouldRotateBaselinePoseAroundHostYAxisInYUp()
+ {
+ var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion worldCorrection = adapter.CreateCanonicalRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
+
+ Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
+ new Vector3(5, 0, 0),
+ Vector3.UnitZ,
+ convention,
+ out Quaternion baselineRotation));
+
+ Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForwardWithWorldCorrection(
+ new Vector3(5, 0, 0),
+ Vector3.UnitZ,
+ convention,
+ worldCorrection,
+ out Quaternion correctedRotation));
+
+ Matrix4x4 baseline = Matrix4x4.CreateFromQuaternion(baselineRotation);
+
+ AssertColumn(baseline, 0, 1, 0, 0);
+ AssertColumn(baseline, 1, 0, 0, 1);
+ AssertColumn(baseline, 2, 0, -1, 0);
+
+ Quaternion expectedRotation = Quaternion.Normalize(worldCorrection * baselineRotation);
+ AssertQuaternionEquivalent(expectedRotation, correctedRotation);
+ }
+
private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
{
switch (column)
diff --git a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
index 9f77b59..12d773a 100644
--- a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
+++ b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs
@@ -158,6 +158,74 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(0.0, rotatedForward.Z, 1e-6);
}
+ [TestMethod]
+ public void YUp_HostRotationCorrection_ShouldKeepHostYAxisInvariantInHostSpace()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
+
+ Vector3 rotatedUp = Vector3.Transform(Vector3.UnitY, hostCorrection);
+ AssertVector(rotatedUp, 0.0, 1.0, 0.0);
+ }
+
+ [TestMethod]
+ public void ComposeHostQuaternion_ShouldApplyHostCorrectionAfterBaseline()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ var baseline = Quaternion.Identity;
+
+ Quaternion composed = adapter.ComposeHostQuaternion(
+ baseline,
+ new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
+
+ Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
+
+ 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(0.0, linear.M12, 1e-6);
+ Assert.AreEqual(1.0, linear.M22, 1e-6);
+ Assert.AreEqual(0.0, linear.M32, 1e-6);
+ }
+
+ [TestMethod]
+ public void ZUp_HostRotationCorrection_ShouldKeepHostZAxisInvariantInHostSpace()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
+
+ Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
+ AssertVector(rotatedUp, 0.0, 0.0, 1.0);
+ }
+
+ [TestMethod]
+ public void ZUp_ComposeHostQuaternion_ShouldApplyHostZCorrectionAfterBaseline()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ var baseline = Quaternion.Identity;
+
+ Quaternion composed = adapter.ComposeHostQuaternion(
+ baseline,
+ new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
+
+ Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
+
+ Assert.AreEqual(0.0, linear.M11, 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(0.0, linear.M22, 1e-6);
+ Assert.AreEqual(0.0, linear.M32, 1e-6);
+
+ Assert.AreEqual(0.0, linear.M13, 1e-6);
+ Assert.AreEqual(0.0, linear.M23, 1e-6);
+ Assert.AreEqual(1.0, linear.M33, 1e-6);
+ }
+
private static void AssertPoint(Vector3 point, double x, double y, double z)
{
Assert.AreEqual(x, point.X, 1e-9);
diff --git a/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs b/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs
new file mode 100644
index 0000000..e6785ae
--- /dev/null
+++ b/UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs
@@ -0,0 +1,90 @@
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using NavisworksTransport.Utils.CoordinateSystem;
+using System.Numerics;
+
+namespace NavisworksTransport.UnitTests.CoordinateSystem
+{
+ [TestClass]
+ public class RotatedObjectExtentHelperTests
+ {
+ [TestMethod]
+ public void YUp_HostY90_ForRealObject_ShouldKeepUpExtentUnchanged()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
+ Quaternion correction = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
+
+ var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
+ convention,
+ forwardSize: 6.0,
+ sideSize: 4.0,
+ upSize: 2.0,
+ correctionQuaternion: correction);
+
+ Assert.AreEqual(4.0, result.forwardExtent, 1e-6);
+ Assert.AreEqual(6.0, result.sideExtent, 1e-6);
+ Assert.AreEqual(2.0, result.upExtent, 1e-6);
+ }
+
+ [TestMethod]
+ public void YUp_HostZ90_ForRealObject_ShouldPromoteForwardSizeToUpExtent()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
+ Quaternion correction = adapter.CreateHostRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
+
+ var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
+ convention,
+ forwardSize: 6.0,
+ sideSize: 4.0,
+ upSize: 2.0,
+ correctionQuaternion: correction);
+
+ Assert.AreEqual(2.0, result.forwardExtent, 1e-6);
+ Assert.AreEqual(4.0, result.sideExtent, 1e-6);
+ Assert.AreEqual(6.0, result.upExtent, 1e-6);
+ }
+
+ [TestMethod]
+ public void ZUp_HostY90_ShouldPromoteForwardSizeToUpExtent()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
+ Quaternion correction = adapter.CreateCanonicalRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
+
+ var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
+ convention,
+ forwardSize: 6.0,
+ sideSize: 4.0,
+ upSize: 2.0,
+ correctionQuaternion: correction);
+
+ Assert.AreEqual(2.0, result.forwardExtent, 1e-6);
+ Assert.AreEqual(4.0, result.sideExtent, 1e-6);
+ Assert.AreEqual(6.0, result.upExtent, 1e-6);
+ }
+
+ [TestMethod]
+ public void ZUp_HostZ90_ShouldKeepUpExtentUnchanged()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
+ Quaternion correction = adapter.CreateCanonicalRotationCorrection(
+ new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
+
+ var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
+ convention,
+ forwardSize: 6.0,
+ sideSize: 4.0,
+ upSize: 2.0,
+ correctionQuaternion: correction);
+
+ Assert.AreEqual(4.0, result.forwardExtent, 1e-6);
+ Assert.AreEqual(6.0, result.sideExtent, 1e-6);
+ Assert.AreEqual(2.0, result.upExtent, 1e-6);
+ }
+ }
+}
diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs
index 3650993..7e0fb20 100644
--- a/src/Core/Animation/PathAnimationManager.cs
+++ b/src/Core/Animation/PathAnimationManager.cs
@@ -695,7 +695,7 @@ namespace NavisworksTransport.Core.Animation
if (_route.PathType == PathType.Hoisting)
{
// 吊装路径：第一个路径点（起吊点）是地面位置，动画跟踪点统一使用几何中心
- startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
+ startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
LogManager.Debug($"[移动到起点] 吊装路径：起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
}
else if (_route.PathType == PathType.Rail)
@@ -730,7 +730,7 @@ namespace NavisworksTransport.Core.Animation
else
{
// 地面路径：路径点在接触面上，动画跟踪点统一使用几何中心
- startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
+ startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
}
@@ -956,7 +956,9 @@ namespace NavisworksTransport.Core.Animation
}
// 🔥 空中路径：根据路径类型和线段索引调整物体位置
- double objectHeight = GetAnimatedObjectHeight();
+ double objectHeight = _route.PathType == PathType.Rail
+ ? GetAnimatedObjectHeight()
+ : GetAnimatedObjectGroundContactHeight();
if (_route.PathType == PathType.Hoisting)
{
@@ -1038,11 +1040,11 @@ namespace NavisworksTransport.Core.Animation
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
}
- framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectHeight());
+ framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectGroundContactHeight());
yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
- previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectHeight());
- nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectHeight());
+ previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectGroundContactHeight());
+ nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectGroundContactHeight());
if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
i > 0 &&
@@ -1944,14 +1946,14 @@ namespace NavisworksTransport.Core.Animation
{
expectedTrackedEndPoint = ResolveHoistingTrackedCenter(
terminalAnchorPoint,
- GetAnimatedObjectHeight(),
+ GetAnimatedObjectGroundContactHeight(),
0.0);
}
else
{
expectedTrackedEndPoint = ResolveGroundTrackedCenter(
terminalAnchorPoint,
- GetAnimatedObjectHeight());
+ GetAnimatedObjectGroundContactHeight());
}
Vector3D forward = new Vector3D(
@@ -2462,7 +2464,7 @@ namespace NavisworksTransport.Core.Animation
{
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
currentPositionForTransform = actualHostPosition;
- LogManager.Debug(
+ LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
}
catch (Exception ex)
@@ -2471,15 +2473,15 @@ namespace NavisworksTransport.Core.Animation
}
var currentLinear = new Transform3D(currentRotation).Linear;
var targetLinear = new Transform3D(newRotation).Linear;
- LogManager.Debug(
+ LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
- LogManager.Debug(
+ LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " +
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
$"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " +
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
- LogManager.Debug(
+ LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " +
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
$"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " +
@@ -3299,6 +3301,56 @@ namespace NavisworksTransport.Core.Animation
return ModelItemTransformHelper.GetHostHeight(boundingBox, adapter);
}
+ ///
+ /// 获取当前姿态下，地面/吊装路径用于“接触点 -> 几何中心”换算的有效法线尺寸（模型单位）。
+ /// 与通行空间使用的旋转后法线尺寸保持一致，避免物体旋转后仍按原始高度抬升中心。
+ ///
+ private double GetAnimatedObjectGroundContactHeight()
+ {
+ double rawHeight = GetAnimatedObjectHeight();
+ if (_objectRotationCorrection.IsZero)
+ {
+ return rawHeight;
+ }
+
+ var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
+ ModelAxisConvention axisConvention;
+ double forwardSize;
+ double sideSize;
+ double upSize;
+
+ if (IsVirtualObjectMode)
+ {
+ axisConvention = ModelAxisConvention.CreateVirtualObjectAssetConvention();
+ forwardSize = _virtualObjectLength;
+ sideSize = _virtualObjectWidth;
+ upSize = _virtualObjectHeight;
+ }
+ else if (IsRealObjectMode && _realObjectLength > 0.0 && _realObjectWidth > 0.0 && _realObjectHeight > 0.0)
+ {
+ axisConvention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
+ forwardSize = _realObjectLength;
+ sideSize = _realObjectWidth;
+ upSize = _realObjectHeight;
+ }
+ else
+ {
+ return rawHeight;
+ }
+
+ Quaternion correctionQuaternion = IsVirtualObjectMode
+ ? adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
+ : adapter.CreateHostRotationCorrection(_objectRotationCorrection);
+ var projectedExtents = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
+ axisConvention,
+ forwardSize,
+ sideSize,
+ upSize,
+ correctionQuaternion);
+
+ return projectedExtents.upExtent;
+ }
+
///
/// 获取当前碰撞检测精度
///
@@ -3523,7 +3575,6 @@ namespace NavisworksTransport.Core.Animation
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
- Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
Vector3 forward = canonicalNext - canonicalPrevious;
if (forward.LengthSquared() < 1e-6f)
@@ -3531,6 +3582,25 @@ namespace NavisworksTransport.Core.Animation
forward = canonicalNext - canonicalCurrent;
}
+ if (IsRealObjectMode)
+ {
+ if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
+ forward,
+ HostCoordinateAdapter.CanonicalUpVector3,
+ convention,
+ out var baselineQuaternion))
+ {
+ return false;
+ }
+
+ Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
+ Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
+ Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
+ rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
+ return true;
+ }
+
+ Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
forward,
HostCoordinateAdapter.CanonicalUpVector3,
@@ -3552,6 +3622,24 @@ namespace NavisworksTransport.Core.Animation
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
var convention = GetCurrentModelAxisConvention();
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
+ if (IsRealObjectMode)
+ {
+ if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
+ canonicalForward,
+ HostCoordinateAdapter.CanonicalUpVector3,
+ convention,
+ out var baselineQuaternion))
+ {
+ return false;
+ }
+
+ Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
+ Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
+ Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
+ rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
+ return true;
+ }
+
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
canonicalForward,
diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
index ffd37f2..ac82b2c 100644
--- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
+++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
@@ -4350,8 +4350,10 @@ namespace NavisworksTransport.UI.WPF.ViewModels
return (0, 0, 0);
}
- Quaternion correctionQuaternion = CoordinateSystemManager.Instance.CreateHostAdapter()
- .CreateCanonicalRotationCorrection(_objectRotationCorrection);
+ var hostAdapter = CoordinateSystemManager.Instance.CreateHostAdapter();
+ Quaternion correctionQuaternion = UseVirtualObject
+ ? hostAdapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
+ : hostAdapter.CreateHostRotationCorrection(_objectRotationCorrection);
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
axisConvention,
forwardSize,
diff --git a/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs b/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs
index a7f36dc..bd6417e 100644
--- a/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs
+++ b/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs
@@ -103,5 +103,36 @@ namespace NavisworksTransport.Utils.CoordinateSystem
normalizedUp);
return true;
}
+
+ public static bool TryCreateQuaternionFromForwardWithWorldCorrection(
+ Vector3 desiredForward,
+ Vector3 canonicalUp,
+ ModelAxisConvention convention,
+ Quaternion worldCorrectionQuaternion,
+ out Quaternion rotation)
+ {
+ rotation = Quaternion.Identity;
+
+ if (!TryCreateQuaternionFromForward(
+ desiredForward,
+ canonicalUp,
+ convention,
+ out var baselineRotation))
+ {
+ return false;
+ }
+
+ if (Math.Abs(worldCorrectionQuaternion.X) < 1e-9f &&
+ Math.Abs(worldCorrectionQuaternion.Y) < 1e-9f &&
+ Math.Abs(worldCorrectionQuaternion.Z) < 1e-9f &&
+ Math.Abs(worldCorrectionQuaternion.W - 1.0f) < 1e-9f)
+ {
+ rotation = baselineRotation;
+ return true;
+ }
+
+ rotation = Quaternion.Normalize(worldCorrectionQuaternion * baselineRotation);
+ return true;
+ }
}
}
diff --git a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
index 8cd0146..7a97e95 100644
--- a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
+++ b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs
@@ -188,6 +188,34 @@ namespace NavisworksTransport.Utils.CoordinateSystem
hostLinear.M31, hostLinear.M32, hostLinear.M33);
}
+ public Rotation3D FromHostQuaternion(Quaternion hostRotation)
+ {
+ Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostRotation);
+ return CreateRotationFromLinearComponents(
+ hostLinear.M11, hostLinear.M12, hostLinear.M13,
+ hostLinear.M21, hostLinear.M22, hostLinear.M23,
+ hostLinear.M31, hostLinear.M32, hostLinear.M33);
+ }
+
+ public Quaternion CreateHostRotationCorrection(LocalEulerRotationCorrection correction)
+ {
+ if (correction.IsZero)
+ {
+ return Quaternion.Identity;
+ }
+
+ Quaternion qx = Quaternion.CreateFromAxisAngle(Vector3.UnitX, DegreesToRadians(correction.XDegrees));
+ Quaternion qy = Quaternion.CreateFromAxisAngle(Vector3.UnitY, DegreesToRadians(correction.YDegrees));
+ Quaternion qz = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, DegreesToRadians(correction.ZDegrees));
+ return Quaternion.Normalize(qz * qy * qx);
+ }
+
+ public Quaternion ComposeHostQuaternion(Quaternion baselineQuaternion, LocalEulerRotationCorrection correction)
+ {
+ Quaternion correctionQuaternion = CreateHostRotationCorrection(correction);
+ return Quaternion.Normalize(correctionQuaternion * baselineQuaternion);
+ }
+
///
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
/// 约定：先绕宿主 X，再绕宿主 Y，最后绕宿主 Z。
diff --git a/src/Utils/ModelItemTransformHelper.cs b/src/Utils/ModelItemTransformHelper.cs
index 1e6d59f..d76da64 100644
--- a/src/Utils/ModelItemTransformHelper.cs
+++ b/src/Utils/ModelItemTransformHelper.cs
@@ -540,24 +540,24 @@ namespace NavisworksTransport.Utils
targetPosition.Y - rotatedCurrentPosition.Y,
targetPosition.Z - rotatedCurrentPosition.Z);
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " +
$"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
$"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})");
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 当前旋转: " +
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
$"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " +
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 目标旋转: " +
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
$"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " +
$"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})");
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 增量旋转: " +
$"X=({deltaLinear.Get(0, 0):F4},{deltaLinear.Get(1, 0):F4},{deltaLinear.Get(2, 0):F4}), " +
$"Y=({deltaLinear.Get(0, 1):F4},{deltaLinear.Get(1, 1):F4},{deltaLinear.Get(2, 1):F4}), " +
@@ -600,13 +600,13 @@ namespace NavisworksTransport.Utils
var targetLinear = new Transform3D(targetRotation).Linear;
var actualPosition = actualBounds.Center;
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 立即读回位置(可能滞后): " +
$"实际=({actualPosition.X:F3},{actualPosition.Y:F3},{actualPosition.Z:F3}), " +
$"期望=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
$"偏差=({actualPosition.X - targetPosition.X:F3},{actualPosition.Y - targetPosition.Y:F3},{actualPosition.Z - targetPosition.Z:F3})");
- LogManager.Debug(
+ LogManager.Info(
$"[模型增量姿态] {item.DisplayName} 立即读回旋转(可能滞后/不反映override): " +
$"实际X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " +
$"实际Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " +