tian
66e68c6bd1
贴地后 overrideQ 是绝对旋转，但 _objectRotationCorrection 语义是 相对于 baseline(pathYaw) 的增量，且 PathAnimationManager 增量链从 CAD 姿态出发：finalQ = qup(pathYaw-currentYaw) * correctionHostQ * cadQ。 之前直接转 overrideQ 导致确认/动画时重复旋转。 核心改动： - ComposeHostCorrection 接受 cadQ，从中提取 currentYaw， correctionHostQ = qup(currentYaw - pathYaw) * overrideQ - HostQuaternionToCanonical 用相似变换 R_canon = M^-1 * R_host * M （非列向量映射，保证 Identity 不变性） - CanonicalQuaternionToHostEulerCorrection 加 hostType 参数重载， 去除全局 CoordinateSystemManager 依赖 - ComputeGroundLiftForCorrection：与自动调整同方法实测底边算 lift， finally 用新 correction+lift 完整重建（skipCadRestore=false）， 使物体停留在贴地等价姿态 - 新增 5 个增量链闭环单测（含 CAD 歪斜场景）
425 lines
22 KiB
C#
425 lines
22 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);
|
||
}
|
||
|
||
// ----- 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);
|
||
|
||
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}");
|
||
}
|
||
}
|
||
}
|
||
}
|