From f86e8c8bf45d530a73d2ecf1487ed7f681b79e67 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Tue, 23 Jun 2026 23:09:59 +0800
Subject: [PATCH] =?UTF-8?q?=EF=BB=BFfix:=20=E8=B4=B4=E5=90=88=E5=9C=B0?=
=?UTF-8?q?=E9=9D=A2=E5=90=8E=E7=BB=95=E5=9E=82=E7=9B=B4=E8=BD=B4=E6=90=9C?=
=?UTF-8?q?=E7=B4=A2=E6=9C=80=E5=B0=8F=E6=88=AA=E9=9D=A2=20yaw=EF=BC=8C?=
=?UTF-8?q?=E5=AF=B9=E9=BD=90=E8=B7=AF=E5=BE=84=E6=96=B9=E5=90=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
贴地旋转已固定俯仰/翻滚，只剩绕宿主 Up 的 yaw 自由度。
对 CAD 姿态天然歪斜的物体，假设 local +X 是长轴会失败，
改为搜索 [0,360) 使物体在路径方向的截面投影面积最小，
与自动调整同目标但单自由度搜索更稳定。
- 新增 AlignToGroundMinCrossSectionYawSearcher（粗搜5°+细搜1°）
- 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式
- 新增 7 个单测覆盖平地/倾斜/CAD歪斜/垂直退化/立方体
- 经多物体实测验证：Chair Lounge Couch / Chair Sitting Square 均正确对齐
---
NavisworksTransport.UnitTests.csproj | 1 +
TransportPlugin.csproj | 1 +
...ToGroundMinCrossSectionYawSearcherTests.cs | 223 ++++++++++++++++++
doc/requirement/todo_features.md | 6 +
.../ViewModels/AnimationControlViewModel.cs | 44 +++-
...AlignToGroundMinCrossSectionYawSearcher.cs | 131 ++++++++++
6 files changed, 398 insertions(+), 8 deletions(-)
create mode 100644 UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
create mode 100644 src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
diff --git a/NavisworksTransport.UnitTests.csproj b/NavisworksTransport.UnitTests.csproj
index c78f83f..9408696 100644
--- a/NavisworksTransport.UnitTests.csproj
+++ b/NavisworksTransport.UnitTests.csproj
@@ -66,6 +66,7 @@
+
diff --git a/TransportPlugin.csproj b/TransportPlugin.csproj
index 88dde49..95c37bc 100644
--- a/TransportPlugin.csproj
+++ b/TransportPlugin.csproj
@@ -344,6 +344,7 @@
+
diff --git a/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs b/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
new file mode 100644
index 0000000..0220b1a
--- /dev/null
+++ b/UnitTests/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcherTests.cs
@@ -0,0 +1,223 @@
+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;
+
+ ///
+ /// 计算在 q 姿态下物体沿 hostSide/hostUp 的截面投影面积。
+ /// 与 Searcher 内部 EvaluateCrossSectionArea 公式一致，用于独立验证搜索结果。
+ ///
+ 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;
+ }
+
+ ///
+ /// 从结果四元数反算绕 hostUp 的 deltaYaw（度）。
+ /// deltaQ = inverse(faceDown) * result；其旋转轴应近似 hostUp。
+ ///
+ 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);
+
+ 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);
+
+ 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);
+
+ 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);
+
+ // 验证：搜索结果的截面面积 <= 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);
+
+ // 验证：搜索结果面积 <= 不搜索(只用 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
+
+ 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);
+
+ // 立方体任何 yaw 面积相同，只要返回有效四元数即可
+ Assert.AreEqual(1.0, Quaternion.Normalize(q).Length(), 1e-5);
+ }
+ }
+}
diff --git a/doc/requirement/todo_features.md b/doc/requirement/todo_features.md
index 9440cb7..65cd59a 100644
--- a/doc/requirement/todo_features.md
+++ b/doc/requirement/todo_features.md
@@ -2,6 +2,12 @@
## 功能点
+### [2026/6/10]
+
+1. [ ] （功能）对任意三维姿态的模型，点选面自动贴合地面
+2. [ ] （功能）自定义分层属性，使用配置文件
+3. [x] （优化）用单点取面，取代Rail路径中的三点取端面和2点取安装面
+
### [2026/5/27]
1. [x] （功能）对任意三维姿态的模型，自动调整最小投影到路径方向
diff --git a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
index 4e8473a..bdd53a5 100644
--- a/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
+++ b/src/UI/WPF/ViewModels/AnimationControlViewModel.cs
@@ -2299,12 +2299,13 @@ namespace NavisworksTransport.UI.WPF.ViewModels
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));
- // 路径 yaw 补偿
+ // 贴地后绕宿主 Up 搜索 yaw，使物体在路径方向的截面积最小
+ // （与"自动调整"同目标，但贴地已固定俯仰/翻滚，只剩 yaw 单自由度）
var routePoints = CurrentPathRoute?.Points;
+ double? searchedYawDegreesForLog = null;
if ((CurrentPathRoute?.PathType == PathType.Ground || CurrentPathRoute?.PathType == PathType.Hoisting)
&& routePoints != null && routePoints.Count >= 2)
{
- // 照抄 TryResolveStartPathDirection 的取点方式
var start = routePoints[0];
var startVector = new Vector3((float)start.X, (float)start.Y, (float)start.Z);
Vector3 hostForward = Vector3.Zero;
@@ -2318,12 +2319,34 @@ namespace NavisworksTransport.UI.WPF.ViewModels
break;
}
}
- if (hostForward.LengthSquared() > 1e-8f
- && PathTargetFrameResolver.TryResolvePlanarHostYaw(
- hostForward, CoordinateSystemManager.Instance.ResolvedType, out double yawRadians))
+ if (hostForward.LengthSquared() > 1e-8f)
{
- overrideQ = Quaternion.Normalize(
- Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)yawRadians) * overrideQ);
+ 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);
+
+ // 从结果反算实际 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;
}
}
@@ -2354,7 +2377,12 @@ namespace NavisworksTransport.UI.WPF.ViewModels
_objectRotationCorrection = resultCorrection;
OnPropertyChanged(nameof(ObjectRotationCorrection));
- 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}) correction={_objectRotationCorrection}");
+ 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"
diff --git a/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs b/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
new file mode 100644
index 0000000..1acdffb
--- /dev/null
+++ b/src/Utils/CoordinateSystem/AlignToGroundMinCrossSectionYawSearcher.cs
@@ -0,0 +1,131 @@
+using System;
+using System.Numerics;
+
+namespace NavisworksTransport.Utils.CoordinateSystem
+{
+ ///
+ /// 贴合地面后的最小截面 yaw 搜索器。
+ /// 在贴地旋转(把用户选定的面旋到 -hostUp)基础上，绕宿主 Up 轴搜索一个 yaw，
+ /// 使物体在"垂直路径方向 × 沿宿主 Up"截面上的投影面积最小。
+ /// 与 ObjectPassageProjectionOptimizer 同目标，但只在 yaw 单自由度上搜索——
+ /// 贴地旋转已固定俯仰/翻滚，只剩绕垂直轴的旋转自由度。
+ /// 适用于 CAD 姿态天然歪斜、无法靠"假设 local +X 是长轴"对齐路径的物体。
+ ///
+ public static class AlignToGroundMinCrossSectionYawSearcher
+ {
+ private const double CoarseStepDegrees = 5.0;
+ private const double FineStepDegrees = 1.0;
+ private const float HorizontalEpsilon = 1e-8f;
+
+ ///
+ /// 在贴地旋转基础上绕宿主 Up 搜索 yaw，使物体截面投影面积最小。
+ ///
+ /// 贴地旋转(把面法线旋到 -hostUp)，宿主世界空间。
+ /// CAD/复位姿态旋转，宿主世界空间。
+ /// 物体 local +X 方向尺寸(模型单位)。
+ /// 物体 local +Y 方向尺寸(模型单位)。
+ /// 物体 local +Z 方向尺寸(模型单位)。
+ /// 路径起点方向(宿主世界)。
+ /// 宿主坐标系适配器。
+ /// 合成后的宿主世界四元数(已归一化)：yawQ * faceDownRotation。
+ public static Quaternion Search(
+ Quaternion faceDownRotation,
+ Quaternion cadRotation,
+ double sizeX,
+ double sizeY,
+ double sizeZ,
+ Vector3 hostPathForward,
+ HostCoordinateAdapter adapter)
+ {
+ 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, hostSide, hostUp, sizeX, sizeY, sizeZ);
+ if (area < bestArea)
+ {
+ bestArea = area;
+ bestYawRad = yawRad;
+ }
+ }
+
+ // 细搜 [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, hostSide, hostUp, sizeX, sizeY, sizeZ);
+ if (area < bestArea)
+ {
+ bestArea = area;
+ bestYawRad = yawRad;
+ }
+ }
+
+ return Quaternion.Normalize(
+ Quaternion.CreateFromAxisAngle(hostUp, (float)bestYawRad) * faceDownRotation);
+ }
+
+ ///
+ /// 计算给定 deltaYaw 下物体截面投影面积。
+ /// 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式：
+ /// width = Σ |localAxis · hostSide| * sizeAxis (垂直路径方向宽度)
+ /// height = Σ |localAxis · hostUp | * sizeAxis (沿宿主 Up 高度)
+ /// area = width * height
+ ///
+ private static double EvaluateCrossSectionArea(
+ Quaternion faceDownRotation,
+ Quaternion cadRotation,
+ double deltaYawRadians,
+ Vector3 hostSide,
+ Vector3 hostUp,
+ double sizeX,
+ double sizeY,
+ double sizeZ)
+ {
+ 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));
+
+ 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;
+ }
+ }
+}