zengzy / hhrcs_service · Files

using System.Numerics;
using System.Text;
using Rcs.Domain.Entities;
using Rcs.Domain.ValueObjects;

namespace Rcs.Shared.Utils
{
    public static class CraHandle
    {
        /// <summary>
        /// 切向角度阈值。
        /// </summary>
        public static double TangentAngleThresholdRad { get; set; } = AngleConstants.AllowedDeviationTheta;
        /// <summary>
        /// 计算圆弧中点坐标
        /// </summary>
        /// <param name="startX">起点X坐标</param>
        /// <param name="startY">起点Y坐标</param>
        /// <param name="endX">终点X坐标</param>
        /// <param name="endY">终点Y坐标</param>
        /// <param name="centerX">圆心X坐标</param>
        /// <param name="centerY">圆心Y坐标</param>
        /// <returns>中点坐标 (x, y)</returns>
        public static (double x, double y) GetArcMidPoint(
            double startX, double startY,
            double endX, double endY,
            double centerX, double centerY)
        {
            // 计算起点和终点相对于圆心的极角
            double thetaStart = Math.Atan2(startY - centerY, startX - centerX);
            double thetaEnd = Math.Atan2(endY - centerY, endX - centerX);

            // 确保角度按顺时针或逆时针方向正确
            double deltaTheta = thetaEnd - thetaStart;

            // 调整角度范围到 [-π, π]
            if (deltaTheta > Math.PI)
                deltaTheta -= 2 * Math.PI;
            else if (deltaTheta < -Math.PI)
                deltaTheta += 2 * Math.PI;

            // 中点角
            double thetaMid = thetaStart + deltaTheta / 2;

            // 半径
            double r = Math.Sqrt((startX - centerX) * (startX - centerX) +
                                 (startY - centerY) * (startY - centerY));

            // 计算中点坐标
            double midX = centerX + r * Math.Cos(thetaMid);
            double midY = centerY + r * Math.Sin(thetaMid);

            return (midX, midY);
        }
        public static Vector2 GetCircleCenter(Vector2 P0, Vector2 P1, Vector2 P2)
        {
            float x0 = P0.X, y0 = P0.Y;
            float x1 = P1.X, y1 = P1.Y;
            float x2 = P2.X, y2 = P2.Y;

            float midX01 = (x0 + x1) / 2;
            float midY01 = (y0 + y1) / 2;
            float midX12 = (x1 + x2) / 2;
            float midY12 = (y1 + y2) / 2;

            float k01 = (x1 - x0) != 0 ? (y1 - y0) / (x1 - x0) : float.NaN;
            float k12 = (x2 - x1) != 0 ? (y2 - y1) / (x2 - x1) : float.NaN;

            float cx, cy;

            if (float.IsNaN(k01)) // P0P1 垂直
            {
                cx = midX01;
                float k12_perp = (k12 != 0) ? -1 / k12 : float.NaN;
                cy = float.IsNaN(k12_perp) ? midY12 : k12_perp * (cx - midX12) + midY12;
            }
            else if (float.IsNaN(k12)) // P1P2 垂直
            {
                cx = midX12;
                float k01_perp = (k01 != 0) ? -1 / k01 : float.NaN;
                cy = float.IsNaN(k01_perp) ? midY01 : k01_perp * (cx - midX01) + midY01;
            }
            else
            {
                float k01_perp = (k01 != 0) ? -1 / k01 : float.NaN;
                float k12_perp = (k12 != 0) ? -1 / k12 : float.NaN;

                cx = (k01_perp * midX01 - k12_perp * midX12 + midY12 - midY01) / (k01_perp - k12_perp);
                cy = k01_perp * (cx - midX01) + midY01;
            }

            return new Vector2(cx, cy);
        }

