NavisworksTransport/UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RealObjectRailExtentResolverTests
    {
        [TestMethod]
        public void YUp_ZeroCorrection_ShouldKeepResolvedRailUpExtent()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
            Quaternion baseline = Quaternion.Identity;

            bool ok = RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.YUp,
                forwardSize: 6.0,
                sideSize: 2.0,
                upSize: 4.0,
                baseline,
                baseline,
                out ModelAxisConvention convention,
                out var extents);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis);
            Assert.AreEqual(6.0, extents.forwardExtent, 1e-6);
            Assert.AreEqual(2.0, extents.sideExtent, 1e-6);
            Assert.AreEqual(4.0, extents.upExtent, 1e-6);
        }

        [TestMethod]
        public void YUp_HostY90_ShouldKeepResolvedRailUpExtentAndSwapForwardSide()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
            Quaternion baseline = Quaternion.Identity;
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion final = adapter.ComposeHostQuaternion(
                baseline,
                new LocalEulerRotationCorrection(0.0, 90.0, 0.0));

            bool ok = RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.YUp,
                forwardSize: 6.0,
                sideSize: 2.0,
                upSize: 4.0,
                baseline,
                final,
                out _,
                out var extents);

            Assert.IsTrue(ok);
            Assert.AreEqual(2.0, extents.forwardExtent, 1e-6);
            Assert.AreEqual(6.0, extents.sideExtent, 1e-6);
            Assert.AreEqual(4.0, extents.upExtent, 1e-6);
        }

        [TestMethod]
        public void YUp_DualAxisCorrection_WithRailBaseline_ShouldProjectAgainstFinalRailPose()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));

            Quaternion baseline = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(new Matrix4x4(
                0.9900f, -0.1403f, -0.0161f, 0f,
                0.1403f, 0.9901f, -0.0023f, 0f,
                0.0163f, 0.0000f, 0.9999f, 0f,
                0f, 0f, 0f, 1f)));

            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion final = adapter.ComposeHostQuaternion(
                baseline,
                new LocalEulerRotationCorrection(0.0, 90.0, 90.0));

            bool ok = RealObjectRailExtentResolver.TryResolveProjectedSemanticExtents(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.YUp,
                forwardSize: 6.0,
                sideSize: 2.0,
                upSize: 4.0,
                baseline,
                final,
                out ModelAxisConvention convention,
                out var extents);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis);

            Vector3 localSize = convention.CreateScaleVector3(6.0, 2.0, 4.0);
            Vector3 rotatedLocalX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, final));
            Vector3 rotatedLocalY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, final));
            Vector3 rotatedLocalZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, final));
            Vector3 targetForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, baseline));
            Vector3 targetUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, baseline));
            Vector3 targetSide = Vector3.Normalize(Vector3.Cross(targetForward, targetUp));

            double expectedForward = ProjectExtent(localSize, rotatedLocalX, rotatedLocalY, rotatedLocalZ, targetForward);
            double expectedSide = ProjectExtent(localSize, rotatedLocalX, rotatedLocalY, rotatedLocalZ, targetSide);
            double expectedUp = ProjectExtent(localSize, rotatedLocalX, rotatedLocalY, rotatedLocalZ, targetUp);

            Assert.AreEqual(expectedForward, extents.forwardExtent, 1e-5);
            Assert.AreEqual(expectedSide, extents.sideExtent, 1e-5);
            Assert.AreEqual(expectedUp, extents.upExtent, 1e-5);
            Assert.AreNotEqual(4.0, extents.upExtent, 1e-3, "双轴旋转后，法向尺寸不应仍停留在单轴结果。");
        }

        private static double ProjectExtent(
            Vector3 localSize,
            Vector3 rotatedLocalX,
            Vector3 rotatedLocalY,
            Vector3 rotatedLocalZ,
            Vector3 targetAxis)
        {
            return Math.Abs(Vector3.Dot(rotatedLocalX, targetAxis)) * localSize.X +
                   Math.Abs(Vector3.Dot(rotatedLocalY, targetAxis)) * localSize.Y +
                   Math.Abs(Vector3.Dot(rotatedLocalZ, targetAxis)) * localSize.Z;
        }
    }
}