diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj index c58c383..7832f72 100644 --- a/NavisworksTransport.UnitTests.csproj +++ b/NavisworksTransport.UnitTests.csproj @@ -55,6 +55,8 @@ + + diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj index 9db0354..adb5d43 100644 --- a/TransportPlugin.csproj +++ b/TransportPlugin.csproj @@ -332,11 +332,14 @@ + + + diff --git a/UnitTests/CoordinateSystem/CanonicalRailOffsetResolverTests.cs b/UnitTests/CoordinateSystem/CanonicalRailOffsetResolverTests.cs new file mode 100644 index 0000000..ca843f2 --- /dev/null +++ b/UnitTests/CoordinateSystem/CanonicalRailOffsetResolverTests.cs @@ -0,0 +1,101 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Core; +using NavisworksTransport.Utils.CoordinateSystem; +using System.Numerics; + +namespace NavisworksTransport.UnitTests.CoordinateSystem +{ + [TestClass] + public class CanonicalRailOffsetResolverTests + { + [TestMethod] + public void OverRail_ShouldOffsetTrackedCenterAlongNormalByHalfHeight() + { + PathRoute route = new PathRoute + { + PathType = PathType.Rail, + RailMountMode = RailMountMode.OverRail, + RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine + }; + + RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ); + Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter( + route, + Vector3.Zero, + frame, + 4.0); + + AssertVector(trackedCenter, 0.0, 0.0, 2.0); + } + + [TestMethod] + public void UnderRail_ShouldOffsetTrackedCenterOppositeToNormalByHalfHeight() + { + PathRoute route = new PathRoute + { + PathType = PathType.Rail, + RailMountMode = RailMountMode.UnderRail, + RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine + }; + + RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ); + Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter( + route, + Vector3.Zero, + frame, + 4.0); + + AssertVector(trackedCenter, 0.0, 0.0, -2.0); + } + + [TestMethod] + public void SlopedNormal_ShouldMoveTrackedCenterAlongRailNormalInsteadOfWorldUp() + { + PathRoute route = new PathRoute + { + PathType = PathType.Rail, + RailMountMode = RailMountMode.OverRail, + RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine + }; + + Vector3 normal = Vector3.Normalize(new Vector3(0f, 1f, 1f)); + RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, normal); + + Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter( + route, + new Vector3(10f, 20f, 30f), + frame, + 4.0); + + Vector3 expected = new Vector3(10f, 20f, 30f) + normal * 2f; + AssertVector(trackedCenter, expected.X, expected.Y, expected.Z); + } + + [TestMethod] + public void AnchorSemantic_ShouldOnlyUseHalfHeightOffset_WhenReferencePointIsAlreadyAnchor() + { + PathRoute route = new PathRoute + { + PathType = PathType.Rail, + RailMountMode = RailMountMode.OverRail, + RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine + }; + + RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ); + Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter( + route, + Vector3.Zero, + frame, + 4.0); + + AssertVector(trackedCenter, 0.0, 0.0, 2.0); + } + + private static void AssertVector(Vector3 actual, double x, double y, double z) + { + Assert.AreEqual(x, actual.X, 1e-6); + Assert.AreEqual(y, actual.Y, 1e-6); + Assert.AreEqual(z, actual.Z, 1e-6); + } + } +} diff --git a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs index b337260..3e2fca1 100644 --- a/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs +++ b/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs @@ -64,6 +64,24 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem Assert.AreEqual(0.0, Vector3.Dot(lateral, up), 1e-6); } + [TestMethod] + public void SlopedPath_ShouldCreateRailLocalFrameWithStableNormalCloseToCanonicalUp() + { + bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame( + new Vector3(0, 0, 0), + new Vector3(10, 0, 1), + new Vector3(20, 0, 2), + Vector3.UnitZ, + out RailLocalFrame frame); + + Assert.IsTrue(ok); + Assert.IsTrue(Vector3.Dot(frame.Forward, Vector3.UnitX) > 0.99f); + Assert.IsTrue(Vector3.Dot(frame.Normal, Vector3.UnitZ) > 0.99f); + Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Normal), 1e-6); + Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Lateral), 1e-6); + Assert.AreEqual(0.0, Vector3.Dot(frame.Lateral, frame.Normal), 1e-6); + } + private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z) { switch (column) diff --git a/UnitTests/CoordinateSystem/CanonicalTrackedPositionResolverTests.cs b/UnitTests/CoordinateSystem/CanonicalTrackedPositionResolverTests.cs new file mode 100644 index 0000000..674e192 --- /dev/null +++ b/UnitTests/CoordinateSystem/CanonicalTrackedPositionResolverTests.cs @@ -0,0 +1,65 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +using NavisworksTransport.Utils.CoordinateSystem; +using System.Numerics; + +namespace NavisworksTransport.UnitTests.CoordinateSystem +{ + [TestClass] + public class CanonicalTrackedPositionResolverTests + { + [TestMethod] + public void GroundReference_ShouldOffsetHalfHeightAlongUp() + { + Vector3 result = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter( + new Vector3(10f, 20f, 30f), + Vector3.UnitZ, + 4.0); + + AssertVector(result, 10.0, 20.0, 32.0); + } + + [TestMethod] + public void TopSuspensionReference_ShouldOffsetNegativeHalfHeightAlongUp() + { + Vector3 result = CanonicalTrackedPositionResolver.ResolveTopSuspensionTrackedCenter( + new Vector3(10f, 20f, 30f), + Vector3.UnitZ, + 4.0); + + AssertVector(result, 10.0, 20.0, 28.0); + } + + [TestMethod] + public void ArbitraryNormalReference_ShouldOffsetAlongProvidedNormal() + { + Vector3 normal = Vector3.Normalize(new Vector3(0f, 1f, 1f)); + Vector3 result = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference( + new Vector3(5f, 6f, 7f), + normal, + 4.0, + 0.0); + + Vector3 expected = new Vector3(5f, 6f, 7f) + normal * 2f; + AssertVector(result, expected.X, expected.Y, expected.Z); + } + + [TestMethod] + public void MidSurfaceFactor_ShouldProduceNoOffset() + { + Vector3 result = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference( + new Vector3(1f, 2f, 3f), + Vector3.UnitZ, + 8.0, + 0.5); + + AssertVector(result, 1.0, 2.0, 3.0); + } + + private static void AssertVector(Vector3 actual, double x, double y, double z) + { + Assert.AreEqual(x, actual.X, 1e-6); + Assert.AreEqual(y, actual.Y, 1e-6); + Assert.AreEqual(z, actual.Z, 1e-6); + } + } +} diff --git a/deploy-plugin.bat b/deploy-plugin.bat index eaf9e8a..9c39165 100644 --- a/deploy-plugin.bat +++ b/deploy-plugin.bat @@ -7,6 +7,22 @@ powershell -NoProfile -ExecutionPolicy Bypass -Command ^ "$ErrorActionPreference = 'Stop';" ^ "$targetDir = [System.IO.Path]::GetFullPath('%TARGET_DIR%');" ^ "$buildDir = [System.IO.Path]::GetFullPath('%BUILD_DIR%');" ^ + "function Test-FileUnlocked([string]$path) {" ^ + " if (-not (Test-Path $path)) { return $true }" ^ + " try {" ^ + " $stream = [System.IO.File]::Open($path, [System.IO.FileMode]::Open, [System.IO.FileAccess]::ReadWrite, [System.IO.FileShare]::None);" ^ + " $stream.Close();" ^ + " return $true;" ^ + " } catch {" ^ + " return $false;" ^ + " }" ^ + "}" ^ + "function Wait-FileUnlocked([string]$path, [datetime]$deadline) {" ^ + " while (-not (Test-FileUnlocked $path)) {" ^ + " if ((Get-Date) -ge $deadline) { throw ('Target file still locked: ' + $path) }" ^ + " Start-Sleep -Milliseconds 500;" ^ + " }" ^ + "}" ^ "" ^ "Get-Process Roamer -ErrorAction SilentlyContinue | Stop-Process -Force -ErrorAction SilentlyContinue;" ^ "$deadline = (Get-Date).AddSeconds(15);" ^ @@ -14,6 +30,13 @@ powershell -NoProfile -ExecutionPolicy Bypass -Command ^ " if ((Get-Date) -ge $deadline) { throw 'Roamer.exe still running after 15 seconds.' }" ^ " Start-Sleep -Milliseconds 500;" ^ "}" ^ + "$keyTargetFiles = @(" ^ + " (Join-Path $targetDir 'TransportPlugin.dll')," ^ + " (Join-Path $targetDir 'geometry4Sharp.dll')," ^ + " (Join-Path (Join-Path $targetDir 'resources') 'unit_cube.nwc')," ^ + " (Join-Path (Join-Path $targetDir 'resources') 'unit_cylinder.nwc')" ^ + ");" ^ + "foreach ($keyFile in $keyTargetFiles) { Wait-FileUnlocked $keyFile $deadline }" ^ "" ^ "New-Item -ItemType Directory -Force -Path $targetDir | Out-Null;" ^ "Copy-Item (Join-Path $buildDir '*.dll') $targetDir -Force;" ^ diff --git a/doc/design/2026/coordinate-system-canonical-space-design.md b/doc/design/2026/coordinate-system-canonical-space-design.md index 4ab4395..7aa02af 100644 --- a/doc/design/2026/coordinate-system-canonical-space-design.md +++ b/doc/design/2026/coordinate-system-canonical-space-design.md @@ -181,6 +181,59 @@ flowchart LR 3. 业务基准定义 4. 模型局部轴约定 +### 3.5 Rail 局部坐标系与动画跟踪点 + +`Rail` 路径不能再只理解成“沿世界 up 做上下偏移”。 + +对 `Rail` 来说，必须显式建立一套局部坐标系： + +- `Forward` + - 沿轨道前进方向 +- `Normal` + - 安装法向 + - 可以是任意空间角度 + - 但应尽量贴近项目 up +- `Lateral` + - 由 `Normal x Forward` 或正交化后确定 + +后续所有 `Rail` 相关计算都应建立在这套 `RailLocalFrame` 之上： + +- 物体姿态 +- 通行空间姿态 +- 路径参考点到动画跟踪点的偏移 +- 终点贴合语义 + +### 3.6 动画跟踪点统一语义 + +动画系统内部不应混用： + +- 底面中心 +- 顶面中心 +- 包围盒中心 +- 路径参考点 + +统一规则应为： + +- **动画跟踪点 = 当前动画主链路使用的唯一位置语义** +- 当前阶段建议统一为：**几何中心** + +这样做的原因是： + +- 中心点对三维旋转最中性 +- 不依赖“当前哪一面朝上” +- 不依赖宿主 `Y-up / Z-up` +- 物体姿态与碰撞恢复、截图回放更容易共用同一套语义 + +不同路径的业务需求通过“参考点 -> 跟踪中心点”的显式偏移来表达： + +- 地面/吊装：通常沿 `+up / -up` +- Rail：沿 `RailLocalFrame.Normal` + +这样： + +- 业务层只表达“路径参考点”和“法向偏移” +- 动画层只认“中心点 + 姿态” + ## 4. 核心设计原则 ### 4.1 Navisworks 世界坐标统一视为外部坐标 diff --git a/doc/guide/design_principles.md b/doc/guide/design_principles.md index 33a66bd..d3961d1 100644 --- a/doc/guide/design_principles.md +++ b/doc/guide/design_principles.md @@ -2521,6 +2521,30 @@ DialogHelper.ShowDialog(new MyDialog()); - 最终部署前应至少确认主项目完整构建成功 - 必要时校验关键 WPF 视图资源是否真的编入程序集 +### 额外经验 3：三维碰撞验证前禁止先重置宿主姿态 + +对受 `PathAnimationManager` 控制的三维动画对象，`ClashDetective` 候选验证前不能先调用： + +- `doc.Models.ResetPermanentTransform(modelItems)` + +原因： + +- `PathAnimationManager` 的内部跟踪姿态记录的是“当前动画目标姿态” +- 一旦先把宿主物体重置回 CAD 原始姿态，宿主真实姿态就会变成单位姿态 +- 如果随后仍复用 `PathAnimationManager.MoveAnimatedObjectToPose(...)` +- 则它会把“缓存姿态”误当成当前姿态，导致增量旋转退化成单位旋转，只剩平移 + +实际后果： + +- 动画结束后，碰撞检测汇总一启动，真实物体会被摆平或姿态跳变 +- 后续所有 `ClashDetective` 验证都在错误姿态上进行 + +正确做法： + +- 对三维 `PAM` 主链路对象，直接复用 `MoveAnimatedObjectToPose(...)` +- 不再在验证入口额外执行 `ResetPermanentTransform` +- `ResetPermanentTransform` 只保留给旧二维/非 `PAM` 恢复分支 + --- *本文档将随着项目发展持续更新，确保设计指导的有效性和实用性。* diff --git a/doc/working/coordinate-system-canonical-space-implementation-plan.md b/doc/working/coordinate-system-canonical-space-implementation-plan.md index fc7c3f9..cf3928a 100644 --- a/doc/working/coordinate-system-canonical-space-implementation-plan.md +++ b/doc/working/coordinate-system-canonical-space-implementation-plan.md @@ -242,6 +242,48 @@ - Rail 虚拟物体与真实模型在 Y-up / Z-up 项目中的姿态语义一致 - 真实物体起点贴合和动画第一帧使用同一份固定物理尺寸语义 - 不再允许从“已旋转后的当前 AABB”重新推导 Rail 物体高度 +- Rail 动画不再直接在业务层手写 `forward/up/offset`，而是复用基础工具 +- `ClashDetective` 三维候选验证必须直接复用 `PathAnimationManager` 主链路恢复，不允许先 `ResetPermanentTransform` 再复用 `PAM` + +### Task 7.2 新增 `RailLocalFrame` + +目标： + +- 将 `Rail` 局部坐标系正式提升为基础框架对象 + +最低要求： + +- `Forward` +- `Normal` +- `Lateral` + +实施要求： + +- 先在纯数学层构建并测试 +- 再由 `RailPathPoseHelper`、动画、通行空间复用 + +验收： + +- 不再由业务代码自己拼切向/法向/侧向 +- `Rail` 的姿态、法向偏移、通行空间统一使用同一套局部坐标系 + +### Task 7.3 新增“参考点 -> 跟踪中心点”偏移解析器 + +目标： + +- 将路径参考点到动画跟踪中心点的偏移，从业务代码中抽离成基础工具 + +实施要求： + +- 先以几何中心为统一动画跟踪点 +- 地面/吊装路径使用宿主 up 语义 +- Rail 路径使用 `RailLocalFrame.Normal` +- 先补单测再接业务 + +验收： + +- `PathAnimationManager` 不再直接散落手写半高偏移 +- `AnimatedObjectTrackedPosition` 明确表示动画跟踪中心点 ### Task 7.1 明确模型局部轴约定 diff --git a/src/Commands/GenerateCollisionReportCommand.cs b/src/Commands/GenerateCollisionReportCommand.cs index 9f39b72..1eb3f4e 100644 --- a/src/Commands/GenerateCollisionReportCommand.cs +++ b/src/Commands/GenerateCollisionReportCommand.cs @@ -471,8 +471,11 @@ namespace NavisworksTransport.Commands int successCount = 0; // 保存动画物体原始状态（用于截图后恢复） + // 对受 PathAnimationManager 控制的对象，直接复用与 ClashDetective 相同的保存/恢复主链路， + // 避免报告生成额外引入第二套状态语义。 ModelItem animatedObject = null; ModelItemTransformHelper.ObjectStateSnapshot savedObjectState = null; + bool restoreViaAnimationManager = false; // 找到第一个有位置信息的碰撞来获取动画物体 foreach (var c in collisionData.AllCollisions) @@ -480,7 +483,18 @@ namespace NavisworksTransport.Commands if (c.Item1 != null && c.HasPositionInfo) { animatedObject = c.Item1; - savedObjectState = CollisionSceneHelper.SaveAnimatedObjectState(animatedObject); + var pam = PathAnimationManager.GetInstance(); + restoreViaAnimationManager = pam != null && pam.ControlsAnimatedObject(animatedObject); + + if (restoreViaAnimationManager) + { + pam.SaveObjectState(animatedObject); + LogManager.Info(string.Format("已通过动画主链路保存物体状态: {0}", animatedObject.DisplayName)); + } + else + { + savedObjectState = CollisionSceneHelper.SaveAnimatedObjectState(animatedObject); + } break; } } @@ -544,7 +558,19 @@ namespace NavisworksTransport.Commands } // 恢复动画物体到原始状态 - CollisionSceneHelper.RestoreAnimatedObjectState(animatedObject, savedObjectState); + if (animatedObject != null) + { + var pam = PathAnimationManager.GetInstance(); + if (restoreViaAnimationManager && pam != null && pam.ControlsAnimatedObject(animatedObject)) + { + pam.RestoreAnimatedObjectState(animatedObject); + LogManager.Info(string.Format("已通过动画主链路恢复物体状态: {0}", animatedObject.DisplayName)); + } + else + { + CollisionSceneHelper.RestoreAnimatedObjectState(animatedObject, savedObjectState); + } + } LogManager.Info(string.Format("已生成 {0}/{1} 个碰撞对视角截图", successCount, collisionData.AllCollisions.Count)); } @@ -623,8 +649,7 @@ namespace NavisworksTransport.Commands } var bounds = animatedObject.BoundingBox(); - var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); - return ModelItemTransformHelper.GetHostBottomAnchorPoint(bounds, adapter); + return bounds?.Center ?? new Point3D(0, 0, 0); } diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index e25d6c8..b1f5246 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Linq; +using System.Numerics; using System.Text; using System.Windows.Threading; using Autodesk.Navisworks.Api; @@ -429,7 +430,7 @@ namespace NavisworksTransport.Core.Animation _hasTrackedRotation = true; var originalBoundingBox = objectToRestore.BoundingBox(); - _trackedPosition = GetTrackedObjectPosition(objectToRestore, _route?.PathType == PathType.Rail && !isVirtual); + _trackedPosition = GetTrackedObjectPosition(objectToRestore); string objectName = isVirtual ? "虚拟物体" : objectToRestore.DisplayName; LogManager.Info($"[归位] {objectName} 已彻底恢复到原始位置, yaw={_currentYaw:F3}"); @@ -604,7 +605,7 @@ namespace NavisworksTransport.Core.Animation _animatedObject = animatedObject; _originalTransform = animatedObject.Transform; _originalCenter = animatedObject.BoundingBox().Center; - _trackedPosition = GetTrackedObjectPosition(animatedObject, _route?.PathType == PathType.Rail && !_isVirtualObject); + _trackedPosition = GetTrackedObjectPosition(animatedObject); // 统一逻辑：从当前 Transform 中提取实际朝向（无论虚拟物体还是普通物体） _currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform); @@ -657,17 +658,17 @@ namespace NavisworksTransport.Core.Animation Point3D startPosition = _pathPoints[0]; if (_route.PathType == PathType.Hoisting) { - // 吊装路径：第一个路径点（起吊点）是地面位置，物体底面应该在这里 - // 不需要向下移动物体高度 - LogManager.Debug($"[移动到起点] 吊装路径：起吊点是地面位置，物体底面Z={startPosition.Z:F2}"); + // 吊装路径：第一个路径点（起吊点）是地面位置，动画跟踪点统一使用几何中心 + startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight()); + LogManager.Debug($"[移动到起点] 吊装路径：起吊点已转换为物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})"); } else if (_route.PathType == PathType.Rail) { double objectHeight = GetAnimatedObjectHeight(); Point3D previousPoint = _pathPoints[0]; Point3D nextPoint = _pathPoints.Count > 1 ? _pathPoints[1] : _pathPoints[0]; - startPosition = RailPathPoseHelper.ResolveBottomPosition(_route, startPosition, previousPoint, nextPoint, objectHeight); - LogManager.Debug($"[移动到起点] Rail路径调整: 参考点=({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2}), 物体底面=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), 物体高度={objectHeight:F2}, 安装={_route.RailMountMode}, 对接={(PathRoute.IsTopPayloadAnchorForMountMode(_route.RailMountMode) ? "顶面对接" : "底面对接")}, 偏移={_route.RailReferenceToAnchorOffset:F2}"); + startPosition = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(_route, startPosition, previousPoint, nextPoint, objectHeight); + LogManager.Debug($"[移动到起点] Rail路径调整: 参考点=({_pathPoints[0].X:F2},{_pathPoints[0].Y:F2},{_pathPoints[0].Z:F2}), 物体中心=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), 物体高度={objectHeight:F2}, 安装={_route.RailMountMode}, 对接={(PathRoute.IsTopPayloadAnchorForMountMode(_route.RailMountMode) ? "顶面对接" : "底面对接")}"); if (RailPathPoseHelper.TryCreateRailRotation( previousPoint, @@ -690,8 +691,9 @@ namespace NavisworksTransport.Core.Animation } else { - // 地面路径：points[0] 是地面位置，物体底面应该在这里 - LogManager.Debug($"[移动到起点] 地面路径: pos=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})"); + // 地面路径：路径点在接触面上，动画跟踪点统一使用几何中心 + startPosition = ResolveGroundTrackedCenter(startPosition, GetAnimatedObjectHeight()); + LogManager.Debug($"[移动到起点] 地面路径中心点=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})"); } // 使用 UpdateObjectPosition 统一处理移动和旋转 @@ -925,33 +927,25 @@ namespace NavisworksTransport.Core.Animation if (segmentIndex == firstSegment) { - // 起吊段：从地面逐渐上升到悬挂点-物体高度 - // 进度0时（地面）：物体底面在地面，不向下移动 - // 进度1时（悬挂点）：物体顶面在悬挂点，物体底面=悬挂点-物体高度 - double heightOffset = segmentProgress * objectHeight; - framePosition = new Point3D(framePosition.X, framePosition.Y, framePosition.Z - heightOffset); - LogManager.Debug($"[吊装路径-起吊段] 进度={segmentProgress:F2}, 高度偏移={heightOffset:F2}, 物体底面Z={framePosition.Z:F2}"); + framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, segmentProgress); + LogManager.Debug($"[吊装路径-起吊段] 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})"); } else if (segmentIndex > firstSegment && segmentIndex < lastSegment) { - // 平移段（中间的所有线段）：全程都是悬挂点，物体顶面在悬挂点 - framePosition = new Point3D(framePosition.X, framePosition.Y, framePosition.Z - objectHeight); - LogManager.Debug($"[吊装路径-平移段] 线段{segmentIndex}, 进度={segmentProgress:F2}, 物体底面Z={framePosition.Z:F2}"); + // 平移段（中间的所有线段）：参考点位于顶部悬挂点 + framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, 1.0); + LogManager.Debug($"[吊装路径-平移段] 线段{segmentIndex}, 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})"); } else if (segmentIndex == lastSegment) { - // 下降段：从悬挂点-物体高度逐渐下降到地面 - // 进度0时（悬挂点）：物体顶面在悬挂点，物体底面=悬挂点-物体高度 - // 进度1时（地面）：物体底面在地面，不向下移动 - double heightOffset = (1 - segmentProgress) * objectHeight; - framePosition = new Point3D(framePosition.X, framePosition.Y, framePosition.Z - heightOffset); - LogManager.Debug($"[吊装路径-下降段] 进度={segmentProgress:F2}, 高度偏移={heightOffset:F2}, 物体底面Z={framePosition.Z:F2}"); + framePosition = ResolveHoistingTrackedCenter(framePosition, objectHeight, 1.0 - segmentProgress); + LogManager.Debug($"[吊装路径-下降段] 进度={segmentProgress:F2}, 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2})"); } } else if (_route.PathType == PathType.Rail) { - framePosition = RailPathPoseHelper.ResolveBottomPosition(_route, framePosition, p1, p2, objectHeight); - LogManager.Debug($"[Rail路径] 调整物体位置: 参考点=({p1.X:F2},{p1.Y:F2},{p1.Z:F2})->({p2.X:F2},{p2.Y:F2},{p2.Z:F2}), 物体底面=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2}), 安装={_route.RailMountMode}, 对接={(PathRoute.IsTopPayloadAnchorForMountMode(_route.RailMountMode) ? "顶面对接" : "底面对接")}"); + framePosition = RailPathPoseHelper.ResolveObjectSpaceCenterPosition(_route, framePosition, p1, p2, objectHeight); + LogManager.Debug($"[Rail路径] 调整物体位置: 参考点=({p1.X:F2},{p1.Y:F2},{p1.Z:F2})->({p2.X:F2},{p2.Y:F2},{p2.Z:F2}), 物体中心=({framePosition.X:F2},{framePosition.Y:F2},{framePosition.Z:F2}), 安装={_route.RailMountMode}, 对接={(PathRoute.IsTopPayloadAnchorForMountMode(_route.RailMountMode) ? "顶面对接" : "底面对接")}"); } } else @@ -977,6 +971,8 @@ namespace NavisworksTransport.Core.Animation framePosition = InterpolateOnArcEdge(edge, edgeProgress); } + framePosition = ResolveGroundTrackedCenter(framePosition, GetAnimatedObjectHeight()); + yawRadians = ComputeYawOnPath(i, allSampledPoints, edgeIndex, edgeProgress); previousFramePoint = framePosition; nextFramePoint = framePosition; @@ -1023,19 +1019,19 @@ namespace NavisworksTransport.Core.Animation // 计算偏移量（当前包围盒中心到framePosition的偏移） var offsetX = framePosition.X - originalCenter.X; var offsetY = framePosition.Y - originalCenter.Y; - var offsetZ = framePosition.Z - currentObjectBoundingBox.Min.Z; // Z方向：framePosition是底面，originalCenter是中心 + var offsetZ = framePosition.Z - originalCenter.Z; - // 创建新的包围盒（将当前包围盒移动到framePosition） + // 创建新的包围盒（将当前包围盒中心移动到framePosition） var virtualBoundingBox = new BoundingBox3D( new Point3D( currentObjectBoundingBox.Min.X + offsetX, currentObjectBoundingBox.Min.Y + offsetY, - framePosition.Z // 底面Z坐标 + currentObjectBoundingBox.Min.Z + offsetZ ), new Point3D( currentObjectBoundingBox.Max.X + offsetX, currentObjectBoundingBox.Max.Y + offsetY, - framePosition.Z + (currentObjectBoundingBox.Max.Z - currentObjectBoundingBox.Min.Z) // 保持高度 + currentObjectBoundingBox.Max.Z + offsetZ ) ); @@ -1112,18 +1108,12 @@ namespace NavisworksTransport.Core.Animation (virtualBoundingBox.Min.Y + virtualBoundingBox.Max.Y + colliderBox.Min.Y + colliderBox.Max.Y) / 4, (virtualBoundingBox.Min.Z + virtualBoundingBox.Max.Z + colliderBox.Min.Z + colliderBox.Max.Z) / 4 ), - // 碰撞结果中的 AnimatedObjectTrackedPosition 必须与后续恢复链路的动画跟踪点语义一致： - // 1. 二维 yaw 路径沿用成熟旧逻辑，记录包围盒中心； - // 2. 三维 customRotation 路径记录动画跟踪点（framePosition）。 - AnimatedObjectTrackedPosition = frame.HasCustomRotation - ? new Point3D( - framePosition.X, - framePosition.Y, - framePosition.Z) - : new Point3D( - framePosition.X, - framePosition.Y, - framePosition.Z + (virtualBoundingBox.Max.Z - virtualBoundingBox.Min.Z) / 2), + // AnimatedObjectTrackedPosition 统一定义为动画主链路的跟踪点位置。 + // 当前动画主链路已统一使用几何中心作为跟踪点，因此所有路径都直接记录 framePosition。 + AnimatedObjectTrackedPosition = new Point3D( + framePosition.X, + framePosition.Y, + framePosition.Z), Item2Position = GetObjectPosition(collider), AnimatedObjectTrackedYawRadians = actualYawRadians, // 记录动画跟踪点对应的运动物体实际播放朝向 AnimatedObjectTrackedRotation = frame.HasCustomRotation ? frame.Rotation : Rotation3D.Identity, @@ -1133,7 +1123,7 @@ namespace NavisworksTransport.Core.Animation LogManager.Debug( $"[保存碰撞结果] 帧={i}, " + - $"位置语义={(frame.HasCustomRotation ? "轨迹跟踪点" : "包围盒中心")}, " + + $"位置语义=动画跟踪中心点, " + $"原始Yaw={yawRadians * 180.0 / Math.PI:F2}°, " + $"实际Yaw={actualYawRadians * 180.0 / Math.PI:F2}°, " + $"customRotation={frame.HasCustomRotation}, " + @@ -1152,6 +1142,15 @@ namespace NavisworksTransport.Core.Animation var totalCollisions = _animationFrames.Sum(f => f.Collisions.Count); // 检查碰撞结果是否包含位置信息 var collisionsWithPosition = _allCollisionResults.Count(c => c.HasPositionInfo && c.AnimatedObjectTrackedPosition != null); + + if (_route.PathType == PathType.Rail && _animationFrames.Count > 0) + { + AnimationFrame firstRailFrame = _animationFrames[0]; + AnimationFrame lastRailFrame = _animationFrames[_animationFrames.Count - 1]; + LogManager.Info( + $"[Rail帧诊断] 首帧跟踪中心=({firstRailFrame.Position.X:F3},{firstRailFrame.Position.Y:F3},{firstRailFrame.Position.Z:F3}), " + + $"末帧跟踪中心=({lastRailFrame.Position.X:F3},{lastRailFrame.Position.Y:F3},{lastRailFrame.Position.Z:F3}), 总帧数={_animationFrames.Count}"); + } LogManager.Info($"=== 预计算完成 ==="); LogManager.Info($"总帧数: {_animationFrames.Count}"); @@ -1724,9 +1723,6 @@ namespace NavisworksTransport.Core.Animation LogManager.Info("动画播放自然结束，物体保持在最终位置"); LogRailEndAlignmentDiagnostics(); - // 直接设置为完成状态，避免中间状态切换 - SetState(AnimationState.Finished); - // 清除所有高亮（包括物体和碰撞） ModelHighlightHelper.ClearAllHighlights(); LogManager.Info("动画完成：已清除所有高亮"); @@ -1735,12 +1731,6 @@ namespace NavisworksTransport.Core.Animation var actualTotalFrames = _animationFrames?.Count ?? 0; LogManager.Info($"动画播放完成，总帧数: {actualTotalFrames}, 平均FPS: {_actualFPS:F1}"); - // 更新 TimeLiner 任务状态 - if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId)) - { - _timeLinerManager.UpdateTaskProgress(_currentTaskId, 1.0, AnimationState.Finished); - } - // 🔥 检查是否使用历史记录，如果是则跳过ClashDetective测试 if (_isUsingHistoryRecord) { @@ -1789,6 +1779,16 @@ namespace NavisworksTransport.Core.Animation LogManager.Info("碰撞检测被取消"); } } + + // 后处理全部完成后再进入 Finished。 + // 否则 UI 会在碰撞汇总仍在运行时启动报告生成，和当前物体恢复链路并发，导致结束后姿态被再次改写。 + SetState(AnimationState.Finished); + + // 更新 TimeLiner 任务状态 + if (_timeLinerManager != null && !string.IsNullOrEmpty(_currentTaskId)) + { + _timeLinerManager.UpdateTaskProgress(_currentTaskId, 1.0, AnimationState.Finished); + } } catch (Exception ex) { @@ -1819,7 +1819,7 @@ namespace NavisworksTransport.Core.Animation Point3D finalTrackedPoint = _trackedPosition; double objectHeight = GetAnimatedObjectHeight(); - Point3D expectedTrackedEndPoint = RailPathPoseHelper.ResolveBottomPosition( + Point3D expectedTrackedEndPoint = RailPathPoseHelper.ResolveObjectSpaceCenterPosition( _route, terminalAnchorPoint, previousAnchorPoint, @@ -2318,6 +2318,7 @@ namespace NavisworksTransport.Core.Animation { try { + bool isRailRealObject = _route?.PathType == PathType.Rail && !_isVirtualObject && _animatedObject != null; if (_isVirtualObject) { VirtualObjectManager.Instance.MoveVirtualObject(newPosition, newRotation); @@ -2330,10 +2331,10 @@ namespace NavisworksTransport.Core.Animation : _animatedObject.Transform.Factor().Rotation; try { - var actualHostPosition = GetTrackedObjectPosition(_animatedObject, _route?.PathType == PathType.Rail && !_isVirtualObject); + var actualHostPosition = GetTrackedObjectPosition(_animatedObject); currentPositionForTransform = actualHostPosition; - LogManager.Info( - $"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})"); + LogManager.Debug( + $"[动画姿态入口] {_animatedObject.DisplayName} 宿主即时读回点=({actualHostPosition.X:F3},{actualHostPosition.Y:F3},{actualHostPosition.Z:F3})"); } catch (Exception ex) { @@ -2341,19 +2342,19 @@ namespace NavisworksTransport.Core.Animation } var currentLinear = new Transform3D(currentRotation).Linear; var targetLinear = new Transform3D(newRotation).Linear; - 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=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " + - $"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " + - $"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})"); + LogManager.Debug( + $"[动画姿态入口] {_animatedObject.DisplayName} 跟踪点=({_trackedPosition.X:F3},{_trackedPosition.Y:F3},{_trackedPosition.Z:F3}), " + + $"目标点=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3})"); + LogManager.Debug( + $"[动画姿态入口] {_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.Debug( + $"[动画姿态入口] {_animatedObject.DisplayName} 目标姿态: " + + $"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " + + $"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " + + $"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})"); ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation( _animatedObject, currentPositionForTransform, @@ -2370,6 +2371,15 @@ namespace NavisworksTransport.Core.Animation _trackedRotation = newRotation; _hasTrackedRotation = true; _currentYaw = ModelItemTransformHelper.GetYawFromRotation(newRotation); + + if (isRailRealObject) + { + Point3D hostActualAfter = GetTrackedObjectPosition(_animatedObject); + LogManager.Debug( + $"[Rail姿态应用] 宿主最终跟踪中心=({hostActualAfter.X:F3},{hostActualAfter.Y:F3},{hostActualAfter.Z:F3}), " + + $"目标跟踪中心=({newPosition.X:F3},{newPosition.Y:F3},{newPosition.Z:F3}), " + + $"偏差=({hostActualAfter.X - newPosition.X:F3},{hostActualAfter.Y - newPosition.Y:F3},{hostActualAfter.Z - newPosition.Z:F3})"); + } } catch (Exception ex) { @@ -2492,6 +2502,16 @@ namespace NavisworksTransport.Core.Animation _savedObjectHasCustomRotation = _hasTrackedRotation; _hasSavedObjectState = true; LogManager.Info($"已保存物体状态: pos=({position.X:F2},{position.Y:F2},{position.Z:F2}), yaw={yaw * 180 / Math.PI:F2}度, customRotation={_savedObjectHasCustomRotation}"); + if (_savedObjectHasCustomRotation) + { + var linear = new Transform3D(_savedObjectRotation).Linear; + LogManager.Info( + $"[PAM保存姿态] {obj.DisplayName} 保存目标姿态: " + + $"位置=({_savedObjectPosition.X:F3},{_savedObjectPosition.Y:F3},{_savedObjectPosition.Z:F3}), " + + $"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " + + $"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " + + $"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})"); + } } catch (Exception ex) { @@ -2522,10 +2542,18 @@ namespace NavisworksTransport.Core.Animation var originalAnimatedObject = _animatedObject; _animatedObject = obj; - _trackedPosition = _savedObjectPosition; - _currentYaw = _savedObjectYaw; - _trackedRotation = _savedObjectRotation; - _hasTrackedRotation = _savedObjectHasCustomRotation; + LogHostActualPoseAxes(obj, "[PAM恢复前宿主姿态]", false); + + if (_savedObjectHasCustomRotation) + { + var linear = new Transform3D(_savedObjectRotation).Linear; + LogManager.Info( + $"[PAM恢复姿态] {obj.DisplayName} 恢复目标姿态: " + + $"位置=({_savedObjectPosition.X:F3},{_savedObjectPosition.Y:F3},{_savedObjectPosition.Z:F3}), " + + $"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " + + $"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " + + $"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})"); + } if (_savedObjectHasCustomRotation) { @@ -2536,6 +2564,13 @@ namespace NavisworksTransport.Core.Animation UpdateObjectPosition(_savedObjectPosition, _savedObjectYaw); } + _trackedPosition = _savedObjectPosition; + _currentYaw = _savedObjectYaw; + _trackedRotation = _savedObjectRotation; + _hasTrackedRotation = _savedObjectHasCustomRotation; + + LogHostActualPoseAxes(obj, "[PAM恢复后宿主姿态]", false); + _animatedObject = originalAnimatedObject; LogManager.Info($"已恢复物体状态: pos=({_savedObjectPosition.X:F2},{_savedObjectPosition.Y:F2},{_savedObjectPosition.Z:F2}), yaw={_savedObjectYaw * 180 / Math.PI:F2}度, customRotation={_savedObjectHasCustomRotation}"); } @@ -2596,6 +2631,8 @@ namespace NavisworksTransport.Core.Animation _animatedObject = obj; } + LogHostActualPoseAxes(obj, "[动画姿态复用前宿主姿态]", false); + if (hasCustomRotation) { UpdateObjectPosition(targetPosition, targetRotation); @@ -2605,6 +2642,8 @@ namespace NavisworksTransport.Core.Animation UpdateObjectPosition(targetPosition, targetYawRadians); } + LogHostActualPoseAxes(obj, "[动画姿态复用后宿主姿态]", false); + _animatedObject = originalAnimatedObject; LogManager.Info( @@ -2618,6 +2657,37 @@ namespace NavisworksTransport.Core.Animation } } + private void LogHostActualPoseAxes(ModelItem obj, string prefix, bool important) + { + if (obj == null) + { + return; + } + + try + { + Transform3D transform = obj.Transform; + Matrix3 linear = transform.Linear; + string message = + $"{prefix} {obj.DisplayName}: " + + $"X=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " + + $"Y=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " + + $"Z=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})"; + if (important) + { + LogManager.Info(message); + } + else + { + LogManager.Debug(message); + } + } + catch (Exception ex) + { + LogManager.Warning($"{prefix} 读取失败: {ex.Message}"); + } + } + /// /// 获取播放方向（1=正向，-1=反向） /// @@ -3180,7 +3250,7 @@ namespace NavisworksTransport.Core.Animation { _originalTransform = animatedObject.Transform; _originalCenter = animatedObject.BoundingBox().Center; - _trackedPosition = GetTrackedObjectPosition(animatedObject, _route?.PathType == PathType.Rail && !_isVirtualObject); + _trackedPosition = GetTrackedObjectPosition(animatedObject); // 保持当前的 _currentYaw（因为物体可能已经被 MoveObjectToPathStart 旋转） // 不要从 Transform 中提取，因为 Transform 返回的是原始值，不是当前值 @@ -3197,7 +3267,7 @@ namespace NavisworksTransport.Core.Animation LogManager.Info($"[CreateAnimation] 动画已创建，状态设置为Ready"); } - private Point3D GetTrackedObjectPosition(ModelItem item, bool useStableRailAnchor) + private Point3D GetTrackedObjectPosition(ModelItem item) { if (item == null) { @@ -3205,8 +3275,35 @@ namespace NavisworksTransport.Core.Animation } var bounds = item.BoundingBox(); + return bounds?.Center ?? new Point3D(0, 0, 0); + } + + private Point3D ResolveGroundTrackedCenter(Point3D groundContactPoint, double objectHeight) + { var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); - return ModelItemTransformHelper.GetHostBottomAnchorPoint(bounds, adapter); + var canonicalReferencePoint = ToNumerics(adapter.ToCanonicalPoint(groundContactPoint)); + var canonicalCenter = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter( + canonicalReferencePoint, + HostCoordinateAdapter.CanonicalUpVector3, + objectHeight); + return adapter.FromCanonicalPoint(new Point3D(canonicalCenter.X, canonicalCenter.Y, canonicalCenter.Z)); + } + + private Point3D ResolveHoistingTrackedCenter(Point3D referencePoint, double objectHeight, double referenceContactFactor) + { + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + var canonicalReferencePoint = ToNumerics(adapter.ToCanonicalPoint(referencePoint)); + var canonicalCenter = CanonicalTrackedPositionResolver.ResolveCenterFromContactReference( + canonicalReferencePoint, + HostCoordinateAdapter.CanonicalUpVector3, + objectHeight, + referenceContactFactor); + return adapter.FromCanonicalPoint(new Point3D(canonicalCenter.X, canonicalCenter.Y, canonicalCenter.Z)); + } + + private static Vector3 ToNumerics(Point3D point) + { + return new Vector3((float)point.X, (float)point.Y, (float)point.Z); } /// diff --git a/src/Core/Collision/ClashDetectiveIntegration.cs b/src/Core/Collision/ClashDetectiveIntegration.cs index 1e52bdb..ac693d4 100644 --- a/src/Core/Collision/ClashDetectiveIntegration.cs +++ b/src/Core/Collision/ClashDetectiveIntegration.cs @@ -801,29 +801,34 @@ namespace NavisworksTransport var targetYaw = candidate.AnimatedObjectTrackedYawRadians; var targetRotation = candidate.AnimatedObjectTrackedRotation; - // 🔥 关键：为了最高精度，先回到CAD原始位置，再移动到目标位置 - // 避免累积误差影响碰撞检测精度 - doc.Models.ResetPermanentTransform(modelItems); - if (candidate.AnimatedObjectHasTrackedRotation && targetRotation != null) { - var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); - var originalBounds = testAnimatedObject.BoundingBox(); - var originalTrackedPosition = ModelItemTransformHelper.GetHostBottomAnchorPoint(originalBounds, adapter); - var originalRotation = testAnimatedObject.Transform.Factor().Rotation; LogManager.Info( $"[ClashDetective验证] 三维候选恢复: " + - $"当前跟踪点=({originalTrackedPosition.X:F3},{originalTrackedPosition.Y:F3},{originalTrackedPosition.Z:F3}), " + + $"对象={testAnimatedObject.DisplayName}, " + $"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3})"); - ModelItemTransformHelper.MoveItemIncrementallyToPositionAndRotation( - testAnimatedObject, - originalTrackedPosition, - originalRotation, - targetPosition, - targetRotation); + + var pam = PathAnimationManager.GetInstance(); + if (pam != null && pam.ControlsAnimatedObject(testAnimatedObject)) + { + pam.MoveAnimatedObjectToPose( + testAnimatedObject, + targetPosition, + targetYaw, + targetRotation, + true); + } + else + { + throw new InvalidOperationException( + $"ClashDetective三维候选恢复失败：对象 {testAnimatedObject.DisplayName} 不受当前动画管理器控制。"); + } } else { + // 旧二维候选验证仍沿用“回到 CAD 原始位置再应用增量”的成熟语义。 + doc.Models.ResetPermanentTransform(modelItems); + var originalBounds = testAnimatedObject.BoundingBox(); var originalPos = new Point3D( (originalBounds.Min.X + originalBounds.Max.X) / 2, diff --git a/src/Core/PathDataManager.cs b/src/Core/PathDataManager.cs index 36df9f2..17036f3 100644 --- a/src/Core/PathDataManager.cs +++ b/src/Core/PathDataManager.cs @@ -50,7 +50,6 @@ namespace NavisworksTransport public string pathType { get; set; } public string railMountMode { get; set; } public string railPathDefinitionMode { get; set; } - public double railReferenceToAnchorOffset { get; set; } public double totalLength { get; set; } public JsonObjectLimits objectLimits { get; set; } public double gridSize { get; set; } @@ -280,7 +279,6 @@ namespace NavisworksTransport pathType = route.PathType.ToString(), railMountMode = route.RailMountMode.ToString(), railPathDefinitionMode = route.RailPathDefinitionMode.ToString(), - railReferenceToAnchorOffset = Math.Round(route.RailReferenceToAnchorOffset, exportSettings?.Precision ?? 3), totalLength = Math.Round(route.TotalLength, exportSettings?.Precision ?? 3), objectLimits = new { @@ -530,8 +528,6 @@ namespace NavisworksTransport route.RailPathDefinitionMode = railPathDefinitionMode; } - route.RailReferenceToAnchorOffset = jsonRoute.railReferenceToAnchorOffset; - // 解析路径点 var points = new List(); foreach (var jsonPoint in jsonRoute.points) @@ -1391,7 +1387,6 @@ namespace NavisworksTransport routeElement.SetAttribute("pathType", route.PathType.ToString()); routeElement.SetAttribute("railMountMode", route.RailMountMode.ToString()); routeElement.SetAttribute("railPathDefinitionMode", route.RailPathDefinitionMode.ToString()); - routeElement.SetAttribute("railReferenceToAnchorOffset", route.RailReferenceToAnchorOffset.ToString("F3")); routeElement.SetAttribute("totalLength", route.TotalLength.ToString("F3")); routeElement.SetAttribute("maxObjectLength", route.MaxObjectLength.ToString("F3")); routeElement.SetAttribute("maxObjectWidth", route.MaxObjectWidth.ToString("F3")); @@ -1534,11 +1529,6 @@ namespace NavisworksTransport route.RailPathDefinitionMode = railPathDefinitionMode; } - if (double.TryParse(routeNode.Attributes?["railReferenceToAnchorOffset"]?.Value, out double railReferenceToAnchorOffset)) - { - route.RailReferenceToAnchorOffset = railReferenceToAnchorOffset; - } - // 解析创建时间 if (DateTime.TryParse(routeNode.Attributes?["created"]?.Value, out DateTime createdTime)) { diff --git a/src/Core/PathDatabase.cs b/src/Core/PathDatabase.cs index 5b03432..44af0c6 100644 --- a/src/Core/PathDatabase.cs +++ b/src/Core/PathDatabase.cs @@ -101,7 +101,6 @@ namespace NavisworksTransport LiftHeight REAL, RailMountMode INTEGER, RailPathDefinitionMode INTEGER, - RailReferenceToAnchorOffset REAL, CreatedTime DATETIME, LastModified DATETIME ) @@ -300,8 +299,8 @@ namespace NavisworksTransport // 12. 设置数据库版本（SQLite内置user_version） // 版本号格式：主版本*10000 + 次版本*100 + 修订号 - // 例如：2.1.5 = 20105 - ExecuteNonQuery("PRAGMA user_version = 20105"); // v2.1.5 - 碰撞对象位姿字段命名统一为动画跟踪点语义 + // 例如：2.1.6 = 20106 + ExecuteNonQuery("PRAGMA user_version = 20106"); // v2.1.6 - 删除无效的 RailReferenceToAnchorOffset 字段 // 创建索引 ExecuteNonQuery("CREATE INDEX IF NOT EXISTS idx_reports_route ON CollisionReports(RouteId)"); @@ -464,8 +463,8 @@ namespace NavisworksTransport // 路径长度由 PathRoute.TotalLength 计算属性实时从 Edges/Points 计算 var sql = @" INSERT OR REPLACE INTO PathRoutes - (Id, Name, EstimatedTime, TurnRadius, IsCurved, MaxObjectLength, MaxObjectWidth, MaxObjectHeight, SafetyMargin, GridSize, PathType, LiftHeight, RailMountMode, RailPathDefinitionMode, RailReferenceToAnchorOffset, CreatedTime, LastModified) - VALUES (@id, @name, @time, @turnRadius, @isCurved, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @pathType, @liftHeightMeters, @railMountMode, @railPathDefinitionMode, @railReferenceToAnchorOffset, @created, @modified) + (Id, Name, EstimatedTime, TurnRadius, IsCurved, MaxObjectLength, MaxObjectWidth, MaxObjectHeight, SafetyMargin, GridSize, PathType, LiftHeight, RailMountMode, RailPathDefinitionMode, CreatedTime, LastModified) + VALUES (@id, @name, @time, @turnRadius, @isCurved, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @pathType, @liftHeightMeters, @railMountMode, @railPathDefinitionMode, @created, @modified) "; using (var cmd = new SQLiteCommand(sql, _connection)) @@ -484,7 +483,6 @@ namespace NavisworksTransport cmd.Parameters.AddWithValue("@liftHeightMeters", route.LiftHeight); cmd.Parameters.AddWithValue("@railMountMode", (int)route.RailMountMode); cmd.Parameters.AddWithValue("@railPathDefinitionMode", (int)route.RailPathDefinitionMode); - cmd.Parameters.AddWithValue("@railReferenceToAnchorOffset", route.RailReferenceToAnchorOffset); cmd.Parameters.AddWithValue("@created", route.CreatedTime); cmd.Parameters.AddWithValue("@modified", DateTime.Now); cmd.ExecuteNonQuery(); @@ -1542,7 +1540,6 @@ namespace NavisworksTransport LiftHeight = Convert.ToDouble(reader["LiftHeight"]), RailMountMode = (RailMountMode)Convert.ToInt32(reader["RailMountMode"]), RailPathDefinitionMode = (RailPathDefinitionMode)Convert.ToInt32(reader["RailPathDefinitionMode"]), - RailReferenceToAnchorOffset = Convert.ToDouble(reader["RailReferenceToAnchorOffset"]), CreatedTime = Convert.ToDateTime(reader["CreatedTime"]), LastModified = Convert.ToDateTime(reader["LastModified"]) }; @@ -2724,7 +2721,7 @@ namespace NavisworksTransport cmd.CommandText = @" SELECT Id, Name, CreatedTime, LastModified, TurnRadius, IsCurved, MaxObjectLength, MaxObjectWidth, MaxObjectHeight, - SafetyMargin, GridSize, PathType, LiftHeight, RailMountMode, RailPathDefinitionMode, RailReferenceToAnchorOffset + SafetyMargin, GridSize, PathType, LiftHeight, RailMountMode, RailPathDefinitionMode FROM PathRoutes WHERE Id = @Id"; @@ -2750,8 +2747,7 @@ namespace NavisworksTransport PathType = (PathType)Convert.ToInt32(reader["PathType"]), LiftHeight = Convert.ToDouble(reader["LiftHeight"]), RailMountMode = (RailMountMode)Convert.ToInt32(reader["RailMountMode"]), - RailPathDefinitionMode = (RailPathDefinitionMode)Convert.ToInt32(reader["RailPathDefinitionMode"]), - RailReferenceToAnchorOffset = Convert.ToDouble(reader["RailReferenceToAnchorOffset"]) + RailPathDefinitionMode = (RailPathDefinitionMode)Convert.ToInt32(reader["RailPathDefinitionMode"]) }; // 加载路径点和路径边 @@ -2788,8 +2784,7 @@ namespace NavisworksTransport var columnsToAdd = new Dictionary { ["railmountmode"] = "ALTER TABLE PathRoutes ADD COLUMN RailMountMode INTEGER DEFAULT 0", - ["railpathdefinitionmode"] = "ALTER TABLE PathRoutes ADD COLUMN RailPathDefinitionMode INTEGER DEFAULT 0", - ["railreferencetoanchoroffset"] = "ALTER TABLE PathRoutes ADD COLUMN RailReferenceToAnchorOffset REAL DEFAULT 0" + ["railpathdefinitionmode"] = "ALTER TABLE PathRoutes ADD COLUMN RailPathDefinitionMode INTEGER DEFAULT 0" }; foreach (var column in columnsToAdd) diff --git a/src/Core/PathPlanningModels.cs b/src/Core/PathPlanningModels.cs index 188f8a2..e3ba6bb 100644 --- a/src/Core/PathPlanningModels.cs +++ b/src/Core/PathPlanningModels.cs @@ -737,13 +737,6 @@ namespace NavisworksTransport /// public RailPathDefinitionMode RailPathDefinitionMode { get; set; } - /// - /// Rail 参考线到构件对接点的偏移距离（模型单位） - /// 在 LegacySuspensionPoint 模式下通常为 0。 - /// 在 RailCenterLine 模式下，结合 RailMountMode 解释为相对轨道中心线的上下偏移。 - /// - public double RailReferenceToAnchorOffset { get; set; } - // 数据库分析相关属性 /// /// 碰撞数量（从数据库加载） @@ -784,7 +777,6 @@ namespace NavisworksTransport IsCurved = false; RailMountMode = RailMountMode.UnderRail; RailPathDefinitionMode = RailPathDefinitionMode.LegacySuspensionPoint; - RailReferenceToAnchorOffset = 0.0; } /// @@ -807,7 +799,6 @@ namespace NavisworksTransport Description = string.Empty; RailMountMode = RailMountMode.UnderRail; RailPathDefinitionMode = RailPathDefinitionMode.LegacySuspensionPoint; - RailReferenceToAnchorOffset = 0.0; } /// @@ -1042,8 +1033,7 @@ namespace NavisworksTransport MaxObjectHeight = MaxObjectHeight, SafetyMargin = SafetyMargin, RailMountMode = RailMountMode, - RailPathDefinitionMode = RailPathDefinitionMode, - RailReferenceToAnchorOffset = RailReferenceToAnchorOffset + RailPathDefinitionMode = RailPathDefinitionMode }; // 克隆所有路径点 diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs index e22a601..59c3aa1 100644 --- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs +++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs @@ -1885,12 +1885,9 @@ namespace NavisworksTransport.UI.WPF.ViewModels _clashIntegration.ClearWasLastTestCanceled(); break; } - - // 先清除所有碰撞高亮（包括预计算和ClashDetective） + ModelHighlightHelper.ClearCollisionHighlights(); - // 生成碰撞报告（无论有无碰撞）并等待完成 await GenerateAndSaveCollisionReport(); - // 高亮ClashDetective结果（只在动画自然完成时进行，确保报告生成完成） HighlightClashDetectiveResults(); break; case NavisworksTransport.Core.Animation.AnimationState.Stopped: diff --git a/src/Utils/CollisionSceneHelper.cs b/src/Utils/CollisionSceneHelper.cs index ffe839b..2d2e7b4 100644 --- a/src/Utils/CollisionSceneHelper.cs +++ b/src/Utils/CollisionSceneHelper.cs @@ -118,6 +118,21 @@ namespace NavisworksTransport.Utils LogManager.Info(string.Format("已保存动画物体状态: {0}, 位置=({1:F2},{2:F2},{3:F2}), 朝向={4:F2}°, customRotation={5}", animatedObject.DisplayName, state.Position.X, state.Position.Y, state.Position.Z, currentYaw * 180 / Math.PI, state.HasCustomRotation)); + if (state.HasCustomRotation && state.Rotation != null) + { + var linear = new Transform3D(state.Rotation).Linear; + LogManager.Info(string.Format( + "[报告姿态保存] {0} 保存姿态: " + + "位置=({1:F3},{2:F3},{3:F3}), " + + "X=({4:F4},{5:F4},{6:F4}), " + + "Y=({7:F4},{8:F4},{9:F4}), " + + "Z=({10:F4},{11:F4},{12:F4})", + animatedObject.DisplayName, + state.Position.X, state.Position.Y, state.Position.Z, + linear.Get(0, 0), linear.Get(1, 0), linear.Get(2, 0), + linear.Get(0, 1), linear.Get(1, 1), linear.Get(2, 1), + linear.Get(0, 2), linear.Get(1, 2), linear.Get(2, 2))); + } return state; } @@ -139,6 +154,22 @@ namespace NavisworksTransport.Utils try { + if (state.HasCustomRotation && state.Rotation != null) + { + var linear = new Transform3D(state.Rotation).Linear; + LogManager.Info(string.Format( + "[报告姿态恢复] {0} 恢复目标姿态: " + + "位置=({1:F3},{2:F3},{3:F3}), " + + "X=({4:F4},{5:F4},{6:F4}), " + + "Y=({7:F4},{8:F4},{9:F4}), " + + "Z=({10:F4},{11:F4},{12:F4})", + animatedObject.DisplayName, + state.Position.X, state.Position.Y, state.Position.Z, + linear.Get(0, 0), linear.Get(1, 0), linear.Get(2, 0), + linear.Get(0, 1), linear.Get(1, 1), linear.Get(2, 1), + linear.Get(0, 2), linear.Get(1, 2), linear.Get(2, 2))); + } + var pam = PathAnimationManager.GetInstance(); if (pam != null && pam.ControlsAnimatedObject(animatedObject)) { diff --git a/src/Utils/CoordinateSystem/CanonicalRailOffsetResolver.cs b/src/Utils/CoordinateSystem/CanonicalRailOffsetResolver.cs new file mode 100644 index 0000000..1b6e757 --- /dev/null +++ b/src/Utils/CoordinateSystem/CanonicalRailOffsetResolver.cs @@ -0,0 +1,48 @@ +using System; +using System.Numerics; +using NavisworksTransport.Core; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// 负责在 Canonical Space 中，将 Rail 路径参考点转换成动画跟踪中心点。 + /// 这层只处理纯数学偏移，不依赖 Navisworks API。 + /// + public static class CanonicalRailOffsetResolver + { + public static double GetCenterTrackedOffset(PathRoute route, double objectHeight) + { + // 路径点本身已经是安装参考点（终点锚点）语义。 + // 动画跟踪中心只需要相对安装参考点沿轨道法向偏移半个高度， + // 不能叠加任何“参考线 -> 锚点”旧语义偏移，否则会把终点/逐帧参考点重复平移。 + return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode) + ? -objectHeight / 2.0 + : objectHeight / 2.0; + } + + public static Vector3 ResolveTrackedCenter( + PathRoute route, + Vector3 canonicalReferencePoint, + RailLocalFrame localFrame, + double objectHeight) + { + if (route == null || route.PathType != PathType.Rail) + { + return canonicalReferencePoint; + } + + if (localFrame == null) + { + throw new ArgumentNullException(nameof(localFrame)); + } + + double centerOffset = GetCenterTrackedOffset(route, objectHeight); + double referenceContactFactor = 0.5 - (centerOffset / objectHeight); + return CanonicalTrackedPositionResolver.ResolveCenterFromContactReference( + canonicalReferencePoint, + localFrame.Normal, + objectHeight, + referenceContactFactor); + } + } +} diff --git a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs index a4f82b3..405f01f 100644 --- a/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs +++ b/src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs @@ -11,6 +11,31 @@ namespace NavisworksTransport.Utils.CoordinateSystem { private const float TangentEpsilon = 1e-6f; + public static bool TryCreateLocalFrame( + Vector3 previousPoint, + Vector3 currentPoint, + Vector3 nextPoint, + Vector3 canonicalUp, + out RailLocalFrame frame) + { + frame = null; + + if (!TryCreateBasis( + previousPoint, + currentPoint, + nextPoint, + canonicalUp, + out var forward, + out var lateral, + out var up)) + { + return false; + } + + frame = new RailLocalFrame(forward, lateral, up); + return true; + } + public static bool TryCreateBasis( Vector3 previousPoint, Vector3 currentPoint, diff --git a/src/Utils/CoordinateSystem/CanonicalTrackedPositionResolver.cs b/src/Utils/CoordinateSystem/CanonicalTrackedPositionResolver.cs new file mode 100644 index 0000000..fb43c6c --- /dev/null +++ b/src/Utils/CoordinateSystem/CanonicalTrackedPositionResolver.cs @@ -0,0 +1,43 @@ +using System.Numerics; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// 在 Canonical Space 中，将路径参考点解析为动画跟踪中心点。 + /// 统一表达“参考点位于物体法向哪一侧接触面”的语义。 + /// + public static class CanonicalTrackedPositionResolver + { + /// + /// 将任意法向上的参考点解析为几何中心。 + /// referenceContactFactor 的语义： + /// 0 = 参考点位于底部/负法向接触面 + /// 1 = 参考点位于顶部/正法向接触面 + /// 中心点偏移 = normal * height * (0.5 - referenceContactFactor) + /// + public static Vector3 ResolveCenterFromContactReference( + Vector3 referencePoint, + Vector3 normal, + double objectHeight, + double referenceContactFactor) + { + return referencePoint + Vector3.Normalize(normal) * (float)(objectHeight * (0.5 - referenceContactFactor)); + } + + public static Vector3 ResolveGroundTrackedCenter( + Vector3 groundContactPoint, + Vector3 canonicalUp, + double objectHeight) + { + return ResolveCenterFromContactReference(groundContactPoint, canonicalUp, objectHeight, 0.0); + } + + public static Vector3 ResolveTopSuspensionTrackedCenter( + Vector3 suspensionReferencePoint, + Vector3 canonicalUp, + double objectHeight) + { + return ResolveCenterFromContactReference(suspensionReferencePoint, canonicalUp, objectHeight, 1.0); + } + } +} diff --git a/src/Utils/CoordinateSystem/RailLocalFrame.cs b/src/Utils/CoordinateSystem/RailLocalFrame.cs new file mode 100644 index 0000000..5306db3 --- /dev/null +++ b/src/Utils/CoordinateSystem/RailLocalFrame.cs @@ -0,0 +1,33 @@ +using System; +using System.Numerics; + +namespace NavisworksTransport.Utils.CoordinateSystem +{ + /// + /// Rail 路径在 Canonical Space 中的局部坐标系。 + /// Forward 沿路径前进方向，Normal 表示安装法向，Lateral 由二者叉乘确定。 + /// + public sealed class RailLocalFrame + { + public RailLocalFrame(Vector3 forward, Vector3 lateral, Vector3 normal) + { + Forward = Normalize(forward, nameof(forward)); + Lateral = Normalize(lateral, nameof(lateral)); + Normal = Normalize(normal, nameof(normal)); + } + + public Vector3 Forward { get; } + public Vector3 Lateral { get; } + public Vector3 Normal { get; } + + private static Vector3 Normalize(Vector3 value, string name) + { + if (value.LengthSquared() < 1e-12f) + { + throw new ArgumentException("局部坐标轴长度不能为零。", name); + } + + return Vector3.Normalize(value); + } + } +} diff --git a/src/Utils/ModelItemTransformHelper.cs b/src/Utils/ModelItemTransformHelper.cs index 0ad8a0f..1e6d59f 100644 --- a/src/Utils/ModelItemTransformHelper.cs +++ b/src/Utils/ModelItemTransformHelper.cs @@ -436,7 +436,7 @@ namespace NavisworksTransport.Utils /// 适用于碰撞位置还原等场景 /// /// 要移动的物体 - /// 目标位置（地面位置，即包围盒底面） + /// 目标位置（动画跟踪点，当前统一为几何中心） /// 目标朝向（弧度，绕Z轴） public static void MoveItemToPositionAndYaw(ModelItem item, Point3D targetPosition, double targetYaw) { @@ -448,18 +448,14 @@ namespace NavisworksTransport.Utils // 获取CAD原始状态 var originalBounds = item.BoundingBox(); - var originalGroundPos = new Point3D( - originalBounds.Center.X, - originalBounds.Center.Y, - originalBounds.Min.Z - ); + var originalCenterPos = originalBounds.Center; var originalYaw = GetYawFromTransform(item.Transform); // 计算从CAD原始位置到目标位置的增量 var deltaPos = new Vector3D( - targetPosition.X - originalGroundPos.X, - targetPosition.Y - originalGroundPos.Y, - targetPosition.Z - originalGroundPos.Z + targetPosition.X - originalCenterPos.X, + targetPosition.Y - originalCenterPos.Y, + targetPosition.Z - originalCenterPos.Z ); double deltaYaw = targetYaw - originalYaw; @@ -470,13 +466,13 @@ namespace NavisworksTransport.Utils // 有旋转：需要补偿绕原点旋转带来的位置偏移 double cos = Math.Cos(deltaYaw); double sin = Math.Sin(deltaYaw); - double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin; - double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos; + double rotatedX = originalCenterPos.X * cos - originalCenterPos.Y * sin; + double rotatedY = originalCenterPos.X * sin + originalCenterPos.Y * cos; var compensatedTranslation = new Vector3D( targetPosition.X - rotatedX, targetPosition.Y - rotatedY, - deltaPos.Z + targetPosition.Z - originalCenterPos.Z ); var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)); @@ -544,24 +540,24 @@ namespace NavisworksTransport.Utils targetPosition.Y - rotatedCurrentPosition.Y, targetPosition.Z - rotatedCurrentPosition.Z); - LogManager.Info( + LogManager.Debug( $"[模型增量姿态] {item.DisplayName} 当前=({currentPosition.X:F3},{currentPosition.Y:F3},{currentPosition.Z:F3}), " + $"目标=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " + $"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})"); - LogManager.Info( + LogManager.Debug( $"[模型增量姿态] {item.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( + LogManager.Debug( $"[模型增量姿态] {item.DisplayName} 目标旋转: " + $"X=({targetLinear.Get(0, 0):F4},{targetLinear.Get(1, 0):F4},{targetLinear.Get(2, 0):F4}), " + $"Y=({targetLinear.Get(0, 1):F4},{targetLinear.Get(1, 1):F4},{targetLinear.Get(2, 1):F4}), " + $"Z=({targetLinear.Get(0, 2):F4},{targetLinear.Get(1, 2):F4},{targetLinear.Get(2, 2):F4})"); - LogManager.Info( + LogManager.Debug( $"[模型增量姿态] {item.DisplayName} 增量旋转: " + $"X=({deltaLinear.Get(0, 0):F4},{deltaLinear.Get(1, 0):F4},{deltaLinear.Get(2, 0):F4}), " + $"Y=({deltaLinear.Get(0, 1):F4},{deltaLinear.Get(1, 1):F4},{deltaLinear.Get(2, 1):F4}), " + @@ -602,18 +598,15 @@ namespace NavisworksTransport.Utils var actualRotation = actualTransform.Factor().Rotation; var actualLinear = new Transform3D(actualRotation).Linear; var targetLinear = new Transform3D(targetRotation).Linear; - var actualPosition = new Point3D( - actualBounds.Center.X, - actualBounds.Center.Y, - actualBounds.Min.Z); + var actualPosition = actualBounds.Center; - LogManager.Info( + LogManager.Debug( $"[模型增量姿态] {item.DisplayName} 立即读回位置(可能滞后): " + $"实际=({actualPosition.X:F3},{actualPosition.Y:F3},{actualPosition.Z:F3}), " + $"期望=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " + $"偏差=({actualPosition.X - targetPosition.X:F3},{actualPosition.Y - targetPosition.Y:F3},{actualPosition.Z - targetPosition.Z:F3})"); - LogManager.Info( + LogManager.Debug( $"[模型增量姿态] {item.DisplayName} 立即读回旋转(可能滞后/不反映override): " + $"实际X=({actualLinear.Get(0, 0):F4},{actualLinear.Get(1, 0):F4},{actualLinear.Get(2, 0):F4}), " + $"实际Y=({actualLinear.Get(0, 1):F4},{actualLinear.Get(1, 1):F4},{actualLinear.Get(2, 1):F4}), " + @@ -812,29 +805,22 @@ namespace NavisworksTransport.Utils var originalComponents = originalTransform.Factor(); var originalRotation = originalComponents.Rotation; var originalBounds = item.BoundingBox(); - var originalUp = GetUpDirectionFromTransform(originalTransform); - var localSize = EstimateLocalBoxSize(originalBounds, originalTransform); - double halfHeight = localSize.Z / 2.0; - var originalGroundPos = new Point3D( - originalBounds.Center.X - originalUp.X * halfHeight, - originalBounds.Center.Y - originalUp.Y * halfHeight, - originalBounds.Center.Z - originalUp.Z * halfHeight - ); + var originalCenterPos = originalBounds.Center; Rotation3D deltaRotation = BuildDeltaRotation(originalRotation, targetRotation); var rotationTransform = new Transform3D(deltaRotation); var linear = rotationTransform.Linear; - var rotatedGroundPos = new Point3D( - linear.Get(0, 0) * originalGroundPos.X + linear.Get(0, 1) * originalGroundPos.Y + linear.Get(0, 2) * originalGroundPos.Z, - linear.Get(1, 0) * originalGroundPos.X + linear.Get(1, 1) * originalGroundPos.Y + linear.Get(1, 2) * originalGroundPos.Z, - linear.Get(2, 0) * originalGroundPos.X + linear.Get(2, 1) * originalGroundPos.Y + linear.Get(2, 2) * originalGroundPos.Z + var rotatedCenterPos = new Point3D( + linear.Get(0, 0) * originalCenterPos.X + linear.Get(0, 1) * originalCenterPos.Y + linear.Get(0, 2) * originalCenterPos.Z, + linear.Get(1, 0) * originalCenterPos.X + linear.Get(1, 1) * originalCenterPos.Y + linear.Get(1, 2) * originalCenterPos.Z, + linear.Get(2, 0) * originalCenterPos.X + linear.Get(2, 1) * originalCenterPos.Y + linear.Get(2, 2) * originalCenterPos.Z ); var translation = new Vector3D( - targetPosition.X - rotatedGroundPos.X, - targetPosition.Y - rotatedGroundPos.Y, - targetPosition.Z - rotatedGroundPos.Z + targetPosition.X - rotatedCenterPos.X, + targetPosition.Y - rotatedCenterPos.Y, + targetPosition.Z - rotatedCenterPos.Z ); Transform3D transform; @@ -852,7 +838,7 @@ namespace NavisworksTransport.Utils transform = new Transform3D(deltaRotation, translation); } - LogAbsoluteTransformDiagnostics(item, originalRotation, targetRotation, deltaRotation, originalGroundPos, targetPosition, translation, halfHeight, originalUp); + LogAbsoluteTransformDiagnostics(item, originalRotation, targetRotation, deltaRotation, originalCenterPos, targetPosition, translation); doc.Models.OverridePermanentTransform(modelItems, transform, false); } @@ -870,11 +856,9 @@ namespace NavisworksTransport.Utils Rotation3D originalRotation, Rotation3D targetRotation, Rotation3D deltaRotation, - Point3D originalGroundPos, + Point3D originalTrackedPosition, Point3D targetPosition, - Vector3D translation, - double halfHeight, - Vector3D originalUp) + Vector3D translation) { try { @@ -899,9 +883,8 @@ namespace NavisworksTransport.Utils $"X=({deltaLinear.Get(0, 0):F4},{deltaLinear.Get(1, 0):F4},{deltaLinear.Get(2, 0):F4}), " + $"Y=({deltaLinear.Get(0, 1):F4},{deltaLinear.Get(1, 1):F4},{deltaLinear.Get(2, 1):F4}), " + $"Z=({deltaLinear.Get(0, 2):F4},{deltaLinear.Get(1, 2):F4},{deltaLinear.Get(2, 2):F4}), " + - $"原始Up=({originalUp.X:F4},{originalUp.Y:F4},{originalUp.Z:F4}), 半高={halfHeight:F3}, " + - $"原始底面中心=({originalGroundPos.X:F3},{originalGroundPos.Y:F3},{originalGroundPos.Z:F3}), " + - $"目标底面中心=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " + + $"原始跟踪点=({originalTrackedPosition.X:F3},{originalTrackedPosition.Y:F3},{originalTrackedPosition.Z:F3}), " + + $"目标跟踪点=({targetPosition.X:F3},{targetPosition.Y:F3},{targetPosition.Z:F3}), " + $"平移=({translation.X:F3},{translation.Y:F3},{translation.Z:F3})"); } catch (Exception ex) @@ -931,18 +914,14 @@ namespace NavisworksTransport.Utils // 获取CAD原始状态 var originalBounds = item.BoundingBox(); - var originalGroundPos = new Point3D( - originalBounds.Center.X, - originalBounds.Center.Y, - originalBounds.Min.Z - ); + var originalCenterPos = originalBounds.Center; var originalYaw = GetYawFromTransform(item.Transform); // 计算从CAD原始位置到目标位置的增量 var deltaPos = new Vector3D( - targetPosition.X - originalGroundPos.X, - targetPosition.Y - originalGroundPos.Y, - targetPosition.Z - originalGroundPos.Z + targetPosition.X - originalCenterPos.X, + targetPosition.Y - originalCenterPos.Y, + targetPosition.Z - originalCenterPos.Z ); double deltaYaw = targetYaw - originalYaw; @@ -964,13 +943,13 @@ namespace NavisworksTransport.Utils { double cos = Math.Cos(deltaYaw); double sin = Math.Sin(deltaYaw); - double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin; - double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos; + double rotatedX = originalCenterPos.X * cos - originalCenterPos.Y * sin; + double rotatedY = originalCenterPos.X * sin + originalCenterPos.Y * cos; components.Translation = new Vector3D( targetPosition.X - rotatedX, targetPosition.Y - rotatedY, - deltaPos.Z + targetPosition.Z - originalCenterPos.Z ); } else @@ -1079,18 +1058,14 @@ namespace NavisworksTransport.Utils // 2. 从CAD原始位置移动到保存的位置 // 获取CAD原始状态 var originalBounds = item.BoundingBox(); - var originalGroundPos = new Point3D( - originalBounds.Center.X, - originalBounds.Center.Y, - originalBounds.Min.Z - ); + var originalCenterPos = originalBounds.Center; var originalYaw = GetYawFromTransform(item.Transform); // 计算到保存位置的增量 var deltaPos = new Vector3D( - state.Position.X - originalGroundPos.X, - state.Position.Y - originalGroundPos.Y, - state.Position.Z - originalGroundPos.Z + state.Position.X - originalCenterPos.X, + state.Position.Y - originalCenterPos.Y, + state.Position.Z - originalCenterPos.Z ); double deltaYaw = state.YawRadians - originalYaw; @@ -1100,13 +1075,13 @@ namespace NavisworksTransport.Utils { double cos = Math.Cos(deltaYaw); double sin = Math.Sin(deltaYaw); - double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin; - double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos; + double rotatedX = originalCenterPos.X * cos - originalCenterPos.Y * sin; + double rotatedY = originalCenterPos.X * sin + originalCenterPos.Y * cos; var compensatedTranslation = new Vector3D( state.Position.X - rotatedX, state.Position.Y - rotatedY, - deltaPos.Z + state.Position.Z - originalCenterPos.Z ); var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)); @@ -1141,18 +1116,14 @@ namespace NavisworksTransport.Utils // 获取CAD原始状态 var originalBounds = item.BoundingBox(); - var originalGroundPos = new Point3D( - originalBounds.Center.X, - originalBounds.Center.Y, - originalBounds.Min.Z - ); + var originalCenterPos = originalBounds.Center; var originalYaw = GetYawFromTransform(item.Transform); // 计算从CAD原始位置到目标位置的增量 var deltaPos = new Vector3D( - targetPosition.X - originalGroundPos.X, - targetPosition.Y - originalGroundPos.Y, - targetPosition.Z - originalGroundPos.Z + targetPosition.X - originalCenterPos.X, + targetPosition.Y - originalCenterPos.Y, + targetPosition.Z - originalCenterPos.Z ); double deltaYaw = targetYaw - originalYaw; @@ -1171,13 +1142,13 @@ namespace NavisworksTransport.Utils { double cos = Math.Cos(deltaYaw); double sin = Math.Sin(deltaYaw); - double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin; - double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos; + double rotatedX = originalCenterPos.X * cos - originalCenterPos.Y * sin; + double rotatedY = originalCenterPos.X * sin + originalCenterPos.Y * cos; components.Translation = new Vector3D( targetPosition.X - rotatedX, targetPosition.Y - rotatedY, - deltaPos.Z + targetPosition.Z - originalCenterPos.Z ); } else diff --git a/src/Utils/RailPathPoseHelper.cs b/src/Utils/RailPathPoseHelper.cs index 378b09f..c668a4d 100644 --- a/src/Utils/RailPathPoseHelper.cs +++ b/src/Utils/RailPathPoseHelper.cs @@ -15,34 +15,11 @@ namespace NavisworksTransport.Utils private const double TangentEpsilon = 1e-9; private static bool _rotationConstructorConventionLogged; - /// - /// 获取 Rail 参考点到构件对接点的有符号偏移（模型单位）。 - /// LegacySuspensionPoint 模式下，路径点本身就是构件对接点，因此偏移为 0。 - /// - public static double GetAnchorOffset(PathRoute route) - { - if (route == null || route.PathType != PathType.Rail) - { - return 0.0; - } - - if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint) - { - return 0.0; - } - - return route.RailMountMode == RailMountMode.OverRail - ? route.RailReferenceToAnchorOffset - : -route.RailReferenceToAnchorOffset; - } - /// /// 计算构件底面中心相对于 Rail 参考点的 Z 偏移（模型单位）。 /// public static double GetBottomZOffset(PathRoute route, double objectHeight) { - double anchorOffset = GetAnchorOffset(route); - if (route == null || route.PathType != PathType.Rail) { return 0.0; @@ -50,10 +27,10 @@ namespace NavisworksTransport.Utils if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)) { - return anchorOffset - objectHeight; + return -objectHeight; } - return anchorOffset; + return 0.0; } /// @@ -63,8 +40,6 @@ namespace NavisworksTransport.Utils /// public static double GetObjectSpaceCenterZOffset(PathRoute route, double objectSpaceHeight) { - double anchorOffset = GetAnchorOffset(route); - if (route == null || route.PathType != PathType.Rail) { return 0.0; @@ -72,10 +47,10 @@ namespace NavisworksTransport.Utils if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)) { - return anchorOffset - objectSpaceHeight / 2.0; + return -objectSpaceHeight / 2.0; } - return anchorOffset + objectSpaceHeight / 2.0; + return objectSpaceHeight / 2.0; } /// @@ -160,11 +135,21 @@ namespace NavisworksTransport.Utils var adapterForRail = CoordinateSystemManager.Instance.CreateHostAdapter(); var canonicalReferencePointForRail = adapterForRail.ToCanonicalPoint(referencePoint); - var normal = ResolveRailNormal(previousPoint, referencePoint, nextPoint); - var canonicalCenterPointForRail = new Point3D( - canonicalReferencePointForRail.X + normal.X * centerOffset, - canonicalReferencePointForRail.Y + normal.Y * centerOffset, - canonicalReferencePointForRail.Z + normal.Z * centerOffset); + if (!TryCreateCanonicalLocalFrame(previousPoint, referencePoint, nextPoint, out RailLocalFrame frame)) + { + var canonicalCenterPointForRailFallback = new Point3D( + canonicalReferencePointForRail.X, + canonicalReferencePointForRail.Y, + canonicalReferencePointForRail.Z + centerOffset); + return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRailFallback); + } + + Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter( + route, + ToNumerics(canonicalReferencePointForRail), + frame, + objectSpaceHeight); + var canonicalCenterPointForRail = new Point3D(trackedCenter.X, trackedCenter.Y, trackedCenter.Z); return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRail); } @@ -210,18 +195,19 @@ namespace NavisworksTransport.Utils var canonicalCurrentPoint = ToNumerics(adapter.ToCanonicalPoint(currentPoint)); var canonicalNextPoint = ToNumerics(adapter.ToCanonicalPoint(nextPoint)); - if (!CanonicalRailPoseBuilder.TryCreateBasis( + if (!CanonicalRailPoseBuilder.TryCreateLocalFrame( canonicalPreviousPoint, canonicalCurrentPoint, canonicalNextPoint, HostCoordinateAdapter.CanonicalUpVector3, - out var canonicalForward, - out var canonicalLateral, - out var canonicalUp)) + out RailLocalFrame frame)) { return false; } + var canonicalForward = frame.Forward; + var canonicalLateral = frame.Lateral; + var canonicalUp = frame.Normal; Quaternion canonicalRotation = axisConvention.CreateQuaternion(canonicalForward, canonicalUp); Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation); @@ -397,53 +383,26 @@ namespace NavisworksTransport.Utils private static double GetBottomOffsetMagnitude(PathRoute route, double objectHeight) { - double anchorOffset = GetAnchorOffset(route); return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode) - ? anchorOffset - objectHeight - : anchorOffset; + ? -objectHeight + : 0.0; } private static double GetObjectSpaceCenterOffsetMagnitude(PathRoute route, double objectSpaceHeight) { - double anchorOffset = GetAnchorOffset(route); return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode) - ? anchorOffset - objectSpaceHeight / 2.0 - : anchorOffset + objectSpaceHeight / 2.0; + ? -objectSpaceHeight / 2.0 + : objectSpaceHeight / 2.0; } private static Vector3D ResolveRailNormal(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint) { - var tangent = ResolveTangent(previousPoint, currentPoint, nextPoint); - double tangentLengthSquared = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z; - - if (tangentLengthSquared < TangentEpsilon) + if (TryCreateCanonicalLocalFrame(previousPoint, currentPoint, nextPoint, out RailLocalFrame frame)) { - return HostCoordinateAdapter.CanonicalUp; + return ToNavVector(frame.Normal); } - tangent = new Vector3D( - tangent.X / Math.Sqrt(tangentLengthSquared), - tangent.Y / Math.Sqrt(tangentLengthSquared), - tangent.Z / Math.Sqrt(tangentLengthSquared)); - - var worldUp = HostCoordinateAdapter.CanonicalUp; - double projection = worldUp.X * tangent.X + worldUp.Y * tangent.Y + worldUp.Z * tangent.Z; - var normal = new Vector3D( - worldUp.X - projection * tangent.X, - worldUp.Y - projection * tangent.Y, - worldUp.Z - projection * tangent.Z); - - double normalLengthSquared = normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z; - if (normalLengthSquared < TangentEpsilon) - { - return HostCoordinateAdapter.CanonicalUp; - } - - double normalLength = Math.Sqrt(normalLengthSquared); - return new Vector3D( - normal.X / normalLength, - normal.Y / normalLength, - normal.Z / normalLength); + return HostCoordinateAdapter.CanonicalUp; } private static Vector3D ResolveTangent(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint) @@ -470,6 +429,21 @@ namespace NavisworksTransport.Utils return tangent; } + private static bool TryCreateCanonicalLocalFrame( + Point3D previousPoint, + Point3D currentPoint, + Point3D nextPoint, + out RailLocalFrame frame) + { + var adapter = CoordinateSystemManager.Instance.CreateHostAdapter(); + return CanonicalRailPoseBuilder.TryCreateLocalFrame( + ToNumerics(adapter.ToCanonicalPoint(previousPoint)), + ToNumerics(adapter.ToCanonicalPoint(currentPoint)), + ToNumerics(adapter.ToCanonicalPoint(nextPoint)), + HostCoordinateAdapter.CanonicalUpVector3, + out frame); + } + private static Vector3D Normalize(Vector3D vector) { double lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;