        /// <summary>
        /// 圆弧上三点计算起点切线向量
        /// </summary>
        /// <param name="P0"></param>
        /// <param name="P1"></param>
        /// <param name="P2"></param>
        /// <returns></returns>
        public static Vector2 GetOnlineTangentAtStart(Vector2 P0, Vector2 P1, Vector2 P2)
        {
            Vector2 center = GetCircleCenter(P0, P1, P2);
            return GetTangentAtStart(P0, P2, center);
        }
        /// <summary>
        /// 起点(0,1)、终点(1,0)、圆心(1,1)的圆弧起点切线向量
        /// </summary>
        public static Vector2 GetTangentAtStart(Vector2 P0, Vector2 P1, Vector2 center)
        {
            Vector2 radiusVec = P0 - center;
            // 判断圆弧方向：使用叉积判断P0->P1->center的转向
            Vector2 vec1 = P1 - P0; //(1,-1)
            Vector2 vec2 = center - P1; //(0,1)
            float crossProduct = vec1.X * vec2.Y - vec1.Y * vec2.X;
            Vector2 tangent;
            if (crossProduct > 0)// 逆时针
            {
                tangent = new Vector2(-radiusVec.Y, radiusVec.X);
            }
            else
            {
                tangent = new Vector2(radiusVec.Y, -radiusVec.X);
            }
            tangent = Vector2.Normalize(tangent);
            return tangent;
        }
        /// <summary>
        /// 起点、终点、圆心的圆弧终点切线向量
        /// </summary>
        public static Vector2 GetTangentAtEnd(Vector2 P0, Vector2 P1, Vector2 center)
        {
            Vector2 radiusVec = P1 - center;
            // 判断圆弧方向：使用叉积判断P0->P1->center的转向
            Vector2 vec1 = P1 - P0;
            Vector2 vec2 = center - P1;
            float crossProduct = vec1.X * vec2.Y - vec1.Y * vec2.X;
            Vector2 tangent;
            if (crossProduct > 0)
            {
                tangent = new Vector2(radiusVec.Y, -radiusVec.X);
            }
            else
            {
                tangent = new Vector2(-radiusVec.Y, radiusVec.X);
            }
            tangent = Vector2.Normalize(tangent);
            return tangent;
        }
        /// <summary>
        /// 圆弧上三点计算终点切线向量
        /// </summary>
        public static Vector2 GetOnlineTangentAtEnd(Vector2 P0, Vector2 P1, Vector2 P2)
        {
            Vector2 center = GetCircleCenter(P0, P1, P2);
            return GetTangentAtEnd(P0, P2, center);
        }
        

        /// <summary>
        /// 获取边在指定节点的“进入方向向量”。
        /// </summary>
        public static Vector2 GetArrivalVector(MapEdge edge, Guid nodeId)
        {
            var pts = edge.ControlPoints;
            if (!edge.IsCurve)
            {
                var from = new Vector2((float)edge.MapNodesFromNode.X, (float)edge.MapNodesFromNode.Y);
                var to = new Vector2((float)edge.MapNodesToNode.X, (float)edge.MapNodesToNode.Y);
                return Vector2.Normalize(nodeId == edge.FromNode ? from - to : to - from);
            }
            if (nodeId == edge.FromNode)
                return GetTangentAtStart(
                    new Vector2((float)pts[0].X, (float)pts[0].Y),
                    new Vector2((float)pts[1].X, (float)pts[1].Y),
                    new Vector2((float)pts[pts.Count - 1].X, (float)pts[pts.Count - 1].Y));
            else
            {
                return GetTangentAtEnd(
                    new Vector2((float)pts[0].X, (float)pts[0].Y),
                    new Vector2((float)pts[1].X, (float)pts[1].Y),
                    new Vector2((float)pts[pts.Count - 1].X, (float)pts[pts.Count - 1].Y)
                    );
            }
        }
        /// <summary>
        /// 获取边在指定节点的“出发方向向量”。
        /// </summary>
        public static Vector2 GetDepartureVector(MapEdge edge, Guid nodeId)
        {
            if (!edge.IsCurve)
            {
                var from = new Vector2((float)edge.MapNodesFromNode.X, (float)edge.MapNodesFromNode.Y);
                var to = new Vector2((float)edge.MapNodesToNode.X, (float)edge.MapNodesToNode.Y);
                return Vector2.Normalize(nodeId == edge.FromNode ? to - from : from - to);
            }
            var pts = edge.ControlPoints;
            if (nodeId == edge.FromNode)
                return GetTangentAtStart(
                    new Vector2((float)pts[0].X, (float)pts[0].Y),
                    new Vector2((float)pts[1].X, (float)pts[1].Y),
                    new Vector2((float)pts[pts.Count - 1].X, (float)pts[pts.Count - 1].Y));
            else
            {
                return GetTangentAtEnd(
                    new Vector2((float)pts[0].X, (float)pts[0].Y),
                    new Vector2((float)pts[1].X, (float)pts[1].Y),
                    new Vector2((float)pts[pts.Count - 1].X, (float)pts[pts.Count - 1].Y)
                    );
            }
        }
        /// <summary>
        /// 计算两个向量的夹角（弧度）。
        /// </summary>
        public static double AngleBetween(Vector2 a, Vector2 b)
        {
            double dot = Vector2.Dot(a, b);
            double len = a.Length() * b.Length();
            if (len == 0) return 0;
            double cos = Math.Clamp(dot / len, -1, 1);
            return Math.Acos(cos);
        }

