NavisworksTransport/UnitTests/CoordinateSystem/HostCoordinateAdapterTests.cs

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);
        }

        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)
        {
            Assert.AreEqual(x, vector.X, 1e-9);
            Assert.AreEqual(y, vector.Y, 1e-9);
            Assert.AreEqual(z, vector.Z, 1e-9);
        }
    }
}