tian
f86e8c8bf4
贴地旋转已固定俯仰/翻滚，只剩绕宿主 Up 的 yaw 自由度。 对 CAD 姿态天然歪斜的物体，假设 local +X 是长轴会失败， 改为搜索 [0,360) 使物体在路径方向的截面投影面积最小， 与自动调整同目标但单自由度搜索更稳定。 - 新增 AlignToGroundMinCrossSectionYawSearcher（粗搜5°+细搜1°） - 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式 - 新增 7 个单测覆盖平地/倾斜/CAD歪斜/垂直退化/立方体 - 经多物体实测验证：Chair Lounge Couch / Chair Sitting Square 均正确对齐
224 lines
12 KiB
C#
224 lines
12 KiB
C#
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);
|
||
}
|
||
}
|
||
}
|