        /// <summary>
        /// 检查两个方向向量是否相切（考虑倒车/前进模式）
        /// </summary>
        /// <param name="vehicleDirection">车辆方向向量</param>
        /// <param name="pathDirection">路径切线方向向量</param>
        /// <param name="isReverse">是否为倒车路径</param>
        /// <returns></returns>
        public static bool IsTangentCompatible(Vector2 vehicleDirection, Vector2 pathDirection, Vector2? prevDirection, bool isReverse = false)
        {
            // 路径夹角
            var angle = prevDirection.HasValue ? AngleBetween(prevDirection.Value, pathDirection)
                : 0;
            // 车辆行驶方向与路径夹角
            var angleAgv = AngleBetween(vehicleDirection, pathDirection);
            var IsTangentAgv = !isReverse ? angleAgv < TangentAngleThresholdRad
                 : Math.PI - TangentAngleThresholdRad < angleAgv && angleAgv <= Math.PI;

            return (angle <= TangentAngleThresholdRad || Math.Abs(Math.PI- angle) <= TangentAngleThresholdRad)
                && IsTangentAgv;
        }

        public static double GetEdgeHeading(MapEdge edge, Guid fromNodeId)
        {
            var vec = GetDepartureVector(edge, fromNodeId);
            return Math.Atan2(vec.Y, vec.X);
        }

        /// <summary>
        /// 计算车体在边上的实际航向角，考虑当前车体朝向和倒车模式
        /// </summary>
        /// <param name="edge">路径边</param>
        /// <param name="fromNodeId">起始节点ID</param>
        /// <param name="currentVehicleHeading">当前车体航向角（弧度）</param>
        /// <returns>车体在该边上的实际航向角</returns>
        public static double GetVehicleHeadingOnEdge(MapEdge edge, Guid fromNodeId, double currentVehicleHeading)
        {
            if (!edge.IsCurve)
            {
                // 车体朝向保持不变（车体不转向，只是沿路径反向移动）
                return currentVehicleHeading;
            }
            else
            {
                // 弧线：车体航向需要根据弧线的转角进行调整
                var arcRotation = GetArcRotationAngle(edge, fromNodeId);
                return currentVehicleHeading + arcRotation;
            }
        }

        /// <summary>
        /// 计算弧线的转角（从起点到终点的角度变化）
        /// </summary>
        /// <param name="edge">弧线边</param>
        /// <param name="fromNodeId">起始节点ID</param>
        /// <returns>转角（弧度），正值表示逆时针，负值表示顺时针</returns>
        public static double GetArcRotationAngle(MapEdge edge, Guid fromNodeId)
        {
            if (!edge.IsCurve)
                return 0; // 直线没有转角

            var pts = edge.ControlPoints;
            if (pts == null || pts.Count < 3)
                return 0;

            var P0 = new Vector2((float)pts[0].X, (float)pts[0].Y);
            var P1 = new Vector2((float)pts[1].X, (float)pts[1].Y);
            var center = new Vector2((float)pts[2].X, (float)pts[2].Y);

            // 计算起点和终点相对于圆心的角度
            var startAngle = Math.Atan2(P0.Y - center.Y, P0.X - center.X);
            var endAngle = Math.Atan2(P1.Y - center.Y, P1.X - center.X);

            // 计算角度差
            var deltaAngle = endAngle - startAngle;

            // 标准化角度到 [-π, π] 范围
            while (deltaAngle > Math.PI) deltaAngle -= 2 * Math.PI;
            while (deltaAngle < -Math.PI) deltaAngle += 2 * Math.PI;

            // 根据fromNodeId确定方向
            if (fromNodeId == edge.FromNode)
            {
                // 从起点到终点
                return deltaAngle;
            }
            else
            {
                // 从终点到起点（反向）
                return -deltaAngle;
            }
        }

