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 e6027ce..81352de 100644 Binary files a/resources/unit_cube.nwc and b/resources/unit_cube.nwc differ diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index b1f5246..9f75699 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -696,8 +696,22 @@ namespace NavisworksTransport.Core.Animation LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})"); } - // 使用 UpdateObjectPosition 统一处理移动和旋转 - UpdateObjectPosition(startPosition, yaw); + if (_route.PathType != PathType.Rail && + TryCreatePlanarPathRotationAtStart(out var planarRotation)) + { + UpdateObjectPosition(startPosition, planarRotation); + LogManager.Info("[移动到起点] 地面/吊装路径已应用完整三维姿态"); + } + else if (_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) + { + LogManager.Error("[移动到起点] 平面路径未能生成完整姿态,已禁止退回旧 yaw 链路"); + return false; + } + else + { + // 使用 UpdateObjectPosition 统一处理移动和旋转 + UpdateObjectPosition(startPosition, yaw); + } string pathTypeName = _route.PathType.GetDisplayName(); LogManager.Info($"物体已初始化到路径起点并对齐朝向: pos=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), yaw={yaw:F3}rad, 路径类型={pathTypeName}"); @@ -961,21 +975,50 @@ namespace NavisworksTransport.Core.Animation double accumulatedLength = segmentLengths.Take(edgeIndex).Sum(); double edgeProgress = (targetDistance - accumulatedLength) / edge.PhysicalLength; + Point3D sampledPreviousPoint; + Point3D sampledNextPoint; + // 在边内插值位置 if (edge.SegmentType == PathSegmentType.Straight) { - framePosition = InterpolateOnStraightEdge(edge, edgeProgress); + int sampleCount = edge.SampledPoints.Count; + int pointIndex = (int)(edgeProgress * (sampleCount - 1)); + pointIndex = Math.Max(0, Math.Min(pointIndex, sampleCount - 1)); + + framePosition = edge.SampledPoints[pointIndex]; + sampledPreviousPoint = edge.SampledPoints[Math.Max(0, pointIndex - 1)]; + sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)]; } else // Arc { - framePosition = InterpolateOnArcEdge(edge, edgeProgress); + int sampleCount = edge.SampledPoints.Count; + int pointIndex = (int)(edgeProgress * (sampleCount - 1)); + pointIndex = Math.Max(0, Math.Min(pointIndex, sampleCount - 1)); + + framePosition = edge.SampledPoints[pointIndex]; + sampledPreviousPoint = edge.SampledPoints[Math.Max(0, pointIndex - 1)]; + sampledNextPoint = edge.SampledPoints[Math.Min(sampleCount - 1, pointIndex + 1)]; } framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectHeight()); yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress); - previousFramePoint = framePosition; - nextFramePoint = framePosition; + previousFramePoint = ResolveGroundTrackedCenter(sampledPreviousPoint, GetAnimatedObjectHeight()); + nextFramePoint = ResolveGroundTrackedCenter(sampledNextPoint, GetAnimatedObjectHeight()); + + if ((_route.PathType == PathType.Ground || _route.PathType == PathType.Hoisting) && + i > 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);