diff --git a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs index f416c13..97fe309 100644 --- a/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs +++ b/UnitTests/CoordinateSystem/RailPreservedPoseTests.cs @@ -92,10 +92,79 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem public void ShouldAllowFragmentPlanarFallback_ShouldDisableHoisting() { Assert.IsFalse(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Hoisting)); - Assert.IsTrue(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Ground)); + Assert.IsFalse(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Ground)); Assert.IsTrue(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Rail)); } + [TestMethod] + public void ShouldUseReferenceBasedRealObjectPlanarPose_ShouldDisableGroundAndHoisting() + { + Assert.IsFalse(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Ground)); + Assert.IsFalse(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Hoisting)); + Assert.IsTrue(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Rail)); + } + + [TestMethod] + public void TryResolveDisplayedPlanarYawFromRotation_ShouldResolveYUpFromDisplayedXAxis() + { + Rotation3D rotation = new Rotation3D( + 0.0, + System.Math.Sin(-System.Math.PI / 4.0), + 0.0, + System.Math.Cos(-System.Math.PI / 4.0)); + + bool ok = PathAnimationManager.TryResolveDisplayedPlanarYawFromRotation( + rotation, + CoordinateSystemType.YUp, + out double yawRadians); + + Assert.IsTrue(ok); + Assert.AreEqual(-System.Math.PI / 2.0, yawRadians, 1e-6); + } + + [TestMethod] + public void ShouldUseGroundRealObjectPureIncrementFrames_ShouldEnableOnlyForGroundRealObjects() + { + Assert.IsTrue(PathAnimationManager.ShouldUseGroundRealObjectPureIncrementFrames(PathType.Ground, true)); + Assert.IsFalse(PathAnimationManager.ShouldUseGroundRealObjectPureIncrementFrames(PathType.Ground, false)); + Assert.IsFalse(PathAnimationManager.ShouldUseGroundRealObjectPureIncrementFrames(PathType.Hoisting, true)); + Assert.IsFalse(PathAnimationManager.ShouldUseGroundRealObjectPureIncrementFrames(PathType.Rail, true)); + } + + [TestMethod] + public void TryResolveGroundHostPlanarYawFromFramePoints_ShouldResolveYUpHostYaw() + { + bool ok = PathAnimationManager.TryResolveGroundHostPlanarYawFromFramePoints( + new Point3D(0, 0, 0), + new Point3D(1, 0, 0), + new Point3D(1, 0, -1), + CoordinateSystemType.YUp, + out double yawRadians); + + Assert.IsTrue(ok); + Assert.AreEqual(-System.Math.PI / 4.0, yawRadians, 1e-6); + } + + [TestMethod] + public void ResolveGroundHostUpCorrectionRadians_ShouldUseYDegreesForYUp() + { + double radians = PathAnimationManager.ResolveGroundHostUpCorrectionRadians( + new LocalEulerRotationCorrection(10.0, 20.0, 30.0), + CoordinateSystemType.YUp); + + Assert.AreEqual(20.0 * System.Math.PI / 180.0, radians, 1e-9); + } + + [TestMethod] + public void ResolveGroundHostUpCorrectionRadians_ShouldUseZDegreesForZUp() + { + double radians = PathAnimationManager.ResolveGroundHostUpCorrectionRadians( + new LocalEulerRotationCorrection(10.0, 20.0, 30.0), + CoordinateSystemType.ZUp); + + Assert.AreEqual(30.0 * System.Math.PI / 180.0, radians, 1e-9); + } + [TestMethod] public void ResolveCurrentRotationBaseline_ShouldPreferActualGeometryForHoisting() { diff --git a/doc/working/2026-04-08-ground-remove-fragment-dependency-plan.md b/doc/working/2026-04-08-ground-remove-fragment-dependency-plan.md index e99339b..1dad3c6 100644 --- a/doc/working/2026-04-08-ground-remove-fragment-dependency-plan.md +++ b/doc/working/2026-04-08-ground-remove-fragment-dependency-plan.md @@ -34,7 +34,15 @@ 2. `Ground` 这条链不应该再扩散 `local/reference/fragment` 概念。 3. `fragment` 现在的问题,不在于“所有地方都要立刻删”,而在于: - Ground 主链还会读它 - - 导致姿态来源不稳定 + - 导致旋转来源不稳定 +4. `Ground` 这条链仍然有旋转目标,但这个旋转目标不再来自 `fragment` 参考姿态解释。 +5. `Ground` 后续只需要求: + - 路径在宿主平面里的方向 + - 对应的宿主平面旋转量 +6. 也就是说,`Ground` 不再求“reference-based pose”,而只求: + - `hostForward` + - `hostUp` + - `host-planar rotation delta` --- @@ -43,7 +51,7 @@ 这轮只保留 3 个具体目标: 1. `Ground` 初始化时,不再优先读 fragment 参考姿态 -2. `Ground` 平面姿态求解时,不再走 fragment 参考旋转入口 +2. `Ground` 平面姿态求解时,不再走 fragment 参考旋转入口,而只求宿主平面旋转量 3. `Ground` 不再允许 fragment planar fallback 只要这 3 点做到,就算这一轮完成。 @@ -63,6 +71,7 @@ - 当 `PathType == Ground` 且是真实物体时 - 不再去走 `TryCaptureRealObjectReferenceRotation(...)` - 直接改用当前实际几何姿态,或现有非 fragment 入口 +- 这一步的目的不是继续建立另一套“参考姿态定义”,而只是拿到当前增量起点 ### 4.2 平面姿态求解入口 @@ -74,7 +83,11 @@ 要求: - `Ground` 不再从这里拿 fragment 参考旋转 -- `Ground` 单独走非 fragment 的姿态来源 +- `Ground` 不再继续求 reference-based pose +- `Ground` 只求宿主平面旋转量: + - 路径 `hostForward` + - 宿主 `hostUp` + - 当前对象在宿主坐标系下要追加的平面旋转量 ### 4.3 fallback 入口 @@ -94,7 +107,8 @@ 1. 先改 `Ground` 初始化入口 2. 再改 `Ground` 平面姿态求解入口 -3. 最后关掉 `Ground` 的 fragment fallback +3. 再改 `Ground` 尺寸/通行空间入口,避免它们继续偷偷走 `reference-based pose` +4. 最后关掉 `Ground` 的 fragment fallback 每一步都要求: @@ -102,6 +116,67 @@ - 只改 `Ground` - 不顺手改别的路径 +### 5.1 2026-04-07 夜间分析结论 + +第一刀已经证明: + +- `Ground` 初始化入口可以先切掉 fragment +- 但这还不够 + +这次日志暴露出的真正问题是: + +- `Ground` 虽然不再直接读 fragment +- 但起点姿态求解仍然在走 `TryCreateRealObjectPlanarPoseSolution(...)` +- 也就是仍然在走 `reference-based pose` 入口 + +所以第二刀必须明确成: + +1. `Ground` 不再进入 `TryCreateRealObjectPlanarPoseSolution(...)` +2. `Ground` 起点旋转改成: + - 当前显示旋转 + - 当前宿主平面 yaw + - 路径目标宿主平面 yaw + - 基于这三者直接求目标旋转 +3. 也就是 `Ground` 只保留: + - 当前显示状态作为增量起点 + - 路径方向作为目标方向 + - 宿主平面旋转量作为唯一旋转语义 + +### 5.2 现有链路与目标链路 + +当前代码里,`Ground + 真实物体` 至少有 3 个入口会接触姿态: + +1. 起点 + - `MoveObjectToPathStart(...)` + - `TryCreatePlanarPathRotationAtStart(...)` + - `TryCreateRealObjectPlanarRotationFromHostForward(...)` +2. 逐帧 + - `ApplyGroundAnimationFrame(...)` +3. 尺寸/通行空间 + - `TryCalculateCurrentRealObjectPlanarProjectedExtents(...)` + +这一轮的目标链路应该统一成: + +1. 初始化 + - 当前显示姿态只用来拿“当前增量起点” + - 不再把它包装成 `referenceRotation` +2. 起点/逐帧旋转 + - 只求 `hostForward` + - 只求宿主平面旋转量 + - 不再进入 `TryCreateRealObjectPlanarPoseSolution(...)` + - 起点应用层不再走 `currentRotation -> targetRotation -> deltaRotation` + - 改成直接施加宿主轴旋转增量,再补平移把 tracked point 拉回目标点 +3. 尺寸/通行空间 + - 对 `Ground` 直接复用固定业务约定: + - `forward = PositiveX` + - `up = HostUp` + - 不再通过 `reference-based pose` 推导 `ModelAxisConvention` + +这 3 条链必须保持同一件事: + +- `Ground` 不再解释 reference pose +- `Ground` 只解释“当前显示状态 + 路径方向 + 宿主平面旋转量” + --- ## 6. 验证标准 @@ -112,9 +187,14 @@ - `Ground + 真实物体` 到起点后不再读 fragment 姿态 2. 逐帧 - - `Ground` 播放时姿态来源不再依赖 fragment + - `Ground` 播放时旋转来源不再依赖 fragment + - `Ground` 只根据路径方向继续求宿主平面旋转量 -3. fallback +3. 尺寸/通行空间 + - `Ground` 的 projected extents 不再因为 fragment/reference pose 缺失而失败 + - `Ground` 的 `ModelAxisConvention` 不再来自 `TryCreateRealObjectPlanarPoseSolution(...)` + +4. fallback - `Ground` 关闭 fragment fallback 后,主链要么成功,要么明确报错 - 不允许再偷偷回退 @@ -134,3 +214,41 @@ - 统一三类路径的所有姿态入口 这些都属于下一轮的事。 + +--- + +## 8. 2026-04-08 夜间新增:Ground 角度调整的最小实现策略 + +在 `Ground + 真实物体` 这条新纯增量链上,角度调整不再通过“完整目标姿态重建”实现,而只允许走最单纯的宿主增量法: + +1. `up` 轴修正 + - 不单独构造三维姿态 + - 直接并入路径目标 `yaw` + - `YUp` 取 `YDegrees` + - `ZUp` 取 `ZDegrees` + +2. 非 `up` 轴修正 + - 不与 `yaw` 一次性组合 + - 只在起点落位后,围绕业务跟踪点按宿主轴逐次增量应用 + - 当前约定: + - `YUp`:应用 `XDegrees`、`ZDegrees` + - `ZUp`:应用 `XDegrees`、`YDegrees` + +3. 逐帧播放 + - 继续只吃“路径宿主平面角 + up 轴修正” + - 不在每帧重复叠加非 `up` 轴修正 + - 非 `up` 轴修正应由起点姿态一次性建立,并在后续 `yaw` 增量中自然保持 + +### 明早优先验证 + +1. `Ground + 真实物体` 到起点 + - 位置是否继续保持正确 + - `X/Y/Z` 角度调整是否真正进入增量旋转链 + +2. `Ground` 逐帧播放 + - 转弯是否仍保持当前已修好的效果 + - `up` 轴修正是否能跟随路径持续生效 + +3. 非 `up` 轴修正 + - 是否表现为“起点一次性按宿主轴旋转” + - 播放过程中不应每帧累加放大 diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index 8cc6b3a..65cc77d 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -77,6 +77,7 @@ namespace NavisworksTransport.Core.Animation public double Progress { get; set; } // 进度(0-1) public Point3D Position { get; set; } // 该帧的位置 public double YawRadians { get; set; } // 绕Z轴的偏航角(弧度) + public double? GroundHostPlanarYawRadians { get; set; } // Ground真实物体逐帧宿主平面角 public Rotation3D Rotation { get; set; } // 可选的完整三维姿态 public bool HasCustomRotation { get; set; } // 是否使用完整三维姿态 public List Collisions { get; set; } // 该帧的碰撞结果 @@ -86,6 +87,7 @@ namespace NavisworksTransport.Core.Animation { Collisions = new List(); YawRadians = 0.0; + GroundHostPlanarYawRadians = null; Rotation = Rotation3D.Identity; HasCustomRotation = false; } @@ -738,8 +740,9 @@ namespace NavisworksTransport.Core.Animation LogManager.Debug($"[移动到起点] 地面/空轨路径使用路径方向: {yaw * 180 / Math.PI:F2}度"); } - // 旧 yaw 链仅保留给未进入完整三维姿态的遗留路径。 - if (!_objectRotationCorrection.IsZero) + // 旧 yaw 链仅保留给未进入 Ground 纯增量主链的遗留路径。 + if (!_objectRotationCorrection.IsZero && + !(_route.PathType == PathType.Ground && IsRealObjectMode)) { double correctionRad = _objectRotationCorrection.ZDegrees * Math.PI / 180.0; yaw += correctionRad; @@ -802,7 +805,41 @@ namespace NavisworksTransport.Core.Animation $"[路径起点诊断] 路径point0=({pathStartPoint.X:F3},{pathStartPoint.Y:F3},{pathStartPoint.Z:F3}), " + $"起点目标trackedPoint=({startPosition.X:F3},{startPosition.Y:F3},{startPosition.Z:F3}), 路径类型={_route.PathType.GetDisplayName()}"); - if (_route.PathType != PathType.Rail && + if (IsRealObjectMode && + _route.PathType == PathType.Ground && + TryApplyGroundStartIncrementalTransform(startPosition, out double groundTargetYawRadians)) + { + LogManager.Info("[移动到起点] Ground 真实物体已应用纯增量旋转+平移"); + + var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject); + LogManager.Info( + $"[路径起点诊断] 起点应用后实际包围盒中心=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), " + + $"相对目标偏差=({startAppliedPoint.X - startPosition.X:F3},{startAppliedPoint.Y - startPosition.Y:F3},{startAppliedPoint.Z - startPosition.Z:F3})"); + + var startOffset = new Vector3D( + startPosition.X - startAppliedPoint.X, + startPosition.Y - startAppliedPoint.Y, + startPosition.Z - startAppliedPoint.Z); + double startOffsetLength = Math.Sqrt( + startOffset.X * startOffset.X + + startOffset.Y * startOffset.Y + + startOffset.Z * startOffset.Z); + + LogManager.Info( + $"[路径起点诊断] 落位偏差=({startOffset.X:F3},{startOffset.Y:F3},{startOffset.Z:F3}), 长度={startOffsetLength:F3} (仅记录,不二次补偿)"); + + _groundRealObjectBaseRotation = _trackedRotation; + _groundRealObjectBaseYaw = groundTargetYawRadians; + _hasGroundRealObjectBasePose = true; + _groundRealObjectStartCompensation = new Vector3D(0, 0, 0); + _hasGroundRealObjectStartCompensation = false; + _hasHoistingRealObjectBasePose = false; + + LogManager.Info( + $"[Ground真实物体基姿态] {animatedObject.DisplayName} BaseYaw={_groundRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " + + $"已记录基姿态={_hasGroundRealObjectBasePose} (纯增量链)"); + } + else if (_route.PathType != PathType.Rail && TryCreatePlanarPathRotationAtStart(out var planarRotation)) { UpdateObjectPosition(startPosition, planarRotation); @@ -1336,7 +1373,23 @@ namespace NavisworksTransport.Core.Animation Collisions = new List() }; - if (ShouldPreservePathRotationForFrames( + if (ShouldUseGroundRealObjectPureIncrementFrames(_route.PathType, IsRealObjectMode)) + { + if (!TryResolveGroundHostPlanarYawFromFramePoints( + previousFramePoint, + framePosition, + nextFramePoint, + CoordinateSystemManager.Instance.ResolvedType, + out double groundHostPlanarYawRadians)) + { + throw new InvalidOperationException( + $"Ground真实物体第 {i} 帧未能解析宿主平面目标角。"); + } + + frame.GroundHostPlanarYawRadians = groundHostPlanarYawRadians; + frame.YawRadians = groundHostPlanarYawRadians; + } + else if (ShouldPreservePathRotationForFrames( _route.PathType, _objectStartPlacementMode, _hasPathPreservedPoseRotation)) @@ -3947,7 +4000,76 @@ namespace NavisworksTransport.Core.Animation internal static bool ShouldAllowFragmentPlanarFallback(PathType pathType) { - return pathType != PathType.Hoisting; + return pathType != PathType.Hoisting && pathType != PathType.Ground; + } + + internal static bool ShouldUseReferenceBasedRealObjectPlanarPose(PathType pathType) + { + return pathType != PathType.Ground && pathType != PathType.Hoisting; + } + + internal static bool ShouldUseGroundRealObjectPureIncrementFrames(PathType pathType, bool isRealObjectMode) + { + return pathType == PathType.Ground && isRealObjectMode; + } + + internal static bool TryResolveDisplayedPlanarYawFromRotation( + Rotation3D rotation, + CoordinateSystemType hostType, + out double yawRadians) + { + yawRadians = 0.0; + if (rotation == null) + { + return false; + } + + Matrix3 linear = new Transform3D(rotation).Linear; + Vector3 hostForward = new Vector3( + (float)linear.Get(0, 0), + (float)linear.Get(1, 0), + (float)linear.Get(2, 0)); + + return PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians); + } + + internal static double ResolveGroundHostUpCorrectionRadians( + LocalEulerRotationCorrection correction, + CoordinateSystemType hostType) + { + double degrees = hostType == CoordinateSystemType.YUp + ? correction.YDegrees + : correction.ZDegrees; + return degrees * Math.PI / 180.0; + } + + internal static bool TryResolveGroundHostPlanarYawFromFramePoints( + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + CoordinateSystemType hostType, + out double yawRadians) + { + yawRadians = 0.0; + + Vector3 hostForward = new Vector3( + (float)(nextPoint.X - previousPoint.X), + (float)(nextPoint.Y - previousPoint.Y), + (float)(nextPoint.Z - previousPoint.Z)); + if (hostForward.LengthSquared() < 1e-6f) + { + hostForward = new Vector3( + (float)(nextPoint.X - currentPoint.X), + (float)(nextPoint.Y - currentPoint.Y), + (float)(nextPoint.Z - currentPoint.Z)); + } + + if (hostForward.LengthSquared() < 1e-6f) + { + return false; + } + + return PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians); } internal static Rotation3D ResolveCurrentRotationBaseline( @@ -4312,6 +4434,134 @@ namespace NavisworksTransport.Core.Animation return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(_route.PathType, hostPoints, hostType, out yawRadians); } + private bool TryApplyGroundStartIncrementalTransform(Point3D targetTrackedPosition, out double targetYawRadians) + { + targetYawRadians = 0.0; + + if (!IsRealObjectMode || + _route?.PathType != PathType.Ground || + CurrentControlledObject == null || + _pathPoints == null || + _pathPoints.Count < 2) + { + return false; + } + + if (!PathTargetFrameResolver.TryResolvePlanarStartHostForward( + PathType.Ground, + _pathPoints, + out var hostForward)) + { + LogManager.Error("[Ground纯增量起点] 无法解析路径起始方向。"); + return false; + } + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, adapter.HostType, out targetYawRadians)) + { + LogManager.Error("[Ground纯增量起点] 无法从路径方向解析宿主平面角。"); + return false; + } + + ApplyGroundNonUpAxisCorrectionsAtTrackedPosition(_trackedPosition, adapter.HostType); + + Rotation3D currentRotation = Rotation3D.Identity; + if (!ModelItemTransformHelper.TryGetCurrentGeometryRotation(CurrentControlledObject, out currentRotation)) + { + if (_hasTrackedRotation) + { + currentRotation = _trackedRotation; + } + else + { + LogManager.Error("[Ground纯增量起点] 无法读取当前显示旋转。"); + return false; + } + } + + if (!TryResolveDisplayedPlanarYawFromRotation(currentRotation, adapter.HostType, out double currentYawRadians)) + { + LogManager.Error("[Ground纯增量起点] 无法从当前显示状态解析宿主平面角。"); + return false; + } + + double upAxisCorrectionRadians = ResolveGroundHostUpCorrectionRadians( + _objectRotationCorrection, + adapter.HostType); + targetYawRadians = NormalizeRadians(targetYawRadians + upAxisCorrectionRadians); + double deltaYawRadians = NormalizeRadians(targetYawRadians - currentYawRadians); + LogManager.Info( + $"[Ground纯增量起点] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, " + + $"目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, 增量Yaw={deltaYawRadians * 180.0 / Math.PI:F2}°, " + + $"当前跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " + + $"目标跟踪点=({targetTrackedPosition.X:F3},{targetTrackedPosition.Y:F3},{targetTrackedPosition.Z:F3})"); + + ModelItemTransformHelper.MoveItemIncrementallyByAxisRotationAndTranslation( + CurrentControlledObject, + _trackedPosition, + adapter.HostUpVector3, + deltaYawRadians, + targetTrackedPosition); + + _trackedPosition = targetTrackedPosition; + + if (ModelItemTransformHelper.TryGetCurrentGeometryRotation(CurrentControlledObject, out var appliedRotation)) + { + _trackedRotation = appliedRotation; + _hasTrackedRotation = true; + } + else + { + _trackedRotation = currentRotation; + _hasTrackedRotation = true; + } + + _currentYaw = targetYawRadians; + LogHostActualPoseAxes(CurrentControlledObject, "[Ground纯增量起点应用后宿主姿态]", false); + return true; + } + + private void ApplyGroundNonUpAxisCorrectionsAtTrackedPosition( + Point3D trackedPosition, + CoordinateSystemType hostType) + { + if (CurrentControlledObject == null || _objectRotationCorrection.IsZero) + { + return; + } + + var correctionSteps = new List<(string AxisName, Vector3 HostAxis, double AngleRadians)>(); + correctionSteps.Add(("X", Vector3.UnitX, _objectRotationCorrection.XDegrees * Math.PI / 180.0)); + + if (hostType == CoordinateSystemType.YUp) + { + correctionSteps.Add(("Z", Vector3.UnitZ, _objectRotationCorrection.ZDegrees * Math.PI / 180.0)); + } + else + { + correctionSteps.Add(("Y", Vector3.UnitY, _objectRotationCorrection.YDegrees * Math.PI / 180.0)); + } + + foreach (var step in correctionSteps) + { + if (Math.Abs(step.AngleRadians) < 1e-9) + { + continue; + } + + LogManager.Info( + $"[Ground纯增量起点角度修正] 宿主{step.AxisName}轴增量={step.AngleRadians * 180.0 / Math.PI:F2}°, " + + $"跟踪点=({trackedPosition.X:F3},{trackedPosition.Y:F3},{trackedPosition.Z:F3})"); + + ModelItemTransformHelper.MoveItemIncrementallyByAxisRotationAndTranslation( + CurrentControlledObject, + trackedPosition, + step.HostAxis, + step.AngleRadians, + trackedPosition); + } + } + private static double NormalizeRadians(double angle) { while (angle > Math.PI) @@ -4509,7 +4759,7 @@ namespace NavisworksTransport.Core.Animation convention = null; extents = (0.0, 0.0, 0.0); - bool requiresReferenceRotation = _route?.PathType != PathType.Hoisting; + bool requiresReferenceRotation = ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground); if (!IsRealObjectMode || (requiresReferenceRotation && !_hasRealObjectReferenceRotation) || _realObjectLength <= 0.0 || @@ -4526,31 +4776,22 @@ namespace NavisworksTransport.Core.Animation } if (_route?.PathType == PathType.Ground && - _hasGroundRealObjectBasePose && - TryCreateGroundRealObjectConstrainedRotationFromHostForward(hostForward, out var constrainedRotation)) + TryCreateRealObjectPlanarRotationFromHostForward(hostForward, out var groundRotation)) { - if (!TryCreateRealObjectPlanarPoseSolution( - targetFrame.Forward, - targetFrame.Up, - out var constrainedSolution)) - { - return false; - } - - LocalAxisDirection constrainedSemanticUpAxis = + LocalAxisDirection groundSemanticUpAxis = adapter.HostType == CoordinateSystemType.YUp ? LocalAxisDirection.PositiveY : LocalAxisDirection.PositiveZ; - convention = new ModelAxisConvention(constrainedSolution.SelectedReferenceAxisDirection, constrainedSemanticUpAxis); + convention = new ModelAxisConvention(LocalAxisDirection.PositiveX, groundSemanticUpAxis); - Quaternion constrainedFinalHostQuaternion = Rotation3DToHostQuaternion(constrainedRotation); + Quaternion groundFinalHostQuaternion = Rotation3DToHostQuaternion(groundRotation); extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents( convention, _realObjectLength, _realObjectWidth, _realObjectHeight, - constrainedSolution.BaselineRotation, - constrainedFinalHostQuaternion, + groundFinalHostQuaternion, + groundFinalHostQuaternion, targetFrame.Forward, targetFrame.Up); return true; @@ -4961,6 +5202,54 @@ namespace NavisworksTransport.Core.Animation return false; } + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + if (_route?.PathType == PathType.Ground) + { + Rotation3D actualRotation = Rotation3D.Identity; + bool hasActualRotation = + _animatedObject != null && + ModelItemTransformHelper.TryGetCurrentGeometryRotation(_animatedObject, out actualRotation); + + if (!hasActualRotation && _hasTrackedRotation) + { + actualRotation = _trackedRotation; + hasActualRotation = true; + } + + if (!hasActualRotation) + { + LogManager.Error("[真实物体起点姿态] Ground 无法读取当前实际几何姿态,禁止回退到 reference-based pose。"); + return false; + } + + if (!TryResolveDisplayedPlanarYawFromRotation(actualRotation, adapter.HostType, out double currentYawRadians)) + { + LogManager.Error("[真实物体起点姿态] Ground 无法从当前显示姿态解析宿主平面角。"); + return false; + } + + if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(hostForward, adapter.HostType, out double targetYawRadians)) + { + LogManager.Error("[真实物体起点姿态] Ground 无法从路径方向解析宿主平面角。"); + return false; + } + + Quaternion targetQuaternion = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose( + Rotation3DToHostQuaternion(actualRotation), + currentYawRadians, + targetYawRadians, + adapter.HostUpVector3); + rotation = adapter.FromHostQuaternionDirect(targetQuaternion); + + Matrix4x4 targetLinear = Matrix4x4.CreateFromQuaternion(targetQuaternion); + LogManager.Debug( + $"[Ground宿主平面旋转] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, 目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, " + + $"hostXAxis=({targetLinear.M11:F4},{targetLinear.M21:F4},{targetLinear.M31:F4}), " + + $"hostYAxis=({targetLinear.M12:F4},{targetLinear.M22:F4},{targetLinear.M32:F4}), " + + $"hostZAxis=({targetLinear.M13:F4},{targetLinear.M23:F4},{targetLinear.M33:F4})"); + return true; + } + if (!TryCreateRealObjectPlanarPoseSolution( targetFrame.Forward, targetFrame.Up, @@ -4969,7 +5258,6 @@ namespace NavisworksTransport.Core.Animation return false; } - var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection(); Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, localCorrection); rotation = adapter.FromHostQuaternionDirect(hostComposedQuaternion); @@ -5015,6 +5303,11 @@ namespace NavisworksTransport.Core.Animation { solution = null; + if (!ShouldUseReferenceBasedRealObjectPlanarPose(_route?.PathType ?? PathType.Ground)) + { + return false; + } + if (!TryGetRealObjectReferenceRotation(out var referenceRotation)) { return false; @@ -5121,7 +5414,8 @@ namespace NavisworksTransport.Core.Animation try { - if (TryCaptureRealObjectReferenceRotation(_animatedObject, out referenceRotation)) + if (_route?.PathType != PathType.Ground && + TryCaptureRealObjectReferenceRotation(_animatedObject, out referenceRotation)) { return true; } @@ -5198,7 +5492,12 @@ namespace NavisworksTransport.Core.Animation try { - Rotation3D fallbackRotation3D = sourceObject.Transform?.Factor().Rotation ?? Rotation3D.Identity; + Rotation3D fallbackRotation3D = Rotation3D.Identity; + if (!ModelItemTransformHelper.TryGetCurrentGeometryRotation(sourceObject, out fallbackRotation3D)) + { + fallbackRotation3D = sourceObject.Transform?.Factor().Rotation ?? Rotation3D.Identity; + } + referenceRotation = new Quaternion( (float)fallbackRotation3D.A, (float)fallbackRotation3D.B, @@ -5240,7 +5539,7 @@ namespace NavisworksTransport.Core.Animation _hasRealObjectReferenceRotation = true; LogManager.Warning( - $"[真实物体参考姿态] {sourceObject.DisplayName} fragment 姿态不可用,已退回默认参考姿态继续业务流程: " + + $"[真实物体参考姿态] {sourceObject.DisplayName} 已改用非 fragment 参考姿态继续业务流程: " + $"FallbackX=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " + $"FallbackY=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " + $"FallbackZ=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " + @@ -5279,6 +5578,23 @@ namespace NavisworksTransport.Core.Animation return; } + if (!isVirtualObject && + _route?.PathType == PathType.Ground && + ModelItemTransformHelper.TryGetCurrentGeometryRotation(sourceObject, out var groundActualGeometryRotation)) + { + _trackedRotation = groundActualGeometryRotation; + _hasTrackedRotation = true; + _currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation); + + var linear = new Transform3D(_trackedRotation).Linear; + LogManager.Info( + $"[真实物体当前姿态] {sourceObject.DisplayName} Ground 路径已改用实际几何姿态同步,不再读取 fragment 参考姿态: " + + $"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " + + $"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " + + $"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})"); + return; + } + if (!isVirtualObject && TryCaptureRealObjectReferenceRotation(sourceObject, out Quaternion referenceRotation)) { var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); @@ -5574,17 +5890,14 @@ namespace NavisworksTransport.Core.Animation double actualYaw = frameData.YawRadians; - if (frameData.HasCustomRotation) + if (ShouldUseGroundRealObjectPureIncrementFrames(_route?.PathType ?? PathType.Ground, IsRealObjectMode) && + frameData.GroundHostPlanarYawRadians.HasValue) { - // 🔥 Ground路径动画播放:使用 _trackedPosition 计算增量,不重新读取包围盒 - if (_route?.PathType == PathType.Ground && IsRealObjectMode) - { - ApplyGroundAnimationFrame(frameData.Position, frameData.Rotation); - } - else - { - UpdateObjectPosition(frameData.Position, frameData.Rotation); - } + ApplyGroundAnimationFrame(frameData.Position, frameData.GroundHostPlanarYawRadians.Value); + } + else if (frameData.HasCustomRotation) + { + UpdateObjectPosition(frameData.Position, frameData.Rotation); } else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) { @@ -5600,28 +5913,41 @@ namespace NavisworksTransport.Core.Animation /// /// 专门用于Ground路径动画帧播放。使用业务跟踪点 _trackedPosition 作为当前位置,不读取实时包围盒中心回写主链。 /// - private void ApplyGroundAnimationFrame(Point3D targetPosition, Rotation3D targetRotation) + private void ApplyGroundAnimationFrame(Point3D targetPosition, double targetYawRadians) { var controlledObject = CurrentControlledObject; if (controlledObject == null) return; - // 使用业务跟踪点 _trackedPosition 作为当前位置,不把实时包围盒中心混入主链 Point3D currentPosition = _trackedPosition; - Rotation3D currentRotation = _hasTrackedRotation ? _trackedRotation : Rotation3D.Identity; + double currentYawRadians = _currentYaw; + targetYawRadians = NormalizeRadians( + targetYawRadians + + ResolveGroundHostUpCorrectionRadians( + _objectRotationCorrection, + CoordinateSystemManager.Instance.ResolvedType)); + double deltaYawRadians = NormalizeRadians(targetYawRadians - currentYawRadians); + Vector3 hostUp = Vector3.Normalize(CoordinateSystemManager.Instance.CreateHostAdapter().HostUpVector3); - // 简单增量变换,不使用包围盒 - ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation( + LogManager.Debug( + $"[Ground纯增量逐帧] 当前Yaw={currentYawRadians * 180.0 / Math.PI:F2}°, " + + $"目标Yaw={targetYawRadians * 180.0 / Math.PI:F2}°, 增量Yaw={deltaYawRadians * 180.0 / Math.PI:F2}°, " + + $"当前跟踪点=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " + + $"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3})"); + + ModelItemTransformHelper.MoveItemIncrementallyByAxisRotationAndTranslation( controlledObject, currentPosition, - currentRotation, - targetPosition, - targetRotation); + hostUp, + deltaYawRadians, + targetPosition); - // 更新跟踪状态 _trackedPosition = targetPosition; - _trackedRotation = targetRotation; - _hasTrackedRotation = true; - _currentYaw = ModelItemTransformHelper.GetYawFromRotation(targetRotation); + if (ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out Rotation3D appliedRotation)) + { + _trackedRotation = appliedRotation; + _hasTrackedRotation = true; + } + _currentYaw = targetYawRadians; } diff --git a/src/Utils/ModelItemTransformHelper.cs b/src/Utils/ModelItemTransformHelper.cs index 52f858c..eb2f8d6 100644 --- a/src/Utils/ModelItemTransformHelper.cs +++ b/src/Utils/ModelItemTransformHelper.cs @@ -1,6 +1,7 @@ using System; using Autodesk.Navisworks.Api; using NavisworksTransport.Utils.CoordinateSystem; +using System.Numerics; namespace NavisworksTransport.Utils { @@ -591,6 +592,74 @@ namespace NavisworksTransport.Utils LogIncrementalTransformActual(item, targetPosition, targetRotation); } + /// + /// 直接对当前显示结果施加宿主轴旋转增量,再补一段平移把业务跟踪点拉回目标点。 + /// 不先构造 targetRotation,也不从 current/target 姿态反推 deltaRotation。 + /// + public static void MoveItemIncrementallyByAxisRotationAndTranslation( + ModelItem item, + Point3D currentPosition, + Vector3 hostAxis, + double deltaAngleRadians, + Point3D targetPosition) + { + if (item == null) + { + throw new ArgumentNullException(nameof(item)); + } + + if (hostAxis.LengthSquared() < 1e-12f) + { + throw new ArgumentException("hostAxis must be non-zero.", nameof(hostAxis)); + } + + Vector3 normalizedHostAxis = Vector3.Normalize(hostAxis); + Rotation3D deltaRotation = new Rotation3D( + new UnitVector3D(normalizedHostAxis.X, normalizedHostAxis.Y, normalizedHostAxis.Z), + deltaAngleRadians); + Matrix3 deltaLinear = new Transform3D(deltaRotation).Linear; + Point3D rotatedCurrentPosition = new Point3D( + deltaLinear.Get(0, 0) * currentPosition.X + deltaLinear.Get(0, 1) * currentPosition.Y + deltaLinear.Get(0, 2) * currentPosition.Z, + deltaLinear.Get(1, 0) * currentPosition.X + deltaLinear.Get(1, 1) * currentPosition.Y + deltaLinear.Get(1, 2) * currentPosition.Z, + deltaLinear.Get(2, 0) * currentPosition.X + deltaLinear.Get(2, 1) * currentPosition.Y + deltaLinear.Get(2, 2) * currentPosition.Z); + + Vector3D compensatedTranslation = new Vector3D( + targetPosition.X - rotatedCurrentPosition.X, + targetPosition.Y - rotatedCurrentPosition.Y, + targetPosition.Z - rotatedCurrentPosition.Z); + + LogManager.Debug( + $"[模型纯增量旋转平移] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " + + $"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " + + $"Axis=({normalizedHostAxis.X:F4},{normalizedHostAxis.Y:F4},{normalizedHostAxis.Z:F4}), " + + $"Angle={deltaAngleRadians * 180.0 / Math.PI:F2}°, " + + $"旋后当前点=({rotatedCurrentPosition.X:F3},{rotatedCurrentPosition.Y:F3},{rotatedCurrentPosition.Z:F3}), " + + $"平移=({compensatedTranslation.X:F3},{compensatedTranslation.Y:F3},{compensatedTranslation.Z:F3})"); + + var doc = Application.ActiveDocument; + var modelItems = new ModelItemCollection { item }; + LogGeometryLevelTransforms(item, "[模型纯增量旋转平移应用前][GeometryAPI]"); + + var toOrigin = Transform3D.CreateTranslation(new Vector3D( + -currentPosition.X, + -currentPosition.Y, + -currentPosition.Z)); + doc.Models.OverridePermanentTransform(modelItems, toOrigin, false); + + var rotationOnlyComponents = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)).Factor(); + rotationOnlyComponents.Rotation = deltaRotation; + var rotationOnly = rotationOnlyComponents.Combine(); + doc.Models.OverridePermanentTransform(modelItems, rotationOnly, false); + + var toTarget = Transform3D.CreateTranslation(new Vector3D( + targetPosition.X, + targetPosition.Y, + targetPosition.Z)); + doc.Models.OverridePermanentTransform(modelItems, toTarget, false); + + LogGeometryLevelTransforms(item, "[模型纯增量旋转平移应用后][GeometryAPI]"); + } + /// /// 读取物体当前实际几何姿态。 /// 优先使用 ModelGeometry.ActiveTransform,因为 ModelItem.Transform 只反映原始设计变换。