Compare commits
21 Commits
2e959c16c9
...
da6b6cb481
| Author | SHA1 | Date | |
|---|---|---|---|
| da6b6cb481 | |||
| 105061bade | |||
| 3cb5d8d423 | |||
| 5308552b40 | |||
| 66e68c6bd1 | |||
| cf6b2ef959 | |||
| 7f3be8495c | |||
| f86e8c8bf4 | |||
| 75afba7670 | |||
| 020e265918 | |||
| e9fb47d9ad | |||
| 82caa3e155 | |||
| 63b59890e4 | |||
| 999fffcaa7 | |||
| 034acb1d65 | |||
| f75dcb1f50 | |||
| 3634551e53 | |||
| c8d48755db | |||
| 10a0ad754c | |||
| 2f16ec0d43 | |||
| 9ea3cc0e02 |
@ -151,6 +151,15 @@ var rotation = new Rotation3D(qx, qy, qz, qw);
|
|||||||
|
|
||||||
## 9. 工具使用提示
|
## 9. 工具使用提示
|
||||||
|
|
||||||
|
### Shell 命令统一规范
|
||||||
|
|
||||||
|
所有 shell 命令、脚本、rg/fd 调用必须通过 `pwsh` (PowerShell 7) 执行,禁止用 cmd/bash:
|
||||||
|
|
||||||
|
```
|
||||||
|
pwsh -Command "rg -n '关键字' src/"
|
||||||
|
pwsh -Command "./build-and-deploy.bat"
|
||||||
|
```
|
||||||
|
|
||||||
### edit 工具的正确格式
|
### edit 工具的正确格式
|
||||||
|
|
||||||
```json
|
```json
|
||||||
|
|||||||
28
CHANGELOG.md
28
CHANGELOG.md
@ -1,5 +1,33 @@
|
|||||||
# NavisworksTransport 变更日志
|
# NavisworksTransport 变更日志
|
||||||
|
|
||||||
|
## [0.16.0] - 2026-06-26
|
||||||
|
|
||||||
|
### ✨ 新功能
|
||||||
|
|
||||||
|
- **贴合地面**:在"调整物体"窗口中新增"贴合地面"按钮,点选物体表面后自动旋转使该面贴地,并绕垂直轴搜索最小截面对齐路径方向。
|
||||||
|
- 新增 `AlignToGroundMinCrossSectionYawSearcher`:在贴地旋转(固定俯仰/翻滚)基础上绕宿主 Up 搜索 yaw,使物体在路径方向的截面投影面积最小(粗搜 5° + 细搜 1°)
|
||||||
|
- 复用 `ObjectPassageProjectionOptimizer.ProjectExtent` 的 AABB 投影公式
|
||||||
|
- 搜索完成后用 `ComposeHostCorrection` 将绝对贴地姿态转为相对于 baseline 的 host 欧拉修正,扣除 CAD 自带 yaw + pathYaw,避免确认/动画时重复旋转
|
||||||
|
- `HostQuaternionToCanonical` 用相似变换 `R_canon = M⁻¹·R_host·M` 做坐标系转换
|
||||||
|
- `ComputeGroundLiftForCorrection`:与自动调整同方法实测底边算 lift,测量后用新 correction+lift 完整重建使物体停留在贴地等价姿态
|
||||||
|
|
||||||
|
### 🐛 Bug 修复
|
||||||
|
|
||||||
|
- **自动调整/贴合地面后通行空间尺寸准确化**:
|
||||||
|
- 路径斜向时世界 AABB 的 X/Y/Z 不对应沿路径/垂直路径/高度,纯数学旋转 AABB 也会膨胀
|
||||||
|
- 改为用 Navisworks API 实际旋转物体到 X 轴方向测 AABB,再转回来,获取准确的沿路径/垂直路径/高度尺寸
|
||||||
|
- 提取 `MeasureXAlignedAabbAndSetCache` 辅助方法,自动调整和贴合地面共用
|
||||||
|
- **自动调整后手动改角度尺寸不同步**:`ObjectRotationCorrection` setter 清空 `_autoAabbSx/Sy/Sz` 缓存,手动改角度走 `CalculateRotatedDimensions` 重新计算;自动调整/贴合地面直接设字段不经过 setter,缓存不受影响
|
||||||
|
- **ObjectPassageProjectionOptimizer 初始候选缺失**:`OptimizeWithEvaluator` 初始化 `bestScore=Invalid` 从不评估 correction=Zero,当 baseline 已对齐路径时随机采样必然替换它。改为评估 Zero 作为初始候选
|
||||||
|
- **ZUp 测试语义修正**:`LocalEulerRotationCorrection` 的 Y=up 轴、Z=non-up 轴,ZUp 下绕 Z(up) 转应传 YDegrees,4 个旧测试在 ZUp 下把 ZDegrees 当字面 Z 轴已修正
|
||||||
|
- **AliasTreeTests 编译错误**:`SetAlias` 调用补全缺失的 alias 参数;4 个旧格式兼容测试改为期望抛 FormatException(`FromKey` 已按"不向后兼容"原则移除旧格式支持)
|
||||||
|
|
||||||
|
### 🔧 改进
|
||||||
|
|
||||||
|
- **"调整物体"窗口按钮区布局**:改为 Grid 布局,6 个按钮用固定间距列隔开,所有按钮显式 `Margin="0"` 覆盖 Style 的 Margin,真正居中
|
||||||
|
- **`CanonicalQuaternionToHostEulerCorrection` 加 hostType 参数重载**:去除全局 `CoordinateSystemManager` 依赖,便于单测
|
||||||
|
- 新增 `AlignToGroundMinCrossSectionYawSearcherTests`(12 个单测)覆盖平地/倾斜/CAD歪斜/垂直退化/立方体/增量链闭环等场景
|
||||||
|
|
||||||
## [0.15.8] - 2026-06-09
|
## [0.15.8] - 2026-06-09
|
||||||
|
|
||||||
### ✨ 新功能
|
### ✨ 新功能
|
||||||
|
|||||||
@ -66,6 +66,7 @@
|
|||||||
<Compile Include="UnitTests\Core\PathPlanningManagerHoistingCompletionTests.cs" />
|
<Compile Include="UnitTests\Core\PathPlanningManagerHoistingCompletionTests.cs" />
|
||||||
<Compile Include="UnitTests\Core\PathPersistenceTests.cs" />
|
<Compile Include="UnitTests\Core\PathPersistenceTests.cs" />
|
||||||
<Compile Include="UnitTests\Core\PathRouteCloneTests.cs" />
|
<Compile Include="UnitTests\Core\PathRouteCloneTests.cs" />
|
||||||
|
<Compile Include="UnitTests\CoordinateSystem\AlignToGroundMinCrossSectionYawSearcherTests.cs" />
|
||||||
<Compile Include="UnitTests\CoordinateSystem\AutoPathPlanningCoordinateSemanticsTests.cs" />
|
<Compile Include="UnitTests\CoordinateSystem\AutoPathPlanningCoordinateSemanticsTests.cs" />
|
||||||
<Compile Include="UnitTests\Integration\AutoPathGridGenerationAutomationTests.cs" />
|
<Compile Include="UnitTests\Integration\AutoPathGridGenerationAutomationTests.cs" />
|
||||||
<Compile Include="UnitTests\Integration\NavisworksTestAutomationClient.cs" />
|
<Compile Include="UnitTests\Integration\NavisworksTestAutomationClient.cs" />
|
||||||
|
|||||||
@ -344,6 +344,7 @@
|
|||||||
<Compile Include="src\Utils\UnitsConverter.cs" />
|
<Compile Include="src\Utils\UnitsConverter.cs" />
|
||||||
<Compile Include="src\Utils\VersionInfo.cs" />
|
<Compile Include="src\Utils\VersionInfo.cs" />
|
||||||
<!-- Coordinate System -->
|
<!-- Coordinate System -->
|
||||||
|
<Compile Include="src\Utils\CoordinateSystem\AlignToGroundMinCrossSectionYawSearcher.cs" />
|
||||||
<Compile Include="src\Utils\CoordinateSystem\CoordinateSystemType.cs" />
|
<Compile Include="src\Utils\CoordinateSystem\CoordinateSystemType.cs" />
|
||||||
<Compile Include="src\Utils\CoordinateSystem\ICoordinateSystem.cs" />
|
<Compile Include="src\Utils\CoordinateSystem\ICoordinateSystem.cs" />
|
||||||
<Compile Include="src\Utils\CoordinateSystem\CanonicalBounds3.cs" />
|
<Compile Include="src\Utils\CoordinateSystem\CanonicalBounds3.cs" />
|
||||||
|
|||||||
@ -0,0 +1,432 @@
|
|||||||
|
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||||
|
using NavisworksTransport.Utils.CoordinateSystem;
|
||||||
|
using System;
|
||||||
|
using System.Numerics;
|
||||||
|
|
||||||
|
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||||
|
{
|
||||||
|
[TestClass]
|
||||||
|
public class AlignToGroundMinCrossSectionYawSearcherTests
|
||||||
|
{
|
||||||
|
// ----- 验证辅助 -----
|
||||||
|
|
||||||
|
private static double Deg(double deg) => deg * Math.PI / 180.0;
|
||||||
|
|
||||||
|
private static double RadToDeg(double rad) => rad * 180.0 / Math.PI;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 计算在 q 姿态下物体沿 hostSide/hostUp 的截面投影面积。
|
||||||
|
/// 与 Searcher 内部 EvaluateCrossSectionArea 公式一致,用于独立验证搜索结果。
|
||||||
|
/// </summary>
|
||||||
|
private static double ComputeCrossSectionArea(
|
||||||
|
Quaternion q,
|
||||||
|
double sizeX, double sizeY, double sizeZ,
|
||||||
|
Vector3 hostSide, Vector3 hostUp)
|
||||||
|
{
|
||||||
|
Vector3 localX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, q));
|
||||||
|
Vector3 localY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, q));
|
||||||
|
Vector3 localZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, q));
|
||||||
|
double width =
|
||||||
|
Math.Abs(Vector3.Dot(localX, hostSide)) * sizeX +
|
||||||
|
Math.Abs(Vector3.Dot(localY, hostSide)) * sizeY +
|
||||||
|
Math.Abs(Vector3.Dot(localZ, hostSide)) * sizeZ;
|
||||||
|
double height =
|
||||||
|
Math.Abs(Vector3.Dot(localX, hostUp)) * sizeX +
|
||||||
|
Math.Abs(Vector3.Dot(localY, hostUp)) * sizeY +
|
||||||
|
Math.Abs(Vector3.Dot(localZ, hostUp)) * sizeZ;
|
||||||
|
return width * height;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从结果四元数反算绕 hostUp 的 deltaYaw(度)。
|
||||||
|
/// deltaQ = inverse(faceDown) * result;其旋转轴应近似 hostUp。
|
||||||
|
/// </summary>
|
||||||
|
private static double ExtractDeltaYawDegrees(Quaternion faceDown, Quaternion result, Vector3 hostUp)
|
||||||
|
{
|
||||||
|
Quaternion deltaQ = Quaternion.Normalize(Quaternion.Inverse(faceDown) * result);
|
||||||
|
// 提取旋转角度(取绝对值,deltaQ 可能是 q 或 -q,表示同一旋转)
|
||||||
|
double wClamped = Math.Min(1.0, Math.Abs(deltaQ.W));
|
||||||
|
double angleRad = 2.0 * Math.Acos(wClamped);
|
||||||
|
// 轴分量与 hostUp 同向为正,反向为负
|
||||||
|
Vector3 axis = new Vector3(deltaQ.X, deltaQ.Y, deltaQ.Z);
|
||||||
|
double sign = Vector3.Dot(axis, hostUp) >= 0 ? 1.0 : -1.0;
|
||||||
|
return sign * RadToDeg(angleRad);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- 回归保护:平地 + 路径沿 local X,长方体长边已对齐路径 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void FlatFace_PathAlongX_LongSideAlreadyAligned_KeepsZeroYaw_ZUp()
|
||||||
|
{
|
||||||
|
// sizeX=8(长) sizeY=4 sizeZ=2,路径沿 +X,ZUp
|
||||||
|
// 贴地=Identity(面已朝下),CAD=Identity
|
||||||
|
// yaw=0 时:width(沿Y)=4, height(沿Z)=2, 面积=8(最小)
|
||||||
|
// yaw=90° 时:width=8, height=2, 面积=16(更大)
|
||||||
|
// 期望搜索结果 ≈ 0°
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.Identity;
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitX, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
|
||||||
|
Assert.AreEqual(0.0, deltaYaw, 1.5,
|
||||||
|
$"长边已对齐路径时 yaw 应保持 0°,实际={deltaYaw:F2}°");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- 关键:平地 + 路径沿 Y,长方体需转 90° 才让长边对齐路径 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void FlatFace_PathAlongY_SearchFinds90DegToAlignLongSide_ZUp()
|
||||||
|
{
|
||||||
|
// sizeX=8(长) sizeY=4 sizeZ=2,路径沿 +Y,ZUp
|
||||||
|
// yaw=0:local X=+X,width(沿hostSide)=|cross(+Y,+Z)·X|*8 + ... = |+X·+X|*8 + 0 + 0 = 8(宽方向是X),height(沿Z)=2,面积=16
|
||||||
|
// 注:pathFwd=+Y, hostUp=+Z, hostSide=cross(+Y,+Z)=+X
|
||||||
|
// yaw=0: localX=+X,localY=+Y,localZ=+Z;width=|X·X|*8+|Y·X|*4+|Z·X|*2=8;height=|X·Z|*8+|Y·Z|*4+|Z·Z|*2=2;面积=16
|
||||||
|
// yaw=90°(绕Z):localX=+Y,localY=-X,localZ=+Z;width=|Y·X|*8+|-X·X|*4+|Z·X|*2=4;height=|Y·Z|*8+|-X·Z|*4+|Z·Z|*2=2;面积=8(最小)
|
||||||
|
// 期望搜索结果 ≈ ±90°(两个解面积相同,取其一)
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.Identity;
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitY, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
|
||||||
|
// 接受 +90 或 -90(面积相同),用 mod 180 容差判断
|
||||||
|
double mod180 = Math.Abs(deltaYaw) % 180.0;
|
||||||
|
Assert.AreEqual(90.0, mod180, 2.0,
|
||||||
|
$"路径沿 Y、长边沿 X 时应搜索到 ±90° yaw 使长边对齐路径,实际={deltaYaw:F2}°");
|
||||||
|
|
||||||
|
// 验证结果面积确实最小(=8),优于 yaw=0 的 16
|
||||||
|
Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitY, Vector3.UnitZ));
|
||||||
|
double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
double zeroYawArea = ComputeCrossSectionArea(faceDown, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
Assert.IsTrue(resultArea < zeroYawArea - 1e-3,
|
||||||
|
$"搜索结果面积 {resultArea:F3} 应小于 yaw=0 面积 {zeroYawArea:F3}");
|
||||||
|
Assert.AreEqual(8.0, resultArea, 0.1,
|
||||||
|
$"最小截面面积应 ≈ 8,实际={resultArea:F3}");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- YUp 宿主下同样行为 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void FlatFace_PathAlongY_SearchFinds90Deg_YUp()
|
||||||
|
{
|
||||||
|
// YUp:hostUp=+Y。sizeX=8(长) sizeY=4 sizeZ=2,路径沿 +Y(=hostUp)?
|
||||||
|
// 不对——路径沿 hostUp 是纯垂直,会退化。改路径沿 +Z(YUp 下水平方向之一)
|
||||||
|
// YUp 下水平面是 XZ,pathFwd=+Z, hostUp=+Y, hostSide=cross(+Z,+Y)=-X
|
||||||
|
// yaw=0: localX=+X,localY=+Y,localZ=+Z
|
||||||
|
// width(沿-X)=|X·-X|*8+|Y·-X|*4+|Z·-X|*2=8;height(沿+Y)=|X·Y|*8+|Y·Y|*4+|Z·Y|*2=4;面积=32
|
||||||
|
// yaw=90°(绕+Y): localX=+Z,localY=+Y,localZ=-X
|
||||||
|
// width(沿-X)=|Z·-X|*8+|Y·-X|*4+|-X·-X|*2=2;height(沿+Y)=0+|Y·Y|*4+0=4;面积=8(最小)
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
Quaternion faceDown = Quaternion.Identity;
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitZ, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
|
||||||
|
double mod180 = Math.Abs(deltaYaw) % 180.0;
|
||||||
|
Assert.AreEqual(90.0, mod180, 2.0,
|
||||||
|
$"YUp 下路径沿 Z、长边沿 X 时应搜索到 ±90° yaw,实际={deltaYaw:F2}°");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- 贴地旋转已固定俯仰,验证搜索仍能找到使截面最小的 yaw -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void TiltedFaceDown_SearchStillFindsMinCrossSection_ZUp()
|
||||||
|
{
|
||||||
|
// 构造贴地旋转:绕 X 轴翻 30°(模拟贴地后俯仰已固定)
|
||||||
|
// 物体 8×4×2,路径沿 +X,ZUp
|
||||||
|
// 搜索应在绕 Z 的 yaw 上找到最小截面
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(30.0));
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitX, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
// 验证:搜索结果的截面面积 <= yaw=0(只用 faceDown) 的面积
|
||||||
|
Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitX, Vector3.UnitZ));
|
||||||
|
double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
double faceDownOnlyArea = ComputeCrossSectionArea(faceDown, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
Assert.IsTrue(resultArea <= faceDownOnlyArea + 1e-3,
|
||||||
|
$"搜索结果面积 {resultArea:F3} 应不大于仅贴地(yaw=0)面积 {faceDownOnlyArea:F3}");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- CAD 姿态天然歪斜:验证搜索在歪斜 CAD 基础上仍找到最小截面 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void SkewedCadRotation_SearchFindsMinCrossSection_ZUp()
|
||||||
|
{
|
||||||
|
// CAD 姿态自带 45° 绕 Z 旋转(天然歪斜),物体 8×4×2,路径沿 +X
|
||||||
|
// 搜索应找到补偿 yaw 使截面最小
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.Identity;
|
||||||
|
Quaternion cadRot = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)Deg(45.0));
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitX, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
// 验证:搜索结果面积 <= 不搜索(只用 faceDown) 的面积
|
||||||
|
Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitX, Vector3.UnitZ));
|
||||||
|
double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
double noYawArea = ComputeCrossSectionArea(faceDown * cadRot, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
|
||||||
|
Assert.IsTrue(resultArea <= noYawArea + 1e-3,
|
||||||
|
$"CAD 歪斜时搜索结果面积 {resultArea:F3} 应不大于不搜索面积 {noYawArea:F3}");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- 退化:纯垂直路径(pathFwd 平行 hostUp),保持贴地姿态 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void VerticalPath_ReturnsFaceDownUnchanged_ZUp()
|
||||||
|
{
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(20.0));
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: Vector3.UnitZ, adapter: adapter, // ZUp 下 +Z = hostUp
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
Assert.AreEqual(faceDown.X, q.X, 1e-6);
|
||||||
|
Assert.AreEqual(faceDown.Y, q.Y, 1e-6);
|
||||||
|
Assert.AreEqual(faceDown.Z, q.Z, 1e-6);
|
||||||
|
Assert.AreEqual(faceDown.W, q.W, 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- 立方体(三轴等长):yaw 不影响面积,应返回有效结果不崩溃 -----
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void Cube_AllAxesEqual_ReturnsValidResult_NoCrash()
|
||||||
|
{
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion faceDown = Quaternion.Identity;
|
||||||
|
Quaternion cadRot = Quaternion.Identity;
|
||||||
|
|
||||||
|
Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadRot, sizeX: 4.0, sizeY: 4.0, sizeZ: 4.0,
|
||||||
|
hostPathForward: Vector3.UnitX, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
// 立方体任何 yaw 面积相同,只要返回有效四元数即可
|
||||||
|
Assert.AreEqual(1.0, Quaternion.Normalize(q).Length(), 1e-5);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ----- ComposeHostCorrection:扣除 CAD yaw + pathYaw,避免重复旋转 -----
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// PathAnimationManager 增量链语义闭环验证:
|
||||||
|
/// finalQ = qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) * cadQ
|
||||||
|
/// 应等于 overrideQ * cadQ(贴地时实际施加的最终姿态)
|
||||||
|
/// cadQ=Identity 时 currentYaw=0,简化为:
|
||||||
|
/// qup(pathYaw) * correctionHostQ == overrideQ
|
||||||
|
/// </summary>
|
||||||
|
private static void AssertIncrementalChainClosure(
|
||||||
|
Quaternion overrideQ,
|
||||||
|
Quaternion cadQ,
|
||||||
|
double pathYawRadians,
|
||||||
|
HostCoordinateAdapter adapter,
|
||||||
|
double tolerance = 1e-3)
|
||||||
|
{
|
||||||
|
LocalEulerRotationCorrection correction =
|
||||||
|
AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
|
||||||
|
overrideQ, cadQ, pathYawRadians, adapter);
|
||||||
|
|
||||||
|
Quaternion correctionHostQ = adapter.CreateHostRotationCorrection(correction);
|
||||||
|
|
||||||
|
// currentYaw 从 cadQ 提取(与 ComposeHostCorrection 内部一致)
|
||||||
|
Matrix4x4 cadLinear = Matrix4x4.CreateFromQuaternion(cadQ);
|
||||||
|
Vector3 cadFwdHost = new Vector3(cadLinear.M11, cadLinear.M21, cadLinear.M31);
|
||||||
|
Vector3 cadFwdCanon = adapter.ToCanonicalVector3(cadFwdHost);
|
||||||
|
cadFwdCanon.Z = 0f;
|
||||||
|
double currentYaw = cadFwdCanon.LengthSquared() > 1e-9f
|
||||||
|
? Math.Atan2(Vector3.Normalize(cadFwdCanon).Y, Vector3.Normalize(cadFwdCanon).X)
|
||||||
|
: 0.0;
|
||||||
|
|
||||||
|
// 增量链:finalQ = qup(pathYaw - currentYaw) * correctionHostQ * cadQ
|
||||||
|
Quaternion deltaQ = Quaternion.CreateFromAxisAngle(
|
||||||
|
adapter.HostUpVector3, (float)(pathYawRadians - currentYaw));
|
||||||
|
Quaternion reconstructedFinal = Quaternion.Normalize(deltaQ * correctionHostQ * cadQ);
|
||||||
|
Quaternion expectedFinal = Quaternion.Normalize(overrideQ * cadQ);
|
||||||
|
|
||||||
|
for (int axis = 0; axis < 3; axis++)
|
||||||
|
{
|
||||||
|
Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
|
||||||
|
Vector3 expected = Vector3.Normalize(Vector3.Transform(v, expectedFinal));
|
||||||
|
Vector3 actual = Vector3.Normalize(Vector3.Transform(v, reconstructedFinal));
|
||||||
|
double dot = Vector3.Dot(expected, actual);
|
||||||
|
Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
|
||||||
|
$"axis {axis}: 增量链闭环偏差,dot={dot:F6}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_ZUp()
|
||||||
|
{
|
||||||
|
// CAD=Identity(currentYaw=0), overrideQ=纯 pathYaw, correction 应为 0
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
double pathYaw = Deg(30.0);
|
||||||
|
Quaternion cadQ = Quaternion.Identity;
|
||||||
|
Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
|
||||||
|
|
||||||
|
LocalEulerRotationCorrection correction =
|
||||||
|
AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
|
||||||
|
overrideQ, cadQ, pathYaw, adapter);
|
||||||
|
|
||||||
|
Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
|
||||||
|
Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
|
||||||
|
Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_YUp()
|
||||||
|
{
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
double pathYaw = Deg(-15.62);
|
||||||
|
Quaternion cadQ = Quaternion.Identity;
|
||||||
|
Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
|
||||||
|
|
||||||
|
LocalEulerRotationCorrection correction =
|
||||||
|
AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
|
||||||
|
overrideQ, cadQ, pathYaw, adapter);
|
||||||
|
|
||||||
|
Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
|
||||||
|
Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
|
||||||
|
Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void ComposeCorrection_CadIdentity_FaceDownPlusPathYaw_Closure_YUp()
|
||||||
|
{
|
||||||
|
// CAD=Identity, overrideQ = pathYawQ * faceDown
|
||||||
|
// 增量链闭环验证
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
double pathYaw = Deg(-15.62);
|
||||||
|
Quaternion cadQ = Quaternion.Identity;
|
||||||
|
Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(15.0));
|
||||||
|
Quaternion pathYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
|
||||||
|
Quaternion overrideQ = Quaternion.Normalize(pathYawQ * faceDown);
|
||||||
|
|
||||||
|
AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void ComposeCorrection_CadIdentity_SearchedYawPlusFaceDown_Closure_YUp()
|
||||||
|
{
|
||||||
|
// 模拟真实贴地:CAD=Identity, Search 出 overrideQ, 验证增量链闭环
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
Quaternion cadQ = Quaternion.Identity;
|
||||||
|
Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(25.0));
|
||||||
|
Vector3 pathFwd = new Vector3(0.96f, 0.0f, -0.28f);
|
||||||
|
|
||||||
|
Quaternion overrideQ = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDown, cadQ, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
|
||||||
|
hostPathForward: pathFwd, adapter: adapter,
|
||||||
|
out _, out _, out _);
|
||||||
|
|
||||||
|
PathTargetFrameResolver.TryResolvePlanarHostYaw(pathFwd, CoordinateSystemType.YUp, out double pathYaw);
|
||||||
|
|
||||||
|
AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void ComposeCorrection_SkewedCad_IncrementalChainClosure_YUp()
|
||||||
|
{
|
||||||
|
// CAD 姿态天然歪斜(带 yaw),验证增量链仍闭环
|
||||||
|
// 这是日志中 25"x25" 的场景:cadQ 自带 -124° yaw
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
// 构造 CAD 姿态:绕 host Y(up) 转 -124° + 一些倾斜
|
||||||
|
Quaternion cadQ = Quaternion.Normalize(
|
||||||
|
Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Deg(-124.0)) *
|
||||||
|
Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(10.0)));
|
||||||
|
double pathYaw = Deg(-15.62);
|
||||||
|
// overrideQ = searchedYawQ * faceDown(贴地搜索结果)
|
||||||
|
Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(20.0));
|
||||||
|
Quaternion searchedYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)Deg(-57.0));
|
||||||
|
Quaternion overrideQ = Quaternion.Normalize(searchedYawQ * faceDown);
|
||||||
|
|
||||||
|
AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void HostQuaternionToCanonical_HostUpRotation_MapsToCanonicalZRotation_YUp()
|
||||||
|
{
|
||||||
|
// YUp: host 绕 Y(up) 转 θ → canonical 绕 Z(up) 转 θ
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
double theta = Deg(37.0);
|
||||||
|
Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)theta);
|
||||||
|
|
||||||
|
Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);
|
||||||
|
|
||||||
|
// canonical 下应是绕 Z 转 θ
|
||||||
|
Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)theta);
|
||||||
|
AssertQuaternionsEqual(expected, canonQ);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void HostQuaternionToCanonical_HostXRotation_MapsToCanonicalXRotation_YUp()
|
||||||
|
{
|
||||||
|
// YUp: host 绕 X(forward) 转 θ → canonical 绕 X(forward) 转 θ(X 轴两坐标系相同)
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
double theta = Deg(42.0);
|
||||||
|
Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);
|
||||||
|
|
||||||
|
Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);
|
||||||
|
|
||||||
|
Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);
|
||||||
|
AssertQuaternionsEqual(expected, canonQ);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void HostQuaternionToCanonical_Identity_StaysIdentity_YUp()
|
||||||
|
{
|
||||||
|
// Identity 在两坐标系都应是 Identity(相似变换的不动点)
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||||||
|
Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(
|
||||||
|
Quaternion.Identity, adapter);
|
||||||
|
AssertQuaternionsEqual(Quaternion.Identity, canonQ);
|
||||||
|
}
|
||||||
|
|
||||||
|
[TestMethod]
|
||||||
|
public void HostQuaternionToCanonical_ZUp_IsIdentity()
|
||||||
|
{
|
||||||
|
// ZUp: canonical=host,转换应返回原四元数
|
||||||
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
|
Quaternion original = Quaternion.CreateFromAxisAngle(
|
||||||
|
new Vector3(0.3f, 0.5f, 0.8f), (float)Deg(55.0f));
|
||||||
|
original = Quaternion.Normalize(original);
|
||||||
|
|
||||||
|
Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(original, adapter);
|
||||||
|
AssertQuaternionsEqual(original, canonQ);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AssertQuaternionsEqual(Quaternion expected, Quaternion actual, double tolerance = 1e-4)
|
||||||
|
{
|
||||||
|
// 四元数有双覆盖性(q 和 -q 表示同一旋转),比较作用后的轴
|
||||||
|
for (int axis = 0; axis < 3; axis++)
|
||||||
|
{
|
||||||
|
Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
|
||||||
|
Vector3 e = Vector3.Normalize(Vector3.Transform(v, expected));
|
||||||
|
Vector3 a = Vector3.Normalize(Vector3.Transform(v, actual));
|
||||||
|
double dot = Vector3.Dot(e, a);
|
||||||
|
Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
|
||||||
|
$"axis {axis}: expected≈actual 失败,dot={dot:F6}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -213,25 +213,27 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void ZUp_HostRotationCorrection_ShouldKeepHostZAxisInvariantInHostSpace()
|
public void ZUp_HostRotationCorrection_YDegrees_ShouldKeepHostZAxisInvariant()
|
||||||
{
|
{
|
||||||
|
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转,Z 轴自身不变。
|
||||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||||
|
|
||||||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
||||||
AssertVector(rotatedUp, 0.0, 0.0, 1.0);
|
AssertVector(rotatedUp, 0.0, 0.0, 1.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void ZUp_ComposeHostQuaternion_ShouldApplyHostZCorrectionAfterBaseline()
|
public void ZUp_ComposeHostQuaternion_YDegrees_ShouldApplyHostZAxisRotation()
|
||||||
{
|
{
|
||||||
|
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转 90°:X→Y, Y→-X, Z 不变。
|
||||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
var baseline = Quaternion.Identity;
|
var baseline = Quaternion.Identity;
|
||||||
|
|
||||||
Quaternion composed = adapter.ComposeHostQuaternion(
|
Quaternion composed = adapter.ComposeHostQuaternion(
|
||||||
baseline,
|
baseline,
|
||||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||||
|
|
||||||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||||||
|
|
||||||
|
|||||||
@ -49,12 +49,14 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void ZUp_HostY90_ShouldPromoteForwardSizeToUpExtent()
|
public void ZUp_HostZDegrees_ShouldPromoteForwardSizeToUpExtent()
|
||||||
{
|
{
|
||||||
|
// ZUp 下 ZDegrees = non-up 轴(=Y)。绕 Y 转 90°:X(forward)→-Z(up), Z(up)→X。
|
||||||
|
// forward(6) 提升到 up,up(2) 降到 forward。
|
||||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||||
|
|
||||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||||
convention,
|
convention,
|
||||||
@ -69,12 +71,14 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
|||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void ZUp_HostZ90_ShouldKeepUpExtentUnchanged()
|
public void ZUp_HostYDegrees_ShouldKeepUpExtentUnchanged()
|
||||||
{
|
{
|
||||||
|
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转 90°:X→Y, Y→-X, Z(up) 不变。
|
||||||
|
// forward 和 side 互换,up 不变。
|
||||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||||
|
|
||||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||||
convention,
|
convention,
|
||||||
|
|||||||
@ -59,19 +59,18 @@ namespace NavisworksTransport.UnitTests.Core
|
|||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void FromKey_OldFormat_ShouldRetroCompatible()
|
[ExpectedException(typeof(FormatException))]
|
||||||
|
public void FromKey_OldFormat_ShouldThrow()
|
||||||
{
|
{
|
||||||
var id = AliasNodeIdentity.FromKey("building/floor/wall#2");
|
// 旧格式(无 || 分隔符)已不再支持,按 AGENTS.md "不向后兼容" 原则抛 FormatException
|
||||||
Assert.AreEqual("", id.IndexPath);
|
AliasNodeIdentity.FromKey("building/floor/wall#2");
|
||||||
Assert.AreEqual("building/floor/wall#2", id.DisplayPath);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void FromKey_OldFormatNoIndex_ShouldParse()
|
[ExpectedException(typeof(FormatException))]
|
||||||
|
public void FromKey_OldFormatNoIndex_ShouldThrow()
|
||||||
{
|
{
|
||||||
var id = AliasNodeIdentity.FromKey("building/floor");
|
AliasNodeIdentity.FromKey("building/floor");
|
||||||
Assert.AreEqual("", id.IndexPath);
|
|
||||||
Assert.AreEqual("building/floor", id.DisplayPath);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
@ -99,10 +98,10 @@ namespace NavisworksTransport.UnitTests.Core
|
|||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void TryParseKey_OldFormat_ShouldSucceed()
|
public void TryParseKey_OldFormat_ShouldReturnFalse()
|
||||||
{
|
{
|
||||||
Assert.IsTrue(AliasNodeIdentity.TryParseKey("path#0", out var id));
|
// 旧格式(无 || 分隔符)已不再支持
|
||||||
Assert.AreEqual("", id.IndexPath);
|
Assert.IsFalse(AliasNodeIdentity.TryParseKey("path#0", out _));
|
||||||
}
|
}
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
@ -279,9 +278,9 @@ namespace NavisworksTransport.UnitTests.Core
|
|||||||
public void DeleteByPathPrefix_ShouldRemoveAllUnderPath()
|
public void DeleteByPathPrefix_ShouldRemoveAllUnderPath()
|
||||||
{
|
{
|
||||||
var store = CreateStore();
|
var store = CreateStore();
|
||||||
store.SetAlias(Id("building/floor/1"));
|
store.SetAlias(Id("building/floor/1"), "别名1");
|
||||||
store.SetAlias(Id("building/floor/2"));
|
store.SetAlias(Id("building/floor/2"), "别名2");
|
||||||
store.SetAlias(Id("building/roof/3"));
|
store.SetAlias(Id("building/roof/3"), "别名3");
|
||||||
|
|
||||||
store.DeleteAliasesByPathPrefix("building/floor");
|
store.DeleteAliasesByPathPrefix("building/floor");
|
||||||
|
|
||||||
@ -343,19 +342,16 @@ namespace NavisworksTransport.UnitTests.Core
|
|||||||
Assert.AreEqual(0, store.Count);
|
Assert.AreEqual(0, store.Count);
|
||||||
}
|
}
|
||||||
|
|
||||||
// ── 新格式 → 旧格式 兼容 ──
|
// ── 旧格式已不支持(AGENTS.md "不向后兼容" 原则)──
|
||||||
|
|
||||||
[TestMethod]
|
[TestMethod]
|
||||||
public void OldFormatKey_StillWorks()
|
[ExpectedException(typeof(FormatException))]
|
||||||
|
public void OldFormatKey_ShouldThrow()
|
||||||
{
|
{
|
||||||
var store = CreateStore();
|
var store = CreateStore();
|
||||||
string oldKey = "building/floor/wall#0";
|
string oldKey = "building/floor/wall#0";
|
||||||
|
// 旧格式 key 无 || 分隔符,FromKey 应抛 FormatException
|
||||||
store.SetAlias(AliasNodeIdentity.FromKey(oldKey), "别名");
|
store.SetAlias(AliasNodeIdentity.FromKey(oldKey), "别名");
|
||||||
Assert.AreEqual("别名", store.GetAlias(AliasNodeIdentity.FromKey(oldKey)));
|
|
||||||
|
|
||||||
// 同一个节点用新格式读取
|
|
||||||
var newId = new AliasNodeIdentity("", "building/floor/wall#0");
|
|
||||||
Assert.AreEqual("别名", store.GetAlias(newId));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,3 +1,3 @@
|
|||||||
# 版本号
|
# 版本号
|
||||||
|
|
||||||
0.15.8
|
0.16.0
|
||||||
|
|||||||
4
build-and-deploy.bat
Normal file
4
build-and-deploy.bat
Normal file
@ -0,0 +1,4 @@
|
|||||||
|
@echo off
|
||||||
|
call "%~dp0compile.bat"
|
||||||
|
if %ERRORLEVEL% NEQ 0 exit /b 1
|
||||||
|
call "%~dp0deploy-plugin.bat"
|
||||||
@ -2,6 +2,12 @@
|
|||||||
|
|
||||||
## 功能点
|
## 功能点
|
||||||
|
|
||||||
|
### [2026/6/10]
|
||||||
|
|
||||||
|
1. [ ] (功能)对任意三维姿态的模型,点选面自动贴合地面
|
||||||
|
2. [ ] (功能)自定义分层属性,使用配置文件
|
||||||
|
3. [x] (优化)用单点取面,取代Rail路径中的三点取端面和2点取安装面
|
||||||
|
|
||||||
### [2026/5/27]
|
### [2026/5/27]
|
||||||
|
|
||||||
1. [x] (功能)对任意三维姿态的模型,自动调整最小投影到路径方向
|
1. [x] (功能)对任意三维姿态的模型,自动调整最小投影到路径方向
|
||||||
|
|||||||
@ -449,6 +449,17 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
var totalQ = adapter.CreateHostRotationCorrection(correction);
|
var totalQ = adapter.CreateHostRotationCorrection(correction);
|
||||||
|
|
||||||
|
// 消除 CAD 世界变换影响:Active = WorldOriginal × Override
|
||||||
|
// 要使 Active = totalQ,需 Override = WorldOriginal⁻¹ × totalQ
|
||||||
|
var geom = item.FindFirstGeometry() ?? item.Geometry;
|
||||||
|
var worldOriginal = geom?.ActiveTransform ?? item.Transform;
|
||||||
|
var originalRot = worldOriginal.Factor().Rotation;
|
||||||
|
var originalQ = new Quaternion((float)originalRot.A, (float)originalRot.B, (float)originalRot.C, (float)originalRot.D);
|
||||||
|
LogManager.Info(
|
||||||
|
$"[校正原位] correction={correction}, hostQ=({totalQ.X:F4},{totalQ.Y:F4},{totalQ.Z:F4},{totalQ.W:F4}), " +
|
||||||
|
$"origQ=({originalQ.X:F4},{originalQ.Y:F4},{originalQ.Z:F4},{originalQ.W:F4})");
|
||||||
|
totalQ = Quaternion.Normalize(Quaternion.Inverse(originalQ) * totalQ);
|
||||||
|
|
||||||
var rotatedTp = Vector3.Transform(tp, totalQ);
|
var rotatedTp = Vector3.Transform(tp, totalQ);
|
||||||
|
|
||||||
var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
|
var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
|
||||||
@ -476,6 +487,34 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
_hasGroundRealObjectBasePose = false;
|
_hasGroundRealObjectBasePose = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 贴合地面确认用:保持当前姿态,仅平移到路径起点。
|
||||||
|
/// </summary>
|
||||||
|
public void TranslateToPathStartInPlace(double verticalLiftModelUnits)
|
||||||
|
{
|
||||||
|
var item = CurrentControlledObject;
|
||||||
|
if (item == null || _pathPoints == null || _pathPoints.Count < 1) return;
|
||||||
|
|
||||||
|
var currentCenter = GetLiveBoundingBoxCenter(item);
|
||||||
|
Point3D pathStartPoint = _pathPoints[0];
|
||||||
|
Point3D target = ResolveGroundTrackedCenter(pathStartPoint, GetAnimatedObjectGroundContactHeight());
|
||||||
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
|
var liftVec = adapter.HostUpVector3 * (float)verticalLiftModelUnits;
|
||||||
|
target = new Point3D(target.X + liftVec.X, target.Y + liftVec.Y, target.Z + liftVec.Z);
|
||||||
|
|
||||||
|
var doc = NavisApplication.ActiveDocument;
|
||||||
|
var modelItems = new ModelItemCollection { item };
|
||||||
|
var translation = Transform3D.CreateTranslation(new Vector3D(
|
||||||
|
target.X - currentCenter.X,
|
||||||
|
target.Y - currentCenter.Y,
|
||||||
|
target.Z - currentCenter.Z));
|
||||||
|
doc.Models.OverridePermanentTransform(modelItems, translation, false);
|
||||||
|
|
||||||
|
LogManager.Debug(
|
||||||
|
$"[贴合平移] current=({currentCenter.X:F3},{currentCenter.Y:F3},{currentCenter.Z:F3}), " +
|
||||||
|
$"target=({target.X:F3},{target.Y:F3},{target.Z:F3})");
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 将动画对象彻底恢复到 CAD 原始位置(清除所有覆盖变换)
|
/// 将动画对象彻底恢复到 CAD 原始位置(清除所有覆盖变换)
|
||||||
/// 常用于模式切换或彻底清除动画干扰
|
/// 常用于模式切换或彻底清除动画干扰
|
||||||
@ -5574,6 +5613,62 @@ namespace NavisworksTransport.Core.Animation
|
|||||||
LogManager.Debug($"[起点摆放] 直接设置地面路径上下偏移: {verticalLiftInMeters:F3}m");
|
LogManager.Debug($"[起点摆放] 直接设置地面路径上下偏移: {verticalLiftInMeters:F3}m");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 给定 correction,计算重建后使物体底边贴合目标高度所需的 lift(米单位)。
|
||||||
|
/// 与自动调整的 ComputedLiftOffsetModelUnits 语义一致:
|
||||||
|
/// neededLift = targetBottom - objectBottomAfterRebuild
|
||||||
|
/// 实际应用 correction 重建后测量底边(与自动调整同方法)。
|
||||||
|
/// 测量后用新 correction + 新 lift 重建,使物体停留在正确的贴地等价姿态
|
||||||
|
/// (与确认按钮后的姿态一致),而不是恢复到贴地前的旧姿态。
|
||||||
|
/// </summary>
|
||||||
|
public double ComputeGroundLiftForCorrection(
|
||||||
|
LocalEulerRotationCorrection correction,
|
||||||
|
double targetLiftInMeters,
|
||||||
|
ModelItem item)
|
||||||
|
{
|
||||||
|
if (_route?.PathType != PathType.Ground || _pathPoints == null || _pathPoints.Count < 1 || item == null)
|
||||||
|
{
|
||||||
|
return targetLiftInMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
var hst = CoordinateSystemManager.Instance.ResolvedType;
|
||||||
|
var savedMode = _objectStartPlacementMode;
|
||||||
|
double computedLiftMeters = targetLiftInMeters;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
// 用 correction + lift=0 重建,测量底边
|
||||||
|
var measureRequest = ObjectStartPlacementRequest.CreateRotationCorrection(correction, 0);
|
||||||
|
ApplyObjectStartPlacementRequest(measureRequest);
|
||||||
|
BoundingBox3D measureBounds = item.BoundingBox();
|
||||||
|
double objectBottomWorld = hst == CoordinateSystemType.YUp
|
||||||
|
? measureBounds.Min.Y : measureBounds.Min.Z;
|
||||||
|
double pathStartHeight = hst == CoordinateSystemType.YUp
|
||||||
|
? _pathPoints[0].Y : _pathPoints[0].Z;
|
||||||
|
double targetBottom = pathStartHeight
|
||||||
|
+ UnitsConverter.ConvertFromMeters(targetLiftInMeters);
|
||||||
|
double neededLiftModelUnits = targetBottom - objectBottomWorld;
|
||||||
|
computedLiftMeters = Math.Round(UnitsConverter.ConvertToMeters(neededLiftModelUnits), 3);
|
||||||
|
|
||||||
|
LogManager.Debug(
|
||||||
|
$"[贴合地面] 计算上下偏移: 重建后底边={objectBottomWorld:F3}, 目标底边={targetBottom:F3}, " +
|
||||||
|
$"pathStart={pathStartHeight:F3}, neededLift={computedLiftMeters:F3}m");
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
// 用新 correction + 新 lift 完整重建(归位 CAD → 增量链旋转+平移),
|
||||||
|
// 使物体停留在与确认按钮一致的贴地等价姿态。
|
||||||
|
// 必须用 skipCadRestore=false:skipCadRestore=true 会清零 correction,
|
||||||
|
// 只施加 yaw 增量而不施加 X/Z 旋转,导致姿态缺失俯仰/翻滚。
|
||||||
|
var applyRequest = ObjectStartPlacementRequest.CreateRotationCorrection(
|
||||||
|
correction, computedLiftMeters);
|
||||||
|
_objectStartPlacementMode = savedMode;
|
||||||
|
ApplyObjectStartPlacementRequest(applyRequest);
|
||||||
|
}
|
||||||
|
|
||||||
|
return computedLiftMeters;
|
||||||
|
}
|
||||||
|
|
||||||
public void SetObjectStartPlacementMode(ObjectStartPlacementMode placementMode)
|
public void SetObjectStartPlacementMode(ObjectStartPlacementMode placementMode)
|
||||||
{
|
{
|
||||||
_objectStartPlacementMode = placementMode;
|
_objectStartPlacementMode = placementMode;
|
||||||
|
|||||||
@ -45,6 +45,21 @@ namespace NavisworksTransport
|
|||||||
LogManager.Debug("[FaceInferTool] FaceInferToolPlugin 实例已创建");
|
LogManager.Debug("[FaceInferTool] FaceInferToolPlugin 实例已创建");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 激活取面工具,准备接收用户点击。
|
||||||
|
/// </summary>
|
||||||
|
public static void Activate()
|
||||||
|
{
|
||||||
|
var pluginRecord = Autodesk.Navisworks.Api.Application.Plugins.FindPlugin(
|
||||||
|
"FaceInferTool.NavisworksTransport");
|
||||||
|
if (pluginRecord != null)
|
||||||
|
{
|
||||||
|
var toolPlugin = pluginRecord.LoadPlugin() as ToolPlugin;
|
||||||
|
if (toolPlugin != null)
|
||||||
|
Autodesk.Navisworks.Api.Application.MainDocument.Tool.SetCustomToolPlugin(toolPlugin);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 鼠标按下事件 — 单次点击内做三次 PickItemFromPoint 推断面法向量
|
/// 鼠标按下事件 — 单次点击内做三次 PickItemFromPoint 推断面法向量
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@ -813,6 +813,12 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
{
|
{
|
||||||
if (SetProperty(ref _objectRotationCorrection, value))
|
if (SetProperty(ref _objectRotationCorrection, value))
|
||||||
{
|
{
|
||||||
|
// 手动改角度时清空自动调整/贴合地面的缓存尺寸,
|
||||||
|
// 让 UpdatePassageSpaceVisualization 走 CalculateRotatedDimensions 重新计算。
|
||||||
|
// 贴地/自动调整直接设字段不经过 setter,不受影响。
|
||||||
|
_autoAabbSx = null;
|
||||||
|
_autoAabbSy = null;
|
||||||
|
_autoAabbSz = null;
|
||||||
// 角度修正值改变时,更新物体朝向和通行空间
|
// 角度修正值改变时,更新物体朝向和通行空间
|
||||||
UpdateObjectRotation();
|
UpdateObjectRotation();
|
||||||
}
|
}
|
||||||
@ -1702,6 +1708,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(0.0);
|
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(0.0);
|
||||||
ObjectRotationCorrection = LocalEulerRotationCorrection.Zero;
|
ObjectRotationCorrection = LocalEulerRotationCorrection.Zero;
|
||||||
_objectGroundLiftHeightInMeters = 0.0;
|
_objectGroundLiftHeightInMeters = 0.0;
|
||||||
|
_autoAabbSx = null; _autoAabbSy = null; _autoAabbSz = null;
|
||||||
LogManager.Debug("[选择物体] 已重置角度修正值为0(新物体)");
|
LogManager.Debug("[选择物体] 已重置角度修正值为0(新物体)");
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@ -2176,14 +2183,25 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
{
|
{
|
||||||
bool enableGroundLift = CurrentPathRoute?.PathType == PathType.Ground;
|
bool enableGroundLift = CurrentPathRoute?.PathType == PathType.Ground;
|
||||||
var autoAdjustmentRequest = CreateObjectPassageProjectionOptimizationRequest();
|
var autoAdjustmentRequest = CreateObjectPassageProjectionOptimizationRequest();
|
||||||
|
LocalEulerRotationCorrection prefillRotation = _objectRotationCorrection;
|
||||||
|
(double, double, double)? prefillAabb = null;
|
||||||
|
|
||||||
|
ShowEditRotationDialog:
|
||||||
var dialog = new Views.EditRotationWindow(
|
var dialog = new Views.EditRotationWindow(
|
||||||
_objectRotationCorrection,
|
prefillRotation,
|
||||||
_objectGroundLiftHeightInMeters,
|
_objectGroundLiftHeightInMeters,
|
||||||
enableGroundLift,
|
enableGroundLift,
|
||||||
autoAdjustmentRequest,
|
autoAdjustmentRequest,
|
||||||
autoAdjustmentRequest == null
|
autoAdjustmentRequest == null
|
||||||
? (Func<ObjectPassageProjectionOptimizationResult>)null
|
? (Func<ObjectPassageProjectionOptimizationResult>)null
|
||||||
: (() => OptimizeObjectPassageProjectionByMeasuredAabb(autoAdjustmentRequest)));
|
: (() => OptimizeObjectPassageProjectionByMeasuredAabb(autoAdjustmentRequest)),
|
||||||
|
alignToGroundProvider: HasSelectedAnimatedObject
|
||||||
|
? (Func<Vector3, ObjectPassageProjectionOptimizationResult>)
|
||||||
|
(faceNormal => OptimizeAlignToGround(faceNormal))
|
||||||
|
: null);
|
||||||
|
if (prefillAabb.HasValue)
|
||||||
|
dialog.AutoAdjustAabbSizes = prefillAabb;
|
||||||
|
|
||||||
if (dialog.ShowDialog() == true)
|
if (dialog.ShowDialog() == true)
|
||||||
{
|
{
|
||||||
var placementRequest = dialog.AdjustmentRequest;
|
var placementRequest = dialog.AdjustmentRequest;
|
||||||
@ -2191,7 +2209,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
bool isAutoAdjusted = dialog.AutoAdjustAabbSizes.HasValue;
|
bool isAutoAdjusted = dialog.AutoAdjustAabbSizes.HasValue;
|
||||||
if (isAutoAdjusted)
|
if (isAutoAdjusted)
|
||||||
{
|
{
|
||||||
// 自动调整:先原地校正(对齐 X),再通过 MoveObjectToPathStart(skipCadRestore) 偏航到路径
|
|
||||||
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(
|
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(
|
||||||
placementRequest.VerticalLiftInMeters);
|
placementRequest.VerticalLiftInMeters);
|
||||||
_pathAnimationManager?.ApplyRotationCorrectionInPlace(
|
_pathAnimationManager?.ApplyRotationCorrectionInPlace(
|
||||||
@ -2208,8 +2225,15 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
_pathAnimationManager?.ApplyObjectStartPlacementRequest(placementRequest);
|
_pathAnimationManager?.ApplyObjectStartPlacementRequest(placementRequest);
|
||||||
ObjectRotationCorrection = placementRequest.RotationCorrection;
|
ObjectRotationCorrection = placementRequest.RotationCorrection;
|
||||||
}
|
}
|
||||||
if (dialog.AutoAdjustAabbSizes.HasValue)
|
if (isAutoAdjusted)
|
||||||
(_autoAabbSx, _autoAabbSy, _autoAabbSz) = dialog.AutoAdjustAabbSizes.Value;
|
{
|
||||||
|
// 自动调整确认后用X轴对齐实测AABB设置尺寸,
|
||||||
|
// dialog.AutoAdjustAabbSizes 是世界AABB的X/Y/Z,路径斜向时不准。
|
||||||
|
var aabbAdapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
|
var aabbDoc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||||
|
var aabbItems = new ModelItemCollection { SelectedAnimatedObject };
|
||||||
|
MeasureXAlignedAabbAndSetCache(SelectedAnimatedObject, aabbItems, aabbAdapter, aabbDoc);
|
||||||
|
}
|
||||||
|
|
||||||
string liftSummary = enableGroundLift
|
string liftSummary = enableGroundLift
|
||||||
? $", 上下偏移={_objectGroundLiftHeightInMeters:F3}m"
|
? $", 上下偏移={_objectGroundLiftHeightInMeters:F3}m"
|
||||||
@ -2226,6 +2250,14 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
// 更新通行空间可视化(考虑旋转后的尺寸)
|
// 更新通行空间可视化(考虑旋转后的尺寸)
|
||||||
UpdatePassageSpaceVisualization();
|
UpdatePassageSpaceVisualization();
|
||||||
}
|
}
|
||||||
|
else if (dialog.AlignToGroundRequested)
|
||||||
|
{
|
||||||
|
// 读取对话框当前上下偏移值
|
||||||
|
_objectGroundLiftHeightInMeters = dialog.AdjustmentRequest.VerticalLiftInMeters;
|
||||||
|
FaceInferToolPlugin.FaceInferred += OnFaceInferredForAlignToGround;
|
||||||
|
FaceInferToolPlugin.Activate();
|
||||||
|
return;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
catch (Exception ex)
|
catch (Exception ex)
|
||||||
{
|
{
|
||||||
@ -2234,6 +2266,354 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private void OnFaceInferredForAlignToGround(object sender, FaceInferResult result)
|
||||||
|
{
|
||||||
|
FaceInferToolPlugin.FaceInferred -= OnFaceInferredForAlignToGround;
|
||||||
|
try { Autodesk.Navisworks.Api.Application.MainDocument.Tool.Value = Tool.Select; } catch { }
|
||||||
|
|
||||||
|
if (!result.IsValid)
|
||||||
|
{
|
||||||
|
MessageBox.Show(result.FailureReason ?? "未能识别有效的面。", "贴合地面", MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
var faceNormal = new Vector3((float)result.Normal.X, (float)result.Normal.Y, (float)result.Normal.Z);
|
||||||
|
ApplyAlignToGround(faceNormal);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 实际旋转物体到X轴方向测AABB,获取沿路径/垂直路径/高度的准确尺寸,再转回来。
|
||||||
|
/// 路径斜向时世界AABB的X/Y/Z不对应语义尺寸,必须实际旋转物体用Navisworks API测量。
|
||||||
|
/// 设置 _autoAabbSx=沿路径, _autoAabbSy=高度, _autoAabbSz=垂直路径。
|
||||||
|
/// </summary>
|
||||||
|
private void MeasureXAlignedAabbAndSetCache(ModelItem item, ModelItemCollection modelItems,
|
||||||
|
HostCoordinateAdapter adapter, Autodesk.Navisworks.Api.Document doc)
|
||||||
|
{
|
||||||
|
if (CurrentPathRoute?.Points == null || CurrentPathRoute.Points.Count < 2) return;
|
||||||
|
|
||||||
|
var pathStart = CurrentPathRoute.Points[0];
|
||||||
|
var pathStartVec = new Vector3((float)pathStart.X, (float)pathStart.Y, (float)pathStart.Z);
|
||||||
|
Vector3 pathFwd = Vector3.Zero;
|
||||||
|
for (int i = 1; i < CurrentPathRoute.Points.Count; i++)
|
||||||
|
{
|
||||||
|
var p = CurrentPathRoute.Points[i];
|
||||||
|
var c = new Vector3((float)p.X, (float)p.Y, (float)p.Z) - pathStartVec;
|
||||||
|
if (c.LengthSquared() > 1e-8f) { pathFwd = c; break; }
|
||||||
|
}
|
||||||
|
if (pathFwd.LengthSquared() <= 1e-8f) return;
|
||||||
|
|
||||||
|
Vector3 hostUp = adapter.HostUpVector3;
|
||||||
|
Vector3 pathFwdH = pathFwd - hostUp * Vector3.Dot(pathFwd, hostUp);
|
||||||
|
if (pathFwdH.LengthSquared() <= 1e-8f) return;
|
||||||
|
pathFwdH = Vector3.Normalize(pathFwdH);
|
||||||
|
|
||||||
|
Vector3 horizontalPerpX = Vector3.Normalize(Vector3.Cross(hostUp, Vector3.UnitX));
|
||||||
|
float angleToX = (float)Math.Atan2(
|
||||||
|
Vector3.Dot(pathFwdH, horizontalPerpX),
|
||||||
|
Vector3.Dot(pathFwdH, Vector3.UnitX));
|
||||||
|
|
||||||
|
// 实际旋转物体到X轴方向,测AABB,再转回来
|
||||||
|
Quaternion yawToX = Quaternion.CreateFromAxisAngle(hostUp, -angleToX);
|
||||||
|
var rotateToX = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)).Factor();
|
||||||
|
rotateToX.Rotation = new Rotation3D(yawToX.X, yawToX.Y, yawToX.Z, yawToX.W);
|
||||||
|
doc.Models.OverridePermanentTransform(modelItems, rotateToX.Combine(), false);
|
||||||
|
|
||||||
|
BoundingBox3D xAlignedBounds = item.BoundingBox();
|
||||||
|
int upIndex = adapter.HostUpAxisIndex;
|
||||||
|
_autoAabbSx = xAlignedBounds.Max.X - xAlignedBounds.Min.X;
|
||||||
|
_autoAabbSy = upIndex == 1 ? xAlignedBounds.Max.Y - xAlignedBounds.Min.Y : xAlignedBounds.Max.Z - xAlignedBounds.Min.Z;
|
||||||
|
_autoAabbSz = upIndex == 1 ? xAlignedBounds.Max.Z - xAlignedBounds.Min.Z : xAlignedBounds.Max.Y - xAlignedBounds.Min.Y;
|
||||||
|
|
||||||
|
// 转回来
|
||||||
|
Quaternion yawBack = Quaternion.CreateFromAxisAngle(hostUp, angleToX);
|
||||||
|
var rotateBack = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)).Factor();
|
||||||
|
rotateBack.Rotation = new Rotation3D(yawBack.X, yawBack.Y, yawBack.Z, yawBack.W);
|
||||||
|
doc.Models.OverridePermanentTransform(modelItems, rotateBack.Combine(), false);
|
||||||
|
|
||||||
|
double unitsToMeters = UnitsConverter.GetUnitsToMetersConversionFactor();
|
||||||
|
LogManager.Debug(
|
||||||
|
$"[X轴对齐AABB] 沿路径={_autoAabbSx * unitsToMeters:F2}m, " +
|
||||||
|
$"垂直路径={_autoAabbSz * unitsToMeters:F2}m, " +
|
||||||
|
$"高度={_autoAabbSy * unitsToMeters:F2}m, angleToX={angleToX * 180.0 / Math.PI:F1}°");
|
||||||
|
}
|
||||||
|
|
||||||
|
private void ApplyAlignToGround(Vector3 faceNormal)
|
||||||
|
{
|
||||||
|
var item = UseVirtualObject
|
||||||
|
? VirtualObjectManager.Instance.CurrentVirtualObject
|
||||||
|
: SelectedAnimatedObject;
|
||||||
|
if (item == null) return;
|
||||||
|
|
||||||
|
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||||
|
var modelItems = new ModelItemCollection { item };
|
||||||
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
|
var geom = item.FindFirstGeometry() ?? item.Geometry;
|
||||||
|
|
||||||
|
// 读路径起点世界变换(复位前)
|
||||||
|
var psActive = geom?.ActiveTransform ?? item.Transform;
|
||||||
|
var psRot = psActive.Factor().Rotation;
|
||||||
|
var psQ = new Quaternion((float)psRot.A, (float)psRot.B, (float)psRot.C, (float)psRot.D);
|
||||||
|
|
||||||
|
// 复位 CAD
|
||||||
|
doc.Models.ResetPermanentTransform(modelItems);
|
||||||
|
var cadActive = geom?.ActiveTransform ?? item.Transform;
|
||||||
|
var cadRot = cadActive.Factor().Rotation;
|
||||||
|
var cadQ = new Quaternion((float)cadRot.A, (float)cadRot.B, (float)cadRot.C, (float)cadRot.D);
|
||||||
|
var localN = Vector3.Normalize(Vector3.Transform(Vector3.Normalize(faceNormal), Quaternion.Inverse(psQ)));
|
||||||
|
var cadWorldN = Vector3.Normalize(Vector3.Transform(localN, cadQ));
|
||||||
|
|
||||||
|
var t = -adapter.HostUpVector3;
|
||||||
|
float d = Vector3.Dot(cadWorldN, t);
|
||||||
|
Quaternion overrideQ;
|
||||||
|
if (d > 0.9999f) overrideQ = Quaternion.Identity;
|
||||||
|
else if (d < -0.9999f) overrideQ = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Math.PI);
|
||||||
|
else overrideQ = Quaternion.CreateFromAxisAngle(Vector3.Normalize(Vector3.Cross(cadWorldN, t)), (float)Math.Acos(d));
|
||||||
|
|
||||||
|
// 贴地后绕宿主 Up 搜索 yaw,使物体在路径方向的截面积最小
|
||||||
|
// (与"自动调整"同目标,但贴地已固定俯仰/翻滚,只剩 yaw 单自由度)
|
||||||
|
var routePoints = CurrentPathRoute?.Points;
|
||||||
|
double? searchedYawDegreesForLog = null;
|
||||||
|
double pathYawRadians = 0.0;
|
||||||
|
bool hasPathYaw = false;
|
||||||
|
// 搜索返回的语义尺寸(模型单位):沿路径 / 垂直路径 / 沿 hostUp
|
||||||
|
double searchedForwardExtent = 0.0;
|
||||||
|
double searchedWidthAcrossPath = 0.0;
|
||||||
|
double searchedHeightAlongUp = 0.0;
|
||||||
|
bool hasSearchedExtents = false;
|
||||||
|
if ((CurrentPathRoute?.PathType == PathType.Ground || CurrentPathRoute?.PathType == PathType.Hoisting)
|
||||||
|
&& routePoints != null && routePoints.Count >= 2)
|
||||||
|
{
|
||||||
|
var start = routePoints[0];
|
||||||
|
var startVector = new Vector3((float)start.X, (float)start.Y, (float)start.Z);
|
||||||
|
Vector3 hostForward = Vector3.Zero;
|
||||||
|
for (int i = 1; i < routePoints.Count; i++)
|
||||||
|
{
|
||||||
|
var point = routePoints[i];
|
||||||
|
var candidate = new Vector3((float)point.X, (float)point.Y, (float)point.Z) - startVector;
|
||||||
|
if (candidate.LengthSquared() > 1e-8f)
|
||||||
|
{
|
||||||
|
hostForward = candidate;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (hostForward.LengthSquared() > 1e-8f
|
||||||
|
&& PathTargetFrameResolver.TryResolvePlanarHostYaw(
|
||||||
|
hostForward, CoordinateSystemManager.Instance.ResolvedType, out pathYawRadians))
|
||||||
|
{
|
||||||
|
hasPathYaw = true;
|
||||||
|
double metersToUnits = UnitsConverter.GetMetersToUnitsConversionFactor();
|
||||||
|
double sizeX, sizeY, sizeZ;
|
||||||
|
if (UseVirtualObject)
|
||||||
|
{
|
||||||
|
sizeX = VirtualObjectLengthInMeters * metersToUnits;
|
||||||
|
sizeY = VirtualObjectWidthInMeters * metersToUnits;
|
||||||
|
sizeZ = VirtualObjectHeightInMeters * metersToUnits;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
sizeX = _objectOriginalLength * metersToUnits;
|
||||||
|
sizeY = _objectOriginalWidth * metersToUnits;
|
||||||
|
sizeZ = _objectOriginalHeight * metersToUnits;
|
||||||
|
}
|
||||||
|
|
||||||
|
Quaternion faceDownOnly = overrideQ;
|
||||||
|
overrideQ = AlignToGroundMinCrossSectionYawSearcher.Search(
|
||||||
|
faceDownOnly, cadQ, sizeX, sizeY, sizeZ, hostForward, adapter,
|
||||||
|
out searchedWidthAcrossPath, out searchedHeightAlongUp, out searchedForwardExtent);
|
||||||
|
hasSearchedExtents = true;
|
||||||
|
|
||||||
|
// 从结果反算实际 yaw 角度用于日志
|
||||||
|
Quaternion deltaQ = Quaternion.Normalize(
|
||||||
|
Quaternion.Inverse(faceDownOnly) * overrideQ);
|
||||||
|
float dot = Vector3.Dot(adapter.HostUpVector3,
|
||||||
|
new Vector3(deltaQ.X, deltaQ.Y, deltaQ.Z));
|
||||||
|
float angle = 2.0f * (float)Math.Acos(Math.Min(1.0, Math.Abs(deltaQ.W)));
|
||||||
|
searchedYawDegreesForLog = (dot >= 0 ? angle : -angle) * 180.0 / Math.PI;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
var center = item.BoundingBox().Center;
|
||||||
|
var tp = new Vector3((float)center.X, (float)center.Y, (float)center.Z);
|
||||||
|
var rtp = Vector3.Transform(tp, overrideQ);
|
||||||
|
var id = Transform3D.CreateTranslation(new Vector3D(0, 0, 0)).Factor();
|
||||||
|
id.Rotation = new Rotation3D(overrideQ.X, overrideQ.Y, overrideQ.Z, overrideQ.W);
|
||||||
|
id.Translation = new Vector3D(center.X - rtp.X, center.Y - rtp.Y, center.Z - rtp.Z);
|
||||||
|
doc.Models.OverridePermanentTransform(modelItems, id.Combine(), true);
|
||||||
|
|
||||||
|
// 旋转后平移:底边贴地 + 用户上下偏移
|
||||||
|
var hst = CoordinateSystemManager.Instance.ResolvedType;
|
||||||
|
var newBounds = item.BoundingBox();
|
||||||
|
var newCenter = newBounds.Center;
|
||||||
|
var ps = CurrentPathRoute?.Points?[0];
|
||||||
|
double objectBottom = hst == CoordinateSystemType.YUp ? newBounds.Min.Y : newBounds.Min.Z;
|
||||||
|
double groundLevel = (hst == CoordinateSystemType.YUp ? ps.Y : ps.Z)
|
||||||
|
+ UnitsConverter.ConvertFromMeters(_objectGroundLiftHeightInMeters);
|
||||||
|
var delta = new Vector3D(
|
||||||
|
ps.X - newCenter.X,
|
||||||
|
hst == CoordinateSystemType.YUp ? groundLevel - objectBottom : ps.Y - newCenter.Y,
|
||||||
|
hst == CoordinateSystemType.YUp ? ps.Z - newCenter.Z : groundLevel - objectBottom);
|
||||||
|
doc.Models.OverridePermanentTransform(modelItems, Transform3D.CreateTranslation(delta), false);
|
||||||
|
|
||||||
|
// 把绝对贴地姿态转成"相对于 baseline 的 host 欧拉修正"。
|
||||||
|
// PathAnimationManager 增量链从 CAD 姿态出发,需要扣除 CAD 自带 yaw + pathYaw。
|
||||||
|
LocalEulerRotationCorrection resultCorrection;
|
||||||
|
if (hasPathYaw)
|
||||||
|
{
|
||||||
|
resultCorrection = AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
|
||||||
|
overrideQ, cadQ, pathYawRadians, adapter);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// 无路径 yaw(纯垂直路径或无路径):直接转 overrideQ,
|
||||||
|
// 无 baseline 可扣除,correction 等价于绝对姿态。
|
||||||
|
Quaternion correctionCanonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(
|
||||||
|
overrideQ, adapter);
|
||||||
|
resultCorrection = ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
|
||||||
|
correctionCanonQ, adapter.HostType);
|
||||||
|
}
|
||||||
|
_pathAnimationManager?.SetObjectRotationCorrectionDirect(resultCorrection);
|
||||||
|
_objectRotationCorrection = resultCorrection;
|
||||||
|
OnPropertyChanged(nameof(ObjectRotationCorrection));
|
||||||
|
|
||||||
|
// 回写上下偏移:贴地时用实际包围盒贴地,但确认/动画后用 correction 重建姿态,
|
||||||
|
// 旋转后物体底边位置会变。与自动调整同方法:应用 correction 重建→测量底边→算 lift,
|
||||||
|
// 测量后用新 correction+lift 完整重建,使物体停留在贴地等价姿态。
|
||||||
|
if (CurrentPathRoute?.PathType == PathType.Ground && CurrentPathRoute.Points?.Count > 0
|
||||||
|
&& _pathAnimationManager != null)
|
||||||
|
{
|
||||||
|
double computedLift = _pathAnimationManager.ComputeGroundLiftForCorrection(
|
||||||
|
resultCorrection, _objectGroundLiftHeightInMeters, item);
|
||||||
|
_objectGroundLiftHeightInMeters = computedLift;
|
||||||
|
_pathAnimationManager.SetObjectStartVerticalLiftDirect(_objectGroundLiftHeightInMeters);
|
||||||
|
|
||||||
|
// 物体已在最终贴地姿态(ComputeGroundLiftForCorrection 重建完成)。
|
||||||
|
// 路径可能斜向,实际旋转物体到X轴方向测AABB获取准确语义尺寸。
|
||||||
|
if (hasSearchedExtents)
|
||||||
|
{
|
||||||
|
MeasureXAlignedAabbAndSetCache(item, modelItems, adapter, doc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
LogManager.Info(
|
||||||
|
$"[贴合地面] faceNormal=({faceNormal.X:F4},{faceNormal.Y:F4},{faceNormal.Z:F4}) " +
|
||||||
|
$"cadWorldN=({cadWorldN.X:F4},{cadWorldN.Y:F4},{cadWorldN.Z:F4}) t=({t.X:F4},{t.Y:F4},{t.Z:F4}) " +
|
||||||
|
$"overrideQ=({overrideQ.X:F4},{overrideQ.Y:F4},{overrideQ.Z:F4},{overrideQ.W:F4}) " +
|
||||||
|
$"searchedYaw={(searchedYawDegreesForLog.HasValue ? searchedYawDegreesForLog.Value.ToString("F2") : "N/A")}° " +
|
||||||
|
$"correction={_objectRotationCorrection}");
|
||||||
|
|
||||||
|
var liftSummary = CurrentPathRoute?.PathType == PathType.Ground
|
||||||
|
? $", 上下偏移={_objectGroundLiftHeightInMeters:F3}m"
|
||||||
|
: string.Empty;
|
||||||
|
UpdateMainStatus($"物体已贴合地面: {_objectRotationCorrection}{liftSummary}");
|
||||||
|
UpdatePassageSpaceVisualization();
|
||||||
|
}
|
||||||
|
|
||||||
|
private ObjectPassageProjectionOptimizationResult OptimizeAlignToGround(Vector3 faceNormal)
|
||||||
|
{
|
||||||
|
if (_pathAnimationManager == null || !HasSelectedAnimatedObject)
|
||||||
|
return ObjectPassageProjectionOptimizationResult.CreateFailure("未选择动画物体。");
|
||||||
|
|
||||||
|
var request = CreateObjectPassageProjectionOptimizationRequest();
|
||||||
|
if (request == null)
|
||||||
|
return ObjectPassageProjectionOptimizationResult.CreateFailure("无法创建优化请求。");
|
||||||
|
|
||||||
|
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||||
|
var hostUp = adapter.HostUpVector3;
|
||||||
|
|
||||||
|
var hst = CoordinateSystemManager.Instance.ResolvedType;
|
||||||
|
var hostSide = Vector3.Cross(Vector3.UnitX, hostUp);
|
||||||
|
|
||||||
|
var originalCorrection = _objectRotationCorrection;
|
||||||
|
double originalLiftInMeters = _objectGroundLiftHeightInMeters;
|
||||||
|
ObjectPassageProjectionOptimizationResult result;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
// 取面在路径起点,校正从 CAD 出发。
|
||||||
|
// 路径起点法向量 → 物体局部法向量 → CAD 世界法向量
|
||||||
|
var item = UseVirtualObject
|
||||||
|
? VirtualObjectManager.Instance.CurrentVirtualObject
|
||||||
|
: SelectedAnimatedObject;
|
||||||
|
|
||||||
|
// 先记录路径起点的 ActiveTransform
|
||||||
|
var psActiveTransform = item?.Transform;
|
||||||
|
var psRot = psActiveTransform?.Factor().Rotation ?? Rotation3D.Identity;
|
||||||
|
var psQ = new Quaternion((float)psRot.A, (float)psRot.B, (float)psRot.C, (float)psRot.D);
|
||||||
|
var invPsQ = Quaternion.Inverse(psQ);
|
||||||
|
|
||||||
|
// 复位到 CAD,读 CAD 原始变换
|
||||||
|
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||||
|
var modelItems = new ModelItemCollection { item };
|
||||||
|
doc.Models.ResetPermanentTransform(modelItems);
|
||||||
|
var cadActiveTransform = item.Transform;
|
||||||
|
var cadRot = cadActiveTransform.Factor().Rotation;
|
||||||
|
var cadQ = new Quaternion((float)cadRot.A, (float)cadRot.B, (float)cadRot.C, (float)cadRot.D);
|
||||||
|
|
||||||
|
// 世界→局部→CAD世界
|
||||||
|
var localNormal = Vector3.Normalize(Vector3.Transform(Vector3.Normalize(faceNormal), invPsQ));
|
||||||
|
var cadFaceNormal = Vector3.Normalize(Vector3.Transform(localNormal, cadQ));
|
||||||
|
|
||||||
|
// CAD 空间算对齐
|
||||||
|
var canonicalFaceNormal = adapter.ToCanonicalVector3(cadFaceNormal);
|
||||||
|
var canonicalTargetDown = adapter.ToCanonicalVector3(-hostUp);
|
||||||
|
float dot = Vector3.Dot(canonicalFaceNormal, canonicalTargetDown);
|
||||||
|
Quaternion alignQuatCanonical;
|
||||||
|
if (dot > 0.999f) alignQuatCanonical = Quaternion.Identity;
|
||||||
|
else if (dot < -0.999f) alignQuatCanonical = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Math.PI);
|
||||||
|
else
|
||||||
|
{
|
||||||
|
var axis = Vector3.Normalize(Vector3.Cross(canonicalFaceNormal, canonicalTargetDown));
|
||||||
|
alignQuatCanonical = Quaternion.CreateFromAxisAngle(axis, (float)Math.Acos(dot));
|
||||||
|
}
|
||||||
|
|
||||||
|
var correction = ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(alignQuatCanonical);
|
||||||
|
LogManager.Info(
|
||||||
|
$"[贴合优化] faceNormal=({faceNormal.X:F4},{faceNormal.Y:F4},{faceNormal.Z:F4}), " +
|
||||||
|
$"canonFace=({canonicalFaceNormal.X:F4},{canonicalFaceNormal.Y:F4},{canonicalFaceNormal.Z:F4}), " +
|
||||||
|
$"canonTarget=({canonicalTargetDown.X:F4},{canonicalTargetDown.Y:F4},{canonicalTargetDown.Z:F4}), " +
|
||||||
|
$"dot={dot:F4}, alignQ=({alignQuatCanonical.X:F4},{alignQuatCanonical.Y:F4},{alignQuatCanonical.Z:F4},{alignQuatCanonical.W:F4}), " +
|
||||||
|
$"correction={correction}");
|
||||||
|
result = ObjectPassageProjectionOptimizationResult.CreateSuccess(correction, new ObjectPassageProjectionScore(0, 0));
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
var restoreRequest = ObjectStartPlacementRequest.CreateRotationCorrection(
|
||||||
|
originalCorrection, originalLiftInMeters);
|
||||||
|
_pathAnimationManager.ApplyObjectStartPlacementRequest(restoreRequest);
|
||||||
|
}
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool IsBetterForAlign(
|
||||||
|
ObjectPassageProjectionScore candidate,
|
||||||
|
ObjectPassageProjectionScore currentBest,
|
||||||
|
double areaRelativeTieTolerance,
|
||||||
|
bool? requireWidthLarger)
|
||||||
|
{
|
||||||
|
if (double.IsInfinity(currentBest.Area)) return true;
|
||||||
|
|
||||||
|
double tolerance = Math.Max(0.0, areaRelativeTieTolerance) * Math.Max(1.0, currentBest.Area);
|
||||||
|
if (candidate.Area < currentBest.Area - tolerance) return true;
|
||||||
|
|
||||||
|
if (Math.Abs(candidate.Area - currentBest.Area) <= tolerance)
|
||||||
|
{
|
||||||
|
if (requireWidthLarger.HasValue)
|
||||||
|
{
|
||||||
|
bool cOk = requireWidthLarger.Value
|
||||||
|
? candidate.WidthAcrossPath >= candidate.HeightAlongHostUp
|
||||||
|
: candidate.HeightAlongHostUp >= candidate.WidthAcrossPath;
|
||||||
|
bool bOk = requireWidthLarger.Value
|
||||||
|
? currentBest.WidthAcrossPath >= currentBest.HeightAlongHostUp
|
||||||
|
: currentBest.HeightAlongHostUp >= currentBest.WidthAcrossPath;
|
||||||
|
if (cOk && !bOk) return true;
|
||||||
|
if (!cOk && bOk) return false;
|
||||||
|
}
|
||||||
|
if (candidate.HeightAlongHostUp < currentBest.HeightAlongHostUp - 1e-9) return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
private ObjectPassageProjectionOptimizationResult OptimizeObjectPassageProjectionByMeasuredAabb(
|
private ObjectPassageProjectionOptimizationResult OptimizeObjectPassageProjectionByMeasuredAabb(
|
||||||
ObjectPassageProjectionOptimizationRequest request)
|
ObjectPassageProjectionOptimizationRequest request)
|
||||||
{
|
{
|
||||||
@ -4767,10 +5147,12 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
|||||||
double effectiveLength, effectiveWidth, effectiveHeight;
|
double effectiveLength, effectiveWidth, effectiveHeight;
|
||||||
if (_autoAabbSx.HasValue)
|
if (_autoAabbSx.HasValue)
|
||||||
{
|
{
|
||||||
|
// 自动调整/贴合地面后实测 AABB:物体已在最终姿态,直接使用世界 AABB 尺寸。
|
||||||
|
// 不清空:贴地后 correction 固定,后续刷新也应保持实测值,
|
||||||
|
// 避免 CalculateRotatedDimensions 用包含 CAD yaw 补偿的 correction 反推失真。
|
||||||
effectiveLength = _autoAabbSx.Value;
|
effectiveLength = _autoAabbSx.Value;
|
||||||
effectiveWidth = _autoAabbSz.Value;
|
effectiveWidth = _autoAabbSz.Value;
|
||||||
effectiveHeight = _autoAabbSy.Value;
|
effectiveHeight = _autoAabbSy.Value;
|
||||||
_autoAabbSx = null;
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|||||||
@ -2,7 +2,7 @@
|
|||||||
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
|
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
|
||||||
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
|
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
|
||||||
xmlns:converters="clr-namespace:NavisworksTransport.UI.WPF.Converters"
|
xmlns:converters="clr-namespace:NavisworksTransport.UI.WPF.Converters"
|
||||||
Title="调整物体" Height="430" Width="470"
|
Title="调整物体" Height="430" Width="480"
|
||||||
WindowStartupLocation="CenterScreen"
|
WindowStartupLocation="CenterScreen"
|
||||||
ResizeMode="NoResize"
|
ResizeMode="NoResize"
|
||||||
ShowInTaskbar="False"
|
ShowInTaskbar="False"
|
||||||
@ -190,38 +190,46 @@
|
|||||||
</Border>
|
</Border>
|
||||||
|
|
||||||
<!-- 按钮栏 -->
|
<!-- 按钮栏 -->
|
||||||
<Border Grid.Row="2" Background="#FFF8FBFF" BorderBrush="#FFD4E7FF" BorderThickness="0,1,0,0" Padding="20,12">
|
<Border Grid.Row="2" Background="#FFF8FBFF" BorderBrush="#FFD4E7FF" BorderThickness="0,1,0,0" Padding="10,12">
|
||||||
<StackPanel Orientation="Horizontal" HorizontalAlignment="Center">
|
<Grid HorizontalAlignment="Center">
|
||||||
<Button Content="初始值"
|
<Grid.ColumnDefinitions>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
<ColumnDefinition Width="8"/>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
<ColumnDefinition Width="8"/>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
<ColumnDefinition Width="8"/>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
<ColumnDefinition Width="8"/>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
<ColumnDefinition Width="8"/>
|
||||||
|
<ColumnDefinition Width="Auto"/>
|
||||||
|
</Grid.ColumnDefinitions>
|
||||||
|
<Button Grid.Column="0" Content="初始值"
|
||||||
Click="OnResetClick"
|
Click="OnResetClick"
|
||||||
Style="{StaticResource SecondaryButtonStyle}"
|
Style="{StaticResource SecondaryButtonStyle}"
|
||||||
Width="64"
|
Width="64" Height="32" Margin="0"/>
|
||||||
Height="32"
|
<Button Grid.Column="2" Content="自动调整"
|
||||||
Margin="0,0,8,0"/>
|
|
||||||
<Button Content="自动调整"
|
|
||||||
Click="OnAutoAdjustClick"
|
Click="OnAutoAdjustClick"
|
||||||
Style="{StaticResource SecondaryButtonStyle}"
|
Style="{StaticResource SecondaryButtonStyle}"
|
||||||
Width="64"
|
Width="64" Height="32" Margin="0"/>
|
||||||
Height="32"
|
<Button Grid.Column="4" Content="贴合地面"
|
||||||
Margin="0,0,8,0"/>
|
Click="OnAlignToGroundClick"
|
||||||
<Button Content="平移"
|
Style="{StaticResource SecondaryButtonStyle}"
|
||||||
|
Width="64" Height="32" Margin="0"/>
|
||||||
|
<Button Grid.Column="6" Content="平移"
|
||||||
Click="OnTranslateClick"
|
Click="OnTranslateClick"
|
||||||
Style="{StaticResource SecondaryButtonStyle}"
|
Style="{StaticResource SecondaryButtonStyle}"
|
||||||
Width="64"
|
Width="64" Height="32" Margin="0"/>
|
||||||
Height="32"
|
<Button Grid.Column="8" Content="确认"
|
||||||
Margin="0,0,8,0"/>
|
|
||||||
<Button Content="确认"
|
|
||||||
Click="OnConfirmClick"
|
Click="OnConfirmClick"
|
||||||
Style="{StaticResource ActionButtonStyle}"
|
Style="{StaticResource ActionButtonStyle}"
|
||||||
Width="64"
|
Width="64" Height="32" Margin="0"/>
|
||||||
Height="32"
|
<Button Grid.Column="10" Content="取消"
|
||||||
Margin="0,0,8,0"/>
|
|
||||||
<Button Content="取消"
|
|
||||||
Click="OnCancelClick"
|
Click="OnCancelClick"
|
||||||
Style="{StaticResource SecondaryButtonStyle}"
|
Style="{StaticResource SecondaryButtonStyle}"
|
||||||
Width="64"
|
Width="64" Height="32" Margin="0"/>
|
||||||
Height="32"/>
|
</Grid>
|
||||||
</StackPanel>
|
|
||||||
</Border>
|
</Border>
|
||||||
</Grid>
|
</Grid>
|
||||||
</Window>
|
</Window>
|
||||||
|
|||||||
@ -8,6 +8,8 @@ using NavisworksTransport.UI.WPF.Converters;
|
|||||||
using NavisworksTransport.Utils;
|
using NavisworksTransport.Utils;
|
||||||
using NavisworksTransport.Utils.CoordinateSystem;
|
using NavisworksTransport.Utils.CoordinateSystem;
|
||||||
|
|
||||||
|
using System.Numerics;
|
||||||
|
|
||||||
namespace NavisworksTransport.UI.WPF.Views
|
namespace NavisworksTransport.UI.WPF.Views
|
||||||
{
|
{
|
||||||
public partial class EditRotationWindow : Window, INotifyPropertyChanged
|
public partial class EditRotationWindow : Window, INotifyPropertyChanged
|
||||||
@ -27,9 +29,12 @@ namespace NavisworksTransport.UI.WPF.Views
|
|||||||
private ObjectStartPlacementRequest _adjustmentRequest;
|
private ObjectStartPlacementRequest _adjustmentRequest;
|
||||||
private readonly ObjectPassageProjectionOptimizationRequest _autoAdjustmentRequest;
|
private readonly ObjectPassageProjectionOptimizationRequest _autoAdjustmentRequest;
|
||||||
private readonly Func<ObjectPassageProjectionOptimizationResult> _autoAdjustmentProvider;
|
private readonly Func<ObjectPassageProjectionOptimizationResult> _autoAdjustmentProvider;
|
||||||
|
private readonly Func<Vector3, ObjectPassageProjectionOptimizationResult> _alignToGroundProvider;
|
||||||
private readonly bool _isGroundLiftEnabled;
|
private readonly bool _isGroundLiftEnabled;
|
||||||
private double _vehicleHeightInMeters = 0.15;
|
private double _vehicleHeightInMeters = 0.15;
|
||||||
private bool _requireWidthLarger = false;
|
private bool _requireWidthLarger = false;
|
||||||
|
private bool _isAlignToGroundResult;
|
||||||
|
private bool _alignToGroundRequested = false;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 门型偏好:true=宽>高,false=高>宽(默认)
|
/// 门型偏好:true=宽>高,false=高>宽(默认)
|
||||||
@ -40,7 +45,10 @@ namespace NavisworksTransport.UI.WPF.Views
|
|||||||
set => _requireWidthLarger = value;
|
set => _requireWidthLarger = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
public (double sx, double sy, double sz)? AutoAdjustAabbSizes { get; private set; }
|
public (double sx, double sy, double sz)? AutoAdjustAabbSizes { get; set; }
|
||||||
|
|
||||||
|
public bool AlignToGroundRequested => _alignToGroundRequested;
|
||||||
|
public bool IsAlignToGroundResult { get => _isAlignToGroundResult; set => _isAlignToGroundResult = value; }
|
||||||
|
|
||||||
public double VehicleHeightInMeters
|
public double VehicleHeightInMeters
|
||||||
{
|
{
|
||||||
@ -119,7 +127,8 @@ namespace NavisworksTransport.UI.WPF.Views
|
|||||||
double currentGroundLiftHeightInMeters,
|
double currentGroundLiftHeightInMeters,
|
||||||
bool isGroundLiftEnabled,
|
bool isGroundLiftEnabled,
|
||||||
ObjectPassageProjectionOptimizationRequest autoAdjustmentRequest = null,
|
ObjectPassageProjectionOptimizationRequest autoAdjustmentRequest = null,
|
||||||
Func<ObjectPassageProjectionOptimizationResult> autoAdjustmentProvider = null)
|
Func<ObjectPassageProjectionOptimizationResult> autoAdjustmentProvider = null,
|
||||||
|
Func<Vector3, ObjectPassageProjectionOptimizationResult> alignToGroundProvider = null)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
@ -130,7 +139,11 @@ namespace NavisworksTransport.UI.WPF.Views
|
|||||||
_isGroundLiftEnabled = isGroundLiftEnabled;
|
_isGroundLiftEnabled = isGroundLiftEnabled;
|
||||||
_autoAdjustmentRequest = autoAdjustmentRequest;
|
_autoAdjustmentRequest = autoAdjustmentRequest;
|
||||||
_autoAdjustmentProvider = autoAdjustmentProvider;
|
_autoAdjustmentProvider = autoAdjustmentProvider;
|
||||||
GroundPathLiftHeightInMeters = currentGroundLiftHeightInMeters;
|
_alignToGroundProvider = alignToGroundProvider;
|
||||||
|
// 默认上下偏移 = 物流车高度(若未设置过)
|
||||||
|
GroundPathLiftHeightInMeters = Math.Abs(currentGroundLiftHeightInMeters) < 1e-9
|
||||||
|
? VehicleHeightInMeters
|
||||||
|
: currentGroundLiftHeightInMeters;
|
||||||
_adjustmentRequest = ObjectStartPlacementRequest.CreateRotationCorrection(currentRotation, currentGroundLiftHeightInMeters);
|
_adjustmentRequest = ObjectStartPlacementRequest.CreateRotationCorrection(currentRotation, currentGroundLiftHeightInMeters);
|
||||||
DataContext = this;
|
DataContext = this;
|
||||||
Loaded += (sender, args) => ZAxisTextBox.Focus();
|
Loaded += (sender, args) => ZAxisTextBox.Focus();
|
||||||
@ -200,6 +213,22 @@ namespace NavisworksTransport.UI.WPF.Views
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private void OnAlignToGroundClick(object sender, RoutedEventArgs e)
|
||||||
|
{
|
||||||
|
LogManager.Info($"[贴合地面] 按钮被点击, provider={_alignToGroundProvider != null}");
|
||||||
|
if (_alignToGroundProvider == null)
|
||||||
|
{
|
||||||
|
MessageBox.Show("当前无法使用贴合地面功能。", "贴合地面", MessageBoxButton.OK, MessageBoxImage.Warning);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_alignToGroundRequested = true;
|
||||||
|
TryCommitInputValues();
|
||||||
|
_adjustmentRequest = ObjectStartPlacementRequest.CreateTranslationOnly(GroundPathLiftHeightInMeters);
|
||||||
|
DialogResult = false;
|
||||||
|
Close();
|
||||||
|
}
|
||||||
|
|
||||||
private void OnQuickAngleClick(object sender, RoutedEventArgs e)
|
private void OnQuickAngleClick(object sender, RoutedEventArgs e)
|
||||||
{
|
{
|
||||||
if (sender is Button button)
|
if (sender is Button button)
|
||||||
|
|||||||
@ -0,0 +1,239 @@
|
|||||||
|
using System;
|
||||||
|
using System.Numerics;
|
||||||
|
using NavisworksTransport.Utils;
|
||||||
|
|
||||||
|
namespace NavisworksTransport.Utils.CoordinateSystem
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 贴合地面后的最小截面 yaw 搜索器。
|
||||||
|
/// 在贴地旋转(把用户选定的面旋到 -hostUp)基础上,绕宿主 Up 轴搜索一个 yaw,
|
||||||
|
/// 使物体在"垂直路径方向 × 沿宿主 Up"截面上的投影面积最小。
|
||||||
|
/// 与 ObjectPassageProjectionOptimizer 同目标,但只在 yaw 单自由度上搜索——
|
||||||
|
/// 贴地旋转已固定俯仰/翻滚,只剩绕垂直轴的旋转自由度。
|
||||||
|
/// 适用于 CAD 姿态天然歪斜、无法靠"假设 local +X 是长轴"对齐路径的物体。
|
||||||
|
/// </summary>
|
||||||
|
public static class AlignToGroundMinCrossSectionYawSearcher
|
||||||
|
{
|
||||||
|
private const double CoarseStepDegrees = 5.0;
|
||||||
|
private const double FineStepDegrees = 1.0;
|
||||||
|
private const float HorizontalEpsilon = 1e-8f;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 在贴地旋转基础上绕宿主 Up 搜索 yaw,使物体截面投影面积最小。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="faceDownRotation">贴地旋转(把面法线旋到 -hostUp),宿主世界空间。</param>
|
||||||
|
/// <param name="cadRotation">CAD/复位姿态旋转,宿主世界空间。</param>
|
||||||
|
/// <param name="sizeX">物体 local +X 方向尺寸(模型单位)。</param>
|
||||||
|
/// <param name="sizeY">物体 local +Y 方向尺寸(模型单位)。</param>
|
||||||
|
/// <param name="sizeZ">物体 local +Z 方向尺寸(模型单位)。</param>
|
||||||
|
/// <param name="hostPathForward">路径起点方向(宿主世界)。</param>
|
||||||
|
/// <param name="adapter">宿主坐标系适配器。</param>
|
||||||
|
/// <param name="bestWidthAcrossPath">最佳 yaw 下垂直路径方向的宽度(模型单位)。</param>
|
||||||
|
/// <param name="bestHeightAlongUp">最佳 yaw 下沿宿主 Up 的高度(模型单位)。</param>
|
||||||
|
/// <param name="bestForwardAlongPath">最佳 yaw 下沿路径方向的长度(模型单位)。</param>
|
||||||
|
/// <returns>合成后的宿主世界四元数(已归一化):yawQ * faceDownRotation。</returns>
|
||||||
|
public static Quaternion Search(
|
||||||
|
Quaternion faceDownRotation,
|
||||||
|
Quaternion cadRotation,
|
||||||
|
double sizeX,
|
||||||
|
double sizeY,
|
||||||
|
double sizeZ,
|
||||||
|
Vector3 hostPathForward,
|
||||||
|
HostCoordinateAdapter adapter,
|
||||||
|
out double bestWidthAcrossPath,
|
||||||
|
out double bestHeightAlongUp,
|
||||||
|
out double bestForwardAlongPath)
|
||||||
|
{
|
||||||
|
bestWidthAcrossPath = 0.0;
|
||||||
|
bestHeightAlongUp = 0.0;
|
||||||
|
bestForwardAlongPath = 0.0;
|
||||||
|
|
||||||
|
if (adapter == null)
|
||||||
|
{
|
||||||
|
throw new ArgumentNullException(nameof(adapter));
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector3 hostUp = adapter.HostUpVector3;
|
||||||
|
|
||||||
|
// 路径方向投影到水平面,构造 hostSide(垂直路径方向,水平)
|
||||||
|
Vector3 pathFwdHorizontal = hostPathForward - hostUp * Vector3.Dot(hostPathForward, hostUp);
|
||||||
|
if (pathFwdHorizontal.LengthSquared() < HorizontalEpsilon)
|
||||||
|
{
|
||||||
|
// 纯垂直路径:无水平方向,yaw 无意义,保持贴地姿态
|
||||||
|
return faceDownRotation;
|
||||||
|
}
|
||||||
|
pathFwdHorizontal = Vector3.Normalize(pathFwdHorizontal);
|
||||||
|
Vector3 hostSide = Vector3.Normalize(Vector3.Cross(pathFwdHorizontal, hostUp));
|
||||||
|
|
||||||
|
// 物体最终姿态 = yawQ * faceDown * cad
|
||||||
|
// local 轴在宿主世界的方向 = totalRotation * UnitX/Y/Z
|
||||||
|
// 在该姿态下,截面 = 沿 hostSide 的宽度 × 沿 hostUp 的高度
|
||||||
|
double bestYawRad = 0.0;
|
||||||
|
double bestArea = double.MaxValue;
|
||||||
|
|
||||||
|
// 粗搜 [0, 360°),步长 5°
|
||||||
|
for (double deg = 0.0; deg < 360.0; deg += CoarseStepDegrees)
|
||||||
|
{
|
||||||
|
double yawRad = deg * Math.PI / 180.0;
|
||||||
|
double area = EvaluateCrossSectionArea(
|
||||||
|
faceDownRotation, cadRotation, yawRad,
|
||||||
|
pathFwdHorizontal, hostSide, hostUp, sizeX, sizeY, sizeZ,
|
||||||
|
out double width, out double height, out double forward);
|
||||||
|
if (area < bestArea)
|
||||||
|
{
|
||||||
|
bestArea = area;
|
||||||
|
bestYawRad = yawRad;
|
||||||
|
bestWidthAcrossPath = width;
|
||||||
|
bestHeightAlongUp = height;
|
||||||
|
bestForwardAlongPath = forward;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 细搜 [best - step, best + step],步长 1°
|
||||||
|
double loDeg = (bestYawRad * 180.0 / Math.PI) - CoarseStepDegrees;
|
||||||
|
double hiDeg = (bestYawRad * 180.0 / Math.PI) + CoarseStepDegrees;
|
||||||
|
for (double deg = loDeg; deg <= hiDeg; deg += FineStepDegrees)
|
||||||
|
{
|
||||||
|
double yawRad = deg * Math.PI / 180.0;
|
||||||
|
double area = EvaluateCrossSectionArea(
|
||||||
|
faceDownRotation, cadRotation, yawRad,
|
||||||
|
pathFwdHorizontal, hostSide, hostUp, sizeX, sizeY, sizeZ,
|
||||||
|
out double width, out double height, out double forward);
|
||||||
|
if (area < bestArea)
|
||||||
|
{
|
||||||
|
bestArea = area;
|
||||||
|
bestYawRad = yawRad;
|
||||||
|
bestWidthAcrossPath = width;
|
||||||
|
bestHeightAlongUp = height;
|
||||||
|
bestForwardAlongPath = forward;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return Quaternion.Normalize(
|
||||||
|
Quaternion.CreateFromAxisAngle(hostUp, (float)bestYawRad) * faceDownRotation);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 计算给定 deltaYaw 下物体截面投影面积。
|
||||||
|
/// 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式:
|
||||||
|
/// width = Σ |localAxis · hostSide| * sizeAxis (垂直路径方向宽度)
|
||||||
|
/// height = Σ |localAxis · hostUp| * sizeAxis (沿宿主 Up 高度)
|
||||||
|
/// forward = Σ |localAxis · pathForward| * sizeAxis (沿路径方向长度)
|
||||||
|
/// area = width * height
|
||||||
|
/// </summary>
|
||||||
|
private static double EvaluateCrossSectionArea(
|
||||||
|
Quaternion faceDownRotation,
|
||||||
|
Quaternion cadRotation,
|
||||||
|
double deltaYawRadians,
|
||||||
|
Vector3 pathForward,
|
||||||
|
Vector3 hostSide,
|
||||||
|
Vector3 hostUp,
|
||||||
|
double sizeX,
|
||||||
|
double sizeY,
|
||||||
|
double sizeZ,
|
||||||
|
out double widthAcrossPath,
|
||||||
|
out double heightAlongUp,
|
||||||
|
out double forwardAlongPath)
|
||||||
|
{
|
||||||
|
Quaternion yawQ = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYawRadians);
|
||||||
|
Quaternion total = Quaternion.Normalize(yawQ * faceDownRotation * cadRotation);
|
||||||
|
|
||||||
|
Vector3 localX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, total));
|
||||||
|
Vector3 localY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, total));
|
||||||
|
Vector3 localZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, total));
|
||||||
|
|
||||||
|
widthAcrossPath =
|
||||||
|
Math.Abs(Vector3.Dot(localX, hostSide)) * sizeX +
|
||||||
|
Math.Abs(Vector3.Dot(localY, hostSide)) * sizeY +
|
||||||
|
Math.Abs(Vector3.Dot(localZ, hostSide)) * sizeZ;
|
||||||
|
heightAlongUp =
|
||||||
|
Math.Abs(Vector3.Dot(localX, hostUp)) * sizeX +
|
||||||
|
Math.Abs(Vector3.Dot(localY, hostUp)) * sizeY +
|
||||||
|
Math.Abs(Vector3.Dot(localZ, hostUp)) * sizeZ;
|
||||||
|
forwardAlongPath =
|
||||||
|
Math.Abs(Vector3.Dot(localX, pathForward)) * sizeX +
|
||||||
|
Math.Abs(Vector3.Dot(localY, pathForward)) * sizeY +
|
||||||
|
Math.Abs(Vector3.Dot(localZ, pathForward)) * sizeZ;
|
||||||
|
|
||||||
|
return widthAcrossPath * heightAlongUp;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 把贴地搜索后的绝对宿主姿态(overrideQ)转成"相对于路径 baseline 的 host 欧拉修正",
|
||||||
|
/// 与自动调整返回的 correction 语义一致,供动画系统在 baseline 之上叠加。
|
||||||
|
///
|
||||||
|
/// 关键:PathAnimationManager 增量链从 CAD 姿态出发:
|
||||||
|
/// finalQ = qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) * cadQ
|
||||||
|
/// 贴地时 finalQ = overrideQ * cadQ,所以:
|
||||||
|
/// qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) = overrideQ
|
||||||
|
/// 而 CreateHostRotationCorrection(correction) = qup(YDeg) * qnonUp(ZDeg) * qx(XDeg),所以:
|
||||||
|
/// qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) = overrideQ
|
||||||
|
/// => correctionHostQ = qup(currentYaw - pathYaw) * overrideQ
|
||||||
|
///
|
||||||
|
/// currentYaw 是 CAD 姿态的 canonical yaw,从 cadQ 提取(与 GetYawFromRotation 一致)。
|
||||||
|
public static LocalEulerRotationCorrection ComposeHostCorrection(
|
||||||
|
Quaternion absoluteHostQ,
|
||||||
|
Quaternion cadRotation,
|
||||||
|
double pathYawRadians,
|
||||||
|
HostCoordinateAdapter adapter)
|
||||||
|
{
|
||||||
|
if (adapter == null)
|
||||||
|
{
|
||||||
|
throw new ArgumentNullException(nameof(adapter));
|
||||||
|
}
|
||||||
|
|
||||||
|
double currentYaw = ExtractCanonicalYaw(cadRotation, adapter);
|
||||||
|
Quaternion preRotation = Quaternion.CreateFromAxisAngle(
|
||||||
|
adapter.HostUpVector3, (float)(currentYaw - pathYawRadians));
|
||||||
|
Quaternion correctionHostQ = Quaternion.Normalize(preRotation * absoluteHostQ);
|
||||||
|
|
||||||
|
Quaternion correctionCanonQ = HostQuaternionToCanonical(correctionHostQ, adapter);
|
||||||
|
return ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
|
||||||
|
correctionCanonQ, adapter.HostType);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从宿主空间四元数提取 canonical yaw(Atan2(canonicalForward.Y, canonicalForward.X))。
|
||||||
|
/// 与 ModelItemTransformHelper.GetYawFromRotation 一致:取 forward 轴(X列),转 canonical,投影 XY 平面。
|
||||||
|
/// </summary>
|
||||||
|
private static double ExtractCanonicalYaw(Quaternion hostQ, HostCoordinateAdapter adapter)
|
||||||
|
{
|
||||||
|
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
|
||||||
|
Vector3 hostForward = new Vector3(hostLinear.M11, hostLinear.M21, hostLinear.M31);
|
||||||
|
Vector3 canonFwd = adapter.ToCanonicalVector3(hostForward);
|
||||||
|
canonFwd.Z = 0f;
|
||||||
|
if (canonFwd.LengthSquared() < 1e-9f)
|
||||||
|
{
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
canonFwd = Vector3.Normalize(canonFwd);
|
||||||
|
return Math.Atan2(canonFwd.Y, canonFwd.X);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 宿主空间四元数 → canonical 空间四元数。
|
||||||
|
/// 旋转的坐标系转换必须用相似变换 R_canon = M⁻¹ · R_host · M
|
||||||
|
/// (M = canonical→host 坐标变换矩阵,正交故 M⁻¹ = Mᵀ)。
|
||||||
|
/// 不能用列向量映射——那只在"基向量定义"场景成立,会破坏 Identity 不变性。
|
||||||
|
/// </summary>
|
||||||
|
public static Quaternion HostQuaternionToCanonical(Quaternion hostQ, HostCoordinateAdapter adapter)
|
||||||
|
{
|
||||||
|
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
|
||||||
|
|
||||||
|
// M: canonical→host 坐标变换(列 = canonical 基在 host 中的表示)
|
||||||
|
Vector3 mColX = adapter.FromCanonicalVector3(Vector3.UnitX);
|
||||||
|
Vector3 mColY = adapter.FromCanonicalVector3(Vector3.UnitY);
|
||||||
|
Vector3 mColZ = adapter.FromCanonicalVector3(Vector3.UnitZ);
|
||||||
|
Matrix4x4 m = new Matrix4x4(
|
||||||
|
mColX.X, mColY.X, mColZ.X, 0f,
|
||||||
|
mColX.Y, mColY.Y, mColZ.Y, 0f,
|
||||||
|
mColX.Z, mColY.Z, mColZ.Z, 0f,
|
||||||
|
0f, 0f, 0f, 1f);
|
||||||
|
// M 正交,M⁻¹ = Mᵀ
|
||||||
|
Matrix4x4 mInv = Matrix4x4.Transpose(m);
|
||||||
|
// R_canon = M⁻¹ · R_host · M
|
||||||
|
Matrix4x4 canonLinear = Matrix4x4.Multiply(Matrix4x4.Multiply(mInv, hostLinear), m);
|
||||||
|
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(canonLinear));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -157,7 +157,9 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
|||||||
}
|
}
|
||||||
|
|
||||||
LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
|
LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
|
||||||
ObjectPassageProjectionScore bestScore = ObjectPassageProjectionScore.Invalid;
|
// 必须评估 correction=Zero 作为初始候选:当 baseline 已对齐路径时,
|
||||||
|
// Zero 就是最优解;若不评估,随机采样必然替换它,refine 也收敛不回 0。
|
||||||
|
ObjectPassageProjectionScore bestScore = evaluator(bestCorrection);
|
||||||
Quaternion bestQuaternion = Quaternion.Identity;
|
Quaternion bestQuaternion = Quaternion.Identity;
|
||||||
|
|
||||||
// S³ 均匀采样四元数 → 转 Euler → 覆盖 SO(3) 无万向节死锁
|
// S³ 均匀采样四元数 → 转 Euler → 覆盖 SO(3) 无万向节死锁
|
||||||
@ -357,7 +359,16 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
|
public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
|
||||||
|
{
|
||||||
|
return CanonicalQuaternionToHostEulerCorrection(q, CoordinateSystemManager.Instance.ResolvedType);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将 canonical 空间的四元数分解为宿主欧拉修正 (X, Y=up, Z=nonUp)。
|
||||||
|
/// 接受显式 hostType 参数,避免依赖全局 CoordinateSystemManager 状态,便于单测。
|
||||||
|
/// </summary>
|
||||||
|
public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q, CoordinateSystemType hostType)
|
||||||
{
|
{
|
||||||
double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z;
|
double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z;
|
||||||
|
|
||||||
@ -373,8 +384,6 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
|||||||
double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
|
double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
|
||||||
double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp);
|
double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp);
|
||||||
|
|
||||||
// Canonical (ZUp) → Host: 使用 CoordinateSystemManager 桥接
|
|
||||||
var hostType = CoordinateSystemManager.Instance.ResolvedType;
|
|
||||||
if (hostType == CoordinateSystemType.ZUp)
|
if (hostType == CoordinateSystemType.ZUp)
|
||||||
{
|
{
|
||||||
return new LocalEulerRotationCorrection(
|
return new LocalEulerRotationCorrection(
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user