NavisworksTransport/UnitTests/CoordinateSystem/CanonicalPlanarPoseBuilderTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class CanonicalPlanarPoseBuilderTests
    {
        [TestMethod]
        public void StraightForward_ZUpConvention_ShouldKeepLocalZAsWorldUp()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 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 StraightForward_YUpConvention_ShouldMapLocalYToWorldUp()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 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 ForwardWithVerticalComponent_ShouldProjectToHorizontalPlane()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(10, 0, 3),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            Assert.AreEqual(0.0, linear.M31, 1e-6);
            Assert.AreEqual(0.0, linear.M32, 1e-6);
            Assert.AreEqual(1.0, linear.M33, 1e-6);
        }

        [TestMethod]
        public void ZeroLocalEulerCorrection_ShouldMatchCurrentPlanarBaseline()
        {
            var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp);

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                convention,
                out Quaternion baselineRotation));

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                convention,
                LocalEulerRotationCorrection.Zero,
                out Quaternion correctedRotation));

            AssertQuaternionEquivalent(baselineRotation, correctedRotation);
        }

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

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                convention,
                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 WorldCorrection_ShouldRotateBaselinePoseAroundHostYAxisInYUp()
        {
            var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion worldCorrection = adapter.CreateCanonicalRotationCorrection(
                new LocalEulerRotationCorrection(0.0, 90.0, 0.0));

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                convention,
                out Quaternion baselineRotation));

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForwardWithWorldCorrection(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                convention,
                worldCorrection,
                out Quaternion correctedRotation));

            Matrix4x4 baseline = Matrix4x4.CreateFromQuaternion(baselineRotation);

            AssertColumn(baseline, 0, 1, 0, 0);
            AssertColumn(baseline, 1, 0, 0, 1);
            AssertColumn(baseline, 2, 0, -1, 0);

            Quaternion expectedRotation = Quaternion.Normalize(worldCorrection * baselineRotation);
            AssertQuaternionEquivalent(expectedRotation, correctedRotation);
        }

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