tian
84f67dbd79
用户反馈：自由路径动画起点物体沿前进轴旋转90°（侧滚）， 期望像地面路径一样只转向前进方向、仅绕垂直轴旋转。 根因：TryCreateFreePathRotationForFrame/AtStart 用了 CanonicalRailPoseBuilder（完整三维姿态，forward=3D切线）， 会绕前进轴滚动。改为 CanonicalPlanarPoseBuilder （forward=切线水平投影，up=世界up，只转yaw）。 撤销此前 ShouldPreservePathRotationForFrames 让 Free 保留姿态的改动。 同步更新 FreePathTests 断言（斜路径投影水平、纯垂直无水平方向返回false）。 257 单测全过。
123 lines
4.7 KiB
C#
123 lines
4.7 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
using System;
|
||
using System.Numerics;
|
||
|
||
namespace NavisworksTransport.UnitTests.Core
|
||
{
|
||
[TestClass]
|
||
public class FreePathTests
|
||
{
|
||
// === PathType 枚举 ===
|
||
|
||
[TestMethod]
|
||
public void PathType_Free_ShouldHaveValue3()
|
||
{
|
||
Assert.AreEqual(3, (int)PathType.Free);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void PathType_Free_GetDisplayName_ShouldReturnFree()
|
||
{
|
||
Assert.AreEqual("自由", PathType.Free.GetDisplayName());
|
||
}
|
||
|
||
// === 自由路径旋转姿态（方案1：forward=切线, up=世界up正交化） ===
|
||
|
||
[TestMethod]
|
||
public void FreePathRotation_StraightHorizontal_ShouldAlignForwardToX_UpToWorldUp()
|
||
{
|
||
// ZUp 约定，水平直路：forward=+X，up=+Z（世界up）
|
||
bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
|
||
new Vector3(0, 0, 0),
|
||
new Vector3(1, 0, 0),
|
||
new Vector3(2, 0, 0),
|
||
Vector3.UnitZ,
|
||
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
||
null, // preferredNormal=null → 使用世界up
|
||
Quaternion.Identity,
|
||
out Quaternion rotation);
|
||
|
||
Assert.IsTrue(ok);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
||
// 局部 X → 世界 X（forward=切线）
|
||
AssertColumn(linear, 0, 1, 0, 0);
|
||
// 局部 Z → 世界 Z（up=世界up）
|
||
AssertColumn(linear, 2, 0, 0, 1);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void FreePathRotation_SlopedPath_ShouldProjectForwardToHorizontal_UpStaysWorldUp()
|
||
{
|
||
// 斜路径（沿X上升）：物体只绕垂直轴转 yaw，forward 投影到水平面，up 恒为世界Z（不侧滚）
|
||
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
new Vector3(2, 0, 1), // 切线（含垂直分量）
|
||
Vector3.UnitZ,
|
||
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
||
LocalEulerRotationCorrection.Zero,
|
||
out Quaternion rotation);
|
||
|
||
Assert.IsTrue(ok);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
|
||
// forward = 切线水平投影 = (1,0,0) → 局部X映射到世界X
|
||
AssertColumn(linear, 0, 1, 0, 0);
|
||
// up = 世界Z（不侧滚，关键：斜路径也不倾斜）
|
||
AssertColumn(linear, 2, 0, 0, 1);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void FreePathRotation_PureVerticalPath_ShouldFail_NoHorizontalDirection()
|
||
{
|
||
// 纯垂直路径无水平分量，无法确定 yaw，应返回 false（保持当前姿态）
|
||
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
new Vector3(0, 0, 2),
|
||
Vector3.UnitZ,
|
||
ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
|
||
LocalEulerRotationCorrection.Zero,
|
||
out _);
|
||
|
||
Assert.IsFalse(ok);
|
||
}
|
||
|
||
// === 自由路径几何中心对齐语义 ===
|
||
|
||
[TestMethod]
|
||
public void FreePath_CenterAlignment_ShouldNotApplyHeightOffset()
|
||
{
|
||
// 自由路径：路径点即物体中心，ResolveGroundTrackedCenter 的半高偏移不应用于自由路径
|
||
// 这里验证 CanonicalTrackedPositionResolver 的语义不影响自由路径
|
||
var resolver = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter(
|
||
new Vector3(5, 5, 5),
|
||
Vector3.UnitZ,
|
||
2.0);
|
||
// Ground 语义：中心上移半高
|
||
Assert.AreEqual(6.0, resolver.Z, 1e-6);
|
||
}
|
||
|
||
private static void AssertColumn(Matrix4x4 m, int col, float x, float y, float z)
|
||
{
|
||
switch (col)
|
||
{
|
||
case 0:
|
||
Assert.AreEqual(x, m.M11, 1e-5f);
|
||
Assert.AreEqual(y, m.M21, 1e-5f);
|
||
Assert.AreEqual(z, m.M31, 1e-5f);
|
||
break;
|
||
case 1:
|
||
Assert.AreEqual(x, m.M12, 1e-5f);
|
||
Assert.AreEqual(y, m.M22, 1e-5f);
|
||
Assert.AreEqual(z, m.M32, 1e-5f);
|
||
break;
|
||
case 2:
|
||
Assert.AreEqual(x, m.M13, 1e-5f);
|
||
Assert.AreEqual(y, m.M23, 1e-5f);
|
||
Assert.AreEqual(z, m.M33, 1e-5f);
|
||
break;
|
||
default:
|
||
Assert.Fail("无效列");
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|