Stabilize hoisting pose adjustment flow
This commit is contained in:
parent
cb56737041
commit
d4c49fc227
@ -63,11 +63,54 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void CreateRotationFromPlanarBasePose_ShouldReturnBaseRotation_WhenYawUnchanged()
|
||||
{
|
||||
Quaternion baseRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.31f) *
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.42f));
|
||||
|
||||
Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
|
||||
baseRotation,
|
||||
baseYawRadians: 1.25,
|
||||
targetYawRadians: 1.25,
|
||||
hostUp: Vector3.UnitY);
|
||||
|
||||
AssertQuaternionEquivalent(resultRotation, baseRotation);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void CreateRotationFromPlanarBasePose_ShouldPreserveTiltAndApplyDeltaYaw()
|
||||
{
|
||||
Quaternion baseRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitY, 0.20f) *
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.35f) *
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.40f));
|
||||
|
||||
Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
|
||||
baseRotation,
|
||||
baseYawRadians: 0.0,
|
||||
targetYawRadians: System.Math.PI / 2.0,
|
||||
hostUp: Vector3.UnitY);
|
||||
|
||||
Quaternion expectedRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)(System.Math.PI / 2.0)) *
|
||||
baseRotation);
|
||||
|
||||
AssertQuaternionEquivalent(resultRotation, expectedRotation);
|
||||
}
|
||||
|
||||
private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
|
||||
{
|
||||
Assert.AreEqual(x, actual.X, tolerance);
|
||||
Assert.AreEqual(y, actual.Y, tolerance);
|
||||
Assert.AreEqual(z, actual.Z, tolerance);
|
||||
}
|
||||
|
||||
private static void AssertQuaternionEquivalent(Quaternion actual, Quaternion expected, double tolerance = 1e-6)
|
||||
{
|
||||
float dot = Quaternion.Dot(Quaternion.Normalize(actual), Quaternion.Normalize(expected));
|
||||
Assert.AreEqual(1.0, System.Math.Abs(dot), tolerance);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -195,6 +195,9 @@ namespace NavisworksTransport.Core.Animation
|
||||
private Rotation3D _groundRealObjectBaseRotation = Rotation3D.Identity;
|
||||
private double _groundRealObjectBaseYaw = 0.0;
|
||||
private bool _hasGroundRealObjectBasePose = false;
|
||||
private Rotation3D _hoistingRealObjectBaseRotation = Rotation3D.Identity;
|
||||
private double _hoistingRealObjectBaseYaw = 0.0;
|
||||
private bool _hasHoistingRealObjectBasePose = false;
|
||||
private Vector3D _groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
|
||||
private bool _hasGroundRealObjectStartCompensation = false;
|
||||
private bool _suppressGroundRealObjectCompensation = false;
|
||||
@ -844,16 +847,25 @@ namespace NavisworksTransport.Core.Animation
|
||||
if (IsRealObjectMode && _route.PathType == PathType.Ground)
|
||||
{
|
||||
_groundRealObjectBaseRotation = planarRotation;
|
||||
_hasGroundRealObjectBasePose = TryResolveGroundRealObjectBaseYaw(out _groundRealObjectBaseYaw);
|
||||
_hasGroundRealObjectBasePose = TryResolvePlanarRealObjectBaseYaw(PathType.Ground, out _groundRealObjectBaseYaw);
|
||||
LogManager.Info(
|
||||
$"[Ground真实物体基姿态] {animatedObject.DisplayName} BaseYaw={_groundRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " +
|
||||
$"已记录基姿态={_hasGroundRealObjectBasePose}, " +
|
||||
$"起点补偿=({_groundRealObjectStartCompensation.X:F3},{_groundRealObjectStartCompensation.Y:F3},{_groundRealObjectStartCompensation.Z:F3}), " +
|
||||
$"已启用补偿={_hasGroundRealObjectStartCompensation}");
|
||||
}
|
||||
else if (IsRealObjectMode && _route.PathType == PathType.Hoisting)
|
||||
{
|
||||
_hoistingRealObjectBaseRotation = planarRotation;
|
||||
_hasHoistingRealObjectBasePose = TryResolvePlanarRealObjectBaseYaw(PathType.Hoisting, out _hoistingRealObjectBaseYaw);
|
||||
LogManager.Debug(
|
||||
$"[Hoisting真实物体基姿态] {(animatedObject ?? _animatedObject)?.DisplayName} BaseYaw={_hoistingRealObjectBaseYaw * 180.0 / Math.PI:F2}°, " +
|
||||
$"已记录基姿态={_hasHoistingRealObjectBasePose}, 姿态将复用于预览/生成/播放");
|
||||
}
|
||||
else
|
||||
{
|
||||
_hasGroundRealObjectBasePose = false;
|
||||
_hasHoistingRealObjectBasePose = false;
|
||||
_groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
|
||||
_hasGroundRealObjectStartCompensation = false;
|
||||
}
|
||||
@ -978,6 +990,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
UpdateObjectPosition(startPosition);
|
||||
SyncTrackedRotationToDisplayedPose(CurrentControlledObject ?? _animatedObject);
|
||||
_hasGroundRealObjectBasePose = false;
|
||||
_hasHoistingRealObjectBasePose = false;
|
||||
_groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
|
||||
_hasGroundRealObjectStartCompensation = false;
|
||||
|
||||
@ -3990,10 +4003,10 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"来源={(hasActualGeometryRotation ? "实际几何姿态" : "跟踪姿态")}");
|
||||
}
|
||||
|
||||
private bool TryResolveGroundRealObjectBaseYaw(out double yawRadians)
|
||||
private bool TryResolvePlanarRealObjectBaseYaw(PathType pathType, out double yawRadians)
|
||||
{
|
||||
yawRadians = 0.0;
|
||||
if (_route?.PathType != PathType.Ground ||
|
||||
if (_route?.PathType != pathType ||
|
||||
!IsRealObjectMode ||
|
||||
_pathPoints == null ||
|
||||
_pathPoints.Count < 2)
|
||||
@ -4008,7 +4021,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
hostPoints.Add(new Vector3((float)_pathPoints[i].X, (float)_pathPoints[i].Y, (float)_pathPoints[i].Z));
|
||||
}
|
||||
|
||||
return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(_route.PathType, hostPoints, hostType, out yawRadians);
|
||||
return PathTargetFrameResolver.TryResolvePlanarStartHostYaw(pathType, hostPoints, hostType, out yawRadians);
|
||||
}
|
||||
|
||||
private bool TryCreateGroundRealObjectConstrainedRotation(
|
||||
@ -4092,6 +4105,45 @@ namespace NavisworksTransport.Core.Animation
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool TryCreateHoistingRealObjectConstrainedRotationFromHostForward(
|
||||
Vector3 hostForward,
|
||||
out Rotation3D rotation)
|
||||
{
|
||||
rotation = Rotation3D.Identity;
|
||||
if (!IsRealObjectMode ||
|
||||
_route?.PathType != PathType.Hoisting ||
|
||||
!_hasHoistingRealObjectBasePose)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
|
||||
hostForward,
|
||||
CoordinateSystemManager.Instance.ResolvedType,
|
||||
out var currentFrame))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
||||
currentFrame.Forward,
|
||||
CoordinateSystemManager.Instance.ResolvedType,
|
||||
out double targetYawRadians))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_hoistingRealObjectBaseRotation);
|
||||
Quaternion targetQuaternion = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
|
||||
baseQuaternion,
|
||||
_hoistingRealObjectBaseYaw,
|
||||
targetYawRadians,
|
||||
Vector3.Normalize(adapter.HostUpVector3));
|
||||
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static Quaternion Rotation3DToHostQuaternion(Rotation3D rotation)
|
||||
{
|
||||
var linear = new Transform3D(rotation).Linear;
|
||||
@ -4681,6 +4733,13 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
rotation = Rotation3D.Identity;
|
||||
|
||||
if (_route?.PathType == PathType.Hoisting &&
|
||||
_hasHoistingRealObjectBasePose &&
|
||||
TryCreateHoistingRealObjectConstrainedRotationFromHostForward(hostForward, out rotation))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_route?.PathType == PathType.Hoisting &&
|
||||
TryCreateHoistingRealObjectRotationFromActualPose(hostForward, out rotation))
|
||||
{
|
||||
@ -4711,7 +4770,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation);
|
||||
Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(hostComposedQuaternion);
|
||||
|
||||
LogManager.Info(
|
||||
LogManager.Debug(
|
||||
$"[真实物体起点姿态] 选中参考轴=({solution.SelectedReferenceAxisLocal.X:F3},{solution.SelectedReferenceAxisLocal.Y:F3},{solution.SelectedReferenceAxisLocal.Z:F3}), " +
|
||||
$"投影前进=({solution.ProjectedForward.X:F3},{solution.ProjectedForward.Y:F3},{solution.ProjectedForward.Z:F3}), " +
|
||||
$"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " +
|
||||
@ -4767,9 +4826,9 @@ namespace NavisworksTransport.Core.Animation
|
||||
_realObjectPlanarSelectedForwardAxis = selectedAxisDirection;
|
||||
_hasRealObjectPlanarSelectedForwardAxis = true;
|
||||
|
||||
LogManager.Info(
|
||||
LogManager.Debug(
|
||||
$"[真实物体平面前进轴] Hoisting 已改用实际几何姿态基线，选中对象轴={selectedAxisDirection}。");
|
||||
LogManager.Info(
|
||||
LogManager.Debug(
|
||||
$"[真实物体起点姿态] 选中参考轴=({selectedAxisLocal.X:F3},{selectedAxisLocal.Y:F3},{selectedAxisLocal.Z:F3}), " +
|
||||
$"投影前进=({projectedForward.X:F3},{projectedForward.Y:F3},{projectedForward.Z:F3}), " +
|
||||
$"宿主修正={_objectRotationCorrection}, 本地修正=X=0.0°,Y=0.0°,Z=0.0°, " +
|
||||
@ -4817,7 +4876,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
_realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||||
_hasRealObjectPlanarSelectedForwardAxis = true;
|
||||
LogManager.Info("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。");
|
||||
LogManager.Debug("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。");
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -5097,6 +5156,16 @@ namespace NavisworksTransport.Core.Animation
|
||||
_hasRealObjectPlanarSelectedForwardAxis = false;
|
||||
}
|
||||
|
||||
private void ResetPlanarRealObjectBasePoseCache()
|
||||
{
|
||||
_groundRealObjectBaseRotation = Rotation3D.Identity;
|
||||
_groundRealObjectBaseYaw = 0.0;
|
||||
_hasGroundRealObjectBasePose = false;
|
||||
_hoistingRealObjectBaseRotation = Rotation3D.Identity;
|
||||
_hoistingRealObjectBaseYaw = 0.0;
|
||||
_hasHoistingRealObjectBasePose = false;
|
||||
}
|
||||
|
||||
private LocalEulerRotationCorrection ResolveRealObjectLocalRotationCorrection()
|
||||
{
|
||||
return HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||||
@ -5114,6 +5183,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
_objectStartPlacementMode = ObjectStartPlacementMode.AlignToPathPose;
|
||||
_hasRailPreservedPoseRotation = false;
|
||||
_objectRotationCorrection = rotationCorrection;
|
||||
ResetPlanarRealObjectBasePoseCache();
|
||||
|
||||
// 如果动画已创建，更新物体到起点的朝向
|
||||
if (_animatedObject != null && _pathPoints != null && _pathPoints.Count > 0)
|
||||
@ -5145,6 +5215,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
public void SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection rotationCorrection)
|
||||
{
|
||||
_objectRotationCorrection = rotationCorrection;
|
||||
ResetPlanarRealObjectBasePoseCache();
|
||||
LogManager.Debug($"[角度修正] 直接设置角度修正值: {_objectRotationCorrection}（不触发旋转）");
|
||||
}
|
||||
|
||||
|
||||
@ -1675,32 +1675,34 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
_pathAnimationManager?.SetRealObjectDimensions(objectLength, objectWidth, objectHeight);
|
||||
LogManager.Debug($"[选择物体] 保存原始尺寸: 长度={_objectOriginalLength:F2}m, 宽度={_objectOriginalWidth:F2}m, 高度={_objectOriginalHeight:F2}m");
|
||||
|
||||
// 3. 先注册到动画管理器，再设置 SelectedAnimatedObject。
|
||||
// SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()；
|
||||
// 如果先移动、后 SetAnimatedObject，会把“已摆到起点后的当前姿态”再次当成参考姿态缓存。
|
||||
_pathAnimationManager?.SetAnimatedObject(newObject);
|
||||
SelectedAnimatedObject = newObject;
|
||||
LogManager.Info($"已选择移动物体: {SelectedAnimatedObject.DisplayName}");
|
||||
|
||||
// 选择实体物体意味着切换到实体模式，避免后续起点同步仍走虚拟物体分支。
|
||||
// 3. 选择实体物体意味着切换到实体模式，避免后续起点同步仍走虚拟物体分支。
|
||||
if (UseVirtualObject)
|
||||
{
|
||||
UseVirtualObject = false;
|
||||
LogManager.Debug("[选择物体] 已切换到实体物体模式");
|
||||
}
|
||||
|
||||
// 只有选择不同的物体时，才重置角度修正值
|
||||
// 4. 对“新物体”先清空角度修正，再触发 SelectedAnimatedObject 的起点落位。
|
||||
// SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()，
|
||||
// 如果角度在后面才归零，就会先按旧角度把新选择的物体摆到起点。
|
||||
if (!isSameObject)
|
||||
{
|
||||
// 重置 ViewModel 中的角度修正值（会自动同步到 PathAnimationManager）
|
||||
_pathAnimationManager?.SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection.Zero);
|
||||
ObjectRotationCorrection = LocalEulerRotationCorrection.Zero;
|
||||
LogManager.Debug("[选择物体] 已重置角度修正值为0（新物体）");
|
||||
}
|
||||
else
|
||||
{
|
||||
LogManager.Debug($"[选择物体] 保持当前角度修正值（同一物体）");
|
||||
LogManager.Debug("[选择物体] 保持当前角度修正值（同一物体）");
|
||||
}
|
||||
|
||||
// 5. 先注册到动画管理器，再设置 SelectedAnimatedObject。
|
||||
// SelectedAnimatedObject 的 setter 会立即触发 MoveAnimatedObjectToPathStart()；
|
||||
// 如果先移动、后 SetAnimatedObject，会把“已摆到起点后的当前姿态”再次当成参考姿态缓存。
|
||||
_pathAnimationManager?.SetAnimatedObject(newObject);
|
||||
SelectedAnimatedObject = newObject;
|
||||
LogManager.Info($"已选择移动物体: {SelectedAnimatedObject.DisplayName}");
|
||||
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
@ -2121,6 +2123,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
// 1. 归位并清理
|
||||
_pathAnimationManager.RestoreObjectToCADPosition();
|
||||
_pathAnimationManager.ClearAnimationResults();
|
||||
_pathAnimationManager.SetObjectRotationCorrectionDirect(LocalEulerRotationCorrection.Zero);
|
||||
_pathAnimationManager.SetObjectStartPlacementMode(ObjectStartPlacementMode.AlignToPathPose);
|
||||
LogManager.Info("已清除PathAnimationManager中的动画数据并归位物体");
|
||||
}
|
||||
|
||||
|
||||
@ -5,6 +5,23 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
public static class HoistingRealObjectPoseHelper
|
||||
{
|
||||
public static Quaternion CreateRotationFromPlanarBasePose(
|
||||
Quaternion baseRotation,
|
||||
double baseYawRadians,
|
||||
double targetYawRadians,
|
||||
Vector3 hostUp)
|
||||
{
|
||||
Vector3 normalizedHostUp = NormalizeSafe(hostUp);
|
||||
if (normalizedHostUp.LengthSquared() < 1e-6f)
|
||||
{
|
||||
return Quaternion.Normalize(baseRotation);
|
||||
}
|
||||
|
||||
float deltaYawRadians = NormalizeRadians(targetYawRadians - baseYawRadians);
|
||||
Quaternion deltaRotation = Quaternion.CreateFromAxisAngle(normalizedHostUp, deltaYawRadians);
|
||||
return Quaternion.Normalize(deltaRotation * baseRotation);
|
||||
}
|
||||
|
||||
public static bool TryCreateRotationFromActualPose(
|
||||
Quaternion actualRotation,
|
||||
Vector3 targetForward,
|
||||
@ -131,5 +148,20 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
|
||||
return angle * sign;
|
||||
}
|
||||
|
||||
private static float NormalizeRadians(double angleRadians)
|
||||
{
|
||||
while (angleRadians > Math.PI)
|
||||
{
|
||||
angleRadians -= 2.0 * Math.PI;
|
||||
}
|
||||
|
||||
while (angleRadians < -Math.PI)
|
||||
{
|
||||
angleRadians += 2.0 * Math.PI;
|
||||
}
|
||||
|
||||
return (float)angleRadians;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user