Display_D_HP/TeamAAS-VM/Core/RectangleCenterCalculator.cs

using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace TeamAAS_VP.Core
{
    /// <summary>
    /// 提供多种方法用于计算由四个角点定义的矩形中心点、长轴角度以及相关矩形参数的辅助类。
    /// 角点顺序约定：左上(0) → 右上(1) → 右下(2) → 左下(3)
    /// 长轴方向：从左上角(0)指向右上角(1)
    /// 角度范围：[-180, 180) 度，0°指向X轴正方向，逆时针为正
    /// </summary>
    public class RectangleCenterCalculator
    {
        /// <summary>
        /// 矩形中心及方向信息的综合结果
        /// </summary>
        public class RectangleResult
        {
            /// <summary>
            /// 矩形中心点坐标
            /// </summary>
            public Vector<double> Center { get; set; }

            /// <summary>
            /// 矩形长轴方向向量（从左上指向右上的单位向量）
            /// </summary>
            public Vector<double> MajorAxis { get; set; }

            /// <summary>
            /// 矩形短轴方向向量（单位向量）
            /// </summary>
            public Vector<double> MinorAxis { get; set; }

            /// <summary>
            /// 矩形长度（沿长轴方向，左上到右上的距离）
            /// </summary>
            public double Length { get; set; }

            /// <summary>
            /// 矩形宽度（沿短轴方向）
            /// </summary>
            public double Width { get; set; }

            /// <summary>
            /// 长轴方向角度[-180, 180)度
            /// 从左上指向右上的向量在XY平面的投影角度
            /// 0° = 指向X轴正方向，正角度逆时针，负角度顺时针
            /// </summary>
            public double MajorAxisAngle { get; set; }

            /// <summary>
            /// 四个角点的坐标（保持输入顺序）
            /// </summary>
            public List<Vector<double>> Corners { get; set; }

            /// <summary>
            /// 左上角点坐标
            /// </summary>
            public Vector<double> TopLeft => Corners != null && Corners.Count >= 1 ? Corners[0] : null;

            /// <summary>
            /// 右上角点坐标
            /// </summary>
            public Vector<double> TopRight => Corners != null && Corners.Count >= 2 ? Corners[1] : null;

            /// <summary>
            /// 右下角点坐标
            /// </summary>
            public Vector<double> BottomRight => Corners != null && Corners.Count >= 3 ? Corners[2] : null;

            /// <summary>
            /// 左下角点坐标
            /// </summary>
            public Vector<double> BottomLeft => Corners != null && Corners.Count >= 4 ? Corners[3] : null;

            /// <summary>
            /// 矩形的面积
            /// </summary>
            public double Area { get; set; }

            /// <summary>
            /// 拟合误差（均方根误差）
            /// </summary>
            public double FitError { get; set; }
        }

        /// <summary>
        /// 验证角点顺序并进行必要的调整
        /// 确保顺序为：左上 → 右上 → 右下 → 左下
        /// </summary>
        private static List<Vector<double>> ValidateAndOrderCorners(List<Vector<double>> corners)
        {
            if (corners == null || corners.Count != 4)
                throw new ArgumentException("需要4个角点");

            return SortCornersByTopBottomHeuristic(corners);
        }

        /// <summary>
        /// 计算从左上指向右上的长轴方向角度
        /// </summary>
        private static double CalculateMajorAxisAngle(Vector<double> topLeft, Vector<double> topRight)
        {
            // 计算方向向量（从左上指向右上）
            Vector<double> direction = topRight - topLeft;

            // 只考虑XY平面上的投影
            double x = direction[0];
            double y = direction[1];

            // 使用Math.Atan2计算角度（弧度）
            double angleRad = Math.Atan2(y, x);

            // 转换为度，得到有符号角度 [-180, 180)
            double angleDeg = angleRad * 180.0 / Math.PI;

            // Math.Atan2的结果已经是[-180, 180]，但确保一下边界
            if (angleDeg >= 180.0)
                angleDeg -= 360.0;
            else if (angleDeg < -180.0)
                angleDeg += 360.0;

            return angleDeg;
        }

        /// <summary>
        /// 使用最小二乘法思想计算最优矩形中心点及相关参数（对外统一入口）
        /// 推荐使用改进的PCA方法，因为它能利用所有点信息进行优化
        /// </summary>
        public static RectangleResult CalculateOptimalCenter(List<Vector<double>> corners)
        {
            // 验证和调整角点顺序
            var orderedCorners = ValidateAndOrderCorners(corners);

            // 使用改进的PCA方法（考虑已知顺序）
            return CalculateByPCAWithKnownOrder(orderedCorners);
        }

        /// <summary>
        /// 方法1：基于已知角点顺序的直接计算（最准确）
        /// 直接使用左上和右上点计算长轴，然后计算其他参数
        /// </summary>
        public static RectangleResult CalculateByDirectMethod(List<Vector<double>> corners)
        {
            var orderedCorners = ValidateAndOrderCorners(corners);

            var topLeft = orderedCorners[0];
            var topRight = orderedCorners[1];
            var bottomRight = orderedCorners[2];
            var bottomLeft = orderedCorners[3];

            // 计算长轴（从左上指向右上）
            Vector<double> majorAxis = (topRight - topLeft).Normalize(2);

            // 计算短轴（从左上指向左下）
            Vector<double> minorAxis = (bottomLeft - topLeft).Normalize(2);

            // 确保短轴与长轴垂直（处理非正交情况）
            minorAxis = (minorAxis - majorAxis * minorAxis.DotProduct(majorAxis)).Normalize(2);

            int dim = orderedCorners[0].Count;
            // 计算中心点（四个角点的平均）
            var center = Vector<double>.Build.DenseOfEnumerable(
                            Enumerable.Range(0, dim)
                            .Select(i => orderedCorners.Select(c => c[i]).Average()));

            // 计算长度和宽度
            double length = Distance(topLeft, topRight);
            double width = Distance(topLeft, bottomLeft);

            // 计算长轴角度
            double angle = CalculateMajorAxisAngle(topLeft, topRight);

            // 计算拟合误差
            double fitError = CalculateFitErrorDirect(center, majorAxis, minorAxis, length, width, orderedCorners);

            return new RectangleResult
            {
                Center = center,
                MajorAxis = majorAxis,
                MinorAxis = minorAxis,
                Length = length,
                Width = width,
                MajorAxisAngle = angle,
                Corners = orderedCorners,
                Area = length * width,
                FitError = fitError
            };
        }

        /// <summary>
        /// 方法2：使用PCA但考虑已知角点顺序（推荐）
        /// 结合PCA的鲁棒性和已知顺序的准确性
        /// </summary>
        public static RectangleResult CalculateByPCAWithKnownOrder(List<Vector<double>> corners)
        {
            //打印输入点以调试
            //Console.WriteLine($"输入点 [1] : {corners[0][0]:F3}，{corners[0][1]:F3}");
            //Console.WriteLine($"输入点 [2] : {corners[1][0]:F3}，{corners[1][1]:F3}");
            //Console.WriteLine($"输入点 [3] : {corners[2][0]:F3}，{corners[2][1]:F3}");
            //Console.WriteLine($"输入点 [4] : {corners[3][0]:F3}，{corners[3][1]:F3}");

            var orderedCorners = ValidateAndOrderCorners(corners);
            Console.WriteLine($"");

            //打印输入点以调试
            //Console.WriteLine($"输出点 [1] : {orderedCorners[0][0]:F3}，{orderedCorners[0][1]:F3}");
            //Console.WriteLine($"输出点 [2] : {orderedCorners[1][0]:F3}，{orderedCorners[1][1]:F3}");
            //Console.WriteLine($"输出点 [3] : {orderedCorners[2][0]:F3}，{orderedCorners[2][1]:F3}");
            //Console.WriteLine($"输出点 [4] : {orderedCorners[3][0]:F3}，{orderedCorners[3][1]:F3}");

            int dim = orderedCorners[0].Count;
            // 步骤1：使用所有点进行PCA得到初步估计
            var matrix = Matrix<double>.Build.DenseOfRows(orderedCorners);
            var mean = Vector<double>.Build.DenseOfEnumerable(
                Enumerable.Range(0, dim).Select(i => matrix.Column(i).Average()));

            var centered = matrix.Clone();
            for (int i = 0; i < 4; i++)
            {
                centered.SetRow(i, centered.Row(i) - mean);
            }

            var covariance = centered.Transpose() * centered / 3.0;
            var evd = covariance.Evd();

            // 获取特征向量
            var eigenvectors = evd.EigenVectors;

            // 步骤2：使用已知顺序确定长轴方向
            var topLeft = orderedCorners[0];
            var topRight = orderedCorners[1];

            // 计算实际的长轴方向（从左上指向右上）
            Vector<double> actualMajorDirection = (topRight - topLeft).Normalize(2);

            // 从PCA的特征向量中找到最接近实际方向的那个
            Vector<double> bestAxis = eigenvectors.Column(0);
            double bestSimilarity = Math.Abs(actualMajorDirection.DotProduct(bestAxis));

            for (int i = 1; i < eigenvectors.ColumnCount; i++)
            {
                var axis = eigenvectors.Column(i);
                double similarity = Math.Abs(actualMajorDirection.DotProduct(axis));
                if (similarity > bestSimilarity)
                {
                    bestSimilarity = similarity;
                    bestAxis = axis;
                }
            }

            // 确保PCA轴的方向与实际方向一致（点积为正）
            Vector<double> majorAxis = bestAxis;
            if (actualMajorDirection.DotProduct(majorAxis) < 0)
            {
                majorAxis = -majorAxis;
            }

            // 计算短轴（与长轴垂直）
            Vector<double> minorAxis = CalculatePerpendicularAxis(majorAxis);

            // 将点投影到轴上计算中心、长度和宽度
            var projMajor = orderedCorners.Select(p => (p - mean).DotProduct(majorAxis)).ToList();
            var projMinor = orderedCorners.Select(p => (p - mean).DotProduct(minorAxis)).ToList();

            double centerMajor = (projMajor.Min() + projMajor.Max()) / 2.0;
            double centerMinor = (projMinor.Min() + projMinor.Max()) / 2.0;
            var center = mean + majorAxis * centerMajor + minorAxis * centerMinor;

            double length = Math.Abs(projMajor.Max() - projMajor.Min());
            double width = Math.Abs(projMinor.Max() - projMinor.Min());

            // 计算长轴角度（使用实际的左上-右上方向）
            double angle = CalculateMajorAxisAngle(topLeft, topRight);

            // 计算拟合误差
            double fitError = CalculateFitError(center, majorAxis, minorAxis, length, width, orderedCorners);

            return new RectangleResult
            {
                Center = center,
                MajorAxis = majorAxis,
                MinorAxis = minorAxis,
                Length = length,
                Width = width,
                MajorAxisAngle = angle,
                Corners = orderedCorners,
                Area = length * width,
                FitError = fitError
            };
        }

        /// <summary>
        /// 方法3：对角线优化方法（考虑已知顺序）
        /// </summary>
        public static RectangleResult CalculateByDiagonalOptimization(List<Vector<double>> corners)
        {
            var orderedCorners = ValidateAndOrderCorners(corners);
            var topLeft = orderedCorners[0];
            var topRight = orderedCorners[1];

            // 直接使用已知顺序计算长轴角度
            double angle = CalculateMajorAxisAngle(topLeft, topRight);

            // 计算长轴方向向量
            Vector<double> majorAxis = (topRight - topLeft).Normalize(2);
            Vector<double> minorAxis = CalculatePerpendicularAxis(majorAxis);

            // 使用所有对角线组合计算最佳中心
            var diagonalCombinations = new[]
            {
            (orderedCorners[0], orderedCorners[2]), // 左上-右下
            (orderedCorners[1], orderedCorners[3])  // 右上-左下
        };

            var centers = diagonalCombinations.Select(p => (p.Item1 + p.Item2) / 2.0).ToList();
            var center = (centers[0] + centers[1]) / 2.0;

            // 计算长度和宽度
            double length = Distance(topLeft, topRight);
            double width = Distance(topLeft, orderedCorners[3]); // 左上-左下

            // 计算拟合误差
            double fitError = CalculateFitError(center, majorAxis, minorAxis, length, width, orderedCorners);

            return new RectangleResult
            {
                Center = center,
                MajorAxis = majorAxis,
                MinorAxis = minorAxis,
                Length = length,
                Width = width,
                MajorAxisAngle = angle,
                Corners = orderedCorners,
                Area = length * width,
                FitError = fitError
            };
        }

        /// <summary>
        /// 辅助方法：计算与给定向量垂直的单位向量
        /// </summary>
        private static Vector<double> CalculatePerpendicularAxis(Vector<double> axis)
        {
            if (axis == null) throw new ArgumentNullException(nameof(axis));
            int n = axis.Count;
            if (n == 0) throw new ArgumentException("axis must have positive dimension", nameof(axis));

            // 2D场景：(-y, x) 为垂直向量
            if (n == 2)
            {
                var perp2 = Vector<double>.Build.DenseOfArray(new[] { -axis[1], axis[0] });
                if (perp2.L2Norm() < 1e-12) throw new ArgumentException("axis is zero vector", nameof(axis));
                return perp2.Normalize(2);
            }

            // 通用n维：选一个不与axis共线的标准基向量
            int idx = 0;
            for (int i = 0; i < n; i++)
            {
                if (Math.Abs(axis[i]) < 0.9) { idx = i; break; }
            }
            var temp = Vector<double>.Build.Dense(n, i => i == idx ? 1.0 : 0.0);

            var perpendicular = temp - axis * temp.DotProduct(axis);
            if (perpendicular.L2Norm() < 1e-12) throw new InvalidOperationException("failed to compute perpendicular vector");
            return perpendicular.Normalize(2);
        }

        /// <summary>
        /// 辅助方法：计算两点距离
        /// </summary>
        private static double Distance(Vector<double> a, Vector<double> b)
        {
            return (a - b).L2Norm();
        }

        /// <summary>
        /// 辅助方法：计算拟合误差
        /// </summary>
        private static double CalculateFitError(Vector<double> center, Vector<double> majorAxis, Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
        {
            // 生成理想矩形的四个角点
            var idealCorners = new List<Vector<double>>
        {
            center + majorAxis * (length / 2) + minorAxis * (width / 2),   // 右上
            center + majorAxis * (length / 2) - minorAxis * (width / 2),   // 右下
            center - majorAxis * (length / 2) - minorAxis * (width / 2),   // 左下
            center - majorAxis * (length / 2) + minorAxis * (width / 2)    // 左上
        };

            // 注意：理想角点顺序可能与输入不同，需要重新排序匹配
            // 这里简单计算所有对应点的最小距离和
            double sumSquared = 0;
            for (int i = 0; i < 4; i++)
            {
                // 找到最近的理想角点
                double minDist = double.MaxValue;
                foreach (var ideal in idealCorners)
                {
                    double dist = Distance(corners[i], ideal);
                    if (dist < minDist) minDist = dist;
                }
                sumSquared += minDist * minDist;
            }

            return Math.Sqrt(sumSquared / 4);
        }

        /// <summary>
        /// 直接计算方法的拟合误差
        /// </summary>
        private static double CalculateFitErrorDirect(Vector<double> center, Vector<double> majorAxis, Vector<double> minorAxis, double length, double width, List<Vector<double>> corners)
        {
            // 对于直接方法，我们知道每个角点的理想位置
            var idealCorners = new List<Vector<double>>
        {
            center - majorAxis * (length / 2) + minorAxis * (width / 2),  // 左上
            center + majorAxis * (length / 2) + minorAxis * (width / 2),  // 右上
            center + majorAxis * (length / 2) - minorAxis * (width / 2),  // 右下
            center - majorAxis * (length / 2) - minorAxis * (width / 2)   // 左下
        };

            double sumSquared = 0;
            for (int i = 0; i < 4; i++)
            {
                sumSquared += Math.Pow(Distance(corners[i], idealCorners[i]), 2);
            }

            return Math.Sqrt(sumSquared / 4);
        }

        /// <summary>
        /// 自动排序角点：左上 → 右上 → 右下 → 左下
        /// </summary>
        /// <param name="unorderedCorners">任意顺序的四个角点</param>
        /// <returns>按标准顺序排序的角点列表</returns>
        public static List<Vector<double>> SortCornersToRectangleOrder(List<Vector<double>> unorderedCorners)
        {
            if (unorderedCorners == null || unorderedCorners.Count != 4)
                throw new ArgumentException("需要4个角点");

            // 方法1：基于上下分组+水平排序的稳健方法
            return SortCornersByTopBottomHeuristic(unorderedCorners);
        }

        /// <summary>
        /// 方法1：基于上下分组与水平排序的稳健方法
        /// </summary>
        private static List<Vector<double>> SortCornersByTopBottomHeuristic(List<Vector<double>> corners)
        {
            if (corners == null || corners.Count != 4)
                throw new ArgumentException("需要4个角点");

            var orderHighY = BuildOrderByTopBottom(corners, topIsHigher: true);
            var orderLowY = BuildOrderByTopBottom(corners, topIsHigher: false);

            double scoreHigh = RectangleOrderScore(orderHighY);
            double scoreLow = RectangleOrderScore(orderLowY);

            return scoreHigh <= scoreLow ? orderHighY : orderLowY;
        }

        private static List<Vector<double>> BuildOrderByTopBottom(List<Vector<double>> corners, bool topIsHigher)
        {
            var sortedByY = topIsHigher
                ? corners.OrderByDescending(p => p[1]).ToList()
                : corners.OrderBy(p => p[1]).ToList();

            var topCandidates = sortedByY.Take(2).ToList();
            var bottomCandidates = sortedByY.Skip(2).Take(2).ToList();

            var top = topCandidates.OrderBy(p => p[0]).ToList();
            var bottom = bottomCandidates.OrderBy(p => p[0]).ToList();

            var topLeft = top[0];
            var topRight = top[1];
            var bottomLeft = bottom[0];
            var bottomRight = bottom[1];

            return new List<Vector<double>> { topLeft, topRight, bottomRight, bottomLeft };
        }

        private static double RectangleOrderScore(List<Vector<double>> ordered)
        {
            if (ordered == null || ordered.Count != 4)
                return double.MaxValue;

            var topLeft = ordered[0];
            var topRight = ordered[1];
            var bottomRight = ordered[2];
            var bottomLeft = ordered[3];

            double top = Distance(topLeft, topRight);
            double bottom = Distance(bottomLeft, bottomRight);
            double left = Distance(topLeft, bottomLeft);
            double right = Distance(topRight, bottomRight);
            double diag1 = Distance(topLeft, bottomRight);
            double diag2 = Distance(topRight, bottomLeft);

            double parallelScore = Math.Abs(top - bottom) + Math.Abs(left - right) + Math.Abs(diag1 - diag2);

            double xOrderPenalty = 0.0;
            if (topLeft[0] > topRight[0]) xOrderPenalty += 1000.0;
            if (bottomLeft[0] > bottomRight[0]) xOrderPenalty += 1000.0;

            return parallelScore + xOrderPenalty;
        }

        /// <summary>
        /// 调整角点顺序为标准矩形顺序（左上→右上→右下→左下）
        /// </summary>
        private static List<Vector<double>> AdjustToRectangleOrder(List<Vector<double>> corners)
        {
            return SortCornersByTopBottomHeuristic(corners);
        }

    }
}