NavisworksTransport/UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RealObjectRailAxisConventionResolverTests
    {
        [TestMethod]
        public void YUp_DefaultAxis_ShouldChoosePositiveXAndPositiveYForRail()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
            Vector3 desiredUp = Vector3.UnitY;

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                LocalAxisDirection.PositiveY,
                desiredForward,
                CoordinateSystemType.YUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out Vector3 selectedForwardWorldAxis,
                out LocalAxisDirection selectedUpAxis,
                out Vector3 selectedUpWorldAxis);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, selectedUpAxis);
            AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0);
            AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0);
        }

        [TestMethod]
        public void ZUp_DefaultAxis_ShouldChoosePositiveXAndPositiveZForRail()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.9f, 0.3f, 0.0f));
            Vector3 desiredUp = Vector3.UnitZ;

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                LocalAxisDirection.PositiveZ,
                desiredForward,
                CoordinateSystemType.ZUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out _,
                out LocalAxisDirection selectedUpAxis,
                out Vector3 selectedUpWorldAxis);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
            AssertVector(selectedUpWorldAxis, 0.0, 0.0, 1.0);
        }

        [TestMethod]
        public void YUp_ShouldNotSelectYAxisFamilyAsForwardCandidate()
        {
            Vector3 referenceAxisX = new Vector3(1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(0.0f, 1.0f, 0.01f));
            Vector3 desiredUp = Vector3.UnitY;

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                LocalAxisDirection.PositiveY,
                desiredForward,
                CoordinateSystemType.YUp,
                out _,
                out LocalAxisDirection selectedForwardAxis,
                out _,
                out _,
                out _);

            Assert.IsTrue(ok);
            Assert.AreNotEqual(LocalAxisDirection.PositiveY, selectedForwardAxis);
            Assert.AreNotEqual(LocalAxisDirection.NegativeY, selectedForwardAxis);
        }

        [TestMethod]
        public void YUp_WhenRemainingZAxisBestMatchesDesiredUp_ShouldChooseZAsUp()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 desiredForward = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 desiredUp = new Vector3(0.0f, 1.0f, 0.0f);

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                LocalAxisDirection.PositiveZ,
                desiredForward,
                CoordinateSystemType.YUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out Vector3 selectedForwardWorldAxis,
                out LocalAxisDirection selectedUpAxis,
                out Vector3 selectedUpWorldAxis);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
            AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0);
            AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0);
        }

        [TestMethod]
        public void YUp_ShouldUseMappedHostUpLocalAxis_AndSelectForwardFromRemainingAxes()
        {
            Vector3 referenceAxisX = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(0.99f, -0.14f, -0.02f));

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                LocalAxisDirection.PositiveZ,
                desiredForward,
                CoordinateSystemType.YUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out Vector3 selectedForwardWorldAxis,
                out LocalAxisDirection selectedUpAxis,
                out Vector3 selectedUpWorldAxis);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
            Assert.AreEqual(LocalAxisDirection.NegativeY, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.NegativeY, convention.ForwardAxis);
            AssertVector(selectedUpWorldAxis, -1.0, 0.0, 0.0);
            AssertVector(selectedForwardWorldAxis, 0.0, -1.0, 0.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);
        }
    }
}