NavisworksTransport/UnitTests/Core/FreePathTests.cs

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

namespace NavisworksTransport.UnitTests.Core
{
    [TestClass]
    public class FreePathTests
    {
        // === PathType 枚举 ===

        [TestMethod]
        public void PathType_Free_ShouldHaveValue3()
        {
            Assert.AreEqual(3, (int)PathType.Free);
        }

        [TestMethod]
        public void PathType_Free_GetDisplayName_ShouldReturnFree()
        {
            Assert.AreEqual("自由", PathType.Free.GetDisplayName());
        }

        // === 自由路径旋转姿态（方案1：forward=切线, up=世界up正交化） ===

        [TestMethod]
        public void FreePathRotation_StraightHorizontal_ShouldAlignForwardToX_UpToWorldUp()
        {
            // ZUp 约定，水平直路：forward=+X，up=+Z（世界up）
            bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                null,  // preferredNormal=null → 使用世界up
                Quaternion.Identity,
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            // 局部 X → 世界 X（forward=切线）
            AssertColumn(linear, 0, 1, 0, 0);
            // 局部 Z → 世界 Z（up=世界up）
            AssertColumn(linear, 2, 0, 0, 1);
        }

        [TestMethod]
        public void FreePathRotation_SlopedPath_ShouldKeepForwardAlongTangent_UpOrthogonalized()
        {
            // 斜路径（沿X上升）：forward 应沿切线，up 应正交且接近世界Z
            bool ok = CanonicalRailPoseBuilder.TryCreateBasis(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0.5f),
                new Vector3(2, 0, 1),
                Vector3.UnitZ,
                out Vector3 forward,
                out Vector3 lateral,
                out Vector3 up);

            Assert.IsTrue(ok);
            Assert.AreEqual(1.0, forward.Length(), 1e-6);
            Assert.AreEqual(1.0, up.Length(), 1e-6);
            Assert.AreEqual(1.0, lateral.Length(), 1e-6);
            // 正交性
            Assert.AreEqual(0.0, Vector3.Dot(forward, up), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(forward, lateral), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(lateral, up), 1e-6);
            // 切线方向沿X上升（(2,0,1)/√5 ≈ (0.894,0,0.447)，X为主）
            Assert.IsTrue(Vector3.Dot(forward, Vector3.UnitX) > 0.80f);
            // up 为世界Z在 forward 上的投影正交化，随坡度倾斜约26.6°，
            // Dot(up, Z) ≈ cos(26.6°) ≈ 0.894，仍以上方为主
            Assert.IsTrue(Vector3.Dot(up, Vector3.UnitZ) > 0.80f);
        }

        [TestMethod]
        public void FreePathRotation_VerticalPath_ShouldStillProduceValidFrame()
        {
            // 纯垂直路径（沿Z上升）：forward 应能生成，up 退化为水平（无世界up投影时用X）
            bool ok = CanonicalRailPoseBuilder.TryCreateBasis(
                new Vector3(0, 0, 0),
                new Vector3(0, 0, 1),
                new Vector3(0, 0, 2),
                Vector3.UnitZ,
                out Vector3 forward,
                out Vector3 lateral,
                out Vector3 up);

            Assert.IsTrue(ok);
            Assert.AreEqual(1.0, forward.Length(), 1e-6);
            Assert.AreEqual(1.0, up.Length(), 1e-6);
            Assert.AreEqual(0.0, Vector3.Dot(forward, up), 1e-6);
            // 纯垂直时 forward=+Z，up 应正交（水平）
            Assert.IsTrue(Math.Abs(Vector3.Dot(forward, Vector3.UnitZ)) > 0.99f);
        }

        // === 自由路径几何中心对齐语义 ===

        [TestMethod]
        public void FreePath_CenterAlignment_ShouldNotApplyHeightOffset()
        {
            // 自由路径：路径点即物体中心，ResolveGroundTrackedCenter 的半高偏移不应用于自由路径
            // 这里验证 CanonicalTrackedPositionResolver 的语义不影响自由路径
            var resolver = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter(
                new Vector3(5, 5, 5),
                Vector3.UnitZ,
                2.0);
            // Ground 语义：中心上移半高
            Assert.AreEqual(6.0, resolver.Z, 1e-6);
        }

        private static void AssertColumn(Matrix4x4 m, int col, float x, float y, float z)
        {
            switch (col)
            {
                case 0:
                    Assert.AreEqual(x, m.M11, 1e-5f);
                    Assert.AreEqual(y, m.M21, 1e-5f);
                    Assert.AreEqual(z, m.M31, 1e-5f);
                    break;
                case 1:
                    Assert.AreEqual(x, m.M12, 1e-5f);
                    Assert.AreEqual(y, m.M22, 1e-5f);
                    Assert.AreEqual(z, m.M32, 1e-5f);
                    break;
                case 2:
                    Assert.AreEqual(x, m.M13, 1e-5f);
                    Assert.AreEqual(y, m.M23, 1e-5f);
                    Assert.AreEqual(z, m.M33, 1e-5f);
                    break;
                default:
                    Assert.Fail("无效列");
                    break;
            }
        }
    }
}