NavisworksTransport/UnitTests/CoordinateSystem/HoistingRealObjectPoseHelperTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class HoistingRealObjectPoseHelperTests
    {
        [TestMethod]
        public void ShouldKeepActualUpAxis_WhenAligningHoistingForward()
        {
            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
            Vector3 hostUp = Vector3.UnitY;
            Vector3 targetForward = -Vector3.UnitX;

            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
                actualRotation,
                targetForward,
                hostUp,
                out Quaternion resultRotation,
                out LocalAxisDirection selectedDirection,
                out Vector3 selectedAxisLocal,
                out Vector3 projectedForward);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
            AssertVector(projectedForward, 1.0, 0.0, 0.0);

            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);

            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
        }

        [TestMethod]
        public void ShouldChooseHorizontalAxisFromActualPose_ForHoisting()
        {
            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
            Vector3 hostUp = Vector3.UnitY;
            Vector3 targetForward = Vector3.UnitX;

            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
                actualRotation,
                targetForward,
                hostUp,
                out Quaternion resultRotation,
                out LocalAxisDirection selectedDirection,
                out Vector3 selectedAxisLocal,
                out Vector3 projectedForward);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
            AssertVector(projectedForward, 1.0, 0.0, 0.0);

            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);

            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
        }

        [TestMethod]
        public void CreateRotationFromPlanarBasePose_ShouldReturnBaseRotation_WhenYawUnchanged()
        {
            Quaternion baseRotation = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.31f) *
                Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.42f));

            Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
                baseRotation,
                baseYawRadians: 1.25,
                targetYawRadians: 1.25,
                hostUp: Vector3.UnitY);

            AssertQuaternionEquivalent(resultRotation, baseRotation);
        }

        [TestMethod]
        public void CreateRotationFromPlanarBasePose_ShouldPreserveTiltAndApplyDeltaYaw()
        {
            Quaternion baseRotation = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitY, 0.20f) *
                Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.35f) *
                Quaternion.CreateFromAxisAngle(Vector3.UnitX, -0.40f));

            Quaternion resultRotation = HoistingRealObjectPoseHelper.CreateRotationFromPlanarBasePose(
                baseRotation,
                baseYawRadians: 0.0,
                targetYawRadians: System.Math.PI / 2.0,
                hostUp: Vector3.UnitY);

            Quaternion expectedRotation = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)(System.Math.PI / 2.0)) *
                baseRotation);

            AssertQuaternionEquivalent(resultRotation, expectedRotation);
        }

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

        private static void AssertQuaternionEquivalent(Quaternion actual, Quaternion expected, double tolerance = 1e-6)
        {
            float dot = Quaternion.Dot(Quaternion.Normalize(actual), Quaternion.Normalize(expected));
            Assert.AreEqual(1.0, System.Math.Abs(dot), tolerance);
        }
    }
}