Interpret fragment up for real object pose
This commit is contained in:
parent
8d10f959b2
commit
290df34dec
29
AGENTS.md
29
AGENTS.md
@ -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 虚拟物体
|
||||
|
||||
这是当前架构里最容易被改坏的地方。
|
||||
|
||||
@ -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" />
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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 三维参考杆/任意向量对齐的正确用法
|
||||
|
||||
适用场景:
|
||||
|
||||
@ -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 坐标系定义必须包含的语义
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
90
src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs
Normal file
90
src/Utils/CoordinateSystem/FragmentDefaultUpContext.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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 取原始 Z,Z 由右手系重建。
|
||||
semanticAxisY = rawFrame.AxisZ;
|
||||
semanticAxisZ = Vector3.Normalize(Vector3.Cross(semanticAxisX, semanticAxisY));
|
||||
}
|
||||
else
|
||||
{
|
||||
// fragment 按 YUp 解释,宿主按 ZUp 解释:X 保持,Z 取原始 Y,Y 由右手系重建。
|
||||
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>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user