修正真实物体的旋转和通行空间不适配的问题
This commit is contained in:
parent
d52b1aef08
commit
eaf24420b7
@ -102,6 +102,37 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
AssertVector(mappedUp, 0, 0, 1);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void WorldCorrection_ShouldRotateBaselinePoseAroundHostYAxisInYUp()
|
||||
{
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||
Quaternion worldCorrection = adapter.CreateCanonicalRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
new Vector3(5, 0, 0),
|
||||
Vector3.UnitZ,
|
||||
convention,
|
||||
out Quaternion baselineRotation));
|
||||
|
||||
Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForwardWithWorldCorrection(
|
||||
new Vector3(5, 0, 0),
|
||||
Vector3.UnitZ,
|
||||
convention,
|
||||
worldCorrection,
|
||||
out Quaternion correctedRotation));
|
||||
|
||||
Matrix4x4 baseline = Matrix4x4.CreateFromQuaternion(baselineRotation);
|
||||
|
||||
AssertColumn(baseline, 0, 1, 0, 0);
|
||||
AssertColumn(baseline, 1, 0, 0, 1);
|
||||
AssertColumn(baseline, 2, 0, -1, 0);
|
||||
|
||||
Quaternion expectedRotation = Quaternion.Normalize(worldCorrection * baselineRotation);
|
||||
AssertQuaternionEquivalent(expectedRotation, correctedRotation);
|
||||
}
|
||||
|
||||
private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
|
||||
{
|
||||
switch (column)
|
||||
|
||||
@ -158,6 +158,74 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
Assert.AreEqual(0.0, rotatedForward.Z, 1e-6);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void YUp_HostRotationCorrection_ShouldKeepHostYAxisInvariantInHostSpace()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitY, hostCorrection);
|
||||
AssertVector(rotatedUp, 0.0, 1.0, 0.0);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ComposeHostQuaternion_ShouldApplyHostCorrectionAfterBaseline()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||
var baseline = Quaternion.Identity;
|
||||
|
||||
Quaternion composed = adapter.ComposeHostQuaternion(
|
||||
baseline,
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||||
|
||||
Assert.AreEqual(0.0, linear.M11, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M21, 1e-6);
|
||||
Assert.AreEqual(1.0, linear.M31, 1e-6);
|
||||
|
||||
Assert.AreEqual(0.0, linear.M12, 1e-6);
|
||||
Assert.AreEqual(1.0, linear.M22, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M32, 1e-6);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostRotationCorrection_ShouldKeepHostZAxisInvariantInHostSpace()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
||||
AssertVector(rotatedUp, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_ComposeHostQuaternion_ShouldApplyHostZCorrectionAfterBaseline()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
var baseline = Quaternion.Identity;
|
||||
|
||||
Quaternion composed = adapter.ComposeHostQuaternion(
|
||||
baseline,
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||||
|
||||
Assert.AreEqual(0.0, linear.M11, 1e-6);
|
||||
Assert.AreEqual(-1.0, linear.M21, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M31, 1e-6);
|
||||
|
||||
Assert.AreEqual(1.0, linear.M12, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M22, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M32, 1e-6);
|
||||
|
||||
Assert.AreEqual(0.0, linear.M13, 1e-6);
|
||||
Assert.AreEqual(0.0, linear.M23, 1e-6);
|
||||
Assert.AreEqual(1.0, linear.M33, 1e-6);
|
||||
}
|
||||
|
||||
private static void AssertPoint(Vector3 point, double x, double y, double z)
|
||||
{
|
||||
Assert.AreEqual(x, point.X, 1e-9);
|
||||
|
||||
90
UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs
Normal file
90
UnitTests/CoordinateSystem/RotatedObjectExtentHelperTests.cs
Normal file
@ -0,0 +1,90 @@
|
||||
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||
using NavisworksTransport.Utils.CoordinateSystem;
|
||||
using System.Numerics;
|
||||
|
||||
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
{
|
||||
[TestClass]
|
||||
public class RotatedObjectExtentHelperTests
|
||||
{
|
||||
[TestMethod]
|
||||
public void YUp_HostY90_ForRealObject_ShouldKeepUpExtentUnchanged()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
|
||||
Quaternion correction = adapter.CreateHostRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
forwardSize: 6.0,
|
||||
sideSize: 4.0,
|
||||
upSize: 2.0,
|
||||
correctionQuaternion: correction);
|
||||
|
||||
Assert.AreEqual(4.0, result.forwardExtent, 1e-6);
|
||||
Assert.AreEqual(6.0, result.sideExtent, 1e-6);
|
||||
Assert.AreEqual(2.0, result.upExtent, 1e-6);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void YUp_HostZ90_ForRealObject_ShouldPromoteForwardSizeToUpExtent()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
|
||||
Quaternion correction = adapter.CreateHostRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
forwardSize: 6.0,
|
||||
sideSize: 4.0,
|
||||
upSize: 2.0,
|
||||
correctionQuaternion: correction);
|
||||
|
||||
Assert.AreEqual(2.0, result.forwardExtent, 1e-6);
|
||||
Assert.AreEqual(4.0, result.sideExtent, 1e-6);
|
||||
Assert.AreEqual(6.0, result.upExtent, 1e-6);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostY90_ShouldPromoteForwardSizeToUpExtent()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
forwardSize: 6.0,
|
||||
sideSize: 4.0,
|
||||
upSize: 2.0,
|
||||
correctionQuaternion: correction);
|
||||
|
||||
Assert.AreEqual(2.0, result.forwardExtent, 1e-6);
|
||||
Assert.AreEqual(4.0, result.sideExtent, 1e-6);
|
||||
Assert.AreEqual(6.0, result.upExtent, 1e-6);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostZ90_ShouldKeepUpExtentUnchanged()
|
||||
{
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
forwardSize: 6.0,
|
||||
sideSize: 4.0,
|
||||
upSize: 2.0,
|
||||
correctionQuaternion: correction);
|
||||
|
||||
Assert.AreEqual(4.0, result.forwardExtent, 1e-6);
|
||||
Assert.AreEqual(6.0, result.sideExtent, 1e-6);
|
||||
Assert.AreEqual(2.0, result.upExtent, 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -695,7 +695,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
if (_route.PathType == PathType.Hoisting)
|
||||
{
|
||||
// 吊装路径:第一个路径点(起吊点)是地面位置,动画跟踪点统一使用几何中心
|
||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
|
||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||||
LogManager.Debug($"[移动到起点] 吊装路径:起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||||
}
|
||||
else if (_route.PathType == PathType.Rail)
|
||||
@ -730,7 +730,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
else
|
||||
{
|
||||
// 地面路径:路径点在接触面上,动画跟踪点统一使用几何中心
|
||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
|
||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||||
LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||||
}
|
||||
|
||||
@ -956,7 +956,9 @@ namespace NavisworksTransport.Core.Animation
|
||||
}
|
||||
|
||||
// 🔥 空中路径:根据路径类型和线段索引调整物体位置
|
||||
double objectHeight = GetAnimatedObjectHeight();
|
||||
double objectHeight = _route.PathType == PathType.Rail
|
||||
? GetAnimatedObjectHeight()
|
||||
: GetAnimatedObjectGroundContactHeight();
|
||||
|
||||
if (_route.PathType == PathType.Hoisting)
|
||||
{
|
||||
@ -1038,11 +1040,11 @@ namespace NavisworksTransport.Core.Animation
|
||||
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
|
||||
}
|
||||
|
||||
framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectHeight());
|
||||
framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectGroundContactHeight());
|
||||
|
||||
yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
|
||||
previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectHeight());
|
||||
nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectHeight());
|
||||
previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectGroundContactHeight());
|
||||
nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectGroundContactHeight());
|
||||
|
||||
if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
||||
i > 0 &&
|
||||
@ -1944,14 +1946,14 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
expectedTrackedEndPoint = ResolveHoistingTrackedCenter(
|
||||
terminalAnchorPoint,
|
||||
GetAnimatedObjectHeight(),
|
||||
GetAnimatedObjectGroundContactHeight(),
|
||||
0.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
expectedTrackedEndPoint = ResolveGroundTrackedCenter(
|
||||
terminalAnchorPoint,
|
||||
GetAnimatedObjectHeight());
|
||||
GetAnimatedObjectGroundContactHeight());
|
||||
}
|
||||
|
||||
Vector3D forward = new Vector3D(
|
||||
@ -2462,7 +2464,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
{
|
||||
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
|
||||
currentPositionForTransform = actualHostPosition;
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
|
||||
}
|
||||
catch (Exception ex)
|
||||
@ -2471,15 +2473,15 @@ namespace NavisworksTransport.Core.Animation
|
||||
}
|
||||
var currentLinear = new Transform3D(currentRotation).Linear;
|
||||
var targetLinear = new Transform3D(newRotation).Linear;
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||||
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " +
|
||||
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " +
|
||||
$"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}), " +
|
||||
@ -3299,6 +3301,56 @@ namespace NavisworksTransport.Core.Animation
|
||||
return ModelItemTransformHelper.GetHostHeight(boundingBox, adapter);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前姿态下,地面/吊装路径用于“接触点 -> 几何中心”换算的有效法线尺寸(模型单位)。
|
||||
/// 与通行空间使用的旋转后法线尺寸保持一致,避免物体旋转后仍按原始高度抬升中心。
|
||||
/// </summary>
|
||||
private double GetAnimatedObjectGroundContactHeight()
|
||||
{
|
||||
double rawHeight = GetAnimatedObjectHeight();
|
||||
if (_objectRotationCorrection.IsZero)
|
||||
{
|
||||
return rawHeight;
|
||||
}
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
ModelAxisConvention axisConvention;
|
||||
double forwardSize;
|
||||
double sideSize;
|
||||
double upSize;
|
||||
|
||||
if (IsVirtualObjectMode)
|
||||
{
|
||||
axisConvention = ModelAxisConvention.CreateVirtualObjectAssetConvention();
|
||||
forwardSize = _virtualObjectLength;
|
||||
sideSize = _virtualObjectWidth;
|
||||
upSize = _virtualObjectHeight;
|
||||
}
|
||||
else if (IsRealObjectMode && _realObjectLength > 0.0 && _realObjectWidth > 0.0 && _realObjectHeight > 0.0)
|
||||
{
|
||||
axisConvention = ModelAxisConvention.CreateDefaultForHost(adapter.HostType);
|
||||
forwardSize = _realObjectLength;
|
||||
sideSize = _realObjectWidth;
|
||||
upSize = _realObjectHeight;
|
||||
}
|
||||
else
|
||||
{
|
||||
return rawHeight;
|
||||
}
|
||||
|
||||
Quaternion correctionQuaternion = IsVirtualObjectMode
|
||||
? adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
|
||||
: adapter.CreateHostRotationCorrection(_objectRotationCorrection);
|
||||
var projectedExtents = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
axisConvention,
|
||||
forwardSize,
|
||||
sideSize,
|
||||
upSize,
|
||||
correctionQuaternion);
|
||||
|
||||
return projectedExtents.upExtent;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前碰撞检测精度
|
||||
/// </summary>
|
||||
@ -3523,7 +3575,6 @@ namespace NavisworksTransport.Core.Animation
|
||||
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
||||
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
||||
Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
|
||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
|
||||
Vector3 forward = canonicalNext - canonicalPrevious;
|
||||
if (forward.LengthSquared() < 1e-6f)
|
||||
@ -3531,6 +3582,25 @@ namespace NavisworksTransport.Core.Animation
|
||||
forward = canonicalNext - canonicalCurrent;
|
||||
}
|
||||
|
||||
if (IsRealObjectMode)
|
||||
{
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
forward,
|
||||
HostCoordinateAdapter.CanonicalUpVector3,
|
||||
convention,
|
||||
out var baselineQuaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
|
||||
Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
|
||||
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
|
||||
return true;
|
||||
}
|
||||
|
||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
forward,
|
||||
HostCoordinateAdapter.CanonicalUpVector3,
|
||||
@ -3552,6 +3622,24 @@ namespace NavisworksTransport.Core.Animation
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
var convention = GetCurrentModelAxisConvention();
|
||||
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
|
||||
if (IsRealObjectMode)
|
||||
{
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
canonicalForward,
|
||||
HostCoordinateAdapter.CanonicalUpVector3,
|
||||
convention,
|
||||
out var baselineQuaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
|
||||
Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
|
||||
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
|
||||
return true;
|
||||
}
|
||||
|
||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
canonicalForward,
|
||||
|
||||
@ -4350,8 +4350,10 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
return (0, 0, 0);
|
||||
}
|
||||
|
||||
Quaternion correctionQuaternion = CoordinateSystemManager.Instance.CreateHostAdapter()
|
||||
.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
var hostAdapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
Quaternion correctionQuaternion = UseVirtualObject
|
||||
? hostAdapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
|
||||
: hostAdapter.CreateHostRotationCorrection(_objectRotationCorrection);
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
axisConvention,
|
||||
forwardSize,
|
||||
|
||||
@ -103,5 +103,36 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
normalizedUp);
|
||||
return true;
|
||||
}
|
||||
|
||||
public static bool TryCreateQuaternionFromForwardWithWorldCorrection(
|
||||
Vector3 desiredForward,
|
||||
Vector3 canonicalUp,
|
||||
ModelAxisConvention convention,
|
||||
Quaternion worldCorrectionQuaternion,
|
||||
out Quaternion rotation)
|
||||
{
|
||||
rotation = Quaternion.Identity;
|
||||
|
||||
if (!TryCreateQuaternionFromForward(
|
||||
desiredForward,
|
||||
canonicalUp,
|
||||
convention,
|
||||
out var baselineRotation))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Math.Abs(worldCorrectionQuaternion.X) < 1e-9f &&
|
||||
Math.Abs(worldCorrectionQuaternion.Y) < 1e-9f &&
|
||||
Math.Abs(worldCorrectionQuaternion.Z) < 1e-9f &&
|
||||
Math.Abs(worldCorrectionQuaternion.W - 1.0f) < 1e-9f)
|
||||
{
|
||||
rotation = baselineRotation;
|
||||
return true;
|
||||
}
|
||||
|
||||
rotation = Quaternion.Normalize(worldCorrectionQuaternion * baselineRotation);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -188,6 +188,34 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
hostLinear.M31, hostLinear.M32, hostLinear.M33);
|
||||
}
|
||||
|
||||
public Rotation3D FromHostQuaternion(Quaternion hostRotation)
|
||||
{
|
||||
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostRotation);
|
||||
return CreateRotationFromLinearComponents(
|
||||
hostLinear.M11, hostLinear.M12, hostLinear.M13,
|
||||
hostLinear.M21, hostLinear.M22, hostLinear.M23,
|
||||
hostLinear.M31, hostLinear.M32, hostLinear.M33);
|
||||
}
|
||||
|
||||
public Quaternion CreateHostRotationCorrection(LocalEulerRotationCorrection correction)
|
||||
{
|
||||
if (correction.IsZero)
|
||||
{
|
||||
return Quaternion.Identity;
|
||||
}
|
||||
|
||||
Quaternion qx = Quaternion.CreateFromAxisAngle(Vector3.UnitX, DegreesToRadians(correction.XDegrees));
|
||||
Quaternion qy = Quaternion.CreateFromAxisAngle(Vector3.UnitY, DegreesToRadians(correction.YDegrees));
|
||||
Quaternion qz = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, DegreesToRadians(correction.ZDegrees));
|
||||
return Quaternion.Normalize(qz * qy * qx);
|
||||
}
|
||||
|
||||
public Quaternion ComposeHostQuaternion(Quaternion baselineQuaternion, LocalEulerRotationCorrection correction)
|
||||
{
|
||||
Quaternion correctionQuaternion = CreateHostRotationCorrection(correction);
|
||||
return Quaternion.Normalize(correctionQuaternion * baselineQuaternion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
|
||||
/// 约定:先绕宿主 X,再绕宿主 Y,最后绕宿主 Z。
|
||||
|
||||
@ -540,24 +540,24 @@ namespace NavisworksTransport.Utils
|
||||
targetPosition.Y - rotatedCurrentPosition.Y,
|
||||
targetPosition.Z - rotatedCurrentPosition.Z);
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " +
|
||||
$"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
|
||||
$"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})");
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 当前旋转: " +
|
||||
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({currentLinear.Get(0, 1):F4},{currentLinear.Get(1, 1):F4},{currentLinear.Get(2, 1):F4}), " +
|
||||
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 目标旋转: " +
|
||||
$"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})");
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 增量旋转: " +
|
||||
$"X=({deltaLinear.Get(0, 0):F4},{deltaLinear.Get(1, 0):F4},{deltaLinear.Get(2, 0):F4}), " +
|
||||
$"Y=({deltaLinear.Get(0, 1):F4},{deltaLinear.Get(1, 1):F4},{deltaLinear.Get(2, 1):F4}), " +
|
||||
@ -600,13 +600,13 @@ namespace NavisworksTransport.Utils
|
||||
var targetLinear = new Transform3D(targetRotation).Linear;
|
||||
var actualPosition = actualBounds.Center;
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 立即读回位置(可能滞后): " +
|
||||
$"实际=({actualPosition.X:F3},{actualPosition.Y:F3},{actualPosition.Z:F3}), " +
|
||||
$"期望=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
|
||||
$"偏差=({actualPosition.X - targetPosition.X:F3},{actualPosition.Y - targetPosition.Y:F3},{actualPosition.Z - targetPosition.Z:F3})");
|
||||
|
||||
LogManager.Debug(
|
||||
LogManager.Info(
|
||||
$"[模型增量姿态] {item.DisplayName} 立即读回旋转(可能滞后/不反映override): " +
|
||||
$"实际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}), " +
|
||||
|
||||
Loading…
Reference in New Issue
Block a user