From 042b9a2804c1be6207483310d072300b1d84b2f8 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Sun, 22 Mar 2026 00:00:16 +0800 Subject: [PATCH] Refine canonical pose flow and handoff notes --- NavisworksTransport.UnitTests.csproj | 1 + TransportPlugin.csproj | 1 + .../CanonicalPlanarPoseBuilderTests.cs | 83 +++++++ .../HostCoordinateAdapterTests.cs | 91 ++++++++ doc/working/current-engineering-state.md | 117 ++++++++++ resources/unit_cube.nwc | Bin 1762 -> 1783 bytes src/Core/Animation/PathAnimationManager.cs | 214 ++++++++++++++++-- src/Core/VirtualObjectManager.cs | 37 +++ .../CanonicalPlanarPoseBuilder.cs | 57 +++++ .../CoordinateSystem/HostCoordinateAdapter.cs | 70 ++++++ 10 files changed, 656 insertions(+), 15 deletions(-) create mode 100644 UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs create mode 100644 doc/working/current-engineering-state.md create mode 100644 src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index 7832f72..619c514 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -55,6 +55,7 @@ + diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj index adb5d43..f1e1a5c 100644 --- a/TransportPlugin.csproj +++ b/TransportPlugin.csproj @@ -332,6 +332,7 @@ + diff --git a/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs b/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs new file mode 100644 index 0000000..b0b6b0a --- /dev/null +++ b/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs @@ -0,0 +1,83 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Utils.CoordinateSystem; +using System.Numerics; + +namespace NavisworksTransport.UnitTests.CoordinateSystem +{ + [TestClass] + public class CanonicalPlanarPoseBuilderTests + { + [TestMethod] + public void StraightForward_ZUpConvention_ShouldKeepLocalZAsWorldUp() + { + bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + new Vector3(5, 0, 0), + Vector3.UnitZ, + ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp), + out Quaternion rotation); + + Assert.IsTrue(ok); + Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation); + AssertColumn(linear, 0, 1, 0, 0); + AssertColumn(linear, 1, 0, 1, 0); + AssertColumn(linear, 2, 0, 0, 1); + } + + [TestMethod] + public void StraightForward_YUpConvention_ShouldMapLocalYToWorldUp() + { + bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + new Vector3(5, 0, 0), + Vector3.UnitZ, + ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp), + out Quaternion rotation); + + Assert.IsTrue(ok); + Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation); + AssertColumn(linear, 0, 1, 0, 0); + AssertColumn(linear, 1, 0, 0, 1); + AssertColumn(linear, 2, 0, -1, 0); + } + + [TestMethod] + public void ForwardWithVerticalComponent_ShouldProjectToHorizontalPlane() + { + bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + new Vector3(10, 0, 3), + Vector3.UnitZ, + ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp), + out Quaternion rotation); + + Assert.IsTrue(ok); + Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation); + Assert.AreEqual(0.0, linear.M31, 1e-6); + Assert.AreEqual(0.0, linear.M32, 1e-6); + Assert.AreEqual(1.0, linear.M33, 1e-6); + } + + private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z) + { + switch (column) + { + case 0: + Assert.AreEqual(x, matrix.M11, 1e-6); + Assert.AreEqual(y, matrix.M21, 1e-6); + Assert.AreEqual(z, matrix.M31, 1e-6); + break; + case 1: + Assert.AreEqual(x, matrix.M12, 1e-6); + Assert.AreEqual(y, matrix.M22, 1e-6); + Assert.AreEqual(z, matrix.M32, 1e-6); + break; + case 2: + Assert.AreEqual(x, matrix.M13, 1e-6); + Assert.AreEqual(y, matrix.M23, 1e-6); + Assert.AreEqual(z, matrix.M33, 1e-6); + break; + default: + Assert.Fail("Only first 3 columns are valid."); + break; + } + } + } +} diff --git a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs index 0a97569..3d54d44 100644 --- a/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs +++ b/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs @@ -1,6 +1,7 @@ using Microsoft.VisualStudio.TestTools.UnitTesting; using NavisworksTransport.Utils.CoordinateSystem; using System.Numerics; +using Autodesk.Navisworks.Api; namespace NavisworksTransport.UnitTests.CoordinateSystem { @@ -52,6 +53,96 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem AssertPoint(restoredBounds.Max, 5.0, 7.0, 11.0); } + [TestMethod] + public void YUp_CanonicalRotation_ShouldConvertToHostYUpBasis() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp); + Quaternion canonicalRotation = convention.CreateQuaternion(new Vector3(1, 0, 0), Vector3.UnitZ); + + Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(canonicalRotation)); + + Assert.AreEqual(1.0, hostLinear.M11, 1e-6); + Assert.AreEqual(0.0, hostLinear.M21, 1e-6); + Assert.AreEqual(0.0, hostLinear.M31, 1e-6); + + Assert.AreEqual(0.0, hostLinear.M12, 1e-6); + Assert.AreEqual(0.0, hostLinear.M22, 1e-6); + Assert.AreEqual(-1.0, hostLinear.M32, 1e-6); + + Assert.AreEqual(0.0, hostLinear.M13, 1e-6); + Assert.AreEqual(1.0, hostLinear.M23, 1e-6); + Assert.AreEqual(0.0, hostLinear.M33, 1e-6); + } + + [TestMethod] + public void YUp_CanonicalRotation_WithPlanarForward_ShouldProduceExpectedHostAxes() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp); + + Vector3 hostForward = Vector3.Normalize(new Vector3(-0.997320f, 0.0f, 0.073164f)); + Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward); + + bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + canonicalForward, + HostCoordinateAdapter.CanonicalUpVector3, + convention, + out Quaternion canonicalRotation); + + Assert.IsTrue(ok); + + Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation); + Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(canonicalLinear); + Quaternion hostQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear); + Matrix4x4 reconstructedHostLinear = Matrix4x4.CreateFromQuaternion(hostQuaternion); + + Assert.AreEqual(hostForward.X, reconstructedHostLinear.M11, 1e-3); + Assert.AreEqual(hostForward.Y, reconstructedHostLinear.M21, 1e-3); + Assert.AreEqual(hostForward.Z, reconstructedHostLinear.M31, 1e-3); + + Assert.AreEqual(0.0, reconstructedHostLinear.M13, 1e-3); + Assert.AreEqual(1.0, reconstructedHostLinear.M23, 1e-3); + Assert.AreEqual(0.0, reconstructedHostLinear.M33, 1e-3); + } + + [TestMethod] + public void YUp_HostLinear_FromActualGroundPathPoints_ShouldMatchExpectedHostAxes() + { + var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp); + var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp); + + Vector3 previousPoint = new Vector3(-181.82637032765f, 14.8333330154419f, 3.38710203888167f); + Vector3 currentPoint = new Vector3(-181.82637032765f, 14.8333330154419f, 3.38710203888167f); + Vector3 nextPoint = new Vector3(-202.828908853644f, 16.3772966612769f, 14.304117291825f); + + Vector3 canonicalPrevious = adapter.ToCanonicalPoint3(previousPoint); + Vector3 canonicalCurrent = adapter.ToCanonicalPoint3(currentPoint); + Vector3 canonicalNext = adapter.ToCanonicalPoint3(nextPoint); + + Vector3 canonicalForward = canonicalNext - canonicalPrevious; + Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + canonicalForward, + HostCoordinateAdapter.CanonicalUpVector3, + convention, + out Quaternion canonicalRotation)); + + Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation); + Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(canonicalLinear); + + Assert.AreEqual(-0.8873, hostLinear.M11, 1e-3); + Assert.AreEqual(0.0000, hostLinear.M21, 1e-3); + Assert.AreEqual(0.4612, hostLinear.M31, 1e-3); + + Assert.AreEqual(0.4612, hostLinear.M12, 1e-3); + Assert.AreEqual(0.0000, hostLinear.M22, 1e-3); + Assert.AreEqual(0.8873, hostLinear.M32, 1e-3); + + Assert.AreEqual(0.0000, hostLinear.M13, 1e-3); + Assert.AreEqual(1.0000, hostLinear.M23, 1e-3); + Assert.AreEqual(0.0000, hostLinear.M33, 1e-3); + } + private static void AssertPoint(Vector3 point, double x, double y, double z) { Assert.AreEqual(x, point.X, 1e-9); diff --git a/doc/working/current-engineering-state.md b/doc/working/current-engineering-state.md new file mode 100644 index 0000000..f6a1035 --- /dev/null +++ b/doc/working/current-engineering-state.md @@ -0,0 +1,117 @@ +# 当前工程状态 + +更新时间：2026-03-21 + +## 1. 当前稳定状态 + +- `Rail` 路径主链路已稳定： + - `ZUp` 模型下，轨上/轨下路径的真实物体与虚拟物体起点、动画、终点贴合正确。 + - `YUp` 模型下，终端安装仿真、`Rail` 姿态、真实物体与虚拟物体通行空间、起点与动画主链路已基本跑通。 +- 地面路径在 `YUp` 模型下： + - 虚拟物体起点姿态、动画姿态、转弯姿态已恢复正常。 + - 当前已禁止地面/吊装路径偷偷退回旧 `yaw` 链路。 +- 碰撞检测/恢复主链路已稳定： + - `ClashDetective` 三维恢复不能再先 `ResetPermanentTransform`。 + - 碰撞恢复、自动报告、自动截图已重新对齐到动画主链路。 +- 虚拟物体资源问题已确认并修复： + - 旧 `unit_cube.nwc` 局部几何中心不在原点，会导致虚拟物体中心偏差。 + - 新 `unit_cube.nwc` 已替换为几何中心在原点的版本。 + +## 2. 当前坐标系架构 + +- 外部坐标： + - `Navisworks` 世界坐标，统一视为外部输入坐标。 +- 内部统一坐标： + - `Canonical Space`，固定 `Z-up`。 +- 业务基准层： + - `ProjectReferenceFrame` + - 球心、项目 `up`、默认模型轴约定 +- 模型局部轴约定： + - `ModelAxisConvention` + - 明确 `ForwardAxis / UpAxis` +- `Rail` 局部坐标系： + - `RailLocalFrame` + - `Forward / Normal / Lateral` + +## 3. 当前关键基础工具 + +- `src/Utils/CoordinateSystem/HostCoordinateAdapter.cs` +- `src/Utils/CoordinateSystem/ProjectReferenceFrame.cs` +- `src/Utils/CoordinateSystem/ModelAxisConvention.cs` +- `src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs` +- `src/Utils/CoordinateSystem/RailLocalFrame.cs` +- `src/Utils/CoordinateSystem/CanonicalRailOffsetResolver.cs` +- `src/Utils/CoordinateSystem/CanonicalTrackedPositionResolver.cs` +- `src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs` + +## 4. 当前必须记住的根因与规则 + +### 4.1 Rotation / 矩阵语义 + +- `Rotation3D(a, b, c, d)` 的参数顺序是四元数 `x, y, z, w`。 +- 不要再随意在 `System.Numerics` 矩阵、四元数、Navisworks `Rotation3D` 之间猜行列语义。 +- 新的姿态构造应优先复用已经验证过的坐标框架工具，不要临时手拼矩阵。 + +### 4.2 碰撞恢复 + +- 对受 `PathAnimationManager` 控制的对象，`ClashDetective` 验证前绝不能先 `ResetPermanentTransform`。 +- 否则宿主姿态会被清回单位姿态，后续增量恢复会错误地认为“不需要再旋转”。 + +### 4.3 动画跟踪点语义 + +- `AnimatedObjectTrackedPosition` 的唯一语义： + - 当前动画主链路使用的跟踪点位置。 +- 当前主链路已经切到： + - 几何中心跟踪。 +- 如果后续动画跟踪点再变，数据库和碰撞结果语义必须同步更新。 + +### 4.4 虚拟物体资源 + +- 虚拟物体资源的局部几何中心必须在原点。 +- 如果资源局部原点不对，上层姿态和坐标框架再正确，也会表现为固定偏差。 +- 日志里优先看： + - 包围盒中心 + - 目标中心 + - 应用后偏差 +- 不要再用虚拟物体 `Transform` 的即时读回值判断 override 后的真实姿态。 + +### 4.5 地面/吊装路径 + +- 地面/吊装路径已经开始走完整姿态链路，不再允许悄悄退回旧 `yaw` 方案。 +- 如果完整姿态生成失败，应直接暴露错误，而不是 fallback。 + +## 5. 当前保留的日志策略 + +- 保留： + - 终点诊断 + - 保存/恢复姿态 + - 关键碰撞恢复 + - 虚拟物体应用后中心/偏差 +- 已降级或删除： + - 大量重复逐帧宿主姿态轴日志 + - 虚拟物体 `Transform` 即时读回日志（容易误导） + +## 6. 当前还值得继续观察的点 + +- 空轨路径里仍有一个旧 warning： + - `[空轨] 双轨几何中心线提取失败，退回 OBB 主轴中线` +- 这与当前 `YUp` 地面路径问题无关，但后续可以单独处理。 + +## 7. 下一步建议 + +- 继续按“先框架、先测试、后接业务”的方式推进。 +- 新问题优先： + 1. 先看日志 + 2. 若日志不足，先补日志 + 3. 再补针对性的回归测试 + 4. 最后再改业务代码 + +## 8. 当前阶段的工作边界 + +- 不要回到“直接补旧 `yaw` 链路”的方式。 +- 不要再增加隐藏错误的 fallback。 +- 不要再混淆： + - 外部宿主坐标 + - 内部 `Canonical` 坐标 + - 业务参考点 + - 模型局部轴约定 diff --git a/resources/unit_cube.nwc b/resources/unit_cube.nwc index e6027ce47495797ebe1ff8ec9c4142c716e11cbd..81352deb7d52fddb0e9bad05eb41fd6d221d17cf 100644 GIT binary patch delta 1094 zcmV-M1iAa-4fhR@IDfH913?gm@3hduMmsB?AP0gr*QO0o;p`5U$?il}=WdsoTSML; zWnRH25KBRPn~0BKr*mFX3I6cyEX%+D{0G^f8)aP9w}LEng+vY35Tn#nc=WOe{2&!W zD=~04V(>I-LTWob9ZJn8A+gn4YJF#Hnqd)U_nP^w>hmw(-hX{Rzw%+b%d_qQE&(@4*d7Vv01fsU zc26xCp*#RvQV%+KoMUBRU|Q7oMU8w0000G00MZN3uKd311f*f zElSMH(ap^-%_{+tDXGOVMXAO4rA5g=R%u>lNqll?QmS5Fc{0$<UPhcpy?u!qrA4BdZzDZaEv6<2W}PCur7 zOlT^ZP{cl9xQh*qCJtbl0|gDp^6ZoK10Mu3oi0z47zA*C%?iRW5QOtAdu|1_T7x23xg5h+b6XJx%NbaTzs>X9!r5=+CR=|HO*(hC)6Y1 zDFs1<7gT-b=)+~_?Fll8fTc<4-qD>e`!8-xQu@2@fm!BnXuebP{fzo<%EkA6=$g&* z-Kjap&s`4~EXHI98GEd}gFgP(+gX_qvezpL(~}TX+m(Mof2VoX2UhOU5YE_`*Xr)I z%AII#9i6Nt3tFhmC3a$MQuP M1poj50001xG^|Vd9RL6T delta 1032 zcmV+j1o!**4dM-uIDautLop17U+Dk?OWByXK`JB`-h|Y(Dh!V;QsgEEr6ezr{X*3n zbnF#4078r$sEAvD>1!t>e(94X`}c1jSG{h8Nv-c0iFJb^MAVQo>lr+IUM70thM?t~ zsMCa;3Rz@bTHhI4=BUzq@hy5CU;E#@@vzT57p8W$w9^C*>3=SiN8i@L1}6(L8ypz) zbtDum%=|Njr-Uj>3IKyVodE&;(sAh-YoAUQT5W`WWSE-o%0 z3IH>yD)xAsV`N}pURV_x8%g~7sk&v!>dWa!|;q9G@+>kbYy z^KBaHX|$s`=#u53PAF-~G9e0UuHw1*n!LN6e4G4d zw!gf;QFbs?BTQpekpv=>KLss+6mG;crfPc7n+MOvFA$}gU|Yo)4<=?^rezgavd}b~ zdhuZ5&-gLkJ$d&JaJLL9E80yao4`C~XV|hV%Z_0fwhWIB)1yO&&=8Cjqgn^Je;CcR zBZI2U8>I%bpbtETToewEQvq2f--~7wV=Zbjfo-p5%(UsAE)14m@0l|O= zkSX$t&erN}>25?s&T__+s@@yAXU#S0kuNk7JYCegU#;>wKh9~l&Bx;tMoDzSKoCL0 zy3YcAwCudDAd?u9JgeM0y0c~f#ZO7je%IYMtNabkcN@MxWr3e_@qHg$vwgnX4L1x5 zpF#qQF*)Hhd8qw4`rCg~y)zX;_Ikx&b{v6fyY?^WL(Qu`u(Hb{I8Da9QMYGRJJH-a zI(cnlp7` 0 && + (Math.Abs(previousFramePoint.X - nextFramePoint.X) > 1e-6 || + Math.Abs(previousFramePoint.Y - nextFramePoint.Y) > 1e-6 || + Math.Abs(previousFramePoint.Z - nextFramePoint.Z) > 1e-6)) + { + LogManager.Debug( + $"[平面转弯诊断] 帧={i}, edge={edgeIndex}, " + + $"prev=({previousFramePoint.X:F3},{previousFramePoint.Y:F3},{previousFramePoint.Z:F3}), " + + $"cur=({framePosition.X:F3},{framePosition.Y:F3},{framePosition.Z:F3}), " + + $"next=({nextFramePoint.X:F3},{nextFramePoint.Y:F3},{nextFramePoint.Z:F3}), " + + $"yaw={yawRadians * 180.0 / Math.PI:F2}°"); + } } // 创建帧并检测碰撞 @@ -999,6 +1042,17 @@ namespace NavisworksTransport.Core.Animation frame.Rotation = railRotation; frame.HasCustomRotation = true; } + else if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) && + TryCreatePlanarPathRotationForFrame(previousFramePoint, framePosition, nextFramePoint, out var planarRotation)) + { + frame.Rotation = planarRotation; + frame.HasCustomRotation = true; + } + else if (_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) + { + throw new InvalidOperationException( + $"平面路径第 {i} 帧未能生成完整姿态，已禁止退回旧 yaw 链路。"); + } // 记录第一帧的角度（用于调试） if (i == 0) @@ -1381,7 +1435,7 @@ namespace NavisworksTransport.Core.Animation bool needsReset; string mismatchSummary; - if (firstFrame.HasCustomRotation && _route?.PathType == PathType.Rail) + if (firstFrame.HasCustomRotation) { var dx = _trackedPosition.X - firstFrame.Position.X; var dy = _trackedPosition.Y - firstFrame.Position.Y; @@ -2380,6 +2434,10 @@ namespace NavisworksTransport.Core.Animation $"目标跟踪中心=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3}), " + $"偏差=({hostActualAfter.X - newPosition.X:F3},{hostActualAfter.Y - newPosition.Y:F3},{hostActualAfter.Z - newPosition.Z:F3})"); } + else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) + { + LogHostActualPoseAxes(_isVirtualObject ? VirtualObjectManager.Instance.CurrentVirtualObject : _animatedObject, "[平面姿态应用后宿主姿态]", false); + } } catch (Exception ex) { @@ -2754,7 +2812,7 @@ namespace NavisworksTransport.Core.Animation // 更新对象位置和朝向 var frameData = _animationFrames[_currentFrameIndex]; - if (frameData.HasCustomRotation && _route?.PathType == PathType.Rail) + if (frameData.HasCustomRotation) { UpdateObjectPosition(frameData.Position, frameData.Rotation); } @@ -3306,12 +3364,6 @@ namespace NavisworksTransport.Core.Animation return new Vector3((float)point.X, (float)point.Y, (float)point.Z); } - /// - /// Rail 路径的目标姿态以“本地 X 为前进方向、本地 Z 为上方向”为约定。 - /// 真实模型在 Y-up 宿主里通常仍保持“本地 Y 为上方向”的资源语义， - /// 因此需要补一个基准修正：绕本地 X 轴 +90°，使模型的 Y-up 语义映射到 Rail 的 Z-up 期望语义。 - /// 虚拟物体资源本身按 Z-up 构建，不需要此修正。 - /// private ModelAxisConvention GetCurrentRailModelAxisConvention() { if (_isVirtualObject) @@ -3327,6 +3379,133 @@ namespace NavisworksTransport.Core.Animation return convention; } + private ModelAxisConvention GetCurrentModelAxisConvention() + { + if (_isVirtualObject) + { + return ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp); + } + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + return ModelAxisConvention.CreateDefaultForHost(adapter.HostType); + } + + private bool TryCreatePlanarPathRotationAtStart(out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + + if (_pathPoints == null || _pathPoints.Count < 2) + { + return false; + } + + if (_route?.PathType == PathType.Hoisting) + { + 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), + out rotation); + } + + return TryCreatePlanarPathRotationFromHostPoints(_pathPoints[0], _pathPoints[0], _pathPoints[1], out rotation); + } + + private bool TryCreatePlanarPathRotationForFrame(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + + if (_route?.PathType == PathType.Hoisting) + { + if (_pathPoints == null || _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), + out rotation); + } + + return TryCreatePlanarPathRotationFromHostPoints(previousPoint, currentPoint, nextPoint, out rotation); + } + + private bool TryCreatePlanarPathRotationFromHostPoints(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint, out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + var convention = GetCurrentModelAxisConvention(); + Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint)); + Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint)); + Vector3 canonicalNext = ToNumerics(adapter.ToCanonicalPoint(nextPoint)); + + Vector3 forward = canonicalNext - canonicalPrevious; + if (forward.LengthSquared() < 1e-6f) + { + forward = canonicalNext - canonicalCurrent; + } + + forward = ApplyPlanarRotationCorrection(forward); + + if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + forward, + HostCoordinateAdapter.CanonicalUpVector3, + convention, + out var quaternion)) + { + return false; + } + + rotation = adapter.FromCanonicalRotation(quaternion); + return true; + } + + private bool TryCreatePlanarPathRotationFromHostForward(Vector3D hostForward, out Rotation3D rotation) + { + rotation = Rotation3D.Identity; + + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + var convention = GetCurrentModelAxisConvention(); + Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z)); + canonicalForward = ApplyPlanarRotationCorrection(canonicalForward); + + if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward( + canonicalForward, + HostCoordinateAdapter.CanonicalUpVector3, + convention, + out var quaternion)) + { + return false; + } + + rotation = adapter.FromCanonicalRotation(quaternion); + return true; + } + + private Vector3 ApplyPlanarRotationCorrection(Vector3 canonicalForward) + { + if (Math.Abs(_objectRotationCorrection) < 1e-9) + { + return canonicalForward; + } + + float correctionRadians = (float)(_objectRotationCorrection * Math.PI / 180.0); + Quaternion correction = Quaternion.CreateFromAxisAngle( + Vector3.Normalize(HostCoordinateAdapter.CanonicalUpVector3), + correctionRadians); + return Vector3.Transform(canonicalForward, correction); + } + /// /// 设置物体角度修正值（度，顺时针） /// @@ -3488,10 +3667,15 @@ namespace NavisworksTransport.Core.Animation actualYaw += correctionRad; } - if (frameData.HasCustomRotation && _route?.PathType == PathType.Rail) + if (frameData.HasCustomRotation) { UpdateObjectPosition(frameData.Position, frameData.Rotation); } + else if (_route?.PathType == PathType.Ground || _route?.PathType == PathType.Hoisting) + { + throw new InvalidOperationException( + $"平面路径播放帧 {_currentFrameIndex} 缺少完整姿态，禁止退回旧 yaw 链路。"); + } else { UpdateObjectPosition(frameData.Position, actualYaw); diff --git a/src/Core/VirtualObjectManager.cs b/src/Core/VirtualObjectManager.cs index b1ea2dd..2809375 100644 --- a/src/Core/VirtualObjectManager.cs +++ b/src/Core/VirtualObjectManager.cs @@ -119,6 +119,8 @@ namespace NavisworksTransport.Core _virtualObjectModelItem = geometryItem; } + LogVirtualObjectGeometry("[虚拟物体创建] Append后缩放前"); + _currentLengthMeters = lengthMeters; _currentWidthMeters = widthMeters; _currentHeightMeters = heightMeters; @@ -255,7 +257,15 @@ namespace NavisworksTransport.Core try { + LogVirtualObjectGeometry("[虚拟物体姿态] 应用前"); ModelItemTransformHelper.MoveItemToPositionAndRotation(_virtualObjectModelItem, position, rotation); + var actualBounds = _virtualObjectModelItem.BoundingBox(); + Point3D actualCenter = actualBounds?.Center ?? new Point3D(0, 0, 0); + LogManager.Info( + $"[虚拟物体姿态] 应用后中心=({actualCenter.X:F3},{actualCenter.Y:F3},{actualCenter.Z:F3}), " + + $"目标中心=({position.X:F3},{position.Y:F3},{position.Z:F3}), " + + $"偏差=({actualCenter.X - position.X:F3},{actualCenter.Y - position.Y:F3},{actualCenter.Z - position.Z:F3})"); + LogVirtualObjectGeometry("[虚拟物体姿态] 应用后"); LogManager.Debug("虚拟物体已应用完整三维姿态"); } catch (Exception ex) @@ -370,6 +380,33 @@ namespace NavisworksTransport.Core Transform3D newTransform = transformComponents.Combine(); doc.Models.SetModelUnitsAndTransform(_virtualObjectModel, _virtualObjectModel.Units, newTransform, false); + + LogVirtualObjectGeometry("[虚拟物体缩放] 完成后"); + } + + private void LogVirtualObjectGeometry(string prefix) + { + if (_virtualObjectModelItem == null) + { + return; + } + + try + { + var itemBounds = _virtualObjectModelItem.BoundingBox(); + + if (itemBounds != null) + { + LogManager.Info( + $"{prefix} Item包围盒: Min=({itemBounds.Min.X:F3},{itemBounds.Min.Y:F3},{itemBounds.Min.Z:F3}), " + + $"Max=({itemBounds.Max.X:F3},{itemBounds.Max.Y:F3},{itemBounds.Max.Z:F3}), " + + $"Center=({itemBounds.Center.X:F3},{itemBounds.Center.Y:F3},{itemBounds.Center.Z:F3})"); + } + } + catch (Exception ex) + { + LogManager.Warning($"{prefix} 记录虚拟物体几何失败: {ex.Message}"); + } } /// diff --git a/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs b/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs new file mode 100644 index 0000000..1d87f49 --- /dev/null +++ b/src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs @@ -0,0 +1,57 @@ +using System; +using System.Numerics; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// 在内部 Canonical Space 中，为地面/吊装这类“法向取项目 up”的路径构造姿态。 + /// 这一层只处理纯数学的前进方向和上方向，不依赖 Navisworks API。 + /// + public static class CanonicalPlanarPoseBuilder + { + private const float ForwardEpsilon = 1e-6f; + + public static bool TryCreateQuaternionFromPoints( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + ModelAxisConvention convention, + out Quaternion rotation) + { + rotation = Quaternion.Identity; + + Vector3 tangent = nextPoint - previousPoint; + if (tangent.LengthSquared() < ForwardEpsilon) + { + tangent = nextPoint - currentPoint; + } + + return TryCreateQuaternionFromForward(tangent, canonicalUp, convention, out rotation); + } + + public static bool TryCreateQuaternionFromForward( + Vector3 desiredForward, + Vector3 canonicalUp, + ModelAxisConvention convention, + out Quaternion rotation) + { + if (convention == null) + { + throw new ArgumentNullException(nameof(convention)); + } + + rotation = Quaternion.Identity; + + Vector3 projectedForward = desiredForward - Vector3.Dot(desiredForward, canonicalUp) * canonicalUp; + if (projectedForward.LengthSquared() < ForwardEpsilon) + { + return false; + } + + Vector3 normalizedForward = Vector3.Normalize(projectedForward); + rotation = convention.CreateQuaternion(normalizedForward, Vector3.Normalize(canonicalUp)); + return true; + } + } +} diff --git a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs index 4fb862f..fd1f814 100644 --- a/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs +++ b/src/Utils/CoordinateSystem/HostCoordinateAdapter.cs @@ -165,6 +165,76 @@ namespace NavisworksTransport.Utils.CoordinateSystem return FromCanonicalPoint3(canonicalVector); } + public Matrix4x4 FromCanonicalLinearTransform(Matrix4x4 canonicalLinear) + { + Vector3 hostX = FromCanonicalVector3(new Vector3(canonicalLinear.M11, canonicalLinear.M21, canonicalLinear.M31)); + Vector3 hostY = FromCanonicalVector3(new Vector3(canonicalLinear.M12, canonicalLinear.M22, canonicalLinear.M32)); + Vector3 hostZ = FromCanonicalVector3(new Vector3(canonicalLinear.M13, canonicalLinear.M23, canonicalLinear.M33)); + + return new Matrix4x4( + hostX.X, hostY.X, hostZ.X, 0f, + hostX.Y, hostY.Y, hostZ.Y, 0f, + hostX.Z, hostY.Z, hostZ.Z, 0f, + 0f, 0f, 0f, 1f); + } + + public Rotation3D FromCanonicalRotation(Quaternion canonicalRotation) + { + Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation); + Matrix4x4 hostLinear = FromCanonicalLinearTransform(canonicalLinear); + return CreateRotationFromLinearComponents( + hostLinear.M11, hostLinear.M12, hostLinear.M13, + hostLinear.M21, hostLinear.M22, hostLinear.M23, + hostLinear.M31, hostLinear.M32, hostLinear.M33); + } + + private static Rotation3D CreateRotationFromLinearComponents( + double m00, double m01, double m02, + double m10, double m11, double m12, + double m20, double m21, double m22) + { + double qx; + double qy; + double qz; + double qw; + + double trace = m00 + m11 + m22; + if (trace > 0.0) + { + double s = Math.Sqrt(trace + 1.0) * 2.0; + qw = 0.25 * s; + qx = (m21 - m12) / s; + qy = (m02 - m20) / s; + qz = (m10 - m01) / s; + } + else if (m00 > m11 && m00 > m22) + { + double s = Math.Sqrt(1.0 + m00 - m11 - m22) * 2.0; + qw = (m21 - m12) / s; + qx = 0.25 * s; + qy = (m01 + m10) / s; + qz = (m02 + m20) / s; + } + else if (m11 > m22) + { + double s = Math.Sqrt(1.0 + m11 - m00 - m22) * 2.0; + qw = (m02 - m20) / s; + qx = (m01 + m10) / s; + qy = 0.25 * s; + qz = (m12 + m21) / s; + } + else + { + double s = Math.Sqrt(1.0 + m22 - m00 - m11) * 2.0; + qw = (m10 - m01) / s; + qx = (m02 + m20) / s; + qy = (m12 + m21) / s; + qz = 0.25 * s; + } + + return new Rotation3D(qx, qy, qz, qw); + } + public CanonicalBounds3 ToCanonicalBounds3(CanonicalBounds3 hostBounds) { return RebuildBounds3(hostBounds, ToCanonicalPoint3);