tian
cf6b2ef959
ObjectPassageProjectionOptimizer.OptimizeWithEvaluator: - 初始化 bestScore 评估 correction=Zero 作为初始候选 - 旧逻辑 bestScore=Invalid 导致 Zero 从未被评估， 当 baseline 已对齐路径时随机采样必然替换它，refine 收敛不回 0 - 影响 3 个 Optimize 测试：baseline 已对齐时找不到 correction=0 ZUp 测试语义修正（4 个）： - LocalEulerRotationCorrection 语义：Y=up轴/yaw, Z=non-up轴 - ZUp 下 up=Z, non-up=Y，绕 Z(up) 转应使用 YDegrees 不是 ZDegrees - 旧测试在 ZUp 下把 ZDegrees 当字面 Z 轴，与实现语义矛盾
446 lines
21 KiB
C#
446 lines
21 KiB
C#
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
using System.Numerics;
|
||
using System;
|
||
using Autodesk.Navisworks.Api;
|
||
|
||
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||
{
|
||
[TestClass]
|
||
public class HostCoordinateAdapterTests
|
||
{
|
||
[TestMethod]
|
||
public void ZUp_PointRoundTrip_ShouldRemainUnchanged()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||
var hostPoint = new Vector3(1.5f, -2.5f, 3.5f);
|
||
|
||
Vector3 canonicalPoint = adapter.ToCanonicalPoint3(hostPoint);
|
||
Vector3 roundTripPoint = adapter.FromCanonicalPoint3(canonicalPoint);
|
||
|
||
AssertPoint(roundTripPoint, 1.5, -2.5, 3.5);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_PointAndVectorRoundTrip_ShouldMatchExpectedMapping()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var hostPoint = new Vector3(1.0f, 2.0f, 3.0f);
|
||
var hostVector = new Vector3(4.0f, 5.0f, 6.0f);
|
||
|
||
Vector3 canonicalPoint = adapter.ToCanonicalPoint3(hostPoint);
|
||
Vector3 canonicalVector = adapter.ToCanonicalVector3(hostVector);
|
||
|
||
AssertPoint(canonicalPoint, 1.0, -3.0, 2.0);
|
||
AssertVector(canonicalVector, 4.0, -6.0, 5.0);
|
||
|
||
Vector3 restoredPoint = adapter.FromCanonicalPoint3(canonicalPoint);
|
||
Vector3 restoredVector = adapter.FromCanonicalVector3(canonicalVector);
|
||
|
||
AssertPoint(restoredPoint, 1.0, 2.0, 3.0);
|
||
AssertVector(restoredVector, 4.0, 5.0, 6.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_BoundsRoundTrip_ShouldPreserveBounds()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var hostBounds = new CanonicalBounds3(new Vector3(-2.0f, 1.0f, 3.0f), new Vector3(5.0f, 7.0f, 11.0f));
|
||
|
||
CanonicalBounds3 canonicalBounds = adapter.ToCanonicalBounds3(hostBounds);
|
||
CanonicalBounds3 restoredBounds = adapter.FromCanonicalBounds3(canonicalBounds);
|
||
|
||
AssertPoint(restoredBounds.Min, -2.0, 1.0, 3.0);
|
||
AssertPoint(restoredBounds.Max, 5.0, 7.0, 11.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_CanonicalRotation_ShouldConvertToHostYUpBasis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||
Quaternion canonicalRotation = convention.CreateQuaternion(new Vector3(1, 0, 0), Vector3.UnitZ);
|
||
|
||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(canonicalRotation));
|
||
|
||
Assert.AreEqual(1.0, hostLinear.M11, 1e-6);
|
||
Assert.AreEqual(0.0, hostLinear.M21, 1e-6);
|
||
Assert.AreEqual(0.0, hostLinear.M31, 1e-6);
|
||
|
||
Assert.AreEqual(0.0, hostLinear.M12, 1e-6);
|
||
Assert.AreEqual(0.0, hostLinear.M22, 1e-6);
|
||
Assert.AreEqual(-1.0, hostLinear.M32, 1e-6);
|
||
|
||
Assert.AreEqual(0.0, hostLinear.M13, 1e-6);
|
||
Assert.AreEqual(1.0, hostLinear.M23, 1e-6);
|
||
Assert.AreEqual(0.0, hostLinear.M33, 1e-6);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_CanonicalRotation_WithPlanarForward_ShouldProduceExpectedHostAxes()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||
|
||
Vector3 hostForward = Vector3.Normalize(new Vector3(-0.997320f, 0.0f, 0.073164f));
|
||
Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward);
|
||
|
||
bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
canonicalForward,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
convention,
|
||
out Quaternion canonicalRotation);
|
||
|
||
Assert.IsTrue(ok);
|
||
|
||
Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);
|
||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(canonicalLinear);
|
||
Quaternion hostQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
|
||
Matrix4x4 reconstructedHostLinear = Matrix4x4.CreateFromQuaternion(hostQuaternion);
|
||
|
||
Assert.AreEqual(hostForward.X, reconstructedHostLinear.M11, 1e-3);
|
||
Assert.AreEqual(hostForward.Y, reconstructedHostLinear.M21, 1e-3);
|
||
Assert.AreEqual(hostForward.Z, reconstructedHostLinear.M31, 1e-3);
|
||
|
||
Assert.AreEqual(0.0, reconstructedHostLinear.M13, 1e-3);
|
||
Assert.AreEqual(1.0, reconstructedHostLinear.M23, 1e-3);
|
||
Assert.AreEqual(0.0, reconstructedHostLinear.M33, 1e-3);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_HostLinear_FromActualGroundPathPoints_ShouldMatchExpectedHostAxes()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);
|
||
|
||
Vector3 previousPoint = new Vector3(-181.82637032765f, 14.8333330154419f, 3.38710203888167f);
|
||
Vector3 currentPoint = new Vector3(-181.82637032765f, 14.8333330154419f, 3.38710203888167f);
|
||
Vector3 nextPoint = new Vector3(-202.828908853644f, 16.3772966612769f, 14.304117291825f);
|
||
|
||
Vector3 canonicalPrevious = adapter.ToCanonicalPoint3(previousPoint);
|
||
Vector3 canonicalCurrent = adapter.ToCanonicalPoint3(currentPoint);
|
||
Vector3 canonicalNext = adapter.ToCanonicalPoint3(nextPoint);
|
||
|
||
Vector3 canonicalForward = canonicalNext - canonicalPrevious;
|
||
Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||
canonicalForward,
|
||
HostCoordinateAdapter.CanonicalUpVector3,
|
||
convention,
|
||
out Quaternion canonicalRotation));
|
||
|
||
Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);
|
||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(canonicalLinear);
|
||
|
||
Assert.AreEqual(-0.8873, hostLinear.M11, 1e-3);
|
||
Assert.AreEqual(0.0000, hostLinear.M21, 1e-3);
|
||
Assert.AreEqual(0.4612, hostLinear.M31, 1e-3);
|
||
|
||
Assert.AreEqual(0.4612, hostLinear.M12, 1e-3);
|
||
Assert.AreEqual(0.0000, hostLinear.M22, 1e-3);
|
||
Assert.AreEqual(0.8873, hostLinear.M32, 1e-3);
|
||
|
||
Assert.AreEqual(0.0000, hostLinear.M13, 1e-3);
|
||
Assert.AreEqual(1.0000, hostLinear.M23, 1e-3);
|
||
Assert.AreEqual(0.0000, hostLinear.M33, 1e-3);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_HostRotationCorrection_ShouldMapHostYAxisToCanonicalUp()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
|
||
|
||
Quaternion canonicalCorrection = adapter.CreateCanonicalRotationCorrection(correction);
|
||
Vector3 rotatedForward = Vector3.Transform(Vector3.UnitX, canonicalCorrection);
|
||
|
||
Assert.AreEqual(0.0, rotatedForward.X, 1e-6);
|
||
Assert.AreEqual(1.0, rotatedForward.Y, 1e-6);
|
||
Assert.AreEqual(0.0, rotatedForward.Z, 1e-6);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_HostRotationCorrection_ShouldKeepHostYAxisInvariantInHostSpace()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||
|
||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitY, hostCorrection);
|
||
AssertVector(rotatedUp, 0.0, 1.0, 0.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_HostRotationCorrection_X90_ShouldRotateHostZIntoHostY()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||
new LocalEulerRotationCorrection(90.0, 0.0, 0.0));
|
||
|
||
Vector3 rotatedHostZ = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
||
AssertVector(rotatedHostZ, 0.0, -1.0, 0.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_HostRotationCorrection_Z90_ShouldRotateHostXIntoNegativeHostY()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||
new LocalEulerRotationCorrection(0.0, 0.0, 90.0));
|
||
|
||
Vector3 rotatedHostX = Vector3.Transform(Vector3.UnitX, hostCorrection);
|
||
AssertVector(rotatedHostX, 0.0, 1.0, 0.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void ComposeHostQuaternion_ShouldApplyHostCorrectionAfterBaseline()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
var baseline = Quaternion.Identity;
|
||
|
||
Quaternion composed = adapter.ComposeHostQuaternion(
|
||
baseline,
|
||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
Assert.AreEqual(0.0, linear.M11, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M21, 1e-6);
|
||
Assert.AreEqual(-1.0, linear.M31, 1e-6);
|
||
|
||
Assert.AreEqual(0.0, linear.M12, 1e-6);
|
||
Assert.AreEqual(1.0, linear.M22, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M32, 1e-6);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void ZUp_HostRotationCorrection_YDegrees_ShouldKeepHostZAxisInvariant()
|
||
{
|
||
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转，Z 轴自身不变。
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(
|
||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||
|
||
Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection);
|
||
AssertVector(rotatedUp, 0.0, 0.0, 1.0);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void ZUp_ComposeHostQuaternion_YDegrees_ShouldApplyHostZAxisRotation()
|
||
{
|
||
// ZUp 下 YDegrees = up 轴(=Z)。绕 Z 转 90°：X→Y, Y→-X, Z 不变。
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
|
||
var baseline = Quaternion.Identity;
|
||
|
||
Quaternion composed = adapter.ComposeHostQuaternion(
|
||
baseline,
|
||
new LocalEulerRotationCorrection(0.0, 90.0, 0.0));
|
||
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
Assert.AreEqual(0.0, linear.M11, 1e-6);
|
||
Assert.AreEqual(1.0, linear.M21, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M31, 1e-6);
|
||
|
||
Assert.AreEqual(-1.0, linear.M12, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M22, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M32, 1e-6);
|
||
|
||
Assert.AreEqual(0.0, linear.M13, 1e-6);
|
||
Assert.AreEqual(0.0, linear.M23, 1e-6);
|
||
Assert.AreEqual(1.0, linear.M33, 1e-6);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostXAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
|
||
Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(90.0, 0.0, 0.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostYAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
|
||
Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForGroundBaseline_ShouldRotateAxesAroundHostZAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.8987f, 0.0000f, -0.4386f);
|
||
Vector3 baselineY = new Vector3(0.0000f, 1.0000f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.4386f, 0.0000f, -0.8987f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(0.0, 0.0, 90.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForRailBaseline_ShouldRotateAxesAroundHostXAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.9900f, 0.1403f, -0.0163f);
|
||
Vector3 baselineY = new Vector3(0.1403f, 0.9901f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.0161f, -0.0023f, -0.9999f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(90.0, 0.0, 0.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForRailBaseline_ShouldRotateAxesAroundHostYAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.9900f, 0.1403f, -0.0163f);
|
||
Vector3 baselineY = new Vector3(0.1403f, 0.9901f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.0161f, -0.0023f, -0.9999f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void YUp_ComposeHostQuaternion_ForRailBaseline_ShouldRotateAxesAroundHostZAxis()
|
||
{
|
||
var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
|
||
Vector3 baselineX = new Vector3(-0.9900f, 0.1403f, -0.0163f);
|
||
Vector3 baselineY = new Vector3(0.1403f, 0.9901f, 0.0000f);
|
||
Vector3 baselineZ = new Vector3(0.0161f, -0.0023f, -0.9999f);
|
||
Quaternion baseline = CreateQuaternionFromAxes(baselineX, baselineY, baselineZ);
|
||
|
||
var correction = new LocalEulerRotationCorrection(0.0, 0.0, 90.0);
|
||
Quaternion composed = adapter.ComposeHostQuaternion(baseline, correction);
|
||
Quaternion hostCorrection = adapter.CreateHostRotationCorrection(correction);
|
||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(composed);
|
||
|
||
AssertAxis(linear, 1, Vector3.Transform(baselineX, hostCorrection));
|
||
AssertAxis(linear, 2, Vector3.Transform(baselineY, hostCorrection));
|
||
AssertAxis(linear, 3, Vector3.Transform(baselineZ, hostCorrection));
|
||
}
|
||
|
||
[TestMethod]
|
||
public void RemapHostSemanticCorrectionToLocalAxes_ShouldPermuteAnglesFromSemanticAxes()
|
||
{
|
||
LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||
new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
|
||
LocalAxisDirection.PositiveY,
|
||
LocalAxisDirection.PositiveZ,
|
||
LocalAxisDirection.PositiveX);
|
||
|
||
Assert.AreEqual(30.0, mapped.XDegrees, 1e-9);
|
||
Assert.AreEqual(10.0, mapped.YDegrees, 1e-9);
|
||
Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9);
|
||
}
|
||
|
||
[TestMethod]
|
||
public void RemapHostSemanticCorrectionToLocalAxes_ShouldApplySignForNegativeMappedAxes()
|
||
{
|
||
LocalEulerRotationCorrection mapped = HostCoordinateAdapter.RemapHostSemanticCorrectionToLocalAxes(
|
||
new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
|
||
LocalAxisDirection.NegativeY,
|
||
LocalAxisDirection.PositiveZ,
|
||
LocalAxisDirection.NegativeX);
|
||
|
||
Assert.AreEqual(-30.0, mapped.XDegrees, 1e-9);
|
||
Assert.AreEqual(-10.0, mapped.YDegrees, 1e-9);
|
||
Assert.AreEqual(20.0, mapped.ZDegrees, 1e-9);
|
||
}
|
||
|
||
private static void AssertPoint(Vector3 point, double x, double y, double z)
|
||
{
|
||
Assert.AreEqual(x, point.X, 1e-9);
|
||
Assert.AreEqual(y, point.Y, 1e-9);
|
||
Assert.AreEqual(z, point.Z, 1e-9);
|
||
}
|
||
|
||
private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6)
|
||
{
|
||
Assert.AreEqual(x, vector.X, tolerance);
|
||
Assert.AreEqual(y, vector.Y, tolerance);
|
||
Assert.AreEqual(z, vector.Z, tolerance);
|
||
}
|
||
|
||
private static Quaternion CreateQuaternionFromAxes(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
|
||
{
|
||
Matrix4x4 basis = new Matrix4x4(
|
||
xAxis.X, yAxis.X, zAxis.X, 0f,
|
||
xAxis.Y, yAxis.Y, zAxis.Y, 0f,
|
||
xAxis.Z, yAxis.Z, zAxis.Z, 0f,
|
||
0f, 0f, 0f, 1f);
|
||
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(basis));
|
||
}
|
||
|
||
private static void AssertAxis(Matrix4x4 linear, int axisIndex, Vector3 expectedAxis, double tolerance = 1e-4)
|
||
{
|
||
Vector3 normalizedExpected = Vector3.Normalize(expectedAxis);
|
||
switch (axisIndex)
|
||
{
|
||
case 1:
|
||
Assert.AreEqual(normalizedExpected.X, linear.M11, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Y, linear.M21, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Z, linear.M31, tolerance);
|
||
break;
|
||
case 2:
|
||
Assert.AreEqual(normalizedExpected.X, linear.M12, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Y, linear.M22, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Z, linear.M32, tolerance);
|
||
break;
|
||
case 3:
|
||
Assert.AreEqual(normalizedExpected.X, linear.M13, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Y, linear.M23, tolerance);
|
||
Assert.AreEqual(normalizedExpected.Z, linear.M33, tolerance);
|
||
break;
|
||
default:
|
||
Assert.Fail($"未知轴索引: {axisIndex}");
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|