修正真实物体的旋转和通行空间不适配的问题
This commit is contained in:
parent
d52b1aef08
commit
eaf24420b7
@ -102,6 +102,37 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|||||||
AssertVector(mappedUp, 0, 0, 1);
|
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)
|
private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
|
||||||
{
|
{
|
||||||
switch (column)
|
switch (column)
|
||||||
|
|||||||
@ -158,6 +158,74 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|||||||
Assert.AreEqual(0.0, rotatedForward.Z, 1e-6);
|
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)
|
private static void AssertPoint(Vector3 point, double x, double y, double z)
|
||||||
{
|
{
|
||||||
Assert.AreEqual(x, point.X, 1e-9);
|
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)
|
if (_route.PathType == PathType.Hoisting)
|
||||||
{
|
{
|
||||||
// 吊装路径:第一个路径点(起吊点)是地面位置,动画跟踪点统一使用几何中心
|
// 吊装路径:第一个路径点(起吊点)是地面位置,动画跟踪点统一使用几何中心
|
||||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
|
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||||||
LogManager.Debug($"[移动到起点] 吊装路径:起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
LogManager.Debug($"[移动到起点] 吊装路径:起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||||||
}
|
}
|
||||||
else if (_route.PathType == PathType.Rail)
|
else if (_route.PathType == PathType.Rail)
|
||||||
@ -730,7 +730,7 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
// 地面路径:路径点在接触面上,动画跟踪点统一使用几何中心
|
// 地面路径:路径点在接触面上,动画跟踪点统一使用几何中心
|
||||||
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight());
|
startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectGroundContactHeight());
|
||||||
LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
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)
|
if (_route.PathType == PathType.Hoisting)
|
||||||
{
|
{
|
||||||
@ -1038,11 +1040,11 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
|
sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)];
|
||||||
}
|
}
|
||||||
|
|
||||||
framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectHeight());
|
framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectGroundContactHeight());
|
||||||
|
|
||||||
yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
|
yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress);
|
||||||
previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectHeight());
|
previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectGroundContactHeight());
|
||||||
nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectHeight());
|
nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectGroundContactHeight());
|
||||||
|
|
||||||
if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
|
||||||
i > 0 &&
|
i > 0 &&
|
||||||
@ -1944,14 +1946,14 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
{
|
{
|
||||||
expectedTrackedEndPoint = ResolveHoistingTrackedCenter(
|
expectedTrackedEndPoint = ResolveHoistingTrackedCenter(
|
||||||
terminalAnchorPoint,
|
terminalAnchorPoint,
|
||||||
GetAnimatedObjectHeight(),
|
GetAnimatedObjectGroundContactHeight(),
|
||||||
0.0);
|
0.0);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
expectedTrackedEndPoint = ResolveGroundTrackedCenter(
|
expectedTrackedEndPoint = ResolveGroundTrackedCenter(
|
||||||
terminalAnchorPoint,
|
terminalAnchorPoint,
|
||||||
GetAnimatedObjectHeight());
|
GetAnimatedObjectGroundContactHeight());
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3D forward = new Vector3D(
|
Vector3D forward = new Vector3D(
|
||||||
@ -2462,7 +2464,7 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
{
|
{
|
||||||
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
|
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
|
||||||
currentPositionForTransform = actualHostPosition;
|
currentPositionForTransform = actualHostPosition;
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
|
$"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
|
||||||
}
|
}
|
||||||
catch (Exception ex)
|
catch (Exception ex)
|
||||||
@ -2471,15 +2473,15 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
}
|
}
|
||||||
var currentLinear = new Transform3D(currentRotation).Linear;
|
var currentLinear = new Transform3D(currentRotation).Linear;
|
||||||
var targetLinear = new Transform3D(newRotation).Linear;
|
var targetLinear = new Transform3D(newRotation).Linear;
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
|
||||||
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
|
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " +
|
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪姿态: " +
|
||||||
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
|
$"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}), " +
|
$"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})");
|
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " +
|
$"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " +
|
||||||
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
|
$"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}), " +
|
$"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);
|
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>
|
||||||
/// 获取当前碰撞检测精度
|
/// 获取当前碰撞检测精度
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@ -3523,7 +3575,6 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
||||||
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
||||||
Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
|
Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint));
|
||||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
|
||||||
|
|
||||||
Vector3 forward = canonicalNext - canonicalPrevious;
|
Vector3 forward = canonicalNext - canonicalPrevious;
|
||||||
if (forward.LengthSquared() < 1e-6f)
|
if (forward.LengthSquared() < 1e-6f)
|
||||||
@ -3531,6 +3582,25 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
forward = canonicalNext - canonicalCurrent;
|
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(
|
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||||
forward,
|
forward,
|
||||||
HostCoordinateAdapter.CanonicalUpVector3,
|
HostCoordinateAdapter.CanonicalUpVector3,
|
||||||
@ -3552,6 +3622,24 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
var convention = GetCurrentModelAxisConvention();
|
var convention = GetCurrentModelAxisConvention();
|
||||||
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
|
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);
|
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||||
canonicalForward,
|
canonicalForward,
|
||||||
|
|||||||
@ -4350,8 +4350,10 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
return (0, 0, 0);
|
return (0, 0, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
Quaternion correctionQuaternion = CoordinateSystemManager.Instance.CreateHostAdapter()
|
var hostAdapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
Quaternion correctionQuaternion = UseVirtualObject
|
||||||
|
? hostAdapter.CreateCanonicalRotationCorrection(_objectRotationCorrection)
|
||||||
|
: hostAdapter.CreateHostRotationCorrection(_objectRotationCorrection);
|
||||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||||
axisConvention,
|
axisConvention,
|
||||||
forwardSize,
|
forwardSize,
|
||||||
|
|||||||
@ -103,5 +103,36 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
|||||||
normalizedUp);
|
normalizedUp);
|
||||||
return true;
|
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);
|
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>
|
/// <summary>
|
||||||
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
|
/// 将宿主 X/Y/Z 三轴旋转角转换为内部 Canonical Space 中的旋转四元数。
|
||||||
/// 约定:先绕宿主 X,再绕宿主 Y,最后绕宿主 Z。
|
/// 约定:先绕宿主 X,再绕宿主 Y,最后绕宿主 Z。
|
||||||
|
|||||||
@ -540,24 +540,24 @@ namespace NavisworksTransport.Utils
|
|||||||
targetPosition.Y - rotatedCurrentPosition.Y,
|
targetPosition.Y - rotatedCurrentPosition.Y,
|
||||||
targetPosition.Z - rotatedCurrentPosition.Z);
|
targetPosition.Z - rotatedCurrentPosition.Z);
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " +
|
$"[模型增量姿态] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " +
|
||||||
$"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
|
$"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " +
|
||||||
$"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})");
|
$"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})");
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 当前旋转: " +
|
$"[模型增量姿态] {item.DisplayName} 当前旋转: " +
|
||||||
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
|
$"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}), " +
|
$"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})");
|
$"Z=({currentLinear.Get(0, 2):F4},{currentLinear.Get(1, 2):F4},{currentLinear.Get(2, 2):F4})");
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 目标旋转: " +
|
$"[模型增量姿态] {item.DisplayName} 目标旋转: " +
|
||||||
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
|
$"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}), " +
|
$"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})");
|
$"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})");
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 增量旋转: " +
|
$"[模型增量姿态] {item.DisplayName} 增量旋转: " +
|
||||||
$"X=({deltaLinear.Get(0, 0):F4},{deltaLinear.Get(1, 0):F4},{deltaLinear.Get(2, 0):F4}), " +
|
$"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}), " +
|
$"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 targetLinear = new Transform3D(targetRotation).Linear;
|
||||||
var actualPosition = actualBounds.Center;
|
var actualPosition = actualBounds.Center;
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 立即读回位置(可能滞后): " +
|
$"[模型增量姿态] {item.DisplayName} 立即读回位置(可能滞后): " +
|
||||||
$"实际=({actualPosition.X:F3},{actualPosition.Y:F3},{actualPosition.Z:F3}), " +
|
$"实际=({actualPosition.X:F3},{actualPosition.Y:F3},{actualPosition.Z:F3}), " +
|
||||||
$"期望=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.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})");
|
$"偏差=({actualPosition.X - targetPosition.X:F3},{actualPosition.Y - targetPosition.Y:F3},{actualPosition.Z - targetPosition.Z:F3})");
|
||||||
|
|
||||||
LogManager.Debug(
|
LogManager.Info(
|
||||||
$"[模型增量姿态] {item.DisplayName} 立即读回旋转(可能滞后/不反映override): " +
|
$"[模型增量姿态] {item.DisplayName} 立即读回旋转(可能滞后/不反映override): " +
|
||||||
$"实际X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " +
|
$"实际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}), " +
|
$"实际Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " +
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user