NavisworksTransport/UnitTests/Core/PathCurveEngineTests.cs

using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.UnitTests.Core
{
    /// <summary>
    /// PathCurveEngine 核心算法测试
    /// 测试路径曲线化算法的正确性
    /// </summary>
    [TestClass]
    public class PathCurveEngineTests
    {
        #region CalculateFillet 测试

        [TestMethod]
        public void CalculateFillet_RightAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建90度直角转弯
            var pPrev = new Point3D(0, 10, 0);  // 上方点
            var pCurr = new Point3D(0, 0, 0);   // 转弯点
            var pNext = new Point3D(10, 0, 0);  // 右方点
            double turnRadius = 2.0;

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.AreEqual(turnRadius, trajectory.RequestedRadius, 0.01, "请求半径应该正确");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 0, "偏转角应该大于0");
            Assert.IsTrue(trajectory.ArcLength > 0, "圆弧长度应该大于0");

            // 验证切点位置
            Assert.IsTrue(trajectory.Ts.Y > 0, "进入切点应该在转弯点上方");
            Assert.IsTrue(trajectory.Ts.X == 0, "进入切点X坐标应该为0");
            Assert.IsTrue(trajectory.Te.X > 0, "退出切点应该在转弯点右侧");
            Assert.IsTrue(trajectory.Te.Y == 0, "退出切点Y坐标应该为0");
        }

        [TestMethod]
        public void CalculateFillet_AcuteAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建锐角转弯（约60度）
            var pPrev = new Point3D(0, 10, 0);
            var pCurr = new Point3D(0, 0, 0);
            var pNext = new Point3D(8.66, 5, 0);  // 60度方向
            double turnRadius = 2.0;

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 0.5 && trajectory.DeflectionAngle < 1.5, "偏转角应该在60度左右");
        }

        [TestMethod]
        public void CalculateFillet_ObtuseAngleTurn_ReturnsValidArc()
        {
            // Arrange - 创建钝角转弯（约120度）
            var pPrev = new Point3D(0, 10, 0);
            var pCurr = new Point3D(0, 0, 0);
            var pNext = new Point3D(8.66, -5, 0);  // 120度方向
            double turnRadius = 2.0;

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.DeflectionAngle > 1.5 && trajectory.DeflectionAngle < 2.5, "偏转角应该在120度左右");
        }

        [TestMethod]
        public void CalculateFillet_CollinearPoints_ReturnsInvalidArc()
        {
            // Arrange - 创建共线点（几乎直线）
            var pPrev = new Point3D(0, 10, 0);
            var pCurr = new Point3D(0, 0, 0);
            var pNext = new Point3D(0, -10, 0);  // 同一直线
            double turnRadius = 2.0;

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.AreEqual(0, trajectory.ActualRadius, 0.01, "实际半径应该为0");
            Assert.AreEqual(0, trajectory.ArcLength, 0.01, "圆弧长度应该为0");
            Assert.AreEqual(pCurr, trajectory.Ts, "切点应该与转弯点相同");
        }

        [TestMethod]
        public void CalculateFillet_SafetyTruncation_AdjustsRadius()
        {
            // Arrange - 创建短边场景，需要安全截断
            var pPrev = new Point3D(0, 1, 0);   // 短边
            var pCurr = new Point3D(0, 0, 0);
            var pNext = new Point3D(1, 0, 0);   // 短边
            double turnRadius = 2.0;  // 半径大于边长

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius < turnRadius, "实际半径应该小于请求半径");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
        }

        [TestMethod]
        public void CalculateFillet_3DTurn_ReturnsValidArc()
        {
            // Arrange - 创建3D空间中的转弯
            var pPrev = new Point3D(0, 10, 0);
            var pCurr = new Point3D(0, 0, 0);
            var pNext = new Point3D(10, 0, 5);  // 有Z轴变化
            double turnRadius = 2.0;

            // Act
            var trajectory = PathCurveEngine.CalculateFillet(pPrev, pCurr, pNext, turnRadius);

            // Assert
            Assert.IsNotNull(trajectory, "轨迹不应该为null");
            Assert.IsTrue(trajectory.ActualRadius > 0, "实际半径应该大于0");
            Assert.IsTrue(trajectory.ArcCenter.Z != 0, "圆心Z坐标应该不为0");
        }

        #endregion

        #region SampleArc 测试

        [TestMethod]
        public void SampleArc_WithValidTrajectory_ReturnsSampledPoints()
        {
            // Arrange
            var trajectory = new ArcTrajectory
            {
                Ts = new Point3D(0, 2, 0),
                Te = new Point3D(2, 0, 0),
                ArcCenter = new Point3D(0, 0, 0),
                ActualRadius = 2.0,
                DeflectionAngle = Math.PI / 2,  // 90度
                ArcLength = Math.PI  // 半圆周长
            };
            double samplingStep = 0.5;

            // Act
            var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
            Assert.IsTrue(sampledPoints.Count >= 2, "采样点数量应该至少为2");
            Assert.AreEqual(trajectory.Ts, sampledPoints.First(), "第一个点应该是进入切点");
            Assert.AreEqual(trajectory.Te, sampledPoints.Last(), "最后一个点应该是退出切点");
        }

        [TestMethod]
        public void SampleArc_SmallArcLength_ReturnsTwoPoints()
        {
            // Arrange
            var trajectory = new ArcTrajectory
            {
                Ts = new Point3D(0, 0.1, 0),
                Te = new Point3D(0.1, 0, 0),
                ArcCenter = new Point3D(0, 0, 0),
                ActualRadius = 0.1,
                DeflectionAngle = 0.1,
                ArcLength = 0.01  // 非常小的圆弧
            };
            double samplingStep = 0.05;

            // Act
            var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            Assert.IsNotNull(sampledPoints, "采样点列表不应该为null");
            Assert.AreEqual(2, sampledPoints.Count, "小圆弧应该返回2个点");
        }

        [TestMethod]
        public void SampleArc_SamplingStep0_05_ReturnsCorrectCount()
        {
            // Arrange
            var trajectory = new ArcTrajectory
            {
                Ts = new Point3D(0, 2, 0),
                Te = new Point3D(2, 0, 0),
                ArcCenter = new Point3D(0, 0, 0),
                ActualRadius = 2.0,
                DeflectionAngle = Math.PI / 2,
                ArcLength = Math.PI
            };
            double samplingStep = 0.05;

            // Act
            var sampledPoints = PathCurveEngine.SampleArc(trajectory, samplingStep);

            // Assert
            int expectedCount = (int)Math.Ceiling(Math.PI / 0.05) + 1;
            Assert.IsTrue(sampledPoints.Count >= expectedCount - 2 && sampledPoints.Count <= expectedCount + 2,
                $"采样点数量应该在 {expectedCount - 2} 到 {expectedCount + 2} 之间");
        }

        #endregion

        #region ApplyCurvatureToRoute 测试

        [TestMethod]
        public void ApplyCurvatureToRoute_SimplePath_GeneratesEdges()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "测试路径",
                TurnRadius = 2.0
            };
            route.Points = new List<PathPoint>
            {
                new PathPoint(new Point3D(0, 0, 0), "起点", PathPointType.StartPoint),
                new PathPoint(new Point3D(10, 0, 0), "点2", PathPointType.WayPoint),
                new PathPoint(new Point3D(10, 10, 0), "终点", PathPointType.EndPoint)
            };
            route.Points[0].Index = 0;
            route.Points[1].Index = 1;
            route.Points[2].Index = 2;
            double samplingStep = 0.5;

            // Act
            PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);

            // Assert
            Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
            Assert.IsTrue(route.Edges.Count > 0, "应该生成路径边");
            Assert.IsTrue(route.TotalLength > 0, "路径总长度应该大于0");

            // 验证每个边都有采样点
            foreach (var edge in route.Edges)
            {
                Assert.IsNotNull(edge.SampledPoints, "每个边都应该有采样点");
                Assert.IsTrue(edge.SampledPoints.Count > 0, "采样点数量应该大于0");
            }
        }

        [TestMethod]
        public void ApplyCurvatureToRoute_LShapedPath_GeneratesArcEdge()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "L型路径",
                TurnRadius = 1.5
            };
            route.Points = new List<PathPoint>
            {
                new PathPoint(new Point3D(0, 10, 0), "起点", PathPointType.StartPoint),
                new PathPoint(new Point3D(0, 0, 0), "转弯点", PathPointType.WayPoint),
                new PathPoint(new Point3D(10, 0, 0), "终点", PathPointType.EndPoint)
            };
            route.Points[0].Index = 0;
            route.Points[1].Index = 1;
            route.Points[2].Index = 2;
            double samplingStep = 0.5;

            // Act
            PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);

            // Assert
            Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
            Assert.AreEqual(2, route.Edges.Count, "L型路径应该生成2条边");

            // 第一条边是直线段
            Assert.AreEqual(PathSegmentType.Straight, route.Edges[0].SegmentType, "第一条边应该是直线段");

            // 第二条边包含圆弧
            Assert.AreEqual(PathSegmentType.Arc, route.Edges[1].SegmentType, "第二条边应该是圆弧段");
            Assert.IsNotNull(route.Edges[1].Trajectory, "圆弧边应该有轨迹数据");
            Assert.IsTrue(route.Edges[1].Trajectory.ActualRadius > 0, "实际半径应该大于0");
        }

        [TestMethod]
        public void ApplyCurvatureToRoute_ZShapedPath_GeneratesMultipleArcEdges()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "Z型路径",
                TurnRadius = 1.5
            };
            route.Points = new List<PathPoint>
            {
                new PathPoint(new Point3D(0, 10, 0), "起点", PathPointType.StartPoint),
                new PathPoint(new Point3D(0, 0, 0), "转弯点1", PathPointType.WayPoint),
                new PathPoint(new Point3D(10, 0, 0), "转弯点2", PathPointType.WayPoint),
                new PathPoint(new Point3D(10, 10, 0), "终点", PathPointType.EndPoint)
            };
            route.Points[0].Index = 0;
            route.Points[1].Index = 1;
            route.Points[2].Index = 2;
            route.Points[3].Index = 3;
            double samplingStep = 0.5;

            // Act
            PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);

            // Assert
            Assert.IsTrue(route.IsCurved, "路径应该被标记为已曲线化");
            Assert.AreEqual(3, route.Edges.Count, "Z型路径应该生成3条边");

            // 应该有两条圆弧边
            int arcEdgeCount = route.Edges.Count(e => e.SegmentType == PathSegmentType.Arc);
            Assert.IsTrue(arcEdgeCount >= 1, "应该至少有一条圆弧边");
        }

        [TestMethod]
        public void ApplyCurvatureToRoute_NullRoute_DoesNotThrow()
        {
            // Arrange
            PathRoute route = null;
            double samplingStep = 0.5;

            // Act & Assert
            try
            {
                PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);
                Assert.Fail("应该抛出ArgumentNullException");
            }
            catch (ArgumentNullException)
            {
                // Expected
            }
        }

        [TestMethod]
        public void ApplyCurvatureToRoute_LessThanTwoPoints_DoesNotCurve()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "单点路径",
                TurnRadius = 2.0
            };
            route.Points = new List<PathPoint>
            {
                new PathPoint(new Point3D(0, 0, 0), "起点", PathPointType.StartPoint)
            };
            route.Points[0].Index = 0;
            double samplingStep = 0.5;

            // Act
            PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);

            // Assert
            Assert.IsFalse(route.IsCurved, "少于2个点的路径不应该被曲线化");
            Assert.AreEqual(0, route.Edges.Count, "不应该生成任何边");
        }

        #endregion

        #region RecalculateRouteLength 测试

        [TestMethod]
        public void RecalculateRouteLength_StraightEdges_CalculatesCorrectly()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "测试路径"
            };
            route.Edges = new List<PathEdge>
            {
                new PathEdge
                {
                    Id = Guid.NewGuid().ToString(),
                    SegmentType = PathSegmentType.Straight,
                    PhysicalLength = 10.0,
                    SampledPoints = new List<Point3D>()
                },
                new PathEdge
                {
                    Id = Guid.NewGuid().ToString(),
                    SegmentType = PathSegmentType.Straight,
                    PhysicalLength = 5.0,
                    SampledPoints = new List<Point3D>()
                }
            };

            // Act
            PathCurveEngine.RecalculateRouteLength(route);

            // Assert
            Assert.AreEqual(15.0, route.TotalLength, 0.01, "路径总长度应该等于所有边长度之和");
        }

        [TestMethod]
        public void RecalculateRouteLength_MixedEdgeTypes_CalculatesCorrectly()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "测试路径"
            };
            route.Edges = new List<PathEdge>
            {
                new PathEdge
                {
                    Id = Guid.NewGuid().ToString(),
                    SegmentType = PathSegmentType.Straight,
                    PhysicalLength = 10.0,
                    SampledPoints = new List<Point3D>()
                },
                new PathEdge
                {
                    Id = Guid.NewGuid().ToString(),
                    SegmentType = PathSegmentType.Arc,
                    PhysicalLength = Math.PI,  // 半圆
                    SampledPoints = new List<Point3D>()
                },
                new PathEdge
                {
                    Id = Guid.NewGuid().ToString(),
                    SegmentType = PathSegmentType.Straight,
                    PhysicalLength = 5.0,
                    SampledPoints = new List<Point3D>()
                }
            };

            // Act
            PathCurveEngine.RecalculateRouteLength(route);

            // Assert
            double expectedLength = 10.0 + Math.PI + 5.0;
            Assert.AreEqual(expectedLength, route.TotalLength, 0.01, "路径总长度应该正确计算");
        }

        [TestMethod]
        public void RecalculateRouteLength_NullRoute_DoesNotThrow()
        {
            // Arrange
            PathRoute route = null;

            // Act
            PathCurveEngine.RecalculateRouteLength(route);

            // Assert - 不应该抛出异常
            Assert.IsTrue(true, "null路径不应该导致异常");
        }

        [TestMethod]
        public void RecalculateRouteLength_EmptyEdges_SetsZeroLength()
        {
            // Arrange
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "空路径",
                Edges = new List<PathEdge>()
            };

            // Act
            PathCurveEngine.RecalculateRouteLength(route);

            // Assert
            Assert.AreEqual(0, route.TotalLength, 0.01, "空边的路径长度应该为0");
        }

        #endregion

        #region 集成测试

        [TestMethod]
        public void FullWorkflow_CreateCurvePath_CalculatesCorrectLength()
        {
            // Arrange - 创建一个包含转弯的路径
            var route = new PathRoute
            {
                Id = Guid.NewGuid().ToString(),
                Name = "完整测试路径",
                TurnRadius = 2.0
            };
            route.Points = new List<PathPoint>
            {
                new PathPoint(new Point3D(0, 20, 0), "起点", PathPointType.StartPoint),
                new PathPoint(new Point3D(0, 0, 0), "转弯点", PathPointType.WayPoint),
                new PathPoint(new Point3D(20, 0, 0), "终点", PathPointType.EndPoint)
            };
            route.Points[0].Index = 0;
            route.Points[1].Index = 1;
            route.Points[2].Index = 2;
            double samplingStep = 0.5;

            // Act - 应用曲线化
            PathCurveEngine.ApplyCurvatureToRoute(route, samplingStep);

            // Assert
            Assert.IsTrue(route.IsCurved, "路径应该被曲线化");
            Assert.IsTrue(route.TotalLength > 0, "路径总长度应该大于0");

            // 验证路径总长度小于直线距离（因为圆弧比直线短）
            double straightDistance = 20 + 20;  // 两条直线段
            Assert.IsTrue(route.TotalLength < straightDistance, "曲线化后的路径长度应该小于直线距离");
        }

        #endregion
    }
}