NavisworksTransport/UnitTests/CoordinateSystem/CanonicalRailOffsetResolverTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class CanonicalRailOffsetResolverTests
    {
        [TestMethod]
        public void OverRail_ShouldOffsetTrackedCenterAlongNormalByHalfHeight()
        {
            PathRoute route = new PathRoute
            {
                PathType = PathType.Rail,
                RailMountMode = RailMountMode.OverRail,
                RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine
            };

            RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                Vector3.Zero,
                frame,
                4.0);

            AssertVector(trackedCenter, 0.0, 0.0, 2.0);
        }

        [TestMethod]
        public void UnderRail_ShouldOffsetTrackedCenterOppositeToNormalByHalfHeight()
        {
            PathRoute route = new PathRoute
            {
                PathType = PathType.Rail,
                RailMountMode = RailMountMode.UnderRail,
                RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine
            };

            RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                Vector3.Zero,
                frame,
                4.0);

            AssertVector(trackedCenter, 0.0, 0.0, -2.0);
        }

        [TestMethod]
        public void SlopedNormal_ShouldMoveTrackedCenterAlongRailNormalInsteadOfWorldUp()
        {
            PathRoute route = new PathRoute
            {
                PathType = PathType.Rail,
                RailMountMode = RailMountMode.OverRail,
                RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine
            };

            Vector3 normal = Vector3.Normalize(new Vector3(0f, 1f, 1f));
            RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, normal);

            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                new Vector3(10f, 20f, 30f),
                frame,
                4.0);

            Vector3 expected = new Vector3(10f, 20f, 30f) + normal * 2f;
            AssertVector(trackedCenter, expected.X, expected.Y, expected.Z);
        }

        [TestMethod]
        public void AnchorSemantic_ShouldOnlyUseHalfHeightOffset_WhenReferencePointIsAlreadyAnchor()
        {
            PathRoute route = new PathRoute
            {
                PathType = PathType.Rail,
                RailMountMode = RailMountMode.OverRail,
                RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine
            };

            RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                Vector3.Zero,
                frame,
                4.0);

            AssertVector(trackedCenter, 0.0, 0.0, 2.0);
        }

        [TestMethod]
        public void RailNormalOffset_ShouldAddExtraDisplacementAlongRailNormal()
        {
            PathRoute route = new PathRoute
            {
                PathType = PathType.Rail,
                RailMountMode = RailMountMode.OverRail,
                RailPathDefinitionMode = RailPathDefinitionMode.RailCenterLine,
                RailNormalOffset = 1.5
            };

            RailLocalFrame frame = new RailLocalFrame(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                Vector3.Zero,
                frame,
                4.0);

            AssertVector(trackedCenter, 0.0, 0.0, 3.5);
        }

        [TestMethod]
        public void PreservedTrackedCenterOffset_ShouldBeMeasuredFromRailSemanticCenter()
        {
            Vector3 semanticTrackedCenter = new Vector3(10f, 20f, 30f);
            Vector3 actualTrackedCenter = new Vector3(11.25f, 19.5f, 30.75f);

            Vector3 offset = RailPathPoseHelper.CalculatePreservedTrackedCenterOffset(
                actualTrackedCenter,
                semanticTrackedCenter);

            AssertVector(offset, 1.25, -0.5, 0.75);
        }

        [TestMethod]
        public void ResolvePreservedTrackedCenterPosition_ShouldReuseRailSemanticCenterPlusOffset()
        {
            Vector3 semanticTrackedCenter = new Vector3(3f, -2f, 8f);
            Vector3 preservedOffset = new Vector3(0.2f, -0.4f, 0.6f);

            Vector3 preservedTrackedCenter = RailPathPoseHelper.ResolvePreservedTrackedCenterPosition(
                semanticTrackedCenter,
                preservedOffset);

            AssertVector(preservedTrackedCenter, 3.2, -2.4, 8.6);
        }

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