tian/NavisworksTransport
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix ground real-object forward axis and compensation

Browse Source
This commit is contained in:
tian 2026-03-25 17:19:21 +08:00
parent aabf263f0d
commit 921dc07856
9 changed files with 649 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
TransportPlugin.csproj
View File

@ -522,11 +522,6 @@
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
<Link>resources\unit_cube.nwc</Link>
</None>
<!-- Unit Cube YUp NWC Model File -->
<None Include="resources\unit_cube_yup.nwc">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
<Link>resources\unit_cube_yup.nwc</Link>
</None>
<!-- Unit Cylinder NWC Model File -->
<None Include="resources\unit_cylinder.nwc">
<CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>

59
UnitTests/CoordinateSystem/PathTargetFrameResolverTests.cs
View File

@ -1,5 +1,6 @@
using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System.Collections.Generic;
using System.Numerics;
namespace NavisworksTransport.UnitTests.CoordinateSystem
@ -35,6 +36,64 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(1.0, frame.Side.Length(), 1e-6);
}
[TestMethod]
public void Hoisting_StartForward_ShouldUseHorizontalSegmentInsteadOfInitialLift()
{
var pathPoints = new List<Vector3>
{
new Vector3(-105.992f, -28.615f, 77.043f),
new Vector3(-105.992f, 16.230f, 77.043f),
new Vector3(-229.870f, 16.230f, -32.982f)
};
bool ok = PathTargetFrameResolver.TryResolvePlanarStartHostForward(
NavisworksTransport.PathType.Hoisting,
pathPoints,
out Vector3 hostForward);
Assert.IsTrue(ok);
AssertVector(Vector3.Normalize(hostForward), -0.7477, 0.0, -0.6640, 1e-4);
}
[TestMethod]
public void Hoisting_StartFrame_ShouldUseHorizontalSegmentAndKeepHostUp()
{
var pathPoints = new List<Vector3>
{
new Vector3(-105.992f, -28.615f, 77.043f),
new Vector3(-105.992f, 16.230f, 77.043f),
new Vector3(-229.870f, 16.230f, -32.982f)
};
bool ok = PathTargetFrameResolver.TryCreatePlanarStartHostFrame(
NavisworksTransport.PathType.Hoisting,
pathPoints,
CoordinateSystemType.YUp,
out PathTargetFrame frame);
Assert.IsTrue(ok);
AssertVector(frame.Up, 0.0, 1.0, 0.0);
Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
}
[TestMethod]
public void YUp_PlanarYaw_ShouldUseHostXZPlane_NotHostXY()
{
bool ok1 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
new Vector3(1.0f, 0.0f, 0.0f),
CoordinateSystemType.YUp,
out double yaw1);
bool ok2 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
new Vector3(0.0f, 0.0f, 1.0f),
CoordinateSystemType.YUp,
out double yaw2);
Assert.IsTrue(ok1);
Assert.IsTrue(ok2);
Assert.AreEqual(0.0, yaw1, 1e-6);
Assert.AreEqual(-System.Math.PI / 2.0, yaw2, 1e-6);
}
private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
{
Assert.AreEqual(x, actual.X, tolerance);

28
UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs
View File

@ -124,6 +124,34 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
1e-4);
}
[TestMethod]
public void FixedPositiveX_ShouldNotSwitchToZ_WhenPathLeansTowardZ()
{
Quaternion referenceRotation = Quaternion.Identity;
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.690f, 0.0f, 0.724f));
Vector3 desiredUp = Vector3.UnitY;
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
referenceRotation,
desiredForward,
desiredUp,
LocalAxisDirection.PositiveX,
out RealObjectPlanarPoseSolution solution);
Assert.IsTrue(ok);
Assert.AreEqual(LocalAxisDirection.PositiveX, solution.SelectedReferenceAxisDirection);
AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);
Vector3 transformedSelectedAxis = Vector3.Normalize(
Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
AssertVector(
transformedSelectedAxis,
desiredForward.X,
desiredForward.Y,
desiredForward.Z,
1e-4);
}
private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6)
{
Assert.AreEqual(x, vector.X, tolerance);

BIN
resources/unit_cube_yup.nwc
View File

Binary file not shown.

26
resources/unit_cube_yup.obj
View File

@ -1,26 +0,0 @@
# unit cube for virtual object (Y-up)
# dimensions: 1.0 x 1.0 x 1.0
# centered at origin
# local +X = forward, local +Y = up, local +Z = side
# when Navisworks imports OBJ as centimeters, generated NWC becomes 0.01m base size
o unit_cube_yup
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 0.500000 -0.500000
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 0.500000
f 1 2 3
f 1 3 4
f 5 8 7
f 5 7 6
f 1 5 6
f 1 6 2
f 2 6 7
f 2 7 3
f 3 7 8
f 3 8 4
f 4 8 5
f 4 5 1

413
src/Core/Animation/PathAnimationManager.cs
View File

@ -1,4 +1,4 @@
using System;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
@ -185,6 +185,12 @@ namespace NavisworksTransport.Core.Animation
private double _currentYaw = 0.0; // 当前偏航角（弧度）
private Rotation3D _trackedRotation = Rotation3D.Identity; // 动画管理器内部跟踪的完整姿态
private bool _hasTrackedRotation = false; // 当前是否使用完整姿态
private Rotation3D _groundRealObjectBaseRotation = Rotation3D.Identity;
private double _groundRealObjectBaseYaw = 0.0;
private bool _hasGroundRealObjectBasePose = false;
private Vector3D _groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
private bool _hasGroundRealObjectStartCompensation = false;
private bool _suppressGroundRealObjectCompensation = false;
// TimeLiner 集成
private TimeLinerIntegrationManager _timeLinerManager;
@ -464,6 +470,8 @@ namespace NavisworksTransport.Core.Animation
_trackedPosition = GetTrackedObjectPosition(objectToRestore);
string objectName = isVirtual ? "虚拟物体" : objectToRestore.DisplayName;
_groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
_hasGroundRealObjectStartCompensation = false;
LogManager.Info($"[归位] {objectName} 已彻底恢复到原始位置, yaw={_currentYaw:F3}");
}
catch (Exception ex)
@ -718,7 +726,8 @@ namespace NavisworksTransport.Core.Animation
}
// 根据路径类型调整起点位置
Point3D startPosition = _pathPoints[0];
Point3D pathStartPoint = _pathPoints[0];
Point3D startPosition = pathStartPoint;
if (_route.PathType == PathType.Hoisting)
{
// 吊装路径：第一个路径点（起吊点）是地面位置，动画跟踪点统一使用几何中心
@ -758,11 +767,70 @@ namespace NavisworksTransport.Core.Animation
LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
}
LogManager.Info(
$"[路径起点诊断] 路径point0=({pathStartPoint.X:F3},{pathStartPoint.Y:F3},{pathStartPoint.Z:F3}), " +
$"起点目标trackedPoint=({startPosition.X:F3},{startPosition.Y:F3},{startPosition.Z:F3}), 路径类型={_route.PathType.GetDisplayName()}");
if (_route.PathType != PathType.Rail &&
TryCreatePlanarPathRotationAtStart(out var planarRotation))
{
UpdateObjectPosition(startPosition, planarRotation);
LogManager.Info("[移动到起点] 地面/吊装路径已应用完整三维姿态");
var startAppliedPoint = GetTrackedObjectPosition(CurrentControlledObject ?? _animatedObject);
LogManager.Info(
$"[路径起点诊断] 起点应用后实际trackedPoint=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), " +
$"相对目标偏差=({startAppliedPoint.X - startPosition.X:F3},{startAppliedPoint.Y - startPosition.Y:F3},{startAppliedPoint.Z - startPosition.Z:F3})");
if (IsRealObjectMode && _route.PathType == PathType.Ground)
{
var startOffset = new Vector3D(
startPosition.X - startAppliedPoint.X,
startPosition.Y - startAppliedPoint.Y,
startPosition.Z - startAppliedPoint.Z);
double startOffsetLength = Math.Sqrt(
startOffset.X * startOffset.X +
startOffset.Y * startOffset.Y +
startOffset.Z * startOffset.Z);
LogManager.Info(
$"[路径起点补偿] 首次落位偏差=({startOffset.X:F3},{startOffset.Y:F3},{startOffset.Z:F3}), 长度={startOffsetLength:F3}");
if (startOffsetLength > 1e-3)
{
_groundRealObjectStartCompensation = startOffset;
_hasGroundRealObjectStartCompensation = true;
UpdateObjectPosition(startPosition, planarRotation);
var correctedAppliedPoint = GetTrackedObjectPosition(CurrentControlledObject ?? _animatedObject);
LogManager.Info(
$"[路径起点补偿] 补偿后实际trackedPoint=({correctedAppliedPoint.X:F3},{correctedAppliedPoint.Y:F3},{correctedAppliedPoint.Z:F3}), " +
$"相对目标偏差=({correctedAppliedPoint.X - startPosition.X:F3},{correctedAppliedPoint.Y - startPosition.Y:F3},{correctedAppliedPoint.Z - startPosition.Z:F3})");
}
else
{
_groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
_hasGroundRealObjectStartCompensation = false;
}
}
if (IsRealObjectMode && _route.PathType == PathType.Ground)
{
_groundRealObjectBaseRotation = planarRotation;
_hasGroundRealObjectBasePose = TryResolveGroundRealObjectBaseYaw(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
{
_hasGroundRealObjectBasePose = false;
_groundRealObjectStartCompensation = new Vector3D(0, 0, 0);
_hasGroundRealObjectStartCompensation = false;
}
}
else if (_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting)
{
@ -773,6 +841,11 @@ namespace NavisworksTransport.Core.Animation
{
// 使用 UpdateObjectPosition 统一处理移动和旋转
UpdateObjectPosition(startPosition, yaw);
var startAppliedPoint = GetTrackedObjectPosition(CurrentControlledObject ?? _animatedObject);
LogManager.Info(
$"[路径起点诊断] 起点应用后实际trackedPoint=({startAppliedPoint.X:F3},{startAppliedPoint.Y:F3},{startAppliedPoint.Z:F3}), " +
$"相对目标偏差=({startAppliedPoint.X - startPosition.X:F3},{startAppliedPoint.Y - startPosition.Y:F3},{startAppliedPoint.Z - startPosition.Z:F3})");
}
string pathTypeName = _route.PathType.GetDisplayName();
@ -1108,8 +1181,18 @@ namespace NavisworksTransport.Core.Animation
else if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) &&
TryCreatePlanarPathRotationForFrame(previousFramePoint, framePosition, nextFramePoint, out var planarRotation))
{
frame.Rotation = planarRotation;
frame.HasCustomRotation = true;
if (_route.PathType == PathType.Ground &&
IsRealObjectMode &&
TryCreateGroundRealObjectConstrainedRotation(previousFramePoint, framePosition, nextFramePoint, out var constrainedRotation))
{
frame.Rotation = constrainedRotation;
frame.HasCustomRotation = true;
}
else
{
frame.Rotation = planarRotation;
frame.HasCustomRotation = true;
}
}
else if (_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting)
{
@ -1834,8 +1917,18 @@ namespace NavisworksTransport.Core.Animation
// 但后续碰撞链又按最终姿态重新摆正，造成“结束后又往前窜一点”。
if (_animationFrames != null && _animationFrames.Count > 0)
{
_currentFrameIndex = _animationFrames.Count - 1;
ApplyAnimationFramePose(_animationFrames[_currentFrameIndex], _currentFrameIndex);
int lastFrameIndex = _animationFrames.Count - 1;
if (_currentFrameIndex != lastFrameIndex)
{
_currentFrameIndex = lastFrameIndex;
ApplyAnimationFramePose(_animationFrames[_currentFrameIndex], _currentFrameIndex);
LogManager.Info("[动画结束] 当前尚未到最后一帧，已补落最终姿态");
}
else
{
LogManager.Info("[动画结束] 当前已在最后一帧，保持真实结束位置，不再重复落位");
}
ProgressChanged?.Invoke(this, 100.0);
try
{
@ -2496,6 +2589,7 @@ namespace NavisworksTransport.Core.Animation
Point3D currentPositionForTransform = _trackedPosition;
Rotation3D currentRotation;
Rotation3D appliedTargetRotation = newRotation;
Point3D appliedTargetPosition = newPosition;
if (_hasTrackedRotation)
{
currentRotation = _trackedRotation;
@ -2517,17 +2611,54 @@ namespace NavisworksTransport.Core.Animation
currentPositionForTransform = actualHostPosition;
LogManager.Info(
$"[动画姿态入口] {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))
{
var trackedLinear = new Transform3D(currentRotation).Linear;
var actualLinear = new Transform3D(actualGeometryRotation).Linear;
LogManager.Info(
$"[平面姿态基线诊断] {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.Info(
$"[平面姿态基线诊断] {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})");
}
}
catch (Exception ex)
{
LogManager.Warning($"[动画姿态入口] 读取宿主实际状态失败: {ex.Message}");
}
if (IsRealObjectMode &&
_route?.PathType == PathType.Ground &&
!_suppressGroundRealObjectCompensation &&
_hasGroundRealObjectStartCompensation &&
_hasGroundRealObjectBasePose &&
TryRotateGroundRealObjectCompensation(appliedTargetRotation, out var rotatedCompensation))
{
appliedTargetPosition = new Point3D(
newPosition.X - rotatedCompensation.X,
newPosition.Y - rotatedCompensation.Y,
newPosition.Z - rotatedCompensation.Z);
LogManager.Info(
$"[Ground路径补偿] {controlledObject.DisplayName} 原目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3}), " +
$"起点补偿=({_groundRealObjectStartCompensation.X:F3},{_groundRealObjectStartCompensation.Y:F3},{_groundRealObjectStartCompensation.Z:F3}), " +
$"旋转后补偿=({rotatedCompensation.X:F3},{rotatedCompensation.Y:F3},{rotatedCompensation.Z:F3}), " +
$"应用目标点=({appliedTargetPosition.X:F3},{appliedTargetPosition.Y:F3},{appliedTargetPosition.Z:F3})");
}
var currentLinear = new Transform3D(currentRotation).Linear;
var targetLinear = new Transform3D(appliedTargetRotation).Linear;
LogManager.Info(
$"[动画姿态入口] {controlledObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
$"目标点=({appliedTargetPosition.X:F3},{appliedTargetPosition.Y:F3},{appliedTargetPosition.Z:F3})");
LogManager.Info(
$"[动画姿态入口] {controlledObject.DisplayName} 跟踪姿态: " +
$"X=({currentLinear.Get(0, 0):F4},{currentLinear.Get(1, 0):F4},{currentLinear.Get(2, 0):F4}), " +
@ -2542,10 +2673,10 @@ namespace NavisworksTransport.Core.Animation
controlledObject,
currentPositionForTransform,
currentRotation,
newPosition,
appliedTargetPosition,
appliedTargetRotation);
_trackedPosition = newPosition;
_trackedPosition = appliedTargetPosition;
_trackedRotation = appliedTargetRotation;
_hasTrackedRotation = true;
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(appliedTargetRotation);
@ -2737,13 +2868,22 @@ namespace NavisworksTransport.Core.Animation
$"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
}
if (_savedObjectHasCustomRotation)
bool previousSuppressCompensation = _suppressGroundRealObjectCompensation;
_suppressGroundRealObjectCompensation = true;
try
{
UpdateObjectPosition(_savedObjectPosition, _savedObjectRotation);
if (_savedObjectHasCustomRotation)
{
UpdateObjectPosition(_savedObjectPosition, _savedObjectRotation);
}
else
{
UpdateObjectPosition(_savedObjectPosition, _savedObjectYaw);
}
}
else
finally
{
UpdateObjectPosition(_savedObjectPosition, _savedObjectYaw);
_suppressGroundRealObjectCompensation = previousSuppressCompensation;
}
_trackedPosition = _savedObjectPosition;
@ -3533,6 +3673,154 @@ namespace NavisworksTransport.Core.Animation
return adapter.FromCanonicalPoint(new Point3D(canonicalCenter.X, canonicalCenter.Y, canonicalCenter.Z));
}
private bool TryResolveGroundRealObjectBaseYaw(out double yawRadians)
{
yawRadians = 0.0;
if (_route?.PathType != PathType.Ground ||
!IsRealObjectMode ||
_pathPoints == null ||
_pathPoints.Count < 2)
{
return false;
}
var hostType = CoordinateSystemManager.Instance.ResolvedType;
var hostPoints = new List<Vector3>(_pathPoints.Count);
for (int i = 0; i < _pathPoints.Count; i++)
{
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);
}
private bool TryCreateGroundRealObjectConstrainedRotation(
Point3D previousFramePoint,
Point3D currentFramePoint,
Point3D nextFramePoint,
out Rotation3D rotation)
{
rotation = Rotation3D.Identity;
if (!IsRealObjectMode ||
_route?.PathType != PathType.Ground ||
!_hasGroundRealObjectBasePose)
{
return false;
}
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(
new Vector3(
(float)(nextFramePoint.X - previousFramePoint.X),
(float)(nextFramePoint.Y - previousFramePoint.Y),
(float)(nextFramePoint.Z - previousFramePoint.Z)),
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();
Vector3 hostUp = Vector3.Normalize(adapter.HostUpVector3);
double deltaYaw = NormalizeRadians(targetYawRadians - _groundRealObjectBaseYaw);
Quaternion deltaQuaternion = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYaw);
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_groundRealObjectBaseRotation);
Quaternion targetQuaternion = Quaternion.Normalize(deltaQuaternion * baseQuaternion);
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
return true;
}
private bool TryCreateGroundRealObjectConstrainedRotationFromHostForward(
Vector3 hostForward,
out Rotation3D rotation)
{
rotation = Rotation3D.Identity;
if (!IsRealObjectMode ||
_route?.PathType != PathType.Ground ||
!_hasGroundRealObjectBasePose)
{
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();
Vector3 hostUp = Vector3.Normalize(adapter.HostUpVector3);
double deltaYaw = NormalizeRadians(targetYawRadians - _groundRealObjectBaseYaw);
Quaternion deltaQuaternion = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYaw);
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_groundRealObjectBaseRotation);
Quaternion targetQuaternion = Quaternion.Normalize(deltaQuaternion * baseQuaternion);
rotation = adapter.FromHostQuaternionDirect(targetQuaternion);
return true;
}
private static Quaternion Rotation3DToHostQuaternion(Rotation3D rotation)
{
var linear = new Transform3D(rotation).Linear;
var matrix = new Matrix4x4(
(float)linear.Get(0, 0), (float)linear.Get(0, 1), (float)linear.Get(0, 2), 0f,
(float)linear.Get(1, 0), (float)linear.Get(1, 1), (float)linear.Get(1, 2), 0f,
(float)linear.Get(2, 0), (float)linear.Get(2, 1), (float)linear.Get(2, 2), 0f,
0f, 0f, 0f, 1f);
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(matrix));
}
private bool TryRotateGroundRealObjectCompensation(Rotation3D targetRotation, out Vector3D rotatedCompensation)
{
rotatedCompensation = _groundRealObjectStartCompensation;
if (!_hasGroundRealObjectBasePose || !_hasGroundRealObjectStartCompensation)
{
return false;
}
Quaternion baseQuaternion = Rotation3DToHostQuaternion(_groundRealObjectBaseRotation);
Quaternion targetQuaternion = Rotation3DToHostQuaternion(targetRotation);
Quaternion deltaQuaternion = Quaternion.Normalize(targetQuaternion * Quaternion.Inverse(baseQuaternion));
Vector3 baseCompensation = new Vector3(
(float)_groundRealObjectStartCompensation.X,
(float)_groundRealObjectStartCompensation.Y,
(float)_groundRealObjectStartCompensation.Z);
Vector3 rotatedVector = Vector3.Transform(baseCompensation, deltaQuaternion);
rotatedCompensation = new Vector3D(rotatedVector.X, rotatedVector.Y, rotatedVector.Z);
return true;
}
private static double NormalizeRadians(double angle)
{
while (angle > Math.PI)
{
angle -= 2.0 * Math.PI;
}
while (angle < -Math.PI)
{
angle += 2.0 * Math.PI;
}
return angle;
}
private static Vector3 ToNumerics(Point3D point)
{
return new Vector3((float)point.X, (float)point.Y, (float)point.Z);
@ -3672,12 +3960,66 @@ namespace NavisworksTransport.Core.Animation
hostForward = ToNumerics(nextPoint) - ToNumerics(currentPoint);
}
return TryCalculateCurrentRealObjectPlanarProjectedExtents(
hostForward,
out convention,
out extents);
}
private bool TryCalculateCurrentRealObjectPlanarProjectedExtents(
Vector3 hostForward,
out ModelAxisConvention convention,
out (double forwardExtent, double sideExtent, double upExtent) extents)
{
convention = null;
extents = (0.0, 0.0, 0.0);
if (!IsRealObjectMode ||
!_hasRealObjectReferenceRotation ||
_realObjectLength <= 0.0 ||
_realObjectWidth <= 0.0 ||
_realObjectHeight <= 0.0)
{
return false;
}
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
if (!PathTargetFrameResolver.TryCreatePlanarHostFrame(hostForward, adapter.HostType, out var targetFrame))
{
return false;
}
if (_route?.PathType == PathType.Ground &&
_hasGroundRealObjectBasePose &&
TryCreateGroundRealObjectConstrainedRotationFromHostForward(hostForward, out var constrainedRotation))
{
if (!TryCreateRealObjectPlanarPoseSolution(
targetFrame.Forward,
targetFrame.Up,
out var constrainedSolution))
{
return false;
}
LocalAxisDirection constrainedSemanticUpAxis =
adapter.HostType == CoordinateSystemType.YUp
? LocalAxisDirection.PositiveY
: LocalAxisDirection.PositiveZ;
convention = new ModelAxisConvention(constrainedSolution.SelectedReferenceAxisDirection, constrainedSemanticUpAxis);
Quaternion constrainedFinalHostQuaternion = Rotation3DToHostQuaternion(constrainedRotation);
extents = RealObjectProjectedExtentResolver.CalculateProjectedSemanticExtents(
convention,
_realObjectLength,
_realObjectWidth,
_realObjectHeight,
constrainedSolution.BaselineRotation,
constrainedFinalHostQuaternion,
targetFrame.Forward,
targetFrame.Up);
return true;
}
if (!TryCreateRealObjectPlanarPoseSolution(
targetFrame.Forward,
targetFrame.Up,
@ -3724,14 +4066,16 @@ namespace NavisworksTransport.Core.Animation
return false;
}
Point3D previousPoint = _pathPoints[0];
Point3D currentPoint = _pathPoints[0];
Point3D nextPoint = _pathPoints.Count > 1 ? _pathPoints[1] : _pathPoints[0];
if (!PathTargetFrameResolver.TryResolvePlanarStartHostForward(
_route.PathType,
_pathPoints,
out var hostForward))
{
return false;
}
if (!TryCalculateCurrentRealObjectPlanarProjectedExtents(
previousPoint,
currentPoint,
nextPoint,
hostForward,
out _,
out var extents))
{
@ -3858,22 +4202,17 @@ namespace NavisworksTransport.Core.Animation
return false;
}
if (_route?.PathType == PathType.Hoisting)
if (PathTargetFrameResolver.TryResolvePlanarStartHostForward(
_route?.PathType ?? PathType.Ground,
_pathPoints,
out var hostForward))
{
if (_pathPoints.Count < 3)
{
return false;
}
return TryCreatePlanarPathRotationFromHostForward(
new Vector3D(
_pathPoints[2].X - _pathPoints[1].X,
_pathPoints[2].Y - _pathPoints[1].Y,
_pathPoints[2].Z - _pathPoints[1].Z),
new Vector3D(hostForward.X, hostForward.Y, hostForward.Z),
out rotation);
}
return TryCreatePlanarPathRotationFromHostPoints(_pathPoints[0], _pathPoints[0], _pathPoints[1], out rotation);
return false;
}
private bool TryCreatePlanarPathRotationForFrame(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, out Rotation3D rotation)
@ -4043,10 +4382,11 @@ namespace NavisworksTransport.Core.Animation
}
LocalAxisDirection? fixedForwardAxis = null;
if ((_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) &&
_hasRealObjectPlanarSelectedForwardAxis)
if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting)
{
fixedForwardAxis = _realObjectPlanarSelectedForwardAxis;
// Ground/Hoisting 的真实物体前进轴采用固定对象语义：
// 真实物体默认以本地 +X 表示前进方向，不能因为路径更偏向 Z 就切换成 +Z。
fixedForwardAxis = LocalAxisDirection.PositiveX;
}
if (!RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
@ -4060,10 +4400,11 @@ namespace NavisworksTransport.Core.Animation
}
if ((_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) &&
!_hasRealObjectPlanarSelectedForwardAxis)
(!_hasRealObjectPlanarSelectedForwardAxis || _realObjectPlanarSelectedForwardAxis != LocalAxisDirection.PositiveX))
{
_realObjectPlanarSelectedForwardAxis = solution.SelectedReferenceAxisDirection;
_realObjectPlanarSelectedForwardAxis = LocalAxisDirection.PositiveX;
_hasRealObjectPlanarSelectedForwardAxis = true;
LogManager.Info("[真实物体平面前进轴] Ground/Hoisting 已固定使用 PositiveX 作为对象前进轴语义。");
}
return true;
@ -5292,3 +5633,5 @@ namespace NavisworksTransport.Core.Animation
#endregion
}
}

