fix: 贴合地面后正确回写 correction 和 lift，确认/动画不再偏移
贴地后 overrideQ 是绝对旋转，但 _objectRotationCorrection 语义是 相对于 baseline(pathYaw) 的增量，且 PathAnimationManager 增量链从 CAD 姿态出发：finalQ = qup(pathYaw-currentYaw) * correctionHostQ * cadQ。 之前直接转 overrideQ 导致确认/动画时重复旋转。 核心改动： - ComposeHostCorrection 接受 cadQ，从中提取 currentYaw， correctionHostQ = qup(currentYaw - pathYaw) * overrideQ - HostQuaternionToCanonical 用相似变换 R_canon = M^-1 * R_host * M （非列向量映射，保证 Identity 不变性） - CanonicalQuaternionToHostEulerCorrection 加 hostType 参数重载， 去除全局 CoordinateSystemManager 依赖 - ComputeGroundLiftForCorrection：与自动调整同方法实测底边算 lift， finally 用新 correction+lift 完整重建（skipCadRestore=false）， 使物体停留在贴地等价姿态 - 新增 5 个增量链闭环单测（含 CAD 歪斜场景）
This commit is contained in:
parent
cf6b2ef959
commit
66e68c6bd1
@ -219,5 +219,206 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
// 立方体任何 yaw 面积相同，只要返回有效四元数即可
|
||||
Assert.AreEqual(1.0, Quaternion.Normalize(q).Length(), 1e-5);
|
||||
}
|
||||
|
||||
// ----- ComposeHostCorrection：扣除 CAD yaw + pathYaw，避免重复旋转 -----
|
||||
|
||||
/// <summary>
|
||||
/// PathAnimationManager 增量链语义闭环验证：
|
||||
/// finalQ = qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) * cadQ
|
||||
/// 应等于 overrideQ * cadQ（贴地时实际施加的最终姿态）
|
||||
/// cadQ=Identity 时 currentYaw=0，简化为：
|
||||
/// qup(pathYaw) * correctionHostQ == overrideQ
|
||||
/// </summary>
|
||||
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}");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -5613,6 +5613,62 @@ namespace NavisworksTransport.Core.Animation
|
||||
LogManager.Debug($"[起点摆放] 直接设置地面路径上下偏移: {verticalLiftInMeters:F3}m");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 给定 correction，计算重建后使物体底边贴合目标高度所需的 lift（米单位）。
|
||||
/// 与自动调整的 ComputedLiftOffsetModelUnits 语义一致：
|
||||
/// neededLift = targetBottom - objectBottomAfterRebuild
|
||||
/// 实际应用 correction 重建后测量底边（与自动调整同方法）。
|
||||
/// 测量后用新 correction + 新 lift 重建，使物体停留在正确的贴地等价姿态
|
||||
/// （与确认按钮后的姿态一致），而不是恢复到贴地前的旧姿态。
|
||||
/// </summary>
|
||||
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;
|
||||
|
||||
@ -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}) " +
|
||||
|
||||
@ -127,5 +127,83 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
|
||||
return width * height;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把贴地搜索后的绝对宿主姿态(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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从宿主空间四元数提取 canonical yaw（Atan2(canonicalForward.Y, canonicalForward.X)）。
|
||||
/// 与 ModelItemTransformHelper.GetYawFromRotation 一致：取 forward 轴(X列)，转 canonical，投影 XY 平面。
|
||||
/// </summary>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 宿主空间四元数 → canonical 空间四元数。
|
||||
/// 旋转的坐标系转换必须用相似变换 R_canon = M⁻¹ · R_host · M
|
||||
/// （M = canonical→host 坐标变换矩阵，正交故 M⁻¹ = Mᵀ）。
|
||||
/// 不能用列向量映射——那只在"基向量定义"场景成立，会破坏 Identity 不变性。
|
||||
/// </summary>
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -360,6 +360,15 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
}
|
||||
|
||||
public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
|
||||
{
|
||||
return CanonicalQuaternionToHostEulerCorrection(q, CoordinateSystemManager.Instance.ResolvedType);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 canonical 空间的四元数分解为宿主欧拉修正 (X, Y=up, Z=nonUp)。
|
||||
/// 接受显式 hostType 参数，避免依赖全局 CoordinateSystemManager 状态，便于单测。
|
||||
/// </summary>
|
||||
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(
|
||||
|
||||
Loading…
Reference in New Issue
Block a user