NavisworksTransport/src/PathPlanning/AerialPathGenerator.cs

using System;
using System.Collections.Generic;
using Autodesk.Navisworks.Api;
using NavisworksTransport.Utils;
using NavisworksTransport.Utils.CoordinateSystem;

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 空中路径生成器
    /// 用于生成吊装路径（起点吊起 → 平移 → 终点吊下）
    /// 支持智能方向判断和基于地面投影点的动态路径生成
    /// </summary>
    public class AerialPathGenerator
    {
        private readonly HostCoordinateAdapter _adapter;

        /// <summary>
        /// 方向判断容差（米）
        /// 当两个方向的距离差小于此值时，默认选择纵向
        /// </summary>
        private const double DIRECTION_TOLERANCE = 0.001;

        public AerialPathGenerator()
            : this(HostCoordinateAdapter.CreateFromManager())
        {
        }

        public AerialPathGenerator(HostCoordinateAdapter adapter)
        {
            _adapter = adapter ?? throw new ArgumentNullException(nameof(adapter));
        }

        /// <summary>
        /// 生成吊装路径（起点吊起 → 平移 → 终点吊下）
        /// 保留原有方法以保持向后兼容
        /// </summary>
        /// <param name="startPoint">起点（吊起位置）</param>
        /// <param name="endPoint">终点（下降位置）</param>
        /// <param name="liftHeightModelUnits">提升高度（模型单位）</param>
        /// <returns>4个路径点：起吊点、提升点、平移终点、下降点</returns>
        public List<PathPoint> GenerateThreeStepHoistingPath(
            Point3D startPoint,
            Point3D endPoint,
            double liftHeightModelUnits)
        {
            if (startPoint == null)
                throw new ArgumentNullException(nameof(startPoint));
            if (endPoint == null)
                throw new ArgumentNullException(nameof(endPoint));
            if (liftHeightModelUnits <= 0)
                throw new ArgumentException("提升高度必须大于0", nameof(liftHeightModelUnits));

            var pathPoints = new List<PathPoint>();

            // 点1：起吊点（物体初始位置）
            var startPointPath = new PathPoint(
                startPoint,
                "起吊点",
                PathPointType.StartPoint);
            startPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(startPointPath);

            // 点2：提升点（起点正上方，达到提升高度）
            var liftPoint = HoistingCoordinateHelper.OffsetAlongUp(startPoint, liftHeightModelUnits, _adapter);
            var liftPointPath = new PathPoint(
                liftPoint,
                "提升点",
                PathPointType.WayPoint);
            liftPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(liftPointPath);

            // 点3：平移终点（终点正上方，保持提升高度）
            var moveEndPoint = HoistingCoordinateHelper.ProjectToAerialElevation(endPoint, liftPoint, _adapter);
            var moveEndPointPath = new PathPoint(
                moveEndPoint,
                "平移终点",
                PathPointType.WayPoint);
            moveEndPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(moveEndPointPath);

            // 点4：下降点（终点位置，物体最终位置）
            var endPointPath = new PathPoint(
                endPoint,
                "下降点",
                PathPointType.EndPoint);
            endPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(endPointPath);

            return pathPoints;
        }

        /// <summary>
        /// 判断移动方向并生成路径点
        /// 根据用户点击的地面投影位置和上一个路径点，自动判断纵向或横向移动
        /// </summary>
        /// <param name="previousPoint">上一个路径点</param>
        /// <param name="clickedGroundPoint">用户点击的地面投影位置</param>
        /// <param name="targetHeightModelUnits">目标高度（模型单位）</param>
        /// <param name="pointIndex">路径点序号</param>
        /// <returns>生成的路径点</returns>
        public PathPoint GenerateSmartHoistingPoint(
            Point3D previousPoint,
            Point3D clickedGroundPoint,
            double targetHeightModelUnits,
            int pointIndex)
        {
            if (previousPoint == null)
                throw new ArgumentNullException(nameof(previousPoint));
            if (clickedGroundPoint == null)
                throw new ArgumentNullException(nameof(clickedGroundPoint));
            // 注意：targetHeightModelUnits 是绝对高度，可以是负数（如果起点在地下）

            // 计算两个方向的距离
            double deltaX = Math.Abs(clickedGroundPoint.X - previousPoint.X);
            double deltaY = Math.Abs(clickedGroundPoint.Y - previousPoint.Y);

            // 使用目标高度作为Z坐标，XY使用地面点击位置
            Point3D newPoint = HoistingCoordinateHelper.SetElevation(
                clickedGroundPoint,
                targetHeightModelUnits,
                _adapter);

            // 判断移动方向（仅用于记录，不影响位置）
            HoistingPointDirection direction;
            if (Math.Abs(deltaX - deltaY) < DIRECTION_TOLERANCE)
            {
                // 距离相等，默认选择纵向
                direction = HoistingPointDirection.Longitudinal;
            }
            else if (deltaX > deltaY)
            {
                // 更偏向纵向移动（X轴）
                direction = HoistingPointDirection.Longitudinal;
            }
            else
            {
                // 更偏向横向移动（Y轴）
                direction = HoistingPointDirection.Lateral;
            }

            string pointName = $"路径点{pointIndex}";
            var pathPoint = new PathPoint(newPoint, pointName, PathPointType.WayPoint);
            pathPoint.Direction = direction;

            return pathPoint;
        }

        /// <summary>
        /// 基于智能判断生成吊装路径
        /// 支持用户点击地面投影位置，系统自动判断方向并生成空中路径点
        /// </summary>
        /// <param name="startPoint">地面起点（用户点击）</param>
        /// <param name="endPoint">地面终点（用户点击）</param>
        /// <param name="liftHeightMeters">吊装高度（米）</param>
        /// <param name="groundIntermediatePoints">用户点击的地面中间点列表</param>
        /// <returns>生成的路径点列表</returns>
        public List<PathPoint> GenerateSmartHoistingPath(
            Point3D startPoint,
            Point3D endPoint,
            double liftHeightMeters,
            List<Point3D> groundIntermediatePoints)
        {
            if (startPoint == null)
                throw new ArgumentNullException(nameof(startPoint));
            if (endPoint == null)
                throw new ArgumentNullException(nameof(endPoint));
            if (liftHeightMeters <= 0)
                throw new ArgumentException("提升高度必须大于0", nameof(liftHeightMeters));
            if (groundIntermediatePoints == null)
                throw new ArgumentNullException(nameof(groundIntermediatePoints));

            var pathPoints = new List<PathPoint>();
            double liftHeightModelUnits = UnitsConverter.ConvertFromMeters(liftHeightMeters);
            double liftElevation = HoistingCoordinateHelper.GetElevation(startPoint, _adapter) + liftHeightModelUnits;

            // 点1：起吊点（地面起点）
            var startPathPoint = new PathPoint(
                startPoint,
                "起吊点",
                PathPointType.StartPoint);
            startPathPoint.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(startPathPoint);

            // 点2：提升点（起点正上方，吊装高度）
            var liftPoint = HoistingCoordinateHelper.OffsetAlongUp(startPoint, liftHeightModelUnits, _adapter);
            var liftPathPoint = new PathPoint(
                liftPoint,
                "提升点",
                PathPointType.WayPoint);
            liftPathPoint.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(liftPathPoint);

            // 中间点（智能判断方向）
            Point3D previousPoint = liftPoint;

            for (int i = 0; i < groundIntermediatePoints.Count; i++)
            {
                var newPoint = GenerateSmartHoistingPoint(
                    previousPoint,
                    groundIntermediatePoints[i],
                    liftElevation,
                    i + 1);

                pathPoints.Add(newPoint);
                previousPoint = newPoint.Position;
            }

            // 最后一个空中点（用于垂直下降）
            var lastAerialPoint = previousPoint;
            var descendPathPoint = new PathPoint(
                lastAerialPoint,
                "下降点",
                PathPointType.WayPoint);
            descendPathPoint.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(descendPathPoint);

            // 落地点（地面终点）
            var endPathPoint = new PathPoint(
                endPoint,
                "落地点",
                PathPointType.EndPoint);
            endPathPoint.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(endPathPoint);

            return pathPoints;
        }

        /// <summary>
        /// 生成多层阶梯式吊装路径
        /// 支持在多个高度层进行水平移动，可混合上升和下降
        /// </summary>
        /// <param name="startPoint">起点（地面起吊位置）</param>
        /// <param name="endPoint">终点（地面落点位置）</param>
        /// <param name="levels">层级信息列表（按顺序执行）</param>
        /// <returns>路径点列表</returns>
        public List<PathPoint> GenerateMultiLevelHoistingPath(
            Point3D startPoint,
            Point3D endPoint,
            List<HoistingLevel> levels)
        {
            if (startPoint == null)
                throw new ArgumentNullException(nameof(startPoint));
            if (endPoint == null)
                throw new ArgumentNullException(nameof(endPoint));
            if (levels == null || levels.Count == 0)
                throw new ArgumentException("至少需要指定一个层级", nameof(levels));

            var pathPoints = new List<PathPoint>();

            // 点1：起吊点（物体初始位置）
            var startPointPath = new PathPoint(
                startPoint,
                "起吊点",
                PathPointType.StartPoint);
            startPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(startPointPath);

            Point3D currentPoint = startPoint;

            // 按层级顺序生成路径点
            for (int i = 0; i < levels.Count; i++)
            {
                var level = levels[i];
                double levelHeightModelUnits = UnitsConverter.ConvertFromMeters(level.HeightInMeters);
                double currentHeight = HoistingCoordinateHelper.GetElevation(startPoint, _adapter) + levelHeightModelUnits;

                // 垂直移动点（上升或下降到该层级高度）
                var verticalPoint = HoistingCoordinateHelper.SetElevation(currentPoint, currentHeight, _adapter);

                string verticalPointName = level.IsRise ? $"提升点{i + 1}" : $"下降点{i + 1}";
                var verticalPathPoint = new PathPoint(
                    verticalPoint,
                    verticalPointName,
                    PathPointType.WayPoint);
                verticalPathPoint.Direction = HoistingPointDirection.Vertical;
                pathPoints.Add(verticalPathPoint);

                // 水平移动点（移动到该层级的目标位置，保持高度）
                var horizontalPoint = HoistingCoordinateHelper.SetElevation(level.HorizontalTarget, currentHeight, _adapter);

                string horizontalPointName = $"平移点{i + 1}";
                var horizontalPathPoint = new PathPoint(
                    horizontalPoint,
                    horizontalPointName,
                    PathPointType.WayPoint);
                horizontalPathPoint.Direction = HoistingPointDirection.Longitudinal;
                pathPoints.Add(horizontalPathPoint);

                // 更新当前点为水平移动后的位置
                currentPoint = horizontalPoint;
            }

            // 最后：垂直下降到地面终点
            var descendPoint = HoistingCoordinateHelper.ProjectToAerialElevation(endPoint, currentPoint, _adapter);
            var descendPathPoint = new PathPoint(
                descendPoint,
                "最终下降点",
                PathPointType.WayPoint);
            descendPathPoint.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(descendPathPoint);

            // 点N：落地点（物体最终位置）
            var endPointPath = new PathPoint(
                endPoint,
                "落地点",
                PathPointType.EndPoint);
            endPointPath.Direction = HoistingPointDirection.Vertical;
            pathPoints.Add(endPointPath);

            return pathPoints;
        }
    }
}