42
src/Core/VirtualObjectManager.cs
View File

@ -236,19 +236,8 @@ namespace NavisworksTransport.Core
{
var doc = Application.ActiveDocument;
var modelItems = new ModelItemCollection { _virtualObjectModelItem };
var currentTransform = _virtualObjectModelItem.Transform;
var currentComponents = currentTransform.Factor();
var currentScale = currentComponents.Scale;
doc.Models.ResetPermanentTransform(modelItems);
var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
var components = identity.Factor();
components.Scale = currentScale;
Transform3D newTransform = components.Combine();
doc.Models.OverridePermanentTransform(modelItems, newTransform, false);
LogManager.Info("虚拟物体已恢复到CAD原始位置");
LogManager.Info("虚拟物体已恢复到CAD原始位置（仅清除Item姿态，保留Model尺寸）");
}
catch (Exception ex)
{
@ -271,32 +260,8 @@ namespace NavisworksTransport.Core
_currentLengthMeters = lengthMeters;
_currentWidthMeters = widthMeters;
_currentHeightMeters = heightMeters;
// 应用新缩放
ScaleVirtualObject(lengthMeters, widthMeters, heightMeters);
var currentTransform = _virtualObjectModelItem.Transform;
var currentComponents = currentTransform.Factor();
var currentTranslation = currentComponents.Translation;
var currentRotation = currentComponents.Rotation;
bool hasTranslation = currentTranslation.X != 0 || currentTranslation.Y != 0 || currentTranslation.Z != 0;
var rotationResult = currentRotation.ToAxisAndAngle();
bool hasRotation = Math.Abs(rotationResult.Angle) > 0.001;
if (hasTranslation || hasRotation)
{
var newTransform = _virtualObjectModelItem.Transform;
var newComponents = newTransform.Factor();
newComponents.Translation = currentTranslation;
newComponents.Rotation = currentRotation;
Transform3D combinedTransform = newComponents.Combine();
var doc = Application.ActiveDocument;
var modelItems = new ModelItemCollection { _virtualObjectModelItem };
doc.Models.SetModelUnitsAndTransform(_virtualObjectModel, _virtualObjectModel.Units, combinedTransform, false);
}
LogManager.Info($"虚拟物体尺寸已更新");
LogManager.Info("虚拟物体尺寸已更新（仅更新Model尺寸）");
}
finally
{
@ -399,8 +364,7 @@ namespace NavisworksTransport.Core
string[] possiblePaths = new[]
{
Path.Combine(pluginDir, "resources", "unit_cube.nwc"),
Path.Combine(pluginDir, "..", "..", "resources", "unit_cube.nwc"),
@"c:\Users\Tellme\apps\NavisworksTransport\resources\unit_cube.nwc"
Path.Combine(pluginDir, "..", "..", "resources", "unit_cube.nwc")
};
foreach (var path in possiblePaths)

48
src/UI/WPF/ViewModels/PathEditingViewModel.cs
View File

@ -5188,11 +5188,49 @@ namespace NavisworksTransport.UI.WPF.ViewModels
}
else if (animationVm.SelectedAnimatedObject != null)
{
// 使用选择物体时保存的原始尺寸（米），不受旋转影响
objectLengthModel = animationVm.ObjectOriginalLength * metersToUnits;
objectWidthModel = animationVm.ObjectOriginalWidth * metersToUnits;
objectHeightModel = animationVm.ObjectOriginalHeight * metersToUnits;
LogManager.Debug($"[物体参数同步] 使用选择物体保存的原始尺寸: {animationVm.ObjectOriginalLength:F2}m x {animationVm.ObjectOriginalWidth:F2}m x {animationVm.ObjectOriginalHeight:F2}m");
var animationManager = NavisworksTransport.Core.Animation.PathAnimationManager.GetInstance();
if ((pathType == PathType.Ground || pathType == PathType.Hoisting) &&
animationManager != null &&
animationManager.TryGetCurrentRouteRealObjectPlanarProjectedExtents(
out double resolvedPlanarForward,
out double resolvedPlanarSide,
out double resolvedPlanarUp))
{
objectLengthModel = resolvedPlanarForward;
objectWidthModel = resolvedPlanarSide;
objectHeightModel = resolvedPlanarUp;
LogManager.Debug(
$"[物体参数同步] 使用真实物体平面路径最终姿态尺寸: " +
$"沿路径={resolvedPlanarForward / metersToUnits:F2}m, " +
$"垂直路径={resolvedPlanarSide / metersToUnits:F2}m, " +
$"法线={resolvedPlanarUp / metersToUnits:F2}m");
}
else if (pathType == PathType.Rail &&
animationManager != null &&
animationManager.TryGetCurrentRouteRealObjectRailProjectedExtents(
out double resolvedRailForward,
out double resolvedRailSide,
out double resolvedRailUp))
{
objectLengthModel = resolvedRailForward;
objectWidthModel = resolvedRailSide;
objectHeightModel = resolvedRailUp;
LogManager.Debug(
$"[物体参数同步] 使用真实物体 Rail 最终姿态尺寸: " +
$"沿路径={resolvedRailForward / metersToUnits:F2}m, " +
$"垂直路径={resolvedRailSide / metersToUnits:F2}m, " +
$"法线={resolvedRailUp / metersToUnits:F2}m");
}
else
{
// 回退：使用选择物体时保存的原始尺寸（米）
objectLengthModel = animationVm.ObjectOriginalLength * metersToUnits;
objectWidthModel = animationVm.ObjectOriginalWidth * metersToUnits;
objectHeightModel = animationVm.ObjectOriginalHeight * metersToUnits;
LogManager.Debug(
$"[物体参数同步] 回退使用选择物体保存的原始尺寸: " +
$"{animationVm.ObjectOriginalLength:F2}m x {animationVm.ObjectOriginalWidth:F2}m x {animationVm.ObjectOriginalHeight:F2}m");
}
}
else
{

138
src/Utils/CoordinateSystem/PathTargetFrameResolver.cs
View File

@ -1,3 +1,4 @@
using System.Collections.Generic;
using System.Numerics;
using Autodesk.Navisworks.Api;
@ -9,6 +10,107 @@ namespace NavisworksTransport.Utils.CoordinateSystem
/// </summary>
public static class PathTargetFrameResolver
{
public static bool TryResolvePlanarStartHostForward(
NavisworksTransport.PathType pathType,
IReadOnlyList<Point3D> pathPoints,
out Vector3 hostForward)
{
if (pathPoints == null)
{
hostForward = Vector3.Zero;
return false;
}
var hostPoints = new List<Vector3>(pathPoints.Count);
for (int i = 0; i < pathPoints.Count; i++)
{
hostPoints.Add(new Vector3(
(float)pathPoints[i].X,
(float)pathPoints[i].Y,
(float)pathPoints[i].Z));
}
return TryResolvePlanarStartHostForward(pathType, hostPoints, out hostForward);
}
public static bool TryResolvePlanarStartHostForward(
NavisworksTransport.PathType pathType,
IReadOnlyList<Vector3> pathPoints,
out Vector3 hostForward)
{
hostForward = Vector3.Zero;
if (pathPoints == null)
{
return false;
}
if (pathType == NavisworksTransport.PathType.Hoisting)
{
if (pathPoints.Count < 3)
{
return false;
}
hostForward = new Vector3(
(float)(pathPoints[2].X - pathPoints[1].X),
(float)(pathPoints[2].Y - pathPoints[1].Y),
(float)(pathPoints[2].Z - pathPoints[1].Z));
return hostForward.LengthSquared() >= 1e-9f;
}
if (pathPoints.Count < 2)
{
return false;
}
hostForward = new Vector3(
(float)(pathPoints[1].X - pathPoints[0].X),
(float)(pathPoints[1].Y - pathPoints[0].Y),
(float)(pathPoints[1].Z - pathPoints[0].Z));
return hostForward.LengthSquared() >= 1e-9f;
}
public static bool TryCreatePlanarStartHostFrame(
NavisworksTransport.PathType pathType,
IReadOnlyList<Point3D> pathPoints,
CoordinateSystemType hostType,
out PathTargetFrame frame)
{
if (pathPoints == null)
{
frame = null;
return false;
}
var hostPoints = new List<Vector3>(pathPoints.Count);
for (int i = 0; i < pathPoints.Count; i++)
{
hostPoints.Add(new Vector3(
(float)pathPoints[i].X,
(float)pathPoints[i].Y,
(float)pathPoints[i].Z));
}
return TryCreatePlanarStartHostFrame(pathType, hostPoints, hostType, out frame);
}
public static bool TryCreatePlanarStartHostFrame(
NavisworksTransport.PathType pathType,
IReadOnlyList<Vector3> pathPoints,
CoordinateSystemType hostType,
out PathTargetFrame frame)
{
frame = null;
if (!TryResolvePlanarStartHostForward(pathType, pathPoints, out var hostForward))
{
return false;
}
return TryCreatePlanarHostFrame(hostForward, hostType, out frame);
}
public static bool TryCreatePlanarHostFrame(
Vector3 hostForward,
CoordinateSystemType hostType,
@ -28,6 +130,42 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return true;
}
public static bool TryResolvePlanarHostYaw(
Vector3 hostForward,
CoordinateSystemType hostType,
out double yawRadians)
{
yawRadians = 0.0;
if (hostForward.LengthSquared() < 1e-9f)
{
return false;
}
var adapter = new HostCoordinateAdapter(hostType);
Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward);
canonicalForward.Z = 0f;
if (canonicalForward.LengthSquared() < 1e-9f)
{
return false;
}
canonicalForward = Vector3.Normalize(canonicalForward);
yawRadians = System.Math.Atan2(canonicalForward.Y, canonicalForward.X);
return true;
}
public static bool TryResolvePlanarStartHostYaw(
NavisworksTransport.PathType pathType,
IReadOnlyList<Vector3> pathPoints,
CoordinateSystemType hostType,
out double yawRadians)
{
yawRadians = 0.0;
return TryResolvePlanarStartHostForward(pathType, pathPoints, out var hostForward) &&
TryResolvePlanarHostYaw(hostForward, hostType, out yawRadians);
}
public static bool TryCreateRailHostFrame(
PathRoute route,
Point3D previousPoint,