Unify planar collision pose replay
This commit is contained in:
parent
970d3465f4
commit
315fa46043
@ -132,6 +132,24 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
Assert.IsFalse(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Rail, true));
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ResolveAnimationFramePlaybackYawRadians_ShouldPreferPlanarHostYawWithHostCorrection()
|
||||
{
|
||||
var frame = new AnimationFrame
|
||||
{
|
||||
YawRadians = 0.25,
|
||||
PlanarHostYawRadians = 0.75,
|
||||
HasCustomRotation = true
|
||||
};
|
||||
|
||||
double resolved = PathAnimationManager.ResolveAnimationFramePlaybackYawRadians(
|
||||
frame,
|
||||
new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
|
||||
CoordinateSystemType.YUp);
|
||||
|
||||
Assert.AreEqual(0.75 + 20.0 * System.Math.PI / 180.0, resolved, 1e-9);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ApplyRecordedPlanarRealObjectFramePose_ShouldPreserveFullRotation_ForGroundCollisionLogCase()
|
||||
{
|
||||
|
||||
@ -759,7 +759,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"Y=({railLinearTransform.Get(0, 1):F4},{railLinearTransform.Get(1, 1):F4},{railLinearTransform.Get(2, 1):F4}), " +
|
||||
$"Z=({railLinearTransform.Get(0, 2):F4},{railLinearTransform.Get(1, 2):F4},{railLinearTransform.Get(2, 2):F4})");
|
||||
|
||||
UpdateObjectPosition(startPosition, railRotation);
|
||||
ApplyFullPoseUpdate(startPosition, railRotation);
|
||||
LogManager.Info("[移动到起点] Rail路径已应用完整三维旋转姿态");
|
||||
return true;
|
||||
}
|
||||
@ -821,7 +821,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
else if (_route.PathType != PathType.Rail &&
|
||||
TryCreatePlanarPathRotationAtStart(out var planarRotation))
|
||||
{
|
||||
UpdateObjectPosition(startPosition, planarRotation);
|
||||
ApplyFullPoseUpdate(startPosition, planarRotation);
|
||||
LogManager.Info("[移动到起点] 地面/吊装路径已应用完整三维姿态");
|
||||
|
||||
var startAppliedPoint = GetLiveBoundingBoxCenter(CurrentControlledObject ?? _animatedObject);
|
||||
@ -975,7 +975,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
startPosition = ApplyGroundPathObjectLiftOffset(startPosition);
|
||||
}
|
||||
|
||||
UpdateObjectPosition(startPosition);
|
||||
ApplyLegacyYawPoseUpdate(startPosition);
|
||||
SyncTrackedRotationToDisplayedPose(CurrentControlledObject ?? _animatedObject);
|
||||
_hasGroundRealObjectBasePose = false;
|
||||
|
||||
@ -1879,7 +1879,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
_pausedProgress = 0.0; // 重置暂停进度
|
||||
|
||||
// 初始化动画状态时不再强行覆盖 _currentYaw 为第一帧的 yaw
|
||||
// 这样在 UpdateObjectPosition(firstFrame.Position, firstFrame.YawRadians) 时
|
||||
// 这样在 ApplyLegacyYawPoseUpdate(firstFrame.Position, firstFrame.YawRadians) 时
|
||||
// deltaYaw = firstFrame.YawRadians - 物体初始Yaw，从而产生旋转增量让物体转向
|
||||
|
||||
if (_animationFrames != null && _animationFrames.Count > 0)
|
||||
@ -2685,7 +2685,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
/// <summary>
|
||||
/// 更新对象位置和朝向（支持绕物体中心旋转）
|
||||
/// </summary>
|
||||
private void UpdateObjectPosition(Point3D newPosition, double newYaw = double.NaN)
|
||||
private void ApplyLegacyYawPoseUpdate(Point3D newPosition, double newYaw = double.NaN)
|
||||
{
|
||||
try
|
||||
{
|
||||
@ -2741,7 +2741,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
// 纯平移
|
||||
incrementalTransform = Transform3D.CreateTranslation(deltaPos);
|
||||
LogManager.Debug($"[UpdateObjectPosition] 纯平移: ({deltaPos.X:F2},{deltaPos.Y:F2},{deltaPos.Z:F2})");
|
||||
LogManager.Debug($"[LegacyYawPose] 纯平移: ({deltaPos.X:F2},{deltaPos.Y:F2},{deltaPos.Z:F2})");
|
||||
}
|
||||
|
||||
// 应用增量变换（false = 增量模式）
|
||||
@ -2760,7 +2760,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
/// 使用完整三维姿态更新对象位置。
|
||||
/// 当前先用于 Rail 路径，避免影响现有地面/吊装路径的 yaw 流程。
|
||||
/// </summary>
|
||||
private void UpdateObjectPosition(Point3D newPosition, Rotation3D newRotation)
|
||||
private void ApplyFullPoseUpdate(Point3D newPosition, Rotation3D newRotation)
|
||||
{
|
||||
try
|
||||
{
|
||||
@ -3054,19 +3054,13 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
|
||||
}
|
||||
|
||||
if (_savedObjectHasCustomRotation)
|
||||
{
|
||||
UpdateObjectPosition(_savedObjectPosition, _savedObjectRotation);
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdateObjectPosition(_savedObjectPosition, _savedObjectYaw);
|
||||
}
|
||||
|
||||
_trackedPosition = _savedObjectPosition;
|
||||
_currentYaw = _savedObjectYaw;
|
||||
_trackedRotation = _savedObjectRotation;
|
||||
_hasTrackedRotation = _savedObjectHasCustomRotation;
|
||||
ApplyResolvedObjectPose(
|
||||
_savedObjectPosition,
|
||||
_savedObjectYaw,
|
||||
_savedObjectRotation,
|
||||
_savedObjectHasCustomRotation,
|
||||
frameIndex: -1,
|
||||
planarHostYawRadians: _savedObjectYaw);
|
||||
|
||||
LogHostActualPoseAxes(obj, "[PAM恢复后宿主姿态]", false);
|
||||
|
||||
@ -3132,14 +3126,13 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
LogHostActualPoseAxes(obj, "[动画姿态复用前宿主姿态]", false);
|
||||
|
||||
if (hasCustomRotation)
|
||||
{
|
||||
UpdateObjectPosition(targetPosition, targetRotation);
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdateObjectPosition(targetPosition, targetYawRadians);
|
||||
}
|
||||
ApplyResolvedObjectPose(
|
||||
targetPosition,
|
||||
targetYawRadians,
|
||||
targetRotation,
|
||||
hasCustomRotation,
|
||||
frameIndex: -1,
|
||||
planarHostYawRadians: targetYawRadians);
|
||||
|
||||
LogHostActualPoseAxes(obj, "[动画姿态复用后宿主姿态]", false);
|
||||
|
||||
@ -3251,16 +3244,9 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
_currentFrameIndex = frameIndex;
|
||||
|
||||
// 更新对象位置和朝向
|
||||
// 更新对象位置和朝向，复用动画主播放与恢复使用的同一入口。
|
||||
var frameData = _animationFrames[_currentFrameIndex];
|
||||
if (frameData.HasCustomRotation)
|
||||
{
|
||||
UpdateObjectPosition(frameData.Position, frameData.Rotation);
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdateObjectPosition(frameData.Position, ResolveAnimationFramePlaybackYawRadians(frameData));
|
||||
}
|
||||
ApplyAnimationFramePose(frameData, _currentFrameIndex);
|
||||
|
||||
// 更新碰撞高亮
|
||||
UpdateCollisionHighlightFromFrame();
|
||||
@ -5740,27 +5726,57 @@ namespace NavisworksTransport.Core.Animation
|
||||
return;
|
||||
}
|
||||
|
||||
ApplyResolvedObjectPose(
|
||||
frameData.Position,
|
||||
ResolveAnimationFramePlaybackYawRadians(frameData),
|
||||
frameData.Rotation,
|
||||
frameData.HasCustomRotation,
|
||||
frameIndex,
|
||||
frameData.PlanarHostYawRadians);
|
||||
}
|
||||
|
||||
private void ApplyResolvedObjectPose(
|
||||
Point3D targetPosition,
|
||||
double resolvedYawRadians,
|
||||
Rotation3D targetRotation,
|
||||
bool hasCustomRotation,
|
||||
int frameIndex,
|
||||
double? planarHostYawRadians = null)
|
||||
{
|
||||
if (ShouldUsePlanarRealObjectPureIncrementFrames(_route?.PathType ?? PathType.Ground, IsRealObjectMode) &&
|
||||
frameData.PlanarHostYawRadians.HasValue)
|
||||
planarHostYawRadians.HasValue)
|
||||
{
|
||||
ApplyPlanarAnimationFrame(frameData.Position, frameData.PlanarHostYawRadians.Value);
|
||||
ApplyPlanarAnimationFrame(targetPosition, planarHostYawRadians.Value);
|
||||
return;
|
||||
}
|
||||
else if (frameData.HasCustomRotation)
|
||||
|
||||
if (hasCustomRotation)
|
||||
{
|
||||
UpdateObjectPosition(frameData.Position, frameData.Rotation);
|
||||
ApplyFullPoseUpdate(targetPosition, targetRotation);
|
||||
return;
|
||||
}
|
||||
else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||||
|
||||
if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"平面路径播放帧 {frameIndex} 缺少完整姿态，禁止退回旧 yaw 链路。");
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdateObjectPosition(frameData.Position, ResolveAnimationFramePlaybackYawRadians(frameData));
|
||||
}
|
||||
|
||||
ApplyLegacyYawPoseUpdate(targetPosition, resolvedYawRadians);
|
||||
}
|
||||
|
||||
private double ResolveAnimationFramePlaybackYawRadians(AnimationFrame frameData)
|
||||
{
|
||||
return ResolveAnimationFramePlaybackYawRadians(
|
||||
frameData,
|
||||
_objectRotationCorrection,
|
||||
CoordinateSystemManager.Instance.ResolvedType);
|
||||
}
|
||||
|
||||
internal static double ResolveAnimationFramePlaybackYawRadians(
|
||||
AnimationFrame frameData,
|
||||
LocalEulerRotationCorrection rotationCorrection,
|
||||
CoordinateSystemType hostType)
|
||||
{
|
||||
if (frameData == null)
|
||||
{
|
||||
@ -5772,14 +5788,14 @@ namespace NavisworksTransport.Core.Animation
|
||||
return NormalizeRadians(
|
||||
frameData.PlanarHostYawRadians.Value +
|
||||
ResolvePlanarHostUpCorrectionRadians(
|
||||
_objectRotationCorrection,
|
||||
CoordinateSystemManager.Instance.ResolvedType));
|
||||
rotationCorrection,
|
||||
hostType));
|
||||
}
|
||||
|
||||
double resolvedYaw = frameData.YawRadians;
|
||||
if (!frameData.HasCustomRotation && !_objectRotationCorrection.IsZero)
|
||||
if (!frameData.HasCustomRotation && !rotationCorrection.IsZero)
|
||||
{
|
||||
resolvedYaw += _objectRotationCorrection.ZDegrees * Math.PI / 180.0;
|
||||
resolvedYaw += rotationCorrection.ZDegrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
return NormalizeRadians(resolvedYaw);
|
||||
|
||||
@ -804,7 +804,7 @@ namespace NavisworksTransport
|
||||
if (candidate.AnimatedObjectHasTrackedRotation && targetRotation != null)
|
||||
{
|
||||
LogManager.Info(
|
||||
$"[ClashDetective验证] 三维候选恢复: " +
|
||||
$"[ClashDetective验证] 已跟踪姿态候选恢复: " +
|
||||
$"对象={testAnimatedObject.DisplayName}, " +
|
||||
$"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3})");
|
||||
|
||||
@ -821,7 +821,7 @@ namespace NavisworksTransport
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"ClashDetective三维候选恢复失败：对象 {testAnimatedObject.DisplayName} 不受当前动画管理器控制。");
|
||||
$"ClashDetective已跟踪姿态候选恢复失败：对象 {testAnimatedObject.DisplayName} 不受当前动画管理器控制。");
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
Loading…
Reference in New Issue
Block a user