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_ShouldKeepHostZAxisInvariantInHostSpace() { var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp); Quaternion hostCorrection = adapter.CreateHostRotationCorrection( new LocalEulerRotationCorrection(0.0, 0.0, 90.0)); Vector3 rotatedUp = Vector3.Transform(Vector3.UnitZ, hostCorrection); AssertVector(rotatedUp, 0.0, 0.0, 1.0); } [TestMethod] public void ZUp_ComposeHostQuaternion_ShouldApplyHostZCorrectionAfterBaseline() { var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp); var baseline = Quaternion.Identity; Quaternion composed = adapter.ComposeHostQuaternion( baseline, new LocalEulerRotationCorrection(0.0, 0.0, 90.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; } } } }