        /// <summary>
        /// 计算圆弧NURBS控制点、权重和节点向量
        /// 使用三次NURBS（4个控制点）确保精确表示任意角度的圆弧
        /// </summary>
        /// <param name="points">圆弧参数列表：points[0]=start, points[1]=end, points[2]=center</param>
        public static void CalculateArc(List<Vector2> points,
                                               out List<Point> controlPointsStr,
                                               out List<double> weightsStr,
                                               out List<double> knotsStr)
        {
            if (points == null || points.Count < 3)
                throw new ArgumentException("points列表必须包含至少3个元素：start, end, center");

            Vector2 start = points[0];
            Vector2 end = points[1];
            Vector2 center = points[2];

            float R = Vector2.Distance(start, center);
            double thetaStart = Math.Atan2(start.Y - center.Y, start.X - center.X);
            double thetaEnd = Math.Atan2(end.Y - center.Y, end.X - center.X);
            double deltaTheta = thetaEnd - thetaStart;
            
            // 标准化到 [-π, π]
            while (deltaTheta > Math.PI) deltaTheta -= 2 * Math.PI;
            while (deltaTheta < -Math.PI) deltaTheta += 2 * Math.PI;

            Vector2[] controlPoints;
            double[] weights;
            double[] knots;

            // 使用三次NURBS - 4个控制点，适合AGV路径规划
            controlPoints = new Vector2[4];
            weights = new double[4];
            knots = new double[8];

            // 行驶方向（正值为逆时针，负值为顺时针）
            bool isCCW = deltaTheta > 0;

            // 计算切线方向（与行驶方向一致）
            Vector2 rs = start - center;
            Vector2 re = end - center;
            Vector2 startTangent = isCCW
                ? new Vector2(-rs.Y, rs.X)   // 逆时针：半径左转90°
                : new Vector2(rs.Y, -rs.X);  // 顺时针：半径右转90°
            Vector2 endTangent = isCCW
                ? new Vector2(-re.Y, re.X)
                : new Vector2(re.Y, -re.X);

            // 标准化切线向量
            startTangent = Vector2.Normalize(startTangent);
            endTangent = Vector2.Normalize(endTangent);

            // 计算控制点：起点、两个内部控制点、终点
            // 使用经典三次贝塞尔圆弧近似：k = 4/3 * tan(|Δθ|/4)
            float controlDistance = (float)((4.0 / 3.0) * Math.Tan(Math.Abs(deltaTheta) / 4.0) * R);

            controlPoints[0] = start;
            controlPoints[1] = start + startTangent * controlDistance;
            controlPoints[2] = end - endTangent * controlDistance;
            controlPoints[3] = end;

            // 权重：使用精确的NURBS圆弧权重
            double cosHalfAngle = Math.Cos(Math.Abs(deltaTheta) / 4.0);
            weights[0] = 1.0;
            weights[1] = cosHalfAngle;
            weights[2] = cosHalfAngle;
            weights[3] = 1.0;

            // 节点向量：三次NURBS标准配置
            knots = new double[] { 0, 0, 0, 0, 1, 1, 1, 1 };

            // 序列化为字符串
            controlPointsStr = SerializeVectorArray(controlPoints);
            weightsStr = weights.ToList();
            knotsStr = knots.ToList();
        }

