Compare commits
No commits in common. "da6b6cb4810ffeeb9333735f790fb9e9aa56befe" and "2e959c16c9a4f5a9c8a311a24a9164fef2df4c8d" have entirely different histories.
da6b6cb481
...
2e959c16c9
@ -151,15 +151,6 @@ var rotation = new Rotation3D(qx, qy, qz, qw);
|
||||
|
||||
## 9. 工具使用提示
|
||||
|
||||
### Shell 命令统一规范
|
||||
|
||||
所有 shell 命令、脚本、rg/fd 调用必须通过 `pwsh` (PowerShell 7) 执行,禁止用 cmd/bash:
|
||||
|
||||
```
|
||||
pwsh -Command "rg -n '关键字' src/"
|
||||
pwsh -Command "./build-and-deploy.bat"
|
||||
```
|
||||
|
||||
### edit 工具的正确格式
|
||||
|
||||
```json
|
||||
|
||||
28
CHANGELOG.md
28
CHANGELOG.md
@ -1,33 +1,5 @@
|
||||
# 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
|
||||
|
||||
### ✨ 新功能
|
||||
|
||||
@ -66,7 +66,6 @@
|
||||
<Compile Include="UnitTests\Core\PathPlanningManagerHoistingCompletionTests.cs" />
|
||||
<Compile Include="UnitTests\Core\PathPersistenceTests.cs" />
|
||||
<Compile Include="UnitTests\Core\PathRouteCloneTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\AlignToGroundMinCrossSectionYawSearcherTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\AutoPathPlanningCoordinateSemanticsTests.cs" />
|
||||
<Compile Include="UnitTests\Integration\AutoPathGridGenerationAutomationTests.cs" />
|
||||
<Compile Include="UnitTests\Integration\NavisworksTestAutomationClient.cs" />
|
||||
|
||||
@ -344,7 +344,6 @@
|
||||
<Compile Include="src\Utils\UnitsConverter.cs" />
|
||||
<Compile Include="src\Utils\VersionInfo.cs" />
|
||||
<!-- Coordinate System -->
|
||||
<Compile Include="src\Utils\CoordinateSystem\AlignToGroundMinCrossSectionYawSearcher.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\CoordinateSystemType.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\ICoordinateSystem.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\CanonicalBounds3.cs" />
|
||||
|
||||
@ -1,432 +0,0 @@
|
||||
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,27 +213,25 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostRotationCorrection_YDegrees_ShouldKeepHostZAxisInvariant()
|
||||
public void ZUp_HostRotationCorrection_ShouldKeepHostZAxisInvariantInHostSpace()
|
||||
{
|
||||
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转,Z 轴自身不变。
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
||||
AssertVector(rotatedUp, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_ComposeHostQuaternion_YDegrees_ShouldApplyHostZAxisRotation()
|
||||
public void ZUp_ComposeHostQuaternion_ShouldApplyHostZCorrectionAfterBaseline()
|
||||
{
|
||||
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转 90°:X→Y, Y→-X, Z 不变。
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
var baseline = Quaternion.Identity;
|
||||
|
||||
Quaternion composed = adapter.ComposeHostQuaternion(
|
||||
baseline,
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||||
|
||||
|
||||
@ -49,14 +49,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostZDegrees_ShouldPromoteForwardSizeToUpExtent()
|
||||
public void ZUp_HostY90_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 convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
@ -71,14 +69,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ZUp_HostYDegrees_ShouldKeepUpExtentUnchanged()
|
||||
public void ZUp_HostZ90_ShouldKeepUpExtentUnchanged()
|
||||
{
|
||||
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转 90°:X→Y, Y→-X, Z(up) 不变。
|
||||
// forward 和 side 互换,up 不变。
|
||||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||||
Quaternion correction = adapter.CreateCanonicalRotationCorrection(
|
||||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||||
|
||||
var result = RotatedObjectExtentHelper.CalculateProjectedSemanticExtents(
|
||||
convention,
|
||||
|
||||
@ -59,18 +59,19 @@ namespace NavisworksTransport.UnitTests.Core
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
[ExpectedException(typeof(FormatException))]
|
||||
public void FromKey_OldFormat_ShouldThrow()
|
||||
public void FromKey_OldFormat_ShouldRetroCompatible()
|
||||
{
|
||||
// 旧格式(无 || 分隔符)已不再支持,按 AGENTS.md "不向后兼容" 原则抛 FormatException
|
||||
AliasNodeIdentity.FromKey("building/floor/wall#2");
|
||||
var id = AliasNodeIdentity.FromKey("building/floor/wall#2");
|
||||
Assert.AreEqual("", id.IndexPath);
|
||||
Assert.AreEqual("building/floor/wall#2", id.DisplayPath);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
[ExpectedException(typeof(FormatException))]
|
||||
public void FromKey_OldFormatNoIndex_ShouldThrow()
|
||||
public void FromKey_OldFormatNoIndex_ShouldParse()
|
||||
{
|
||||
AliasNodeIdentity.FromKey("building/floor");
|
||||
var id = AliasNodeIdentity.FromKey("building/floor");
|
||||
Assert.AreEqual("", id.IndexPath);
|
||||
Assert.AreEqual("building/floor", id.DisplayPath);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
@ -98,10 +99,10 @@ namespace NavisworksTransport.UnitTests.Core
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void TryParseKey_OldFormat_ShouldReturnFalse()
|
||||
public void TryParseKey_OldFormat_ShouldSucceed()
|
||||
{
|
||||
// 旧格式(无 || 分隔符)已不再支持
|
||||
Assert.IsFalse(AliasNodeIdentity.TryParseKey("path#0", out _));
|
||||
Assert.IsTrue(AliasNodeIdentity.TryParseKey("path#0", out var id));
|
||||
Assert.AreEqual("", id.IndexPath);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
@ -278,9 +279,9 @@ namespace NavisworksTransport.UnitTests.Core
|
||||
public void DeleteByPathPrefix_ShouldRemoveAllUnderPath()
|
||||
{
|
||||
var store = CreateStore();
|
||||
store.SetAlias(Id("building/floor/1"), "别名1");
|
||||
store.SetAlias(Id("building/floor/2"), "别名2");
|
||||
store.SetAlias(Id("building/roof/3"), "别名3");
|
||||
store.SetAlias(Id("building/floor/1"));
|
||||
store.SetAlias(Id("building/floor/2"));
|
||||
store.SetAlias(Id("building/roof/3"));
|
||||
|
||||
store.DeleteAliasesByPathPrefix("building/floor");
|
||||
|
||||
@ -342,16 +343,19 @@ namespace NavisworksTransport.UnitTests.Core
|
||||
Assert.AreEqual(0, store.Count);
|
||||
}
|
||||
|
||||
// ── 旧格式已不支持(AGENTS.md "不向后兼容" 原则)──
|
||||
// ── 新格式 → 旧格式 兼容 ──
|
||||
|
||||
[TestMethod]
|
||||
[ExpectedException(typeof(FormatException))]
|
||||
public void OldFormatKey_ShouldThrow()
|
||||
public void OldFormatKey_StillWorks()
|
||||
{
|
||||
var store = CreateStore();
|
||||
string oldKey = "building/floor/wall#0";
|
||||
// 旧格式 key 无 || 分隔符,FromKey 应抛 FormatException
|
||||
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.16.0
|
||||
0.15.8
|
||||
|
||||
@ -1,4 +0,0 @@
|
||||
@echo off
|
||||
call "%~dp0compile.bat"
|
||||
if %ERRORLEVEL% NEQ 0 exit /b 1
|
||||
call "%~dp0deploy-plugin.bat"
|
||||
@ -2,12 +2,6 @@
|
||||
|
||||
## 功能点
|
||||
|
||||
### [2026/6/10]
|
||||
|
||||
1. [ ] (功能)对任意三维姿态的模型,点选面自动贴合地面
|
||||
2. [ ] (功能)自定义分层属性,使用配置文件
|
||||
3. [x] (优化)用单点取面,取代Rail路径中的三点取端面和2点取安装面
|
||||
|
||||
### [2026/5/27]
|
||||
|
||||
1. [x] (功能)对任意三维姿态的模型,自动调整最小投影到路径方向
|
||||
|
||||
@ -449,17 +449,6 @@ namespace NavisworksTransport.Core.Animation
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
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 identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
|
||||
@ -487,34 +476,6 @@ namespace NavisworksTransport.Core.Animation
|
||||
_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>
|
||||
/// 将动画对象彻底恢复到 CAD 原始位置(清除所有覆盖变换)
|
||||
/// 常用于模式切换或彻底清除动画干扰
|
||||
@ -5613,62 +5574,6 @@ namespace NavisworksTransport.Core.Animation
|
||||
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)
|
||||
{
|
||||
_objectStartPlacementMode = placementMode;
|
||||
|
||||
@ -45,21 +45,6 @@ namespace NavisworksTransport
|
||||
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>
|
||||
/// 鼠标按下事件 — 单次点击内做三次 PickItemFromPoint 推断面法向量
|
||||
/// </summary>
|
||||
|
||||
@ -813,12 +813,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
{
|
||||
if (SetProperty(ref _objectRotationCorrection, value))
|
||||
{
|
||||
// 手动改角度时清空自动调整/贴合地面的缓存尺寸,
|
||||
// 让 UpdatePassageSpaceVisualization 走 CalculateRotatedDimensions 重新计算。
|
||||
// 贴地/自动调整直接设字段不经过 setter,不受影响。
|
||||
_autoAabbSx = null;
|
||||
_autoAabbSy = null;
|
||||
_autoAabbSz = null;
|
||||
// 角度修正值改变时,更新物体朝向和通行空间
|
||||
UpdateObjectRotation();
|
||||
}
|
||||
@ -1708,7 +1702,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(0.0);
|
||||
ObjectRotationCorrection = LocalEulerRotationCorrection.Zero;
|
||||
_objectGroundLiftHeightInMeters = 0.0;
|
||||
_autoAabbSx = null; _autoAabbSy = null; _autoAabbSz = null;
|
||||
LogManager.Debug("[选择物体] 已重置角度修正值为0(新物体)");
|
||||
}
|
||||
else
|
||||
@ -2183,25 +2176,14 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
{
|
||||
bool enableGroundLift = CurrentPathRoute?.PathType == PathType.Ground;
|
||||
var autoAdjustmentRequest = CreateObjectPassageProjectionOptimizationRequest();
|
||||
LocalEulerRotationCorrection prefillRotation = _objectRotationCorrection;
|
||||
(double, double, double)? prefillAabb = null;
|
||||
|
||||
ShowEditRotationDialog:
|
||||
var dialog = new Views.EditRotationWindow(
|
||||
prefillRotation,
|
||||
_objectRotationCorrection,
|
||||
_objectGroundLiftHeightInMeters,
|
||||
enableGroundLift,
|
||||
autoAdjustmentRequest,
|
||||
autoAdjustmentRequest == null
|
||||
? (Func<ObjectPassageProjectionOptimizationResult>)null
|
||||
: (() => OptimizeObjectPassageProjectionByMeasuredAabb(autoAdjustmentRequest)),
|
||||
alignToGroundProvider: HasSelectedAnimatedObject
|
||||
? (Func<Vector3, ObjectPassageProjectionOptimizationResult>)
|
||||
(faceNormal => OptimizeAlignToGround(faceNormal))
|
||||
: null);
|
||||
if (prefillAabb.HasValue)
|
||||
dialog.AutoAdjustAabbSizes = prefillAabb;
|
||||
|
||||
: (() => OptimizeObjectPassageProjectionByMeasuredAabb(autoAdjustmentRequest)));
|
||||
if (dialog.ShowDialog() == true)
|
||||
{
|
||||
var placementRequest = dialog.AdjustmentRequest;
|
||||
@ -2209,6 +2191,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
bool isAutoAdjusted = dialog.AutoAdjustAabbSizes.HasValue;
|
||||
if (isAutoAdjusted)
|
||||
{
|
||||
// 自动调整:先原地校正(对齐 X),再通过 MoveObjectToPathStart(skipCadRestore) 偏航到路径
|
||||
_pathAnimationManager?.SetObjectStartVerticalLiftDirect(
|
||||
placementRequest.VerticalLiftInMeters);
|
||||
_pathAnimationManager?.ApplyRotationCorrectionInPlace(
|
||||
@ -2225,15 +2208,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
_pathAnimationManager?.ApplyObjectStartPlacementRequest(placementRequest);
|
||||
ObjectRotationCorrection = placementRequest.RotationCorrection;
|
||||
}
|
||||
if (isAutoAdjusted)
|
||||
{
|
||||
// 自动调整确认后用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);
|
||||
}
|
||||
if (dialog.AutoAdjustAabbSizes.HasValue)
|
||||
(_autoAabbSx, _autoAabbSy, _autoAabbSz) = dialog.AutoAdjustAabbSizes.Value;
|
||||
|
||||
string liftSummary = enableGroundLift
|
||||
? $", 上下偏移={_objectGroundLiftHeightInMeters:F3}m"
|
||||
@ -2250,14 +2226,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
// 更新通行空间可视化(考虑旋转后的尺寸)
|
||||
UpdatePassageSpaceVisualization();
|
||||
}
|
||||
else if (dialog.AlignToGroundRequested)
|
||||
{
|
||||
// 读取对话框当前上下偏移值
|
||||
_objectGroundLiftHeightInMeters = dialog.AdjustmentRequest.VerticalLiftInMeters;
|
||||
FaceInferToolPlugin.FaceInferred += OnFaceInferredForAlignToGround;
|
||||
FaceInferToolPlugin.Activate();
|
||||
return;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
@ -2266,354 +2234,6 @@ 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(
|
||||
ObjectPassageProjectionOptimizationRequest request)
|
||||
{
|
||||
@ -5147,12 +4767,10 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
double effectiveLength, effectiveWidth, effectiveHeight;
|
||||
if (_autoAabbSx.HasValue)
|
||||
{
|
||||
// 自动调整/贴合地面后实测 AABB:物体已在最终姿态,直接使用世界 AABB 尺寸。
|
||||
// 不清空:贴地后 correction 固定,后续刷新也应保持实测值,
|
||||
// 避免 CalculateRotatedDimensions 用包含 CAD yaw 补偿的 correction 反推失真。
|
||||
effectiveLength = _autoAabbSx.Value;
|
||||
effectiveWidth = _autoAabbSz.Value;
|
||||
effectiveHeight = _autoAabbSy.Value;
|
||||
_autoAabbSx = null;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
|
||||
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
|
||||
xmlns:converters="clr-namespace:NavisworksTransport.UI.WPF.Converters"
|
||||
Title="调整物体" Height="430" Width="480"
|
||||
Title="调整物体" Height="430" Width="470"
|
||||
WindowStartupLocation="CenterScreen"
|
||||
ResizeMode="NoResize"
|
||||
ShowInTaskbar="False"
|
||||
@ -190,46 +190,38 @@
|
||||
</Border>
|
||||
|
||||
<!-- 按钮栏 -->
|
||||
<Border Grid.Row="2" Background="#FFF8FBFF" BorderBrush="#FFD4E7FF" BorderThickness="0,1,0,0" Padding="10,12">
|
||||
<Grid HorizontalAlignment="Center">
|
||||
<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="初始值"
|
||||
<Border Grid.Row="2" Background="#FFF8FBFF" BorderBrush="#FFD4E7FF" BorderThickness="0,1,0,0" Padding="20,12">
|
||||
<StackPanel Orientation="Horizontal" HorizontalAlignment="Center">
|
||||
<Button Content="初始值"
|
||||
Click="OnResetClick"
|
||||
Style="{StaticResource SecondaryButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
<Button Grid.Column="2" Content="自动调整"
|
||||
Width="64"
|
||||
Height="32"
|
||||
Margin="0,0,8,0"/>
|
||||
<Button Content="自动调整"
|
||||
Click="OnAutoAdjustClick"
|
||||
Style="{StaticResource SecondaryButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
<Button Grid.Column="4" Content="贴合地面"
|
||||
Click="OnAlignToGroundClick"
|
||||
Style="{StaticResource SecondaryButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
<Button Grid.Column="6" Content="平移"
|
||||
Width="64"
|
||||
Height="32"
|
||||
Margin="0,0,8,0"/>
|
||||
<Button Content="平移"
|
||||
Click="OnTranslateClick"
|
||||
Style="{StaticResource SecondaryButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
<Button Grid.Column="8" Content="确认"
|
||||
Width="64"
|
||||
Height="32"
|
||||
Margin="0,0,8,0"/>
|
||||
<Button Content="确认"
|
||||
Click="OnConfirmClick"
|
||||
Style="{StaticResource ActionButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
<Button Grid.Column="10" Content="取消"
|
||||
Width="64"
|
||||
Height="32"
|
||||
Margin="0,0,8,0"/>
|
||||
<Button Content="取消"
|
||||
Click="OnCancelClick"
|
||||
Style="{StaticResource SecondaryButtonStyle}"
|
||||
Width="64" Height="32" Margin="0"/>
|
||||
</Grid>
|
||||
Width="64"
|
||||
Height="32"/>
|
||||
</StackPanel>
|
||||
</Border>
|
||||
</Grid>
|
||||
</Window>
|
||||
|
||||
@ -8,8 +8,6 @@ using NavisworksTransport.UI.WPF.Converters;
|
||||
using NavisworksTransport.Utils;
|
||||
using NavisworksTransport.Utils.CoordinateSystem;
|
||||
|
||||
using System.Numerics;
|
||||
|
||||
namespace NavisworksTransport.UI.WPF.Views
|
||||
{
|
||||
public partial class EditRotationWindow : Window, INotifyPropertyChanged
|
||||
@ -29,12 +27,9 @@ namespace NavisworksTransport.UI.WPF.Views
|
||||
private ObjectStartPlacementRequest _adjustmentRequest;
|
||||
private readonly ObjectPassageProjectionOptimizationRequest _autoAdjustmentRequest;
|
||||
private readonly Func<ObjectPassageProjectionOptimizationResult> _autoAdjustmentProvider;
|
||||
private readonly Func<Vector3, ObjectPassageProjectionOptimizationResult> _alignToGroundProvider;
|
||||
private readonly bool _isGroundLiftEnabled;
|
||||
private double _vehicleHeightInMeters = 0.15;
|
||||
private bool _requireWidthLarger = false;
|
||||
private bool _isAlignToGroundResult;
|
||||
private bool _alignToGroundRequested = false;
|
||||
|
||||
/// <summary>
|
||||
/// 门型偏好:true=宽>高,false=高>宽(默认)
|
||||
@ -45,10 +40,7 @@ namespace NavisworksTransport.UI.WPF.Views
|
||||
set => _requireWidthLarger = value;
|
||||
}
|
||||
|
||||
public (double sx, double sy, double sz)? AutoAdjustAabbSizes { get; set; }
|
||||
|
||||
public bool AlignToGroundRequested => _alignToGroundRequested;
|
||||
public bool IsAlignToGroundResult { get => _isAlignToGroundResult; set => _isAlignToGroundResult = value; }
|
||||
public (double sx, double sy, double sz)? AutoAdjustAabbSizes { get; private set; }
|
||||
|
||||
public double VehicleHeightInMeters
|
||||
{
|
||||
@ -127,8 +119,7 @@ namespace NavisworksTransport.UI.WPF.Views
|
||||
double currentGroundLiftHeightInMeters,
|
||||
bool isGroundLiftEnabled,
|
||||
ObjectPassageProjectionOptimizationRequest autoAdjustmentRequest = null,
|
||||
Func<ObjectPassageProjectionOptimizationResult> autoAdjustmentProvider = null,
|
||||
Func<Vector3, ObjectPassageProjectionOptimizationResult> alignToGroundProvider = null)
|
||||
Func<ObjectPassageProjectionOptimizationResult> autoAdjustmentProvider = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
@ -139,11 +130,7 @@ namespace NavisworksTransport.UI.WPF.Views
|
||||
_isGroundLiftEnabled = isGroundLiftEnabled;
|
||||
_autoAdjustmentRequest = autoAdjustmentRequest;
|
||||
_autoAdjustmentProvider = autoAdjustmentProvider;
|
||||
_alignToGroundProvider = alignToGroundProvider;
|
||||
// 默认上下偏移 = 物流车高度(若未设置过)
|
||||
GroundPathLiftHeightInMeters = Math.Abs(currentGroundLiftHeightInMeters) < 1e-9
|
||||
? VehicleHeightInMeters
|
||||
: currentGroundLiftHeightInMeters;
|
||||
GroundPathLiftHeightInMeters = currentGroundLiftHeightInMeters;
|
||||
_adjustmentRequest = ObjectStartPlacementRequest.CreateRotationCorrection(currentRotation, currentGroundLiftHeightInMeters);
|
||||
DataContext = this;
|
||||
Loaded += (sender, args) => ZAxisTextBox.Focus();
|
||||
@ -213,22 +200,6 @@ 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)
|
||||
{
|
||||
if (sender is Button button)
|
||||
|
||||
@ -1,239 +0,0 @@
|
||||
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,9 +157,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
}
|
||||
|
||||
LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
|
||||
// 必须评估 correction=Zero 作为初始候选:当 baseline 已对齐路径时,
|
||||
// Zero 就是最优解;若不评估,随机采样必然替换它,refine 也收敛不回 0。
|
||||
ObjectPassageProjectionScore bestScore = evaluator(bestCorrection);
|
||||
ObjectPassageProjectionScore bestScore = ObjectPassageProjectionScore.Invalid;
|
||||
Quaternion bestQuaternion = Quaternion.Identity;
|
||||
|
||||
// S³ 均匀采样四元数 → 转 Euler → 覆盖 SO(3) 无万向节死锁
|
||||
@ -359,16 +357,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
private static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
|
||||
{
|
||||
double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z;
|
||||
|
||||
@ -384,6 +373,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
|
||||
double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp);
|
||||
|
||||
// Canonical (ZUp) → Host: 使用 CoordinateSystemManager 桥接
|
||||
var hostType = CoordinateSystemManager.Instance.ResolvedType;
|
||||
if (hostType == CoordinateSystemType.ZUp)
|
||||
{
|
||||
return new LocalEulerRotationCorrection(
|
||||
|
||||
Loading…
Reference in New Issue
Block a user