Interpret fragment up for real object pose

This commit is contained in:
tian 2026-03-24 21:04:24 +08:00
parent 8d10f959b2
commit 290df34dec
10 changed files with 494 additions and 214 deletions

View File

@ -133,6 +133,35 @@
禁止再使用“本地坐标系”这种含糊说法。
### 4.1.1 Quaternion / Rotation3D 固定定义
这是项目级硬约束,不允许在任何修复中重新猜测、重新验证或临时改口:
- Navisworks `Rotation3D(double, double, double, double)` 的参数顺序固定为:
- `x, y, z, w`
- `Rotation3D.A/B/C/D` 与 quaternion 分量的对应关系固定为:
- `A = x`
- `B = y`
- `C = z`
- `D = w`
- 当代码里已经拿到 quaternion `(qx, qy, qz, qw)` 时,唯一允许的构造方式是:
```csharp
var rotation = new Rotation3D(qx, qy, qz, qw);
```
- 严禁再写成:
```csharp
var rotation = new Rotation3D(qw, qx, qy, qz); // 错误
```
- 以后遇到姿态问题,禁止再把故障归因到 “Navisworks 四元数分量顺序可能又不一样了”。
- 如果问题涉及真实物体或 fragment 参考姿态的轴语义,优先检查:
- fragment 三轴的业务解释是否正确
- 参考姿态是否应该用显式三轴而不是只传 quaternion
- 宿主坐标系 / 内部坐标系 / 资产坐标系 是否被混用
### 4.2 真实物体 vs 虚拟物体
这是当前架构里最容易被改坏的地方。

View File

@ -340,6 +340,7 @@
<Compile Include="src\Utils\CoordinateSystem\CanonicalRailOffsetResolver.cs" />
<Compile Include="src\Utils\CoordinateSystem\CanonicalRailPoseBuilder.cs" />
<Compile Include="src\Utils\CoordinateSystem\CanonicalTrackedPositionResolver.cs" />
<Compile Include="src\Utils\CoordinateSystem\FragmentDefaultUpContext.cs" />
<Compile Include="src\Utils\CoordinateSystem\FragmentRepresentativePoseHelper.cs" />
<Compile Include="src\Utils\CoordinateSystem\RealObjectPlanarPoseSolver.cs" />
<Compile Include="src\Utils\CoordinateSystem\ObjectSpaceOrientationHelper.cs" />

View File

