NavisworksTransport/UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class CanonicalRailPoseBuilderTests
    {
        [TestMethod]
        public void StraightCanonicalPath_ZUpConvention_ShouldProduceIdentityLikeBasis()
        {
            bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            AssertColumn(linear, 0, 1, 0, 0);
            AssertColumn(linear, 1, 0, 1, 0);
            AssertColumn(linear, 2, 0, 0, 1);
        }

        [TestMethod]
        public void StraightCanonicalPath_YUpConvention_ShouldMapLocalYToCanonicalUp()
        {
            bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            AssertColumn(linear, 0, 1, 0, 0);
            AssertColumn(linear, 1, 0, 0, 1);
            AssertColumn(linear, 2, 0, -1, 0);
        }

        [TestMethod]
        public void SlopedPath_ShouldKeepForwardAlignedWithPathAndUpOrthogonalized()
        {
            bool ok = CanonicalRailPoseBuilder.TryCreateBasis(
                new Vector3(0, 0, 0),
                new Vector3(1, 1, 1),
                new Vector3(2, 2, 2),
                Vector3.UnitZ,
                out Vector3 forward,
                out Vector3 lateral,
                out Vector3 up);

            Assert.IsTrue(ok);
            Assert.AreEqual(1.0, forward.Length(), 1e-6);
            Assert.AreEqual(1.0, lateral.Length(), 1e-6);
            Assert.AreEqual(1.0, up.Length(), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(forward, up), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(forward, lateral), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(lateral, up), 1e-6);
        }

        [TestMethod]
        public void SlopedPath_ShouldCreateRailLocalFrameWithStableNormalCloseToCanonicalUp()
        {
            bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame(
                new Vector3(0, 0, 0),
                new Vector3(10, 0, 1),
                new Vector3(20, 0, 2),
                Vector3.UnitZ,
                out RailLocalFrame frame);

            Assert.IsTrue(ok);
            Assert.IsTrue(Vector3.Dot(frame.Forward, Vector3.UnitX) > 0.99f);
            Assert.IsTrue(Vector3.Dot(frame.Normal, Vector3.UnitZ) > 0.99f);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Normal), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Lateral), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Lateral, frame.Normal), 1e-6);
        }

        [TestMethod]
        public void ZeroLocalUpRotation_ShouldMatchCurrentRailBaseline()
        {
            var convention = ModelAxisConvention.CreateRailAssetConvention();

            Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                convention,
                out Quaternion baselineRotation));

            Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                convention,
                0.0,
                out Quaternion correctedRotation));

            AssertQuaternionEquivalent(baselineRotation, correctedRotation);
        }

        [TestMethod]
        public void LocalUpRotation_ShouldAlignCorrectedLocalForwardWithRailTangent()
        {
            var convention = ModelAxisConvention.CreateRailAssetConvention();

            Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                convention,
                90.0,
                out Quaternion rotation));

            Quaternion localPreRotation = Quaternion.CreateFromAxisAngle(
                Vector3.Normalize(convention.UpUnitVector),
                (float)(90.0 * Math.PI / 180.0));

            Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation));
            Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation));
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);

            Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward);
            Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp);

            AssertVector(mappedForward, 1, 0, 0);
            AssertVector(mappedUp, 0, 0, 1);
        }

        [TestMethod]
        public void LocalEulerCorrection_ShouldAlignCorrectedLocalForwardWithRailTangent()
        {
            var convention = ModelAxisConvention.CreateRailAssetConvention();
            var correction = new LocalEulerRotationCorrection(0.0, 90.0, 0.0);
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(correction);

            Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                convention,
                null,
                correctionQuaternion,
                out Quaternion rotation));

            LocalAxisPoseBuilder.ApplyLocalPreRotation(convention, correctionQuaternion, out var correctedLocalForward, out var correctedLocalUp);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);

            Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward);
            Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp);

            AssertVector(mappedForward, 1, 0, 0);
            AssertVector(mappedUp, 0, 0, 1);
        }

        [TestMethod]
        public void PreferredNormal_ShouldOverrideCanonicalUpForRailFrame()
        {
            bool ok = CanonicalRailPoseBuilder.TryCreateLocalFrame(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                Vector3.UnitY,
                out RailLocalFrame frame);

            Assert.IsTrue(ok);
            AssertVector(frame.Forward, 1, 0, 0);
            AssertVector(frame.Normal, 0, 1, 0);
            AssertVector(frame.Lateral, 0, 0, -1);
        }

        private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
        {
            switch (column)
            {
                case 0:
                    Assert.AreEqual(x, matrix.M11, 1e-6);
                    Assert.AreEqual(y, matrix.M21, 1e-6);
                    Assert.AreEqual(z, matrix.M31, 1e-6);
                    break;
                case 1:
                    Assert.AreEqual(x, matrix.M12, 1e-6);
                    Assert.AreEqual(y, matrix.M22, 1e-6);
                    Assert.AreEqual(z, matrix.M32, 1e-6);
                    break;
                case 2:
                    Assert.AreEqual(x, matrix.M13, 1e-6);
                    Assert.AreEqual(y, matrix.M23, 1e-6);
                    Assert.AreEqual(z, matrix.M33, 1e-6);
                    break;
                default:
                    Assert.Fail("Only first 3 columns are valid.");
                    break;
            }
        }

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

        private static void AssertQuaternionEquivalent(Quaternion expected, Quaternion actual)
        {
            double dot = Math.Abs(
                expected.X * actual.X +
                expected.Y * actual.Y +
                expected.Z * actual.Z +
                expected.W * actual.W);

            Assert.AreEqual(1.0, dot, 1e-6);
        }

        private static Vector3 TransformLocalVector(Matrix4x4 linear, Vector3 localVector)
        {
            Vector3 world = new Vector3(
                linear.M11 * localVector.X + linear.M12 * localVector.Y + linear.M13 * localVector.Z,
                linear.M21 * localVector.X + linear.M22 * localVector.Y + linear.M23 * localVector.Z,
                linear.M31 * localVector.X + linear.M32 * localVector.Y + linear.M33 * localVector.Z);
            return Vector3.Normalize(world);
        }
    }
}