NavisworksTransport/UnitTests/CoordinateSystem/ObjectPassageProjectionOptimizerTests.cs

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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class ObjectPassageProjectionOptimizerTests
    {
        [TestMethod]
        public void Optimize_LongTallBox_ShouldFindThreeAxisPoseBetterThanYawOnly()
        {
            var request = CreateZUpRequest(
                sizeX: 8.0,
                sizeY: 4.0,
                sizeZ: 2.0,
                pathForward: Vector3.UnitX);

            ObjectPassageProjectionOptimizationResult result = ObjectPassageProjectionOptimizer.Optimize(request);
            ObjectPassageProjectionScore yawOnly = ObjectPassageProjectionOptimizer.Evaluate(
                request,
                new LocalEulerRotationCorrection(0.0, 0.0, 90.0));

            Assert.IsTrue(result.Success, result.ErrorMessage);
            Assert.IsTrue(result.Score.Area < yawOnly.Area - 1e-6,
                $"三轴优化面积 {result.Score.Area:F6} 应小于单轴 yaw 面积 {yawOnly.Area:F6}");
            Assert.AreEqual(4.0, result.Score.WidthAcrossPath, 1e-4);
            Assert.AreEqual(2.0, result.Score.HeightAlongHostUp, 1e-4);
        }

        [TestMethod]
        public void Optimize_WhenAreasAreClose_ShouldPreferLowerHeight()
        {
            var request = CreateZUpRequest(
                sizeX: 6.0,
                sizeY: 3.0,
                sizeZ: 2.0,
                pathForward: Vector3.UnitX,
                areaRelativeTieTolerance: 0.05);

            ObjectPassageProjectionOptimizationResult result = ObjectPassageProjectionOptimizer.Optimize(request);

            Assert.IsTrue(result.Success, result.ErrorMessage);
            Assert.AreEqual(3.0, result.Score.WidthAcrossPath, 1e-4);
            Assert.AreEqual(2.0, result.Score.HeightAlongHostUp, 1e-4);
        }

        [TestMethod]
        public void Optimize_WhenBaselineAlreadyAlignedToPath_ShouldKeepZeroCorrection()
        {
            Quaternion baselineRotation = Quaternion.CreateFromAxisAngle(
                Vector3.UnitZ,
                (float)(Math.PI / 4.0));
            var request = new ObjectPassageProjectionOptimizationRequest(
                sizeX: 6.0,
                sizeY: 3.0,
                sizeZ: 2.0,
                hostPathForward: Vector3.Normalize(new Vector3(1.0f, 1.0f, 0.0f)),
                hostUp: Vector3.UnitZ,
                areaRelativeTieTolerance: 0.01,
                baselineHostRotation: baselineRotation);

            ObjectPassageProjectionOptimizationResult result = ObjectPassageProjectionOptimizer.Optimize(request);

            Assert.IsTrue(result.Success, result.ErrorMessage);
            Assert.AreEqual(0.0, result.Correction.XDegrees, 1e-6);
            Assert.AreEqual(0.0, result.Correction.YDegrees, 1e-6);
            Assert.AreEqual(0.0, result.Correction.ZDegrees, 1e-6);
            Assert.AreEqual(3.0, result.Score.WidthAcrossPath, 1e-4);
            Assert.AreEqual(2.0, result.Score.HeightAlongHostUp, 1e-4);
        }

        [TestMethod]
        public void Evaluate_YUp_ShouldUseHostYAsHeight()
        {
            var request = new ObjectPassageProjectionOptimizationRequest(
                sizeX: 6.0,
                sizeY: 2.0,
                sizeZ: 4.0,
                hostPathForward: Vector3.UnitX,
                hostUp: Vector3.UnitY);

            ObjectPassageProjectionScore score = ObjectPassageProjectionOptimizer.Evaluate(
                request,
                LocalEulerRotationCorrection.Zero);

            Assert.AreEqual(4.0, score.WidthAcrossPath, 1e-6);
            Assert.AreEqual(2.0, score.HeightAlongHostUp, 1e-6);
            Assert.AreEqual(8.0, score.Area, 1e-6);
        }

        [TestMethod]
        public void Optimize_SameInputTwice_ShouldReturnSameCorrection()
        {
            var request = CreateZUpRequest(
                sizeX: 9.0,
                sizeY: 4.0,
                sizeZ: 2.0,
                pathForward: Vector3.Normalize(new Vector3(1.0f, 1.0f, 0.0f)));

            ObjectPassageProjectionOptimizationResult first = ObjectPassageProjectionOptimizer.Optimize(request);
            ObjectPassageProjectionOptimizationResult second = ObjectPassageProjectionOptimizer.Optimize(request);

            Assert.IsTrue(first.Success, first.ErrorMessage);
            Assert.IsTrue(second.Success, second.ErrorMessage);
            Assert.AreEqual(first.Correction.XDegrees, second.Correction.XDegrees, 1e-9);
            Assert.AreEqual(first.Correction.YDegrees, second.Correction.YDegrees, 1e-9);
            Assert.AreEqual(first.Correction.ZDegrees, second.Correction.ZDegrees, 1e-9);
            Assert.AreEqual(first.Score.Area, second.Score.Area, 1e-9);
        }

        [TestMethod]
        public void Optimize_PathForwardParallelToHostUp_ShouldFail()
        {
            var request = CreateZUpRequest(
                sizeX: 6.0,
                sizeY: 4.0,
                sizeZ: 2.0,
                pathForward: Vector3.UnitZ);

            ObjectPassageProjectionOptimizationResult result = ObjectPassageProjectionOptimizer.Optimize(request);

            Assert.IsFalse(result.Success);
            StringAssert.Contains(result.ErrorMessage, "路径方向退化");
        }

        private static ObjectPassageProjectionOptimizationRequest CreateZUpRequest(
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 pathForward,
            double areaRelativeTieTolerance = 0.01)
        {
            return new ObjectPassageProjectionOptimizationRequest(
                sizeX,
                sizeY,
                sizeZ,
                pathForward,
                Vector3.UnitZ,
                areaRelativeTieTolerance);
        }
    }
}