@ -98,6 +98,55 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
AssertVector(Vector3.Transform(Vector3.UnitZ, rotation), axisZ, 1e-4);
}
[TestMethod]
public void RepresentativeFrame_ShouldExposeSameAxes_AsRepresentativeRotation()
{
Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f);
Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f);
double[] fragmentMatrix =
{
axisX.X, axisX.Y, axisX.Z, 0.0,
axisY.X, axisY.Y, axisY.Z, 0.0,
axisZ.X, axisZ.Y, axisZ.Z, 0.0,
0.0, 0.0, 0.0, 1.0
};
bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeFrame(
new[] { fragmentMatrix },
out FragmentRepresentativePoseHelper.RepresentativeFrame frame);
Assert.IsTrue(ok);
AssertVector(frame.AxisX, axisX, 1e-4);
AssertVector(frame.AxisY, axisY, 1e-4);
AssertVector(frame.AxisZ, axisZ, 1e-4);
AssertVector(Vector3.Transform(Vector3.UnitX, frame.Rotation), axisX, 1e-4);
AssertVector(Vector3.Transform(Vector3.UnitY, frame.Rotation), axisY, 1e-4);
AssertVector(Vector3.Transform(Vector3.UnitZ, frame.Rotation), axisZ, 1e-4);
}
[TestMethod]
public void TryInterpretRepresentativeFrame_YUpHost_WithFragmentDefaultUpZ_ShouldMapZToHostY()
{
var raw = new FragmentRepresentativePoseHelper.RepresentativeFrame(
new Vector3(1.0f, 0.0f, 0.0f),
new Vector3(0.0f, 0.0f, -1.0f),
new Vector3(0.0f, 1.0f, 0.0f),
Quaternion.Identity);
bool ok = FragmentRepresentativePoseHelper.TryInterpretRepresentativeFrame(
raw,
"Z",
"Y",
out var interpreted);
Assert.IsTrue(ok);
AssertVector(interpreted.AxisX, new Vector3(1.0f, 0.0f, 0.0f), 1e-4);
AssertVector(interpreted.AxisY, new Vector3(0.0f, 1.0f, 0.0f), 1e-4);
AssertVector(interpreted.AxisZ, new Vector3(0.0f, 0.0f, 1.0f), 1e-4);
}
private static double[] CreateFragmentMatrix(Quaternion rotation, Vector3 translation)
{
Vector3 axisX = Vector3.Transform(Vector3.UnitX, rotation);

View File

@ -2,7 +2,6 @@ using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System;
using System.Numerics;
using Autodesk.Navisworks.Api;
namespace NavisworksTransport.UnitTests.CoordinateSystem
{
@ -12,9 +11,7 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
[TestMethod]
public void ShouldChooseClosestCandidateAxis_FromRotatedReferencePose()
{
Quaternion referenceRotation = Quaternion.Normalize(
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0)));
Quaternion referenceRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0));
Vector3 desiredForward = new Vector3(1.0f, 0.2f, 0.1f);
Vector3 desiredUp = Vector3.UnitZ;
@ -26,48 +23,37 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.IsTrue(ok);
AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);
AssertVector(solution.ProjectedForward, 0.97590, 0.19518, 0.09759, 1e-4);
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4);
Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
AssertVector(transformedSelectedAxis, desiredForward.X / desiredForward.Length(), desiredForward.Y / desiredForward.Length(), desiredForward.Z / desiredForward.Length(), 1e-4);
}
[TestMethod]
public void ShouldPreferNegativeAxisWhenItMatchesForwardBest()
{
Quaternion referenceRotation = Quaternion.Identity;
Vector3 desiredForward = new Vector3(-0.1f, -1.0f, 0.05f);
Vector3 desiredUp = Vector3.UnitZ;
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
referenceRotation,
desiredForward,
desiredUp,
Quaternion.Identity,
new Vector3(-0.1f, -1.0f, 0.05f),
Vector3.UnitZ,
out RealObjectPlanarPoseSolution solution);
Assert.IsTrue(ok);
AssertVector(solution.SelectedReferenceAxisLocal, 0.0, -1.0, 0.0);
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4);
}
[TestMethod]
public void ShouldRotateWholeReferencePose_NotFlattenItToHostUpPlane()
public void ShouldRespectDesiredUpPlane_WhenDesiredForwardHasVerticalComponent()
{
Quaternion referenceRotation = Quaternion.Identity;
Vector3 desiredForward = Vector3.Normalize(new Vector3(1.0f, 1.0f, 1.0f));
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
referenceRotation,
Quaternion.Identity,
desiredForward,
Vector3.UnitY,
out RealObjectPlanarPoseSolution solution);
Assert.IsTrue(ok);
AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
}
@ -90,95 +76,16 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.IsTrue(ok);
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
Vector3 transformedX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, solution.BaselineRotation));
Vector3 transformedY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, solution.BaselineRotation));
Vector3 transformedZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation));
Vector3 transformedReferenceUp = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation));
Assert.AreEqual(1.0, transformedReferenceUp.Length(), 1e-4);
Assert.AreEqual(0.0, Vector3.Dot(transformedSelectedAxis, transformedReferenceUp), 1e-4);
}
[TestMethod]
public void YUp_HorizontalRealObject_ToAnotherHorizontalDirection_ShouldKeepVerticalAxis()
{
Vector3 referenceX = new Vector3(1.0f, 0.0f, 0.0f);
Vector3 referenceY = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 referenceZ = new Vector3(0.0f, -1.0f, 0.0f);
Quaternion referenceRotation = CreateQuaternionFromBasis(referenceX, referenceY, referenceZ);
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
Vector3 desiredUp = Vector3.UnitY;
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
referenceRotation,
desiredForward,
desiredUp,
out RealObjectPlanarPoseSolution solution);
Assert.IsTrue(ok);
AssertVector(solution.SelectedReferenceAxisLocal, -1.0, 0.0, 0.0);
Vector3 transformedForward = Vector3.Normalize(
Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
Vector3 transformedVertical = Vector3.Normalize(
Vector3.Transform(-Vector3.UnitZ, solution.BaselineRotation));
AssertVector(transformedForward, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
AssertVector(transformedVertical, 0.0, 1.0, 0.0, 1e-4);
}
private static Quaternion CreateQuaternionFromBasis(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
{
float m00 = axisX.X;
float m01 = axisY.X;
float m02 = axisZ.X;
float m10 = axisX.Y;
float m11 = axisY.Y;
float m12 = axisZ.Y;
float m20 = axisX.Z;
float m21 = axisY.Z;
float m22 = axisZ.Z;
float trace = m00 + m11 + m22;
float qx;
float qy;
float qz;
float qw;
if (trace > 0.0f)
{
float s = (float)Math.Sqrt(trace + 1.0f) * 2.0f;
qw = 0.25f * s;
qx = (m21 - m12) / s;
qy = (m02 - m20) / s;
qz = (m10 - m01) / s;
}
else if (m00 > m11 && m00 > m22)
{
float s = (float)Math.Sqrt(1.0f + m00 - m11 - m22) * 2.0f;
qw = (m21 - m12) / s;
qx = 0.25f * s;
qy = (m01 + m10) / s;
qz = (m02 + m20) / s;
}
else if (m11 > m22)
{
float s = (float)Math.Sqrt(1.0f + m11 - m00 - m22) * 2.0f;
qw = (m02 - m20) / s;
qx = (m01 + m10) / s;
qy = 0.25f * s;
qz = (m12 + m21) / s;
}
else
{
float s = (float)Math.Sqrt(1.0f + m22 - m00 - m11) * 2.0f;
qw = (m10 - m01) / s;
qx = (m02 + m20) / s;
qy = (m12 + m21) / s;
qz = 0.25f * s;
}
return Quaternion.Normalize(new Quaternion(qx, qy, qz, qw));
Assert.AreEqual(1.0, transformedX.Length(), 1e-4);
Assert.AreEqual(1.0, transformedY.Length(), 1e-4);
Assert.AreEqual(1.0, transformedZ.Length(), 1e-4);
Assert.AreEqual(0.0, Vector3.Dot(transformedX, transformedY), 1e-4);
Assert.AreEqual(0.0, Vector3.Dot(transformedY, transformedZ), 1e-4);
Assert.AreEqual(0.0, Vector3.Dot(transformedZ, transformedX), 1e-4);
}
private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6)

View File

@ -779,6 +779,15 @@ var rotation = new Rotation3D(qw, qx, qy, qz); // ❌ 错误
这个结论已经在 Rail 三维姿态、参考杆三维定向的实际运行中验证通过。
**项目级硬约束:**
- 后续任何代码和文档中,都不允许再重新解释 `Rotation3D` 的四元数分量顺序
- 遇到姿态问题时,不要再把问题归因到 `Rotation3D(x,y,z,w)` 是否可能变成了别的顺序
- 如果真实物体或 fragment 参考姿态出现轴翻转/符号错误,应优先检查:
- fragment 三轴的业务语义解释
- 宿主坐标系 / 内部坐标系 / 资产坐标系 的边界
- 是否应该传递显式参考轴,而不是只传 quaternion
#### 11.7.2.2 三维参考杆/任意向量对齐的正确用法
适用场景:

View File

@ -145,6 +145,28 @@ flowchart LR
后续凡是 UI 输入/输出,必须明确是**宿主坐标系**语义;凡是姿态或几何内部计算,必须明确是**内部坐标系**语义;凡是虚拟物体/单位圆柱体资源朝向,必须明确是**资产坐标系**语义。
- 业务锚点
### 3.2.2 Quaternion / Rotation3D 解释约束
坐标系分层之外,还必须固定一条旋转解释规则:
- Navisworks `Rotation3D(double, double, double, double)` 的参数顺序固定为 `x, y, z, w`
- `Rotation3D.A/B/C/D` 固定对应 quaternion 的 `x/y/z/w`
这条规则在项目中视为硬约束,禁止后续代码再次用不同顺序解释四元数。
因此:
- 内部坐标系里算出的 quaternion `(qx, qy, qz, qw)`,写回 Navisworks 时必须使用:
```csharp
var rotation = new Rotation3D(qx, qy, qz, qw);
```
- 后续遇到姿态异常时,不应再次怀疑 `Rotation3D` 分量顺序;应优先检查:
- 宿主坐标系 / 内部坐标系 / 资产坐标系 是否混用
- fragment 参考姿态是否被误当成业务语义姿态
- 真实物体是否错误地只传 quaternion 而没有保留显式参考轴
这层不能偷用世界原点,也不能偷用宿主世界轴。
### 3.3 坐标系定义必须包含的语义

View File

@ -102,6 +102,7 @@ namespace NavisworksTransport.Core.Animation
{
private static PathAnimationManager _instance;
private static readonly Dictionary<string, int> _animationHashToRecordId = new Dictionary<string, int>(); // 动画配置哈希到检测记录ID的映射
private static readonly HashSet<string> _realObjectFragmentUpMismatchHintsShown = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
private ModelItem _animatedObject;
private AnimatedObjectMode _animatedObjectMode = AnimatedObjectMode.None;
private double _virtualObjectLength = 0; // 虚拟物体长度(模型单位)
@ -111,6 +112,9 @@ namespace NavisworksTransport.Core.Animation
private double _realObjectWidth = 0; // 真实物体宽度(模型单位,固定物理尺寸)
private double _realObjectHeight = 0; // 真实物体高度(模型单位,固定物理尺寸)
private Quaternion _realObjectReferenceRotation = Quaternion.Identity;
private Vector3 _realObjectReferenceAxisX = Vector3.UnitX;
private Vector3 _realObjectReferenceAxisY = Vector3.UnitY;
private Vector3 _realObjectReferenceAxisZ = Vector3.UnitZ;
private bool _hasRealObjectReferenceRotation = false;
private List<Point3D> _pathPoints;
private List<ModelItem> _manualCollisionTargets = new List<ModelItem>();
@ -443,12 +447,10 @@ namespace NavisworksTransport.Core.Animation
// 2. 重置内部跟踪变量(同步到原始状态)
// 注意ResetPermanentTransform 后物体的 Transform 属性会自动变回原始值
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(objectToRestore.Transform);
_trackedRotation = objectToRestore.Transform.Factor().Rotation;
_hasTrackedRotation = true;
SyncTrackedRotationToObjectReference(objectToRestore, isVirtual);
var originalBoundingBox = objectToRestore.BoundingBox();
_trackedPosition = GetTrackedObjectPosition(objectToRestore);
var originalBoundingBox = objectToRestore.BoundingBox();
_trackedPosition = GetTrackedObjectPosition(objectToRestore);
string objectName = isVirtual ? "虚拟物体" : objectToRestore.DisplayName;
LogManager.Info($"[归位] {objectName} 已彻底恢复到原始位置, yaw={_currentYaw:F3}");
@ -486,10 +488,7 @@ namespace NavisworksTransport.Core.Animation
_originalTransform = animatedObject.Transform;
_originalCenter = animatedObject.BoundingBox().Center;
_trackedPosition = GetTrackedObjectPosition(animatedObject);
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
_trackedRotation = _originalTransform.Factor().Rotation;
_hasTrackedRotation = true;
TryCaptureRealObjectReferenceRotation(animatedObject, out _);
SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject: false);
}
}
@ -651,16 +650,7 @@ namespace NavisworksTransport.Core.Animation
_originalTransform = animatedObject.Transform;
_originalCenter = animatedObject.BoundingBox().Center;
_trackedPosition = GetTrackedObjectPosition(animatedObject);
// 统一逻辑:从当前 Transform 中提取实际朝向(无论虚拟物体还是普通物体)
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
_trackedRotation = _originalTransform.Factor().Rotation;
_hasTrackedRotation = true;
if (!isVirtualObject)
{
TryCaptureRealObjectReferenceRotation(animatedObject, out _);
}
SyncTrackedRotationToObjectReference(animatedObject, isVirtualObject);
LogManager.Info(
$"[移动到起点] 更新动画对象内部状态: 对象={animatedObject.DisplayName}, 虚拟物体={IsVirtualObjectMode}");
@ -2471,15 +2461,27 @@ namespace NavisworksTransport.Core.Animation
{
VirtualObjectManager.Instance.MoveVirtualObject(newPosition, newRotation);
}
else if (_animatedObject != null)
{
Point3D currentPositionForTransform = _trackedPosition;
var currentRotation = _hasTrackedRotation
? _trackedRotation
: _animatedObject.Transform.Factor().Rotation;
try
{
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
else if (_animatedObject != null)
{
Point3D currentPositionForTransform = _trackedPosition;
Rotation3D currentRotation;
if (_hasTrackedRotation)
{
currentRotation = _trackedRotation;
}
else if (IsRealObjectMode && _hasRealObjectReferenceRotation)
{
currentRotation = CoordinateSystemManager.Instance
.CreateHostAdapter()
.FromHostQuaternion(_realObjectReferenceRotation);
}
else
{
currentRotation = _animatedObject.Transform.Factor().Rotation;
}
try
{
var actualHostPosition = GetTrackedObjectPosition(_animatedObject);
currentPositionForTransform = actualHostPosition;
LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})");
@ -2493,11 +2495,11 @@ namespace NavisworksTransport.Core.Animation
LogManager.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " +
$"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})");
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.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.Info(
$"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " +
$"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " +
@ -3665,10 +3667,10 @@ namespace NavisworksTransport.Core.Animation
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
if (!RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
referenceRotation,
hostForward,
adapter.HostUpVector3,
out var solution))
referenceRotation,
hostForward,
adapter.HostUpVector3,
out var solution))
{
return false;
}
@ -3676,13 +3678,76 @@ namespace NavisworksTransport.Core.Animation
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, _objectRotationCorrection);
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
Matrix4x4 baselineLinear = Matrix4x4.CreateFromQuaternion(solution.BaselineRotation);
Matrix4x4 hostComposedLinear = Matrix4x4.CreateFromQuaternion(hostComposedQuaternion);
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}), " +
$"基姿态已计算");
$"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}), " +
$"HostComposeX=({hostComposedLinear.M11:F4},{hostComposedLinear.M21:F4},{hostComposedLinear.M31:F4}), " +
$"HostComposeY=({hostComposedLinear.M12:F4},{hostComposedLinear.M22:F4},{hostComposedLinear.M32:F4}), " +
$"HostComposeZ=({hostComposedLinear.M13:F4},{hostComposedLinear.M23:F4},{hostComposedLinear.M33:F4})");
return true;
}
private void ShowRealObjectFragmentUpMismatchHintIfNeeded(Vector3 representativeYAxis, Vector3 representativeZAxis)
{
if (_route == null || (_route.PathType != PathType.Ground && _route.PathType != PathType.Hoisting))
{
return;
}
if (!IsRealObjectMode)
{
return;
}
var doc = NavisApplication.ActiveDocument;
string hostUpAxis = FragmentDefaultUpContext.GetCurrentHostUpAxis(doc);
string configuredFragmentUpAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(doc);
Vector3 hostUpVector = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase)
? Vector3.UnitY
: Vector3.UnitZ;
float yAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeYAxis), hostUpVector));
float zAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(representativeZAxis), hostUpVector));
string detectedFragmentUpAxis = zAlignment > yAlignment ? "Z" : "Y";
if (string.Equals(configuredFragmentUpAxis, detectedFragmentUpAxis, StringComparison.OrdinalIgnoreCase))
{
return;
}
string documentKey = FragmentDefaultUpContext.GetCurrentDocumentKey(doc);
if (string.IsNullOrWhiteSpace(documentKey))
{
return;
}
string hintKey = $"{documentKey}|{configuredFragmentUpAxis}|{detectedFragmentUpAxis}";
lock (_realObjectFragmentUpMismatchHintsShown)
{
if (_realObjectFragmentUpMismatchHintsShown.Contains(hintKey))
{
return;
}
_realObjectFragmentUpMismatchHintsShown.Add(hintKey);
}
string message =
$"请到系统管理的坐标系设置中,将 Fragment默认Up 调整为 {detectedFragmentUpAxis}。";
LogManager.Info($"[fragment默认Up提示] 当前值={configuredFragmentUpAxis}, 检测值={detectedFragmentUpAxis}, 模型Up={hostUpAxis}。{message}");
DialogHelper.ShowMessageBox(
message,
"Fragment默认Up提示",
MessageBoxButton.OK,
MessageBoxImage.Information);
}
private bool TryGetRealObjectReferenceRotation(out Quaternion referenceRotation)
{
referenceRotation = Quaternion.Identity;
@ -3760,22 +3825,38 @@ namespace NavisworksTransport.Core.Animation
return false;
}
if (!FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(fragmentMatrices, out referenceRotation))
if (!FragmentRepresentativePoseHelper.TryGetRepresentativeFrame(fragmentMatrices, out var rawRepresentativeFrame))
{
LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 代表姿态求解失败");
return false;
}
var doc = NavisApplication.ActiveDocument;
string fragmentDefaultUpAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis(doc);
string hostUpAxis = FragmentDefaultUpContext.GetCurrentHostUpAxis(doc);
if (!FragmentRepresentativePoseHelper.TryInterpretRepresentativeFrame(
rawRepresentativeFrame,
fragmentDefaultUpAxis,
hostUpAxis,
out var representativeFrame))
{
LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 姿态解释失败");
return false;
}
referenceRotation = representativeFrame.Rotation;
_realObjectReferenceRotation = referenceRotation;
_realObjectReferenceAxisX = representativeFrame.AxisX;
_realObjectReferenceAxisY = representativeFrame.AxisY;
_realObjectReferenceAxisZ = representativeFrame.AxisZ;
_hasRealObjectReferenceRotation = true;
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(referenceRotation);
LogManager.Info(
$"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " +
$"X=({linear.M11:F4},{linear.M21:F4},{linear.M31:F4}), " +
$"Y=({linear.M12:F4},{linear.M22:F4},{linear.M32:F4}), " +
$"Z=({linear.M13:F4},{linear.M23:F4},{linear.M33:F4}), " +
$"fragment数量={fragmentMatrices.Count}");
$"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数量={fragmentMatrices.Count}, Fragment默认Up={fragmentDefaultUpAxis}, 模型Up={hostUpAxis}");
return true;
}
finally
@ -3816,9 +3897,36 @@ namespace NavisworksTransport.Core.Animation
}
}
private void SyncTrackedRotationToObjectReference(ModelItem sourceObject, bool isVirtualObject)
{
if (sourceObject == null)
{
_trackedRotation = Rotation3D.Identity;
_hasTrackedRotation = false;
_currentYaw = 0.0;
return;
}
if (!isVirtualObject && TryCaptureRealObjectReferenceRotation(sourceObject, out Quaternion referenceRotation))
{
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
_trackedRotation = adapter.FromHostQuaternion(referenceRotation);
_hasTrackedRotation = true;
_currentYaw = ModelItemTransformHelper.GetYawFromRotation(_trackedRotation);
return;
}
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(sourceObject.Transform);
_trackedRotation = sourceObject.Transform.Factor().Rotation;
_hasTrackedRotation = true;
}
private void ResetRealObjectReferenceRotation()
{
_realObjectReferenceRotation = Quaternion.Identity;
_realObjectReferenceAxisX = Vector3.UnitX;
_realObjectReferenceAxisY = Vector3.UnitY;
_realObjectReferenceAxisZ = Vector3.UnitZ;
_hasRealObjectReferenceRotation = false;
}

