NavisworksTransport/UnitTests/CoordinateSystem/RealObjectReferencePoseResolverTests.cs

using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System.Collections.Generic;
using System.Numerics;

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RealObjectReferencePoseResolverTests
    {
        [TestMethod]
        public void YUp_HostWithFragmentDefaultZ_ShouldInterpretRepresentativeFrameToHostSemantics()
        {
            var fragmentMatrices = new List<double[]>
            {
                CreateMatrix(
                    axisX: new Vector3(1.0f, 0.0f, 0.0f),
                    axisY: new Vector3(0.0f, 1.0f, 0.0f),
                    axisZ: new Vector3(0.0f, 0.0f, 1.0f))
            };

            bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
                fragmentMatrices,
                fragmentDefaultUpAxis: "Z",
                hostUpAxis: "Y",
                out RealObjectReferencePose pose);

            Assert.IsTrue(ok);
            AssertVector(pose.RawAxisX, 1.0, 0.0, 0.0);
            AssertVector(pose.RawAxisY, 0.0, 1.0, 0.0);
            AssertVector(pose.RawAxisZ, 0.0, 0.0, 1.0);
            AssertVector(pose.AxisX, 1.0, 0.0, 0.0);
            AssertVector(pose.AxisY, 0.0, 0.0, 1.0);
            AssertVector(pose.AxisZ, 0.0, -1.0, 0.0);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldZAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostUpLocalAxis);
        }

        [TestMethod]
        public void YUp_WorldAxisMapping_ShouldFollowRawRepresentativeFrame()
        {
            var fragmentMatrices = new List<double[]>
            {
                CreateMatrix(
                    axisX: new Vector3(0.0f, 0.0f, 1.0f),
                    axisY: new Vector3(1.0f, 0.0f, 0.0f),
                    axisZ: new Vector3(0.0f, 1.0f, 0.0f))
            };

            bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
                fragmentMatrices,
                fragmentDefaultUpAxis: "Z",
                hostUpAxis: "Y",
                out RealObjectReferencePose pose);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostWorldXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldZAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostSemanticZAxisLocalAxis);
        }

        [TestMethod]
        public void DetectDefaultUpAxis_ShouldReturnZ_WhenRepresentativeZMatchesHostUp()
        {
            var fragmentMatrices = new List<double[]>
            {
                CreateMatrix(
                    axisX: new Vector3(1.0f, 0.0f, 0.0f),
                    axisY: new Vector3(0.0f, 0.0f, -1.0f),
                    axisZ: new Vector3(0.0f, 1.0f, 0.0f))
            };

            bool ok = RealObjectReferencePoseResolver.TryDetectDefaultUpAxis(
                fragmentMatrices,
                "Y",
                out string detectedAxis,
                out float yAlignment,
                out float zAlignment);

            Assert.IsTrue(ok);
            Assert.AreEqual("Z", detectedAxis);
            Assert.AreEqual(0.0, yAlignment, 1e-6);
            Assert.AreEqual(1.0, zAlignment, 1e-6);
        }

        [TestMethod]
        public void YUp_WorldAxisMapping_ShouldAcceptSlightlyRotatedRawFragmentFrame()
        {
            var fragmentMatrices = new List<double[]>
            {
                CreateMatrix(
                    axisX: Vector3.Normalize(new Vector3(0.9980f, 0.0638f, 0.0f)),
                    axisY: new Vector3(0.0f, 0.0f, -1.0f),
                    axisZ: Vector3.Normalize(new Vector3(-0.0638f, 0.9980f, 0.0f)))
            };

            bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
                fragmentMatrices,
                fragmentDefaultUpAxis: "Y",
                hostUpAxis: "Y",
                out RealObjectReferencePose pose);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostWorldXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostWorldYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.NegativeY, pose.HostWorldZAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, pose.HostSemanticXAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostSemanticYAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, pose.HostSemanticZAxisLocalAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, pose.HostUpLocalAxis);
        }

        [TestMethod]
        public void YUp_WorldAxisMapping_ShouldRejectAmbiguousRawFragmentFrame()
        {
            var fragmentMatrices = new List<double[]>
            {
                CreateMatrix(
                    axisX: Vector3.Normalize(new Vector3(0.80f, 0.60f, 0.0f)),
                    axisY: new Vector3(0.0f, 0.0f, -1.0f),
                    axisZ: Vector3.Normalize(new Vector3(-0.60f, 0.80f, 0.0f)))
            };

            bool ok = RealObjectReferencePoseResolver.TryResolveFromFragmentMatrices(
                fragmentMatrices,
                fragmentDefaultUpAxis: "Y",
                hostUpAxis: "Y",
                out RealObjectReferencePose pose);

            Assert.IsFalse(ok);
            Assert.IsNull(pose);
        }

        private static double[] CreateMatrix(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
        {
            return new double[]
            {
                axisX.X, axisX.Y, axisX.Z, 0.0,
                axisY.X, axisY.Y, axisY.Z, 0.0,
                axisZ.X, axisZ.Y, axisZ.Z, 0.0,
                0.0,     0.0,     0.0,     1.0
            };
        }

        private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
        {
            Assert.AreEqual(x, actual.X, tolerance);
            Assert.AreEqual(y, actual.Y, tolerance);
            Assert.AreEqual(z, actual.Z, tolerance);
        }
    }
}