fix: 贴合地面后绕垂直轴搜索最小截面 yaw，对齐路径方向
贴地旋转已固定俯仰/翻滚，只剩绕宿主 Up 的 yaw 自由度。 对 CAD 姿态天然歪斜的物体，假设 local +X 是长轴会失败， 改为搜索 [0,360) 使物体在路径方向的截面投影面积最小， 与自动调整同目标但单自由度搜索更稳定。 - 新增 AlignToGroundMinCrossSectionYawSearcher（粗搜5°+细搜1°） - 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式 - 新增 7 个单测覆盖平地/倾斜/CAD歪斜/垂直退化/立方体 - 经多物体实测验证：Chair Lounge Couch / Chair Sitting Square 均正确对齐
This commit is contained in:
parent
75afba7670
commit
f86e8c8bf4
@ -66,6 +66,7 @@
|
||||
<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,6 +344,7 @@
|
||||
<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" />
|
||||
|
||||
@ -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;
|
||||
|
||||
/// <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);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -2,6 +2,12 @@
|
||||
|
||||
## 功能点
|
||||
|
||||
### [2026/6/10]
|
||||
|
||||
1. [ ] （功能）对任意三维姿态的模型，点选面自动贴合地面
|
||||
2. [ ] （功能）自定义分层属性，使用配置文件
|
||||
3. [x] （优化）用单点取面，取代Rail路径中的三点取端面和2点取安装面
|
||||
|
||||
### [2026/5/27]
|
||||
|
||||
1. [x] （功能）对任意三维姿态的模型，自动调整最小投影到路径方向
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -0,0 +1,131 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
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>
|
||||
/// <returns>合成后的宿主世界四元数(已归一化)：yawQ * faceDownRotation。</returns>
|
||||
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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算给定 deltaYaw 下物体截面投影面积。
|
||||
/// 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式：
|
||||
/// width = Σ |localAxis · hostSide| * sizeAxis (垂直路径方向宽度)
|
||||
/// height = Σ |localAxis · hostUp | * sizeAxis (沿宿主 Up 高度)
|
||||
/// area = width * height
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user