        /// <summary>
        /// 生成“前进”方向的三次贝塞尔曲线控制点，用于AGV前进不自旋的轨迹表达。
        /// 输入 points: [0]=start, [1]=end, [2]=center（圆弧弧心）。
        /// 输出为4个控制点（起点、两个控制点、终点）。
        /// </summary>
        public static void CalculateForwardBezier(List<Vector2> points,
                                                   out List<Point> controlPointsStr,
                                                   out List<double> weightsStr,
                                                   out List<double> knotsStr)
        {
            if (points == null || points.Count < 3)
                throw new ArgumentException("points列表必须包含至少3个元素：start, end, center");

            Vector2 start = points[0];
            Vector2 end = points[1];
            Vector2 center = points[2];

            float R = Vector2.Distance(start, center);
            if (R <= 0)
            {
                // 退化处理：返回直线贝塞尔
                Vector2 c1 = start + (end - start) / 3f;
                Vector2 c2 = start + 2f * (end - start) / 3f;
                controlPointsStr = SerializeVectorArray(new[] { start, c1, c2, end });
                weightsStr = new List<double> { 1.0, 1.0, 1.0, 1.0 };
                knotsStr = new List<double> { 0, 0, 0, 0, 1, 1, 1, 1 };
                return;
            }

            double thetaStart = Math.Atan2(start.Y - center.Y, start.X - center.X);
            double thetaEnd = Math.Atan2(end.Y - center.Y, end.X - center.X);
            double deltaTheta = thetaEnd - thetaStart;
            while (deltaTheta > Math.PI) deltaTheta -= 2 * Math.PI;
            while (deltaTheta < -Math.PI) deltaTheta += 2 * Math.PI;

            if (Math.Abs(deltaTheta) < 1e-9)
            {
                // 近似直线
                Vector2 c1 = start + (end - start) / 3f;
                Vector2 c2 = start + 2f * (end - start) / 3f;
                controlPointsStr = SerializeVectorArray(new[] { start, c1, c2, end });
                weightsStr = new List<double> { 1.0, 1.0, 1.0, 1.0 };
                knotsStr = new List<double> { 0, 0, 0, 0, 1, 1, 1, 1 };
                return;
            }

            bool isCCW = deltaTheta > 0;
            Vector2 rs = start - center;
            Vector2 re = end - center;
            Vector2 t0 = isCCW ? new Vector2(-rs.Y, rs.X) : new Vector2(rs.Y, -rs.X);
            Vector2 t1 = isCCW ? new Vector2(-re.Y, re.X) : new Vector2(re.Y, -re.X);
            t0 = Vector2.Normalize(t0);
            t1 = Vector2.Normalize(t1);

            float kR = (float)((4.0 / 3.0) * Math.Tan(Math.Abs(deltaTheta) / 4.0) * R);

            Vector2 p0 = start;
            Vector2 p1 = start + t0 * kR;
            Vector2 p2 = end - t1 * kR;
            Vector2 p3 = end;

            controlPointsStr = SerializeVectorArray(new[] { p0, p1, p2, p3 });
            double cosHalfAngle = Math.Cos(Math.Abs(deltaTheta) / 4.0);
            weightsStr = new List<double> { 1.0, cosHalfAngle, cosHalfAngle, 1.0 };
            knotsStr = new List<double> { 0, 0, 0, 0, 1, 1, 1, 1 };
        }

        /// <summary>
        /// 统一接口：根据是否倒车选择前进（不自旋）或倒车（原NURBS弧）表达。
        /// points: [0]=start, [1]=end, [2]=center
        /// </summary>
        /// <param name="points">start, end, center</param>
        /// <param name="isReverse">true=倒车，false=前进</param>
        /// <param name="controlPointsStr">输出：控制点（4个）</param>
        /// <param name="weightsStr">输出：权重</param>
        /// <param name="knotsStr">输出：节点向量</param>
        public static void CalculateBezierForMotion(List<Vector2> points,
                                                    bool isReverse,
                                                    out List<Point> controlPointsStr,
                                                    out List<double> weightsStr,
                                                    out List<double> knotsStr)
        {
            if (isReverse)
            {
                // 倒车：使用圆弧NURBS表达（车辆方向不变，沿路径反向移动）
                CalculateArc(points, out controlPointsStr, out weightsStr, out knotsStr);
            }
            else
            {
                // 前进：使用前进方向的三次贝塞尔表达，避免不必要自旋
                CalculateForwardBezier(points, out controlPointsStr, out weightsStr, out knotsStr);
            }
        }

        private static List<Point> SerializeVectorArray(Vector2[] arr)
        {
            var ps = new List<Point>(); 
            for (int i = 0; i < arr.Length; i++)
            {
                Point point = new Point();
                point.X = arr[i].X;
                point.Y = arr[i].Y;
                ps.Add(point);
            }
            
            return ps;
        }

        private static string SerializeDoubleArray(double[] arr)
        {
            StringBuilder sb = new StringBuilder();
            sb.Append("[");
            for (int i = 0; i < arr.Length; i++)
            {
                sb.AppendFormat("{0:F6}", arr[i]);
                if (i < arr.Length - 1) sb.Append(", ");
            }
            sb.Append("]");
            return sb.ToString();
        }
    }
}