diff --git a/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs b/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
index 0220b1a..5d46eb2 100644
--- a/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
+++ b/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
@@ -219,5 +219,206 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
// 立方体任何 yaw 面积相同，只要返回有效四元数即可
Assert.AreEqual(1.0, Quaternion.Normalize(q).Length(), 1e-5);
}
+
+ // ----- ComposeHostCorrection：扣除 CAD yaw + pathYaw，避免重复旋转 -----
+
+ ///
+ /// PathAnimationManager 增量链语义闭环验证：
+ /// finalQ = qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) * cadQ
+ /// 应等于 overrideQ * cadQ（贴地时实际施加的最终姿态）
+ /// cadQ=Identity 时 currentYaw=0，简化为：
+ /// qup(pathYaw) * correctionHostQ == overrideQ
+ ///
+ private static void AssertIncrementalChainClosure(
+ Quaternion overrideQ,
+ Quaternion cadQ,
+ double pathYawRadians,
+ HostCoordinateAdapter adapter,
+ double tolerance = 1e-3)
+ {
+ LocalEulerRotationCorrection correction =
+ AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
+ overrideQ, cadQ, pathYawRadians, adapter);
+
+ Quaternion correctionHostQ = adapter.CreateHostRotationCorrection(correction);
+
+ // currentYaw 从 cadQ 提取（与 ComposeHostCorrection 内部一致）
+ Matrix4x4 cadLinear = Matrix4x4.CreateFromQuaternion(cadQ);
+ Vector3 cadFwdHost = new Vector3(cadLinear.M11, cadLinear.M21, cadLinear.M31);
+ Vector3 cadFwdCanon = adapter.ToCanonicalVector3(cadFwdHost);
+ cadFwdCanon.Z = 0f;
+ double currentYaw = cadFwdCanon.LengthSquared() > 1e-9f
+ ? Math.Atan2(Vector3.Normalize(cadFwdCanon).Y, Vector3.Normalize(cadFwdCanon).X)
+ : 0.0;
+
+ // 增量链：finalQ = qup(pathYaw - currentYaw) * correctionHostQ * cadQ
+ Quaternion deltaQ = Quaternion.CreateFromAxisAngle(
+ adapter.HostUpVector3, (float)(pathYawRadians - currentYaw));
+ Quaternion reconstructedFinal = Quaternion.Normalize(deltaQ * correctionHostQ * cadQ);
+ Quaternion expectedFinal = Quaternion.Normalize(overrideQ * cadQ);
+
+ for (int axis = 0; axis < 3; axis++)
+ {
+ Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
+ Vector3 expected = Vector3.Normalize(Vector3.Transform(v, expectedFinal));
+ Vector3 actual = Vector3.Normalize(Vector3.Transform(v, reconstructedFinal));
+ double dot = Vector3.Dot(expected, actual);
+ Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
+ $"axis {axis}: 增量链闭环偏差，dot={dot:F6}");
+ }
+ }
+
+ [TestMethod]
+ public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_ZUp()
+ {
+ // CAD=Identity(currentYaw=0), overrideQ=纯 pathYaw, correction 应为 0
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ double pathYaw = Deg(30.0);
+ Quaternion cadQ = Quaternion.Identity;
+ Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
+
+ LocalEulerRotationCorrection correction =
+ AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
+ overrideQ, cadQ, pathYaw, adapter);
+
+ Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
+ Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
+ Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
+ }
+
+ [TestMethod]
+ public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_YUp()
+ {
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ double pathYaw = Deg(-15.62);
+ Quaternion cadQ = Quaternion.Identity;
+ Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
+
+ LocalEulerRotationCorrection correction =
+ AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
+ overrideQ, cadQ, pathYaw, adapter);
+
+ Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
+ Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
+ Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
+ }
+
+ [TestMethod]
+ public void ComposeCorrection_CadIdentity_FaceDownPlusPathYaw_Closure_YUp()
+ {
+ // CAD=Identity, overrideQ = pathYawQ * faceDown
+ // 增量链闭环验证
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ double pathYaw = Deg(-15.62);
+ Quaternion cadQ = Quaternion.Identity;
+ Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(15.0));
+ Quaternion pathYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
+ Quaternion overrideQ = Quaternion.Normalize(pathYawQ * faceDown);
+
+ AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter);
+ }
+
+ [TestMethod]
+ public void ComposeCorrection_CadIdentity_SearchedYawPlusFaceDown_Closure_YUp()
+ {
+ // 模拟真实贴地：CAD=Identity, Search 出 overrideQ, 验证增量链闭环
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion cadQ = Quaternion.Identity;
+ Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(25.0));
+ Vector3 pathFwd = new Vector3(0.96f, 0.0f, -0.28f);
+
+ Quaternion overrideQ = AlignToGroundMinCrossSectionYawSearcher.Search(
+ faceDown, cadQ, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
+ hostPathForward: pathFwd, adapter: adapter);
+
+ PathTargetFrameResolver.TryResolvePlanarHostYaw(pathFwd, CoordinateSystemType.YUp, out double pathYaw);
+
+ AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
+ }
+
+ [TestMethod]
+ public void ComposeCorrection_SkewedCad_IncrementalChainClosure_YUp()
+ {
+ // CAD 姿态天然歪斜（带 yaw），验证增量链仍闭环
+ // 这是日志中 25"x25" 的场景：cadQ 自带 -124° yaw
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ // 构造 CAD 姿态：绕 host Y(up) 转 -124° + 一些倾斜
+ Quaternion cadQ = Quaternion.Normalize(
+ Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Deg(-124.0)) *
+ Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(10.0)));
+ double pathYaw = Deg(-15.62);
+ // overrideQ = searchedYawQ * faceDown（贴地搜索结果）
+ Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(20.0));
+ Quaternion searchedYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)Deg(-57.0));
+ Quaternion overrideQ = Quaternion.Normalize(searchedYawQ * faceDown);
+
+ AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
+ }
+
+ [TestMethod]
+ public void HostQuaternionToCanonical_HostUpRotation_MapsToCanonicalZRotation_YUp()
+ {
+ // YUp: host 绕 Y(up) 转 θ → canonical 绕 Z(up) 转 θ
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ double theta = Deg(37.0);
+ Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)theta);
+
+ Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);
+
+ // canonical 下应是绕 Z 转 θ
+ Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)theta);
+ AssertQuaternionsEqual(expected, canonQ);
+ }
+
+ [TestMethod]
+ public void HostQuaternionToCanonical_HostXRotation_MapsToCanonicalXRotation_YUp()
+ {
+ // YUp: host 绕 X(forward) 转 θ → canonical 绕 X(forward) 转 θ（X 轴两坐标系相同）
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ double theta = Deg(42.0);
+ Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);
+
+ Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);
+
+ Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);
+ AssertQuaternionsEqual(expected, canonQ);
+ }
+
+ [TestMethod]
+ public void HostQuaternionToCanonical_Identity_StaysIdentity_YUp()
+ {
+ // Identity 在两坐标系都应是 Identity（相似变换的不动点）
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
+ Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(
+ Quaternion.Identity, adapter);
+ AssertQuaternionsEqual(Quaternion.Identity, canonQ);
+ }
+
+ [TestMethod]
+ public void HostQuaternionToCanonical_ZUp_IsIdentity()
+ {
+ // ZUp: canonical=host，转换应返回原四元数
+ var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
+ Quaternion original = Quaternion.CreateFromAxisAngle(
+ new Vector3(0.3f, 0.5f, 0.8f), (float)Deg(55.0f));
+ original = Quaternion.Normalize(original);
+
+ Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(original, adapter);
+ AssertQuaternionsEqual(original, canonQ);
+ }
+
+ private static void AssertQuaternionsEqual(Quaternion expected, Quaternion actual, double tolerance = 1e-4)
+ {
+ // 四元数有双覆盖性（q 和 -q 表示同一旋转），比较作用后的轴
+ for (int axis = 0; axis < 3; axis++)
+ {
+ Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
+ Vector3 e = Vector3.Normalize(Vector3.Transform(v, expected));
+ Vector3 a = Vector3.Normalize(Vector3.Transform(v, actual));
+ double dot = Vector3.Dot(e, a);
+ Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
+ $"axis {axis}: expected≈actual 失败，dot={dot:F6}");
+ }
+ }
}
}
diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs
index 83e99a4..680fb3c 100644
--- a/src/Core/Animation/PathAnimationManager.cs
+++ b/src/Core/Animation/PathAnimationManager.cs
@@ -5613,6 +5613,62 @@ namespace NavisworksTransport.Core.Animation
LogManager.Debug($"[起点摆放] 直接设置地面路径上下偏移: {verticalLiftInMeters:F3}m");
}
+ ///
+ /// 给定 correction，计算重建后使物体底边贴合目标高度所需的 lift（米单位）。
+ /// 与自动调整的 ComputedLiftOffsetModelUnits 语义一致：
+ /// neededLift = targetBottom - objectBottomAfterRebuild
+ /// 实际应用 correction 重建后测量底边（与自动调整同方法）。
+ /// 测量后用新 correction + 新 lift 重建，使物体停留在正确的贴地等价姿态
+ /// （与确认按钮后的姿态一致），而不是恢复到贴地前的旧姿态。
+ ///
+ public double ComputeGroundLiftForCorrection(
+ LocalEulerRotationCorrection correction,
+ double targetLiftInMeters,
+ ModelItem item)
+ {
+ if (_route?.PathType != PathType.Ground || _pathPoints == null || _pathPoints.Count < 1 || item == null)
+ {
+ return targetLiftInMeters;
+ }
+
+ var hst = CoordinateSystemManager.Instance.ResolvedType;
+ var savedMode = _objectStartPlacementMode;
+ double computedLiftMeters = targetLiftInMeters;
+
+ try
+ {
+ // 用 correction + lift=0 重建，测量底边
+ var measureRequest = ObjectStartPlacementRequest.CreateRotationCorrection(correction, 0);
+ ApplyObjectStartPlacementRequest(measureRequest);
+ BoundingBox3D measureBounds = item.BoundingBox();
+ double objectBottomWorld = hst == CoordinateSystemType.YUp
+ ? measureBounds.Min.Y : measureBounds.Min.Z;
+ double pathStartHeight = hst == CoordinateSystemType.YUp
+ ? _pathPoints[0].Y : _pathPoints[0].Z;
+ double targetBottom = pathStartHeight
+ + UnitsConverter.ConvertFromMeters(targetLiftInMeters);
+ double neededLiftModelUnits = targetBottom - objectBottomWorld;
+ computedLiftMeters = Math.Round(UnitsConverter.ConvertToMeters(neededLiftModelUnits), 3);
+
+ LogManager.Debug(
+ $"[贴合地面] 计算上下偏移: 重建后底边={objectBottomWorld:F3}, 目标底边={targetBottom:F3}, " +
+ $"pathStart={pathStartHeight:F3}, neededLift={computedLiftMeters:F3}m");
+ }
+ finally
+ {
+ // 用新 correction + 新 lift 完整重建（归位 CAD → 增量链旋转+平移），
+ // 使物体停留在与确认按钮一致的贴地等价姿态。
+ // 必须用 skipCadRestore=false：skipCadRestore=true 会清零 correction，
+ // 只施加 yaw 增量而不施加 X/Z 旋转，导致姿态缺失俯仰/翻滚。
+ var applyRequest = ObjectStartPlacementRequest.CreateRotationCorrection(
+ correction, computedLiftMeters);
+ _objectStartPlacementMode = savedMode;
+ ApplyObjectStartPlacementRequest(applyRequest);
+ }
+
+ return computedLiftMeters;
+ }
+
public void SetObjectStartPlacementMode(ObjectStartPlacementMode placementMode)
{
_objectStartPlacementMode = placementMode;
diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
index bdd53a5..94a2303 100644
--- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
+++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
@@ -2303,6 +2303,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels
// （与"自动调整"同目标，但贴地已固定俯仰/翻滚，只剩 yaw 单自由度）
var routePoints = CurrentPathRoute?.Points;
double? searchedYawDegreesForLog = null;
+ double pathYawRadians = 0.0;
+ bool hasPathYaw = false;
if ((CurrentPathRoute?.PathType == PathType.Ground || CurrentPathRoute?.PathType == PathType.Hoisting)
&& routePoints != null && routePoints.Count >= 2)
{
@@ -2319,8 +2321,11 @@ namespace NavisworksTransport.UI.WPF.ViewModels
break;
}
}
- if (hostForward.LengthSquared() > 1e-8f)
+ if (hostForward.LengthSquared() > 1e-8f
+ && PathTargetFrameResolver.TryResolvePlanarHostYaw(
+ hostForward, CoordinateSystemManager.Instance.ResolvedType, out pathYawRadians))
{
+ hasPathYaw = true;
double metersToUnits = UnitsConverter.GetMetersToUnitsConversionFactor();
double sizeX, sizeY, sizeZ;
if (UseVirtualObject)
@@ -2372,11 +2377,39 @@ namespace NavisworksTransport.UI.WPF.ViewModels
hst == CoordinateSystemType.YUp ? ps.Z - newCenter.Z : groundLevel - objectBottom);
doc.Models.OverridePermanentTransform(modelItems, Transform3D.CreateTranslation(delta), false);
- var resultCorrection = ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(overrideQ);
+ // 把绝对贴地姿态转成"相对于 baseline 的 host 欧拉修正"。
+ // PathAnimationManager 增量链从 CAD 姿态出发，需要扣除 CAD 自带 yaw + pathYaw。
+ LocalEulerRotationCorrection resultCorrection;
+ if (hasPathYaw)
+ {
+ resultCorrection = AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
+ overrideQ, cadQ, pathYawRadians, adapter);
+ }
+ else
+ {
+ // 无路径 yaw（纯垂直路径或无路径）：直接转 overrideQ，
+ // 无 baseline 可扣除，correction 等价于绝对姿态。
+ Quaternion correctionCanonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(
+ overrideQ, adapter);
+ resultCorrection = ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
+ correctionCanonQ, adapter.HostType);
+ }
_pathAnimationManager?.SetObjectRotationCorrectionDirect(resultCorrection);
_objectRotationCorrection = resultCorrection;
OnPropertyChanged(nameof(ObjectRotationCorrection));
+ // 回写上下偏移：贴地时用实际包围盒贴地，但确认/动画后用 correction 重建姿态，
+ // 旋转后物体底边位置会变。与自动调整同方法：应用 correction 重建→测量底边→算 lift，
+ // 测量后恢复贴地姿态。
+ if (CurrentPathRoute?.PathType == PathType.Ground && CurrentPathRoute.Points?.Count > 0
+ && _pathAnimationManager != null)
+ {
+ double computedLift = _pathAnimationManager.ComputeGroundLiftForCorrection(
+ resultCorrection, _objectGroundLiftHeightInMeters, item);
+ _objectGroundLiftHeightInMeters = computedLift;
+ _pathAnimationManager.SetObjectStartVerticalLiftDirect(_objectGroundLiftHeightInMeters);
+ }
+
LogManager.Info(
$"[贴合地面] faceNormal=({faceNormal.X:F4},{faceNormal.Y:F4},{faceNormal.Z:F4}) " +
$"cadWorldN=({cadWorldN.X:F4},{cadWorldN.Y:F4},{cadWorldN.Z:F4}) t=({t.X:F4},{t.Y:F4},{t.Z:F4}) " +
diff --git a/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs b/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
index 1acdffb..bd77d21 100644
--- a/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
+++ b/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
@@ -127,5 +127,83 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return width * height;
}
+
+ ///
+ /// 把贴地搜索后的绝对宿主姿态(overrideQ)转成"相对于路径 baseline 的 host 欧拉修正"，
+ /// 与自动调整返回的 correction 语义一致，供动画系统在 baseline 之上叠加。
+ ///
+ /// 关键：PathAnimationManager 增量链从 CAD 姿态出发：
+ /// finalQ = qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) * cadQ
+ /// 贴地时 finalQ = overrideQ * cadQ，所以：
+ /// qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) = overrideQ
+ /// 而 CreateHostRotationCorrection(correction) = qup(YDeg) * qnonUp(ZDeg) * qx(XDeg)，所以：
+ /// qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) = overrideQ
+ /// => correctionHostQ = qup(currentYaw - pathYaw) * overrideQ
+ ///
+ /// currentYaw 是 CAD 姿态的 canonical yaw，从 cadQ 提取（与 GetYawFromRotation 一致）。
+ public static LocalEulerRotationCorrection ComposeHostCorrection(
+ Quaternion absoluteHostQ,
+ Quaternion cadRotation,
+ double pathYawRadians,
+ HostCoordinateAdapter adapter)
+ {
+ if (adapter == null)
+ {
+ throw new ArgumentNullException(nameof(adapter));
+ }
+
+ double currentYaw = ExtractCanonicalYaw(cadRotation, adapter);
+ Quaternion preRotation = Quaternion.CreateFromAxisAngle(
+ adapter.HostUpVector3, (float)(currentYaw - pathYawRadians));
+ Quaternion correctionHostQ = Quaternion.Normalize(preRotation * absoluteHostQ);
+
+ Quaternion correctionCanonQ = HostQuaternionToCanonical(correctionHostQ, adapter);
+ return ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
+ correctionCanonQ, adapter.HostType);
+ }
+
+ ///
+ /// 从宿主空间四元数提取 canonical yaw（Atan2(canonicalForward.Y, canonicalForward.X)）。
+ /// 与 ModelItemTransformHelper.GetYawFromRotation 一致：取 forward 轴(X列)，转 canonical，投影 XY 平面。
+ ///
+ private static double ExtractCanonicalYaw(Quaternion hostQ, HostCoordinateAdapter adapter)
+ {
+ Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
+ Vector3 hostForward = new Vector3(hostLinear.M11, hostLinear.M21, hostLinear.M31);
+ Vector3 canonFwd = adapter.ToCanonicalVector3(hostForward);
+ canonFwd.Z = 0f;
+ if (canonFwd.LengthSquared() < 1e-9f)
+ {
+ return 0.0;
+ }
+ canonFwd = Vector3.Normalize(canonFwd);
+ return Math.Atan2(canonFwd.Y, canonFwd.X);
+ }
+
+ ///
+ /// 宿主空间四元数 → canonical 空间四元数。
+ /// 旋转的坐标系转换必须用相似变换 R_canon = M⁻¹ · R_host · M
+ /// （M = canonical→host 坐标变换矩阵，正交故 M⁻¹ = Mᵀ）。
+ /// 不能用列向量映射——那只在"基向量定义"场景成立，会破坏 Identity 不变性。
+ ///
+ public static Quaternion HostQuaternionToCanonical(Quaternion hostQ, HostCoordinateAdapter adapter)
+ {
+ Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
+
+ // M: canonical→host 坐标变换（列 = canonical 基在 host 中的表示）
+ Vector3 mColX = adapter.FromCanonicalVector3(Vector3.UnitX);
+ Vector3 mColY = adapter.FromCanonicalVector3(Vector3.UnitY);
+ Vector3 mColZ = adapter.FromCanonicalVector3(Vector3.UnitZ);
+ Matrix4x4 m = new Matrix4x4(
+ mColX.X, mColY.X, mColZ.X, 0f,
+ mColX.Y, mColY.Y, mColZ.Y, 0f,
+ mColX.Z, mColY.Z, mColZ.Z, 0f,
+ 0f, 0f, 0f, 1f);
+ // M 正交，M⁻¹ = Mᵀ
+ Matrix4x4 mInv = Matrix4x4.Transpose(m);
+ // R_canon = M⁻¹ · R_host · M
+ Matrix4x4 canonLinear = Matrix4x4.Multiply(Matrix4x4.Multiply(mInv, hostLinear), m);
+ return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(canonLinear));
+ }
}
}
diff --git a/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs b/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs
index 29acaee..7a8d9b5 100644
--- a/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs
+++ b/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs
@@ -360,6 +360,15 @@ namespace NavisworksTransport.Utils.CoordinateSystem
}
public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
+ {
+ return CanonicalQuaternionToHostEulerCorrection(q, CoordinateSystemManager.Instance.ResolvedType);
+ }
+
+ ///
+ /// 将 canonical 空间的四元数分解为宿主欧拉修正 (X, Y=up, Z=nonUp)。
+ /// 接受显式 hostType 参数，避免依赖全局 CoordinateSystemManager 状态，便于单测。
+ ///
+ public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q, CoordinateSystemType hostType)
{
double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z;
@@ -375,8 +384,6 @@ namespace NavisworksTransport.Utils.CoordinateSystem
double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp);
- // Canonical (ZUp) → Host: 使用 CoordinateSystemManager 桥接
- var hostType = CoordinateSystemManager.Instance.ResolvedType;
if (hostType == CoordinateSystemType.ZUp)
{
return new LocalEulerRotationCorrection(