NavisworksTransport/UnitTests/CoordinateSystem/AutoPathPlanningCoordinateSemanticsTests.cs

using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Core;
using NavisworksTransport.PathPlanning;
using NavisworksTransport.Utils.CoordinateSystem;
using System.Numerics;
using System.Reflection;
using System.Collections.Generic;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class AutoPathPlanningCoordinateSemanticsTests
    {
        [TestMethod]
        public void YUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostY()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);

            Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 14.83, adapter);

            AssertPoint(point, 2.0, 14.83, 3.0);
        }

        [TestMethod]
        public void ZUp_HostPlanarGridHelper_CreateHostPoint_ShouldApplyElevationOnHostZ()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);

            Vector3 point = HostPlanarGridHelper.CreateHostPoint3(2.0, 3.0, 6.25, adapter);

            AssertPoint(point, 2.0, 3.0, 6.25);
        }

        [TestMethod]
        public void YUp_HostPlanarGridHelper_GetAndSetElevation_ShouldUseHostY()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var hostPoint = new Vector3(4.0f, 15.0f, 6.0f);

            double elevation = HostPlanarGridHelper.GetElevation3(hostPoint, adapter);
            Vector3 adjusted = HostPlanarGridHelper.SetElevation3(hostPoint, 20.0, adapter);

            Assert.AreEqual(15.0, elevation, 1e-6);
            AssertPoint(adjusted, 4.0, 20.0, 6.0);
        }

        [TestMethod]
        public void YUp_HostPlanarGridHelper_GetHorizontalCoords_ShouldProjectToHostXZPlane()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var hostPoint = new Vector3(8.0f, 14.83f, -5.35f);

            (double h1, double h2) = HostPlanarGridHelper.GetHorizontalCoords3(hostPoint, adapter);

            Assert.AreEqual(8.0, h1, 1e-6);
            Assert.AreEqual(-5.35, h2, 1e-6);
        }

        [TestMethod]
        public void YUp_HostPlanarGridHelper_HorizontalRange_ShouldMatchHostXZPlane()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var hostMin = new Vector3(-10.0f, 2.0f, -30.0f);
            var hostMax = new Vector3(40.0f, 12.0f, 5.0f);

            (double min1, double max1, double min2, double max2) =
                HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);

            Assert.AreEqual(-10.0, min1, 1e-6);
            Assert.AreEqual(40.0, max1, 1e-6);
            Assert.AreEqual(-30.0, min2, 1e-6);
            Assert.AreEqual(5.0, max2, 1e-6);
        }

        [TestMethod]
        public void YUp_GridLikeWorldPointConstruction_ShouldWriteElevationToHostY()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            double originH1 = -210.0;
            double originH2 = -10.0;
            double cellSize = 1.0;

            Vector3 worldPoint = HostPlanarGridHelper.CreateHostPoint3(
                originH1 + 4 * cellSize,
                originH2 + 6 * cellSize,
                14.83,
                adapter);

            Assert.AreEqual(-206.0, worldPoint.X, 1e-6);
            Assert.AreEqual(14.83, worldPoint.Y, 1e-6);
            Assert.AreEqual(-4.0, worldPoint.Z, 1e-6);
        }

        [TestMethod]
        public void YUp_GridLikeWorldToGrid_ShouldReadHostXZAsPlanarAxes()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var origin = new Vector3(-210.0f, 14.83f, -10.0f);
            var endPoint = new Vector3(-169.20f, 14.83f, -5.35f);
            const double cellSize = 1.0;

            (double pointH1, double pointH2) = HostPlanarGridHelper.GetHorizontalCoords3(endPoint, adapter);
            (double originH1, double originH2) = HostPlanarGridHelper.GetHorizontalCoords3(origin, adapter);
            int gridX = (int)System.Math.Round((pointH1 - originH1) / cellSize);
            int gridY = (int)System.Math.Round((pointH2 - originH2) / cellSize);

            Assert.AreEqual(41, gridX);
            Assert.AreEqual(5, gridY);
        }

        [TestMethod]
        public void ZUp_GridMap_CellPlanarBounds_ShouldMatchNearestNodeRoundSemantics()
        {
            var bounds = GridMap.CalculateNearestNodePlanarBounds(2.0, 3.0, 1.0);

            Assert.AreEqual(1.5, bounds.minH1, 1e-9);
            Assert.AreEqual(2.5, bounds.maxH1, 1e-9);
            Assert.AreEqual(2.5, bounds.minH2, 1e-9);
            Assert.AreEqual(3.5, bounds.maxH2, 1e-9);
        }

        [TestMethod]
        public void ZUp_GridMap_SegmentIntersection_ShouldDetectInteriorNotBoundaryTouch()
        {
            var bounds = GridMap.CalculateNearestNodePlanarBounds(2.0, 3.0, 1.0);

            bool crossesInterior = GridMap.TryGetSegmentRectangleInteriorIntersection(
                1.75,
                3.0,
                2.25,
                3.0,
                bounds.minH1,
                bounds.maxH1,
                bounds.minH2,
                bounds.maxH2,
                1.0,
                out double enterT,
                out double exitT);

            Assert.IsTrue(crossesInterior);
            Assert.IsTrue(enterT >= 0.0 && enterT <= exitT && exitT <= 1.0);

            bool onlyTouchesBoundary = GridMap.TryGetSegmentRectangleInteriorIntersection(
                1.5,
                2.0,
                1.5,
                4.0,
                bounds.minH1,
                bounds.maxH1,
                bounds.minH2,
                bounds.maxH2,
                1.0,
                out _,
                out _);

            Assert.IsFalse(onlyTouchesBoundary, "贴着归属边界行走不应被当成穿过格子内部。");
        }

        [TestMethod]
        public void YUp_GridMapGenerator_GetObstacleElevationRange_ShouldUseHostYInsteadOfWorldZ()
        {
            var generator = new GridMapGenerator();

            var method = typeof(GridMapGenerator).GetMethod(
                "GetObstacleElevationRange",
                BindingFlags.Instance | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 GetObstacleElevationRange 私有方法。");

            var elevationRange = ((double min, double max))method.Invoke(generator, new object[]
            {
                -200.0, 10.0, -30.0,
                -198.0, 30.0, -10.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(10.0, elevationRange.min, 1e-6, "YUp 下障碍物最小高程应来自宿主 Y。");
            Assert.AreEqual(30.0, elevationRange.max, 1e-6, "YUp 下障碍物最大高程应来自宿主 Y。");
        }

        [TestMethod]
        public void YUp_GridMapGenerator_CalculateBoundingBoxGridCoverage_ShouldProjectToHostXZPlane()
        {
            var generator = new GridMapGenerator();

            var method = typeof(GridMapGenerator).GetMethod(
                "CalculateGridCoverageFromWorldExtents",
                BindingFlags.Instance | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(int), typeof(int), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 CalculateGridCoverageFromWorldExtents 私有方法。");

            var coveredCells = (List<(int x, int y)>)method.Invoke(generator, new object[]
            {
                -200.0, 10.0, -30.0,
                -198.0, 30.0, -10.0,
                -210.0, 14.83, -80.0,
                200, 200, 1.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(63, coveredCells.Count, "YUp 下障碍物包围盒应按宿主 XZ 平面覆盖 3x21 个网格。");
            CollectionAssert.Contains(coveredCells, (10, 50));
            CollectionAssert.Contains(coveredCells, (12, 70));
        }

        [TestMethod]
        public void YUp_GridMapGenerator_CalculateDoorOpeningCoverage_ShouldProjectToHostXZPlane()
        {
            var generator = new GridMapGenerator();

            var method = typeof(GridMapGenerator).GetMethod(
                "CalculateDoorOpeningCoverageFromWorldExtents",
                BindingFlags.Instance | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(int), typeof(int), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 CalculateDoorOpeningCoverageFromWorldExtents 私有方法。");

            var coveredCells = (List<(int x, int y)>)method.Invoke(generator, new object[]
            {
                10.0, 14.0, 20.0,
                12.0, 16.0, 30.0,
                4.0,
                0.0, 0.0, 0.0,
                100, 100, 1.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(15, coveredCells.Count, "YUp 下门开口应按宿主 XZ 平面覆盖 3x5 个网格。");
            CollectionAssert.Contains(coveredCells, (10, 23));
            CollectionAssert.Contains(coveredCells, (12, 27));
        }

        [TestMethod]
        public void YUp_PathPlanningManager_CalculateOptimalGridSize_ShouldUseHostXZHorizontalRange()
        {
            var method = typeof(PathPlanningManager).GetMethod(
                "CalculateOptimalGridSizeFromBounds",
                BindingFlags.Static | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 CalculateOptimalGridSizeFromBounds 私有方法。");

            var gridSize = (double)method.Invoke(null, new object[]
            {
                -10.0, 100.0, -300.0,
                90.0, 120.0, 400.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(2.0, gridSize, 1e-6, "YUp 下网格大小应基于宿主 XZ 平面最大跨度 700 计算。");
        }

        [TestMethod]
        public void YUp_ChannelHeightDetector_GetBoundsElevationRange_ShouldUseHostY()
        {
            var method = typeof(ChannelHeightDetector).GetMethod(
                "GetBoundsElevationRange",
                BindingFlags.Static | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 GetBoundsElevationRange 私有方法。");

            var elevationRange = ((double min, double max))method.Invoke(null, new object[]
            {
                -10.0, 14.83, -30.0,
                20.0, 18.50, 40.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(14.83, elevationRange.min, 1e-6);
            Assert.AreEqual(18.50, elevationRange.max, 1e-6);
        }

        [TestMethod]
        public void YUp_ChannelHeightDetector_CreateHeightProfileSample_ShouldUseHostXZPlaneAndHostYElevation()
        {
            var method = typeof(ChannelHeightDetector).GetMethod(
                "CreateHeightProfileSamplePoint3",
                BindingFlags.Static | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType),
                    typeof(double)
                },
                null);

            Assert.IsNotNull(method, "未找到 CreateHeightProfileSamplePoint3 私有方法。");

            var samplePoint = (Vector3)method.Invoke(null, new object[]
            {
                -10.0, 14.83, -30.0,
                20.0, 18.50, 40.0,
                CoordinateSystemType.YUp,
                0.5
            });

            Assert.AreEqual(5.0, samplePoint.X, 1e-6, "YUp 下采样点第一水平轴应沿宿主 X 插值。");
            Assert.AreEqual(14.83, samplePoint.Y, 1e-6, "YUp 下采样点高程应落在宿主底面 Y。");
            Assert.AreEqual(5.0, samplePoint.Z, 1e-6, "YUp 下采样点第二水平轴应沿宿主 Z 插值。");
        }

        [TestMethod]
        public void YUp_ChannelHeightDetector_GetPickedSurfaceElevation_ShouldUseHostYInsteadOfWorldZ()
        {
            var method = typeof(ChannelHeightDetector).GetMethod(
                "GetPickedSurfaceElevation",
                BindingFlags.Static | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 GetPickedSurfaceElevation 私有方法。");

            var elevation = (double)method.Invoke(null, new object[]
            {
                11.0, 22.0, 33.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(22.0, elevation, 1e-6, "YUp 下拾取表面点的高程应来自宿主 Y。");
        }

        [TestMethod]
        public void YUp_GridMapGenerator_ResolveBoundsMinElevation_ShouldUseHostYInsteadOfWorldZ()
        {
            var method = typeof(GridMapGenerator).GetMethod(
                "ResolveBoundsMinElevation",
                BindingFlags.Static | BindingFlags.NonPublic,
                null,
                new[]
                {
                    typeof(double), typeof(double), typeof(double),
                    typeof(CoordinateSystemType)
                },
                null);

            Assert.IsNotNull(method, "未找到 ResolveBoundsMinElevation 私有方法。");

            var elevation = (double)method.Invoke(null, new object[]
            {
                0.0, 12.5, -100.0,
                CoordinateSystemType.YUp
            });

            Assert.AreEqual(12.5, elevation, 1e-6, "YUp 下边界最小高程应来自宿主 Y，而不是 bounds.Min.Z。");
        }

        private static void AssertPoint(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);
        }
    }
}