NavisworksTransport/UnitTests/CoordinateSystem/FragmentRepresentativePoseHelperTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class FragmentRepresentativePoseHelperTests
    {
        [TestMethod]
        public void ShouldAverageFragmentMatrices_AndIgnoreTranslation()
        {
            Quaternion expectedRotation = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0)));

            double[] fragmentMatrix1 = CreateFragmentMatrix(expectedRotation, new Vector3(10.0f, 20.0f, 30.0f));
            double[] fragmentMatrix2 = CreateFragmentMatrix(expectedRotation, new Vector3(-3.0f, 8.0f, 1.0f));

            bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
                new[] { fragmentMatrix1, fragmentMatrix2 },
                out Quaternion representativeRotation);

            Assert.IsTrue(ok);
            AssertVector(
                Vector3.Transform(Vector3.UnitX, representativeRotation),
                Vector3.Transform(Vector3.UnitX, expectedRotation),
                1e-4);
            AssertVector(
                Vector3.Transform(Vector3.UnitY, representativeRotation),
                Vector3.Transform(Vector3.UnitY, expectedRotation),
                1e-4);
            AssertVector(
                Vector3.Transform(Vector3.UnitZ, representativeRotation),
                Vector3.Transform(Vector3.UnitZ, expectedRotation),
                1e-4);
        }

        [TestMethod]
        public void ShouldIgnoreQuaternionSignWhenAveragingFragments()
        {
            Quaternion expectedRotation = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)(Math.PI / 4.0)));
            Quaternion oppositeSignRotation = new Quaternion(
                -expectedRotation.X,
                -expectedRotation.Y,
                -expectedRotation.Z,
                -expectedRotation.W);

            bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
                new[] { expectedRotation, oppositeSignRotation },
                out Quaternion representativeRotation);

            Assert.IsTrue(ok);
            AssertVector(
                Vector3.Transform(Vector3.UnitY, representativeRotation),
                Vector3.Transform(Vector3.UnitY, expectedRotation),
                1e-4);
            AssertVector(
                Vector3.Transform(Vector3.UnitZ, representativeRotation),
                Vector3.Transform(Vector3.UnitZ, expectedRotation),
                1e-4);
        }

        [TestMethod]
        public void ShouldFail_WhenNoFragmentRotationsAreAvailable()
        {
            bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
                Array.Empty<double[]>(),
                out Quaternion representativeRotation);

            Assert.IsFalse(ok);
            Assert.AreEqual(Quaternion.Identity, representativeRotation);
        }

        [TestMethod]
        public void ShouldPreserveAxes_ForObservedYUpRealObjectFragmentBasis()
        {
            Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f);
            Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f);

            double[] fragmentMatrix =
            {
                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
            };

            bool ok = FragmentRepresentativePoseHelper.TryExtractRotationFromFragmentMatrix(
                fragmentMatrix,
                out Quaternion rotation);

            Assert.IsTrue(ok);
            AssertVector(Vector3.Transform(Vector3.UnitX, rotation), axisX, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitY, rotation), axisY, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitZ, rotation), axisZ, 1e-4);
        }

        private static double[] CreateFragmentMatrix(Quaternion rotation, Vector3 translation)
        {
            Vector3 axisX = Vector3.Transform(Vector3.UnitX, rotation);
            Vector3 axisY = Vector3.Transform(Vector3.UnitY, rotation);
            Vector3 axisZ = Vector3.Transform(Vector3.UnitZ, rotation);

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

        private static void AssertVector(Vector3 actual, Vector3 expected, double tolerance)
        {
            Assert.AreEqual(expected.X, actual.X, tolerance);
            Assert.AreEqual(expected.Y, actual.Y, tolerance);
            Assert.AreEqual(expected.Z, actual.Z, tolerance);
        }
    }
}