View File

@ -27,7 +27,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
#region
private readonly UIStateManager _uiStateManager;
private readonly Dictionary<string, string> _documentFragmentDefaultUpAxes = new Dictionary<string, string>(StringComparer.OrdinalIgnoreCase);
private readonly HashSet<string> _documentRotationHintShown = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
// 日志管理字段
@ -472,7 +471,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
private void RefreshFragmentDefaultUpAxisSelectionFromCurrentDocument()
{
string resolvedAxis = GetCurrentDocumentFragmentDefaultUpAxis();
string resolvedAxis = FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis();
if (_selectedFragmentDefaultUpAxis != resolvedAxis)
{
SetProperty(ref _selectedFragmentDefaultUpAxis, resolvedAxis, nameof(SelectedFragmentDefaultUpAxis));
@ -481,79 +480,27 @@ namespace NavisworksTransport.UI.WPF.ViewModels
private string GetCurrentDocumentFragmentDefaultUpAxis()
{
string documentKey = GetCurrentDocumentKey();
if (!string.IsNullOrWhiteSpace(documentKey) &&
_documentFragmentDefaultUpAxes.TryGetValue(documentKey, out string storedAxis) &&
IsSupportedFragmentDefaultUpAxis(storedAxis))
{
return storedAxis;
}
return GetCurrentHostUpAxis();
return FragmentDefaultUpContext.GetCurrentDocumentFragmentDefaultUpAxis();
}
private void SaveCurrentDocumentFragmentDefaultUpAxis(string axisName)
{
if (!IsSupportedFragmentDefaultUpAxis(axisName))
{
return;
}
string documentKey = GetCurrentDocumentKey();
if (string.IsNullOrWhiteSpace(documentKey))
{
return;
}
_documentFragmentDefaultUpAxes[documentKey] = axisName;
FragmentDefaultUpContext.SetCurrentDocumentFragmentDefaultUpAxis(axisName);
}
private string GetCurrentDocumentKey()
{
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
if (doc == null || doc.IsClear)
{
return string.Empty;
}
if (!string.IsNullOrWhiteSpace(doc.FileName))
{
return doc.FileName;
}
return doc.Title ?? string.Empty;
return FragmentDefaultUpContext.GetCurrentDocumentKey();
}
private string GetCurrentHostUpAxis()
{
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
if (doc != null && !doc.IsClear)
{
UnitVector3D worldUp = doc.CurrentViewpoint.Value.WorldUpVector;
if (!worldUp.IsZero)
{
return Math.Abs(worldUp.Y) >= Math.Abs(worldUp.Z) ? "Y" : "Z";
}
Vector3D documentUp = doc.UpVector;
if (!documentUp.IsZero)
{
return Math.Abs(documentUp.Y) >= Math.Abs(documentUp.Z) ? "Y" : "Z";
}
}
if (string.Equals(_selectedCoordinateSystem, "YUp", StringComparison.OrdinalIgnoreCase))
{
return "Y";
}
return "Z";
return FragmentDefaultUpContext.GetCurrentHostUpAxis();
}
private static bool IsSupportedFragmentDefaultUpAxis(string axisName)
{
return string.Equals(axisName, "Y", StringComparison.OrdinalIgnoreCase) ||
string.Equals(axisName, "Z", StringComparison.OrdinalIgnoreCase);
return FragmentDefaultUpContext.IsSupportedAxis(axisName);
}
private void ShowRootModelRotationHintIfNeeded()

View File

@ -0,0 +1,90 @@
using System;
using System.Collections.Concurrent;
using Autodesk.Navisworks.Api;
namespace NavisworksTransport.Utils.CoordinateSystem
{
/// <summary>
/// 当前文档级的 fragment 默认 Up 运行时状态。
/// 不写入系统配置,只在当前进程内按文档维持。
/// </summary>
internal static class FragmentDefaultUpContext
{
private static readonly ConcurrentDictionary<string, string> DocumentFragmentDefaultUpAxes =
new ConcurrentDictionary<string, string>(StringComparer.OrdinalIgnoreCase);
public static string GetCurrentDocumentKey(Document doc = null)
{
doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument;
if (doc == null || doc.IsClear)
{
return string.Empty;
}
if (!string.IsNullOrWhiteSpace(doc.FileName))
{
return doc.FileName;
}
return doc.Title ?? string.Empty;
}
public static string GetCurrentHostUpAxis(Document doc = null)
{
doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument;
if (doc != null && !doc.IsClear)
{
UnitVector3D worldUp = doc.CurrentViewpoint.Value.WorldUpVector;
if (!worldUp.IsZero)
{
return Math.Abs(worldUp.Y) >= Math.Abs(worldUp.Z) ? "Y" : "Z";
}
Vector3D documentUp = doc.UpVector;
if (!documentUp.IsZero)
{
return Math.Abs(documentUp.Y) >= Math.Abs(documentUp.Z) ? "Y" : "Z";
}
}
return "Z";
}
public static string GetCurrentDocumentFragmentDefaultUpAxis(Document doc = null)
{
doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument;
string documentKey = GetCurrentDocumentKey(doc);
if (!string.IsNullOrWhiteSpace(documentKey) &&
DocumentFragmentDefaultUpAxes.TryGetValue(documentKey, out string storedAxis) &&
IsSupportedAxis(storedAxis))
{
return storedAxis;
}
return GetCurrentHostUpAxis(doc);
}
public static void SetCurrentDocumentFragmentDefaultUpAxis(string axisName, Document doc = null)
{
if (!IsSupportedAxis(axisName))
{
return;
}
doc = doc ?? Autodesk.Navisworks.Api.Application.ActiveDocument;
string documentKey = GetCurrentDocumentKey(doc);
if (string.IsNullOrWhiteSpace(documentKey))
{
return;
}
DocumentFragmentDefaultUpAxes[documentKey] = axisName;
}
public static bool IsSupportedAxis(string axisName)
{
return string.Equals(axisName, "Y", StringComparison.OrdinalIgnoreCase) ||
string.Equals(axisName, "Z", StringComparison.OrdinalIgnoreCase);
}
}
}

View File

@ -20,6 +20,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
{
private const float AxisEpsilon = 1e-6f;
public readonly struct RepresentativeFrame
{
public Vector3 AxisX { get; }
public Vector3 AxisY { get; }
public Vector3 AxisZ { get; }
public Quaternion Rotation { get; }
public RepresentativeFrame(Vector3 axisX, Vector3 axisY, Vector3 axisZ, Quaternion rotation)
{
AxisX = axisX;
AxisY = axisY;
AxisZ = axisZ;
Rotation = rotation;
}
}
/// <summary>
/// 从 ModelItem 的 fragment 几何中提取代表旋转。
/// </summary>
@ -73,6 +89,108 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return TryGetRepresentativeRotation(fragmentRotations, out representativeRotation);
}
public static bool TryGetRepresentativeFrame(
IEnumerable<double[]> fragmentMatrices,
out RepresentativeFrame representativeFrame)
{
representativeFrame = default;
if (fragmentMatrices == null)
{
return false;
}
var rotations = new List<Quaternion>();
foreach (double[] fragmentMatrix in fragmentMatrices)
{
if (TryExtractRotationFromFragmentMatrix(fragmentMatrix, out Quaternion fragmentRotation))
{
rotations.Add(fragmentRotation);
}
}
if (!TryGetRepresentativeRotation(rotations, out Quaternion representativeRotation))
{
return false;
}
Vector3 axisX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, representativeRotation));
Vector3 axisY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, representativeRotation));
Vector3 axisZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, representativeRotation));
representativeFrame = new RepresentativeFrame(axisX, axisY, axisZ, representativeRotation);
return true;
}
public static bool TryInterpretRepresentativeFrame(
RepresentativeFrame rawFrame,
string fragmentDefaultUpAxis,
string hostUpAxis,
out RepresentativeFrame interpretedFrame)
{
interpretedFrame = default;
bool hostIsYUp = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase);
bool fragmentIsYUp = string.Equals(fragmentDefaultUpAxis, "Y", StringComparison.OrdinalIgnoreCase);
Vector3 semanticAxisX = rawFrame.AxisX;
Vector3 semanticAxisY;
Vector3 semanticAxisZ;
if (hostIsYUp == fragmentIsYUp)
{
semanticAxisY = rawFrame.AxisY;
semanticAxisZ = rawFrame.AxisZ;
}
else if (hostIsYUp)
{
// fragment 按 ZUp 解释,宿主按 YUp 解释X 保持Y 取原始 ZZ 由右手系重建。
semanticAxisY = rawFrame.AxisZ;
semanticAxisZ = Vector3.Normalize(Vector3.Cross(semanticAxisX, semanticAxisY));
}
else
{
// fragment 按 YUp 解释,宿主按 ZUp 解释X 保持Z 取原始 YY 由右手系重建。
semanticAxisZ = rawFrame.AxisY;
semanticAxisY = Vector3.Normalize(Vector3.Cross(semanticAxisZ, semanticAxisX));
}
if (!TryNormalize(semanticAxisX, out semanticAxisX) ||
!TryNormalize(semanticAxisY, out semanticAxisY) ||
!TryNormalize(semanticAxisZ, out semanticAxisZ))
{
return false;
}
// 用解释后的三轴重新正交化,保证右手系稳定。
Vector3 axisX = semanticAxisX;
Vector3 axisY = semanticAxisY - Vector3.Dot(semanticAxisY, axisX) * axisX;
if (!TryNormalize(axisY, out axisY))
{
return false;
}
Vector3 axisZ = Vector3.Cross(axisX, axisY);
if (!TryNormalize(axisZ, out axisZ))
{
return false;
}
if (hostIsYUp && Vector3.Dot(axisY, semanticAxisY) < 0.0f)
{
axisY = -axisY;
axisZ = -axisZ;
}
else if (!hostIsYUp && Vector3.Dot(axisZ, semanticAxisZ) < 0.0f)
{
axisY = -axisY;
axisZ = -axisZ;
}
Quaternion rotation = Quaternion.Normalize(CreateQuaternionFromBasis(axisX, axisY, axisZ));
interpretedFrame = new RepresentativeFrame(axisX, axisY, axisZ, rotation);
return true;
}
/// <summary>
/// 从 fragment 旋转集合中提取代表旋转。
/// </summary>