Map real-object angle adjustments from host axes
This commit is contained in:
parent
115d70db66
commit
7851e6affa
@ -362,6 +362,34 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void RemapHostSemanticCorrectionToLocalAxes_ShouldPermuteAnglesFromSemanticAxes()
|
||||
{
|
||||
LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||||
new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
|
||||
LocalAxisDirection.PositiveY,
|
||||
LocalAxisDirection.PositiveZ,
|
||||
LocalAxisDirection.PositiveX);
|
||||
|
||||
Assert.AreEqual(30.0, mapped.XDegrees, 1e-9);
|
||||
Assert.AreEqual(10.0, mapped.YDegrees, 1e-9);
|
||||
Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void RemapHostSemanticCorrectionToLocalAxes_ShouldApplySignForNegativeMappedAxes()
|
||||
{
|
||||
LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||||
new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
|
||||
LocalAxisDirection.NegativeY,
|
||||
LocalAxisDirection.PositiveZ,
|
||||
LocalAxisDirection.NegativeX);
|
||||
|
||||
Assert.AreEqual(-30.0, mapped.XDegrees, 1e-9);
|
||||
Assert.AreEqual(-10.0, mapped.YDegrees, 1e-9);
|
||||
Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9);
|
||||
}
|
||||
|
||||
private static void AssertPoint(Vector3 point, double x, double y, double z)
|
||||
{
|
||||
Assert.AreEqual(x, point.X, 1e-9);
|
||||
|
||||
@ -26,9 +26,45 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
out RealObjectReferencePose pose);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(pose.RawAxisX, 1.0, 0.0, 0.0);
|
||||
AssertVector(pose.RawAxisY, 0.0, 1.0, 0.0);
|
||||
AssertVector(pose.RawAxisZ, 0.0, 0.0, 1.0);
|
||||
AssertVector(pose.AxisX, 1.0, 0.0, 0.0);
|
||||
AssertVector(pose.AxisY, 0.0, 0.0, 1.0);
|
||||
AssertVector(pose.AxisZ, 0.0, -1.0, 0.0);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldXAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldYAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldZAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostUpLocalAxis);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void YUp_WorldAxisMapping_ShouldFollowRawRepresentativeFrame()
|
||||
{
|
||||
var fragmentMatrices = new List<double[]>
|
||||
{
|
||||
CreateMatrix(
|
||||
axisX: new Vector3(0.0f, 0.0f, 1.0f),
|
||||
axisY: new Vector3(1.0f, 0.0f, 0.0f),
|
||||
axisZ: new Vector3(0.0f, 1.0f, 0.0f))
|
||||
};
|
||||
|
||||
bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
|
||||
fragmentMatrices,
|
||||
fragmentDefaultUpAxis: "Z",
|
||||
hostUpAxis: "Y",
|
||||
out RealObjectReferencePose pose);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldXAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldYAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldZAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis);
|
||||
Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
|
||||
@ -115,6 +115,13 @@ namespace NavisworksTransport.Core.Animation
|
||||
private Vector3 _realObjectReferenceAxisX = Vector3.UnitX;
|
||||
private Vector3 _realObjectReferenceAxisY = Vector3.UnitY;
|
||||
private Vector3 _realObjectReferenceAxisZ = Vector3.UnitZ;
|
||||
private LocalAxisDirection _realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
private LocalAxisDirection _realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
private LocalAxisDirection _realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
private LocalAxisDirection _realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
private LocalAxisDirection _realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
private LocalAxisDirection _realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
private LocalAxisDirection _realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY;
|
||||
private bool _hasRealObjectReferenceRotation = false;
|
||||
private LocalAxisDirection _realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||||
private bool _hasRealObjectPlanarSelectedForwardAxis = false;
|
||||
@ -2737,7 +2744,11 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
Point3D currentPositionForTransform = _trackedPosition;
|
||||
Rotation3D currentRotation;
|
||||
Rotation3D currentRotationForTransform;
|
||||
Rotation3D actualGeometryRotation = Rotation3D.Identity;
|
||||
bool hasActualGeometryRotation = false;
|
||||
Rotation3D appliedTargetRotation = newRotation;
|
||||
Rotation3D targetRotationForTransform = newRotation;
|
||||
Point3D appliedTargetPosition = newPosition;
|
||||
if (_hasTrackedRotation)
|
||||
{
|
||||
@ -2762,18 +2773,24 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"[动画姿态入口] {controlledObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
|
||||
|
||||
if (IsRealObjectMode &&
|
||||
(_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) &&
|
||||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out var actualGeometryRotation))
|
||||
(_route?.PathType == PathType.Ground ||
|
||||
_route?.PathType == PathType.Hoisting ||
|
||||
_route?.PathType == PathType.Rail) &&
|
||||
ModelItemTransformHelper.TryGetCurrentGeometryRotation(controlledObject, out actualGeometryRotation))
|
||||
{
|
||||
hasActualGeometryRotation = true;
|
||||
var trackedLinear = new Transform3D(currentRotation).Linear;
|
||||
var actualLinear = new Transform3D(actualGeometryRotation).Linear;
|
||||
string poseTag = _route?.PathType == PathType.Rail
|
||||
? "[Rail姿态基线诊断]"
|
||||
: "[平面姿态基线诊断]";
|
||||
LogManager.Debug(
|
||||
$"[平面姿态基线诊断] {controlledObject.DisplayName} 跟踪旋转: " +
|
||||
$"{poseTag} {controlledObject.DisplayName} 跟踪旋转: " +
|
||||
$"X=({trackedLinear.Get(0, 0):F4},{trackedLinear.Get(1, 0):F4},{trackedLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({trackedLinear.Get(0, 1):F4},{trackedLinear.Get(1, 1):F4},{trackedLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({trackedLinear.Get(0, 2):F4},{trackedLinear.Get(1, 2):F4},{trackedLinear.Get(2, 2):F4})");
|
||||
LogManager.Debug(
|
||||
$"[平面姿态基线诊断] {controlledObject.DisplayName} 实际几何旋转: " +
|
||||
$"{poseTag} {controlledObject.DisplayName} 实际几何旋转: " +
|
||||
$"X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({actualLinear.Get(0, 2):F4},{actualLinear.Get(1, 2):F4},{actualLinear.Get(2, 2):F4})");
|
||||
@ -2783,6 +2800,27 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
LogManager.Warning($"[动画姿态入口] 读取宿主实际状态失败: {ex.Message}");
|
||||
}
|
||||
currentRotationForTransform = currentRotation;
|
||||
|
||||
if (isRailRealObject)
|
||||
{
|
||||
if (ModelItemTransformHelper.TryGetCurrentOverrideRotation(controlledObject, out var currentOverrideRotation))
|
||||
{
|
||||
currentRotationForTransform = currentOverrideRotation;
|
||||
}
|
||||
else
|
||||
{
|
||||
currentRotationForTransform = Rotation3D.Identity;
|
||||
}
|
||||
|
||||
if (!ModelItemTransformHelper.TryResolveOverrideRotationForFinalTarget(
|
||||
controlledObject,
|
||||
appliedTargetRotation,
|
||||
out targetRotationForTransform))
|
||||
{
|
||||
targetRotationForTransform = appliedTargetRotation;
|
||||
}
|
||||
}
|
||||
|
||||
if (IsRealObjectMode &&
|
||||
_route?.PathType == PathType.Ground &&
|
||||
@ -2803,7 +2841,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"应用目标点=({appliedTargetPosition.X:F3},{appliedTargetPosition.Y:F3},{appliedTargetPosition.Z:F3})");
|
||||
}
|
||||
|
||||
var currentLinear = new Transform3D(currentRotation).Linear;
|
||||
var currentLinear = new Transform3D(currentRotationForTransform).Linear;
|
||||
var targetLinear = new Transform3D(appliedTargetRotation).Linear;
|
||||
LogManager.Debug(
|
||||
$"[动画姿态入口] {controlledObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||||
@ -2818,12 +2856,23 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})");
|
||||
|
||||
if (isRailRealObject)
|
||||
{
|
||||
var overrideLinear = new Transform3D(targetRotationForTransform).Linear;
|
||||
LogManager.Info(
|
||||
$"[Rail覆盖姿态] {controlledObject.DisplayName} Override目标: " +
|
||||
$"X=({overrideLinear.Get(0, 0):F4},{overrideLinear.Get(1, 0):F4},{overrideLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({overrideLinear.Get(0, 1):F4},{overrideLinear.Get(1, 1):F4},{overrideLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({overrideLinear.Get(0, 2):F4},{overrideLinear.Get(1, 2):F4},{overrideLinear.Get(2, 2):F4})");
|
||||
}
|
||||
|
||||
ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation(
|
||||
controlledObject,
|
||||
currentPositionForTransform,
|
||||
currentRotation,
|
||||
currentRotationForTransform,
|
||||
appliedTargetPosition,
|
||||
appliedTargetRotation);
|
||||
targetRotationForTransform);
|
||||
|
||||
_trackedPosition = appliedTargetPosition;
|
||||
_trackedRotation = appliedTargetRotation;
|
||||
@ -4044,16 +4093,23 @@ namespace NavisworksTransport.Core.Animation
|
||||
_realObjectReferenceAxisX,
|
||||
_realObjectReferenceAxisY,
|
||||
_realObjectReferenceAxisZ,
|
||||
_realObjectReferenceHostUpLocalAxis,
|
||||
targetFrame.Forward,
|
||||
adapter.HostType,
|
||||
out convention,
|
||||
out var selectedForwardAxis,
|
||||
out var selectedForwardWorldAxis))
|
||||
out var selectedForwardWorldAxis,
|
||||
out var selectedUpAxis,
|
||||
out var selectedUpWorldAxis))
|
||||
{
|
||||
LogManager.Debug(
|
||||
$"[Rail姿态修正] Host={adapter.HostType}, 虚拟物体={IsVirtualObjectMode}, " +
|
||||
$"真实物体参考姿态生效, Forward={convention.ForwardAxis}, Up={convention.UpAxis}, " +
|
||||
$"选中Forward轴={selectedForwardAxis}, 选中Forward世界轴=({selectedForwardWorldAxis.X:F4},{selectedForwardWorldAxis.Y:F4},{selectedForwardWorldAxis.Z:F4})");
|
||||
$"选中Forward轴={selectedForwardAxis}, 选中Forward世界轴=({selectedForwardWorldAxis.X:F4},{selectedForwardWorldAxis.Y:F4},{selectedForwardWorldAxis.Z:F4}), " +
|
||||
$"选中Up轴={selectedUpAxis}, 选中Up世界轴=({selectedUpWorldAxis.X:F4},{selectedUpWorldAxis.Y:F4},{selectedUpWorldAxis.Z:F4}), " +
|
||||
$"宿主Up对应本地轴={_realObjectReferenceHostUpLocalAxis}, " +
|
||||
$"目标Forward=({targetFrame.Forward.X:F4},{targetFrame.Forward.Y:F4},{targetFrame.Forward.Z:F4}), " +
|
||||
$"目标Up=({targetFrame.Up.X:F4},{targetFrame.Up.Y:F4},{targetFrame.Up.Z:F4})");
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@ -4065,6 +4121,25 @@ namespace NavisworksTransport.Core.Animation
|
||||
return true;
|
||||
}
|
||||
|
||||
private static Vector3 ResolveReferenceWorldAxisForLocalDirection(
|
||||
LocalAxisDirection axis,
|
||||
Vector3 referenceAxisX,
|
||||
Vector3 referenceAxisY,
|
||||
Vector3 referenceAxisZ)
|
||||
{
|
||||
switch (axis)
|
||||
{
|
||||
case LocalAxisDirection.PositiveX: return Vector3.Normalize(referenceAxisX);
|
||||
case LocalAxisDirection.NegativeX: return Vector3.Normalize(-referenceAxisX);
|
||||
case LocalAxisDirection.PositiveY: return Vector3.Normalize(referenceAxisY);
|
||||
case LocalAxisDirection.NegativeY: return Vector3.Normalize(-referenceAxisY);
|
||||
case LocalAxisDirection.PositiveZ: return Vector3.Normalize(referenceAxisZ);
|
||||
case LocalAxisDirection.NegativeZ: return Vector3.Normalize(-referenceAxisZ);
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryCalculateCurrentRealObjectRailProjectedExtents(
|
||||
Point3D previousPoint,
|
||||
Point3D currentPoint,
|
||||
@ -4112,14 +4187,16 @@ namespace NavisworksTransport.Core.Animation
|
||||
(float)baselineRotation.B,
|
||||
(float)baselineRotation.C,
|
||||
(float)baselineRotation.D);
|
||||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||||
Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion(
|
||||
baselineHostQuaternion,
|
||||
_objectRotationCorrection);
|
||||
localCorrection);
|
||||
|
||||
return RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents(
|
||||
_realObjectReferenceAxisX,
|
||||
_realObjectReferenceAxisY,
|
||||
_realObjectReferenceAxisZ,
|
||||
_realObjectReferenceHostUpLocalAxis,
|
||||
targetFrame.Forward,
|
||||
adapter.HostType,
|
||||
_realObjectLength,
|
||||
@ -4232,7 +4309,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
Quaternion finalHostQuaternion = adapter.ComposeHostQuaternion(
|
||||
solution.BaselineRotation,
|
||||
_objectRotationCorrection);
|
||||
ResolveRealObjectLocalRotationCorrection());
|
||||
extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
_realObjectLength,
|
||||
@ -4351,9 +4428,10 @@ namespace NavisworksTransport.Core.Animation
|
||||
(float)baselineRotation.B,
|
||||
(float)baselineRotation.C,
|
||||
(float)baselineRotation.D);
|
||||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||||
Quaternion composedHostQuaternion = adapter.ComposeHostQuaternion(
|
||||
baselineHostQuaternion,
|
||||
_objectRotationCorrection);
|
||||
localCorrection);
|
||||
rotation = adapter.FromHostQuaternionDirect(composedHostQuaternion);
|
||||
|
||||
Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(baselineHostQuaternion);
|
||||
@ -4362,6 +4440,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
$"[Rail真实物体角度修正] BaselineX=({baselineLinear.M11:F4},{baselineLinear.M21:F4},{baselineLinear.M31:F4}), " +
|
||||
$"BaselineY=({baselineLinear.M12:F4},{baselineLinear.M22:F4},{baselineLinear.M32:F4}), " +
|
||||
$"BaselineZ=({baselineLinear.M13:F4},{baselineLinear.M23:F4},{baselineLinear.M33:F4}), " +
|
||||
$"宿主修正={_objectRotationCorrection}, 本地修正={localCorrection}, " +
|
||||
$"HostComposeX=({composedLinear.M11:F4},{composedLinear.M21:F4},{composedLinear.M31:F4}), " +
|
||||
$"HostComposeY=({composedLinear.M12:F4},{composedLinear.M22:F4},{composedLinear.M32:F4}), " +
|
||||
$"HostComposeZ=({composedLinear.M13:F4},{composedLinear.M23:F4},{composedLinear.M33:F4})");
|
||||
@ -4547,7 +4626,8 @@ namespace NavisworksTransport.Core.Animation
|
||||
}
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, _objectRotationCorrection);
|
||||
LocalEulerRotationCorrection localCorrection = ResolveRealObjectLocalRotationCorrection();
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, localCorrection);
|
||||
rotation = adapter.FromHostQuaternionDirect(hostComposedQuaternion);
|
||||
|
||||
Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation);
|
||||
@ -4556,6 +4636,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
LogManager.Info(
|
||||
$"[真实物体起点姿态] 选中参考轴=({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}, " +
|
||||
$"BaselineX=({baselineLinear.M11:F4},{baselineLinear.M21:F4},{baselineLinear.M31:F4}), " +
|
||||
$"BaselineY=({baselineLinear.M12:F4},{baselineLinear.M22:F4},{baselineLinear.M32:F4}), " +
|
||||
$"BaselineZ=({baselineLinear.M13:F4},{baselineLinear.M23:F4},{baselineLinear.M33:F4}), " +
|
||||
@ -4732,17 +4813,29 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
referenceRotation = referencePose.Rotation;
|
||||
_realObjectReferenceRotation = referenceRotation;
|
||||
_realObjectReferenceAxisX = referencePose.AxisX;
|
||||
_realObjectReferenceAxisY = referencePose.AxisY;
|
||||
_realObjectReferenceAxisZ = referencePose.AxisZ;
|
||||
_realObjectReferenceAxisX = referencePose.RawAxisX;
|
||||
_realObjectReferenceAxisY = referencePose.RawAxisY;
|
||||
_realObjectReferenceAxisZ = referencePose.RawAxisZ;
|
||||
_realObjectReferenceHostWorldXAxisLocalAxis = referencePose.HostWorldXAxisLocalAxis;
|
||||
_realObjectReferenceHostWorldYAxisLocalAxis = referencePose.HostWorldYAxisLocalAxis;
|
||||
_realObjectReferenceHostWorldZAxisLocalAxis = referencePose.HostWorldZAxisLocalAxis;
|
||||
_realObjectReferenceHostSemanticXAxisLocalAxis = referencePose.HostSemanticXAxisLocalAxis;
|
||||
_realObjectReferenceHostSemanticYAxisLocalAxis = referencePose.HostSemanticYAxisLocalAxis;
|
||||
_realObjectReferenceHostSemanticZAxisLocalAxis = referencePose.HostSemanticZAxisLocalAxis;
|
||||
_realObjectReferenceHostUpLocalAxis = referencePose.HostUpLocalAxis;
|
||||
_hasRealObjectReferenceRotation = true;
|
||||
|
||||
LogManager.Info(
|
||||
$"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " +
|
||||
$"X=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " +
|
||||
$"Y=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " +
|
||||
$"Z=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " +
|
||||
$"fragment数量={referencePose.FragmentCount}, Fragment默认Up={referencePose.FragmentDefaultUpAxis}, 模型Up={referencePose.HostUpAxis}");
|
||||
$"RawX=({_realObjectReferenceAxisX.X:F4},{_realObjectReferenceAxisX.Y:F4},{_realObjectReferenceAxisX.Z:F4}), " +
|
||||
$"RawY=({_realObjectReferenceAxisY.X:F4},{_realObjectReferenceAxisY.Y:F4},{_realObjectReferenceAxisY.Z:F4}), " +
|
||||
$"RawZ=({_realObjectReferenceAxisZ.X:F4},{_realObjectReferenceAxisZ.Y:F4},{_realObjectReferenceAxisZ.Z:F4}), " +
|
||||
$"宿主世界X对应本地轴={referencePose.HostWorldXAxisLocalAxis}, 宿主世界Y对应本地轴={referencePose.HostWorldYAxisLocalAxis}, 宿主世界Z对应本地轴={referencePose.HostWorldZAxisLocalAxis}, " +
|
||||
$"宿主语义X=({referencePose.AxisX.X:F4},{referencePose.AxisX.Y:F4},{referencePose.AxisX.Z:F4}), " +
|
||||
$"宿主语义Y=({referencePose.AxisY.X:F4},{referencePose.AxisY.Y:F4},{referencePose.AxisY.Z:F4}), " +
|
||||
$"宿主语义Z=({referencePose.AxisZ.X:F4},{referencePose.AxisZ.Y:F4},{referencePose.AxisZ.Z:F4}), " +
|
||||
$"宿主语义X对应本地轴={referencePose.HostSemanticXAxisLocalAxis}, 宿主语义Y对应本地轴={referencePose.HostSemanticYAxisLocalAxis}, 宿主语义Z对应本地轴={referencePose.HostSemanticZAxisLocalAxis}, " +
|
||||
$"宿主Up对应本地轴={referencePose.HostUpLocalAxis}, fragment数量={referencePose.FragmentCount}, Fragment默认Up={referencePose.FragmentDefaultUpAxis}, 模型Up={referencePose.HostUpAxis}");
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
@ -4809,11 +4902,27 @@ namespace NavisworksTransport.Core.Animation
|
||||
_realObjectReferenceAxisX = Vector3.UnitX;
|
||||
_realObjectReferenceAxisY = Vector3.UnitY;
|
||||
_realObjectReferenceAxisZ = Vector3.UnitZ;
|
||||
_realObjectReferenceHostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
_realObjectReferenceHostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
_realObjectReferenceHostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
_realObjectReferenceHostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
_realObjectReferenceHostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
_realObjectReferenceHostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
_realObjectReferenceHostUpLocalAxis = LocalAxisDirection.PositiveY;
|
||||
_hasRealObjectReferenceRotation = false;
|
||||
_realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
|
||||
_hasRealObjectPlanarSelectedForwardAxis = false;
|
||||
}
|
||||
|
||||
private LocalEulerRotationCorrection ResolveRealObjectLocalRotationCorrection()
|
||||
{
|
||||
return HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||||
_objectRotationCorrection,
|
||||
_realObjectReferenceHostWorldXAxisLocalAxis,
|
||||
_realObjectReferenceHostWorldYAxisLocalAxis,
|
||||
_realObjectReferenceHostWorldZAxisLocalAxis);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置物体绕宿主 X/Y/Z 轴的角度修正
|
||||
/// </summary>
|
||||
|
||||
@ -277,6 +277,31 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
return Quaternion.Normalize(baselineQuaternion * inverseCorrectionQuaternion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将“宿主语义 X/Y/Z”的角度修正重映射为当前真实物体本地 X/Y/Z 的角度修正。
|
||||
/// </summary>
|
||||
public static LocalEulerRotationCorrection RemapHostSemanticCorrectionToLocalAxes(
|
||||
LocalEulerRotationCorrection hostCorrection,
|
||||
LocalAxisDirection hostSemanticXAxisLocalAxis,
|
||||
LocalAxisDirection hostSemanticYAxisLocalAxis,
|
||||
LocalAxisDirection hostSemanticZAxisLocalAxis)
|
||||
{
|
||||
if (hostCorrection.IsZero)
|
||||
{
|
||||
return LocalEulerRotationCorrection.Zero;
|
||||
}
|
||||
|
||||
double localXDegrees = 0.0;
|
||||
double localYDegrees = 0.0;
|
||||
double localZDegrees = 0.0;
|
||||
|
||||
ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticXAxisLocalAxis, hostCorrection.XDegrees);
|
||||
ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticYAxisLocalAxis, hostCorrection.YDegrees);
|
||||
ApplyMappedAngle(ref localXDegrees, ref localYDegrees, ref localZDegrees, hostSemanticZAxisLocalAxis, hostCorrection.ZDegrees);
|
||||
|
||||
return new LocalEulerRotationCorrection(localXDegrees, localYDegrees, localZDegrees);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
|
||||
///
|
||||
@ -462,5 +487,37 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
return (float)(degrees * Math.PI / 180.0);
|
||||
}
|
||||
|
||||
private static void ApplyMappedAngle(
|
||||
ref double localXDegrees,
|
||||
ref double localYDegrees,
|
||||
ref double localZDegrees,
|
||||
LocalAxisDirection mappedLocalAxis,
|
||||
double hostDegrees)
|
||||
{
|
||||
switch (mappedLocalAxis)
|
||||
{
|
||||
case LocalAxisDirection.PositiveX:
|
||||
localXDegrees += hostDegrees;
|
||||
break;
|
||||
case LocalAxisDirection.NegativeX:
|
||||
localXDegrees -= hostDegrees;
|
||||
break;
|
||||
case LocalAxisDirection.PositiveY:
|
||||
localYDegrees += hostDegrees;
|
||||
break;
|
||||
case LocalAxisDirection.NegativeY:
|
||||
localYDegrees -= hostDegrees;
|
||||
break;
|
||||
case LocalAxisDirection.PositiveZ:
|
||||
localZDegrees += hostDegrees;
|
||||
break;
|
||||
case LocalAxisDirection.NegativeZ:
|
||||
localZDegrees -= hostDegrees;
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(mappedLocalAxis), mappedLocalAxis, null);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,26 +10,56 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
public RealObjectReferencePose(
|
||||
Quaternion rotation,
|
||||
Vector3 rawAxisX,
|
||||
Vector3 rawAxisY,
|
||||
Vector3 rawAxisZ,
|
||||
Vector3 axisX,
|
||||
Vector3 axisY,
|
||||
Vector3 axisZ,
|
||||
LocalAxisDirection hostWorldXAxisLocalAxis,
|
||||
LocalAxisDirection hostWorldYAxisLocalAxis,
|
||||
LocalAxisDirection hostWorldZAxisLocalAxis,
|
||||
LocalAxisDirection hostSemanticXAxisLocalAxis,
|
||||
LocalAxisDirection hostSemanticYAxisLocalAxis,
|
||||
LocalAxisDirection hostSemanticZAxisLocalAxis,
|
||||
LocalAxisDirection hostUpLocalAxis,
|
||||
int fragmentCount,
|
||||
string fragmentDefaultUpAxis,
|
||||
string hostUpAxis)
|
||||
{
|
||||
Rotation = rotation;
|
||||
RawAxisX = rawAxisX;
|
||||
RawAxisY = rawAxisY;
|
||||
RawAxisZ = rawAxisZ;
|
||||
AxisX = axisX;
|
||||
AxisY = axisY;
|
||||
AxisZ = axisZ;
|
||||
HostWorldXAxisLocalAxis = hostWorldXAxisLocalAxis;
|
||||
HostWorldYAxisLocalAxis = hostWorldYAxisLocalAxis;
|
||||
HostWorldZAxisLocalAxis = hostWorldZAxisLocalAxis;
|
||||
HostSemanticXAxisLocalAxis = hostSemanticXAxisLocalAxis;
|
||||
HostSemanticYAxisLocalAxis = hostSemanticYAxisLocalAxis;
|
||||
HostSemanticZAxisLocalAxis = hostSemanticZAxisLocalAxis;
|
||||
HostUpLocalAxis = hostUpLocalAxis;
|
||||
FragmentCount = fragmentCount;
|
||||
FragmentDefaultUpAxis = fragmentDefaultUpAxis;
|
||||
HostUpAxis = hostUpAxis;
|
||||
}
|
||||
|
||||
public Quaternion Rotation { get; }
|
||||
public Vector3 RawAxisX { get; }
|
||||
public Vector3 RawAxisY { get; }
|
||||
public Vector3 RawAxisZ { get; }
|
||||
public Vector3 AxisX { get; }
|
||||
public Vector3 AxisY { get; }
|
||||
public Vector3 AxisZ { get; }
|
||||
public LocalAxisDirection HostWorldXAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostWorldYAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostWorldZAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostSemanticXAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostSemanticYAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostSemanticZAxisLocalAxis { get; }
|
||||
public LocalAxisDirection HostUpLocalAxis { get; }
|
||||
public int FragmentCount { get; }
|
||||
public string FragmentDefaultUpAxis { get; }
|
||||
public string HostUpAxis { get; }
|
||||
|
||||
@ -117,17 +117,169 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryResolveSemanticLocalAxes(
|
||||
rawFrame,
|
||||
interpretedFrame,
|
||||
out var hostSemanticXAxisLocalAxis,
|
||||
out var hostSemanticYAxisLocalAxis,
|
||||
out var hostSemanticZAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryResolveWorldLocalAxes(
|
||||
rawFrame,
|
||||
out var hostWorldXAxisLocalAxis,
|
||||
out var hostWorldYAxisLocalAxis,
|
||||
out var hostWorldZAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
referencePose = new RealObjectReferencePose(
|
||||
interpretedFrame.Rotation,
|
||||
rawFrame.AxisX,
|
||||
rawFrame.AxisY,
|
||||
rawFrame.AxisZ,
|
||||
interpretedFrame.AxisX,
|
||||
interpretedFrame.AxisY,
|
||||
interpretedFrame.AxisZ,
|
||||
hostWorldXAxisLocalAxis,
|
||||
hostWorldYAxisLocalAxis,
|
||||
hostWorldZAxisLocalAxis,
|
||||
hostSemanticXAxisLocalAxis,
|
||||
hostSemanticYAxisLocalAxis,
|
||||
hostSemanticZAxisLocalAxis,
|
||||
string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase)
|
||||
? hostSemanticYAxisLocalAxis
|
||||
: hostSemanticZAxisLocalAxis,
|
||||
fragmentMatrices.Count,
|
||||
fragmentDefaultUpAxis,
|
||||
hostUpAxis);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryResolveWorldLocalAxes(
|
||||
FragmentRepresentativePoseHelper.RepresentativeFrame rawFrame,
|
||||
out LocalAxisDirection hostWorldXAxisLocalAxis,
|
||||
out LocalAxisDirection hostWorldYAxisLocalAxis,
|
||||
out LocalAxisDirection hostWorldZAxisLocalAxis)
|
||||
{
|
||||
hostWorldXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
hostWorldYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
hostWorldZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
|
||||
if (!TryMatchRawAxisDirection(Vector3.UnitX, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldXAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryMatchRawAxisDirection(Vector3.UnitY, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldYAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryMatchRawAxisDirection(Vector3.UnitZ, rawFrame.AxisX, rawFrame.AxisY, rawFrame.AxisZ, out hostWorldZAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryResolveSemanticLocalAxes(
|
||||
FragmentRepresentativePoseHelper.RepresentativeFrame rawFrame,
|
||||
FragmentRepresentativePoseHelper.RepresentativeFrame interpretedFrame,
|
||||
out LocalAxisDirection hostSemanticXAxisLocalAxis,
|
||||
out LocalAxisDirection hostSemanticYAxisLocalAxis,
|
||||
out LocalAxisDirection hostSemanticZAxisLocalAxis)
|
||||
{
|
||||
hostSemanticXAxisLocalAxis = LocalAxisDirection.PositiveX;
|
||||
hostSemanticYAxisLocalAxis = LocalAxisDirection.PositiveY;
|
||||
hostSemanticZAxisLocalAxis = LocalAxisDirection.PositiveZ;
|
||||
|
||||
if (!TryMatchRawAxisDirection(
|
||||
interpretedFrame.AxisX,
|
||||
rawFrame.AxisX,
|
||||
rawFrame.AxisY,
|
||||
rawFrame.AxisZ,
|
||||
out hostSemanticXAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryMatchRawAxisDirection(
|
||||
interpretedFrame.AxisY,
|
||||
rawFrame.AxisX,
|
||||
rawFrame.AxisY,
|
||||
rawFrame.AxisZ,
|
||||
out hostSemanticYAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryMatchRawAxisDirection(
|
||||
interpretedFrame.AxisZ,
|
||||
rawFrame.AxisX,
|
||||
rawFrame.AxisY,
|
||||
rawFrame.AxisZ,
|
||||
out hostSemanticZAxisLocalAxis))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryMatchRawAxisDirection(
|
||||
Vector3 targetAxis,
|
||||
Vector3 rawAxisX,
|
||||
Vector3 rawAxisY,
|
||||
Vector3 rawAxisZ,
|
||||
out LocalAxisDirection axisDirection)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.PositiveX;
|
||||
const float tolerance = 1e-4f;
|
||||
|
||||
if (Vector3.Distance(targetAxis, rawAxisX) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.PositiveX;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (Vector3.Distance(targetAxis, -rawAxisX) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.NegativeX;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (Vector3.Distance(targetAxis, rawAxisY) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.PositiveY;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (Vector3.Distance(targetAxis, -rawAxisY) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.NegativeY;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (Vector3.Distance(targetAxis, rawAxisZ) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.PositiveZ;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (Vector3.Distance(targetAxis, -rawAxisZ) < tolerance)
|
||||
{
|
||||
axisDirection = LocalAxisDirection.NegativeZ;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
public static bool TryDetectDefaultUpAxis(
|
||||
IReadOnlyCollection<double[]> fragmentMatrices,
|
||||
string hostUpAxis,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user