SimulationServerYL/Models/SimulationCommon/ParallellineSearch.cs

namespace SimulationCommon;
public struct Point
{
    public double lat;
    public double lon;
    
    public Point(double lat, double lon)
    {
        this.lat = lat;
        this.lon = lon;
    }
}


public class ParallellineSearch
{
    public static double calculateVectorAngle(double[] vector)
    {
        double vectorCos = vector[0] / Math.Sqrt(vector[0] * vector[0] + vector[1] * vector[1]);
        double vectorAngle = 0;
        if (vector[1] >= 0)
        {
            vectorAngle = Math.Acos(vectorCos);
        }
        else if (vector[1] < 0)
        {
            vectorAngle = 2 * Math.PI - Math.Acos(vectorCos);
        }

        return vectorAngle;
    }

    public static Point movePointByVandA(double angle, double distance, Point startPoint, double[] vector)
    {
        ///定义并输出新坐标点
        Point finalPoint = new Point();
        //计算已知向量与x轴正方向的夹角
        double vectorAngle = calculateVectorAngle(vector);
        //计算目标点向量与x轴正方向的夹角
        vectorAngle += angle;
        //计算目标点向量夹角的正弦、余弦值
        double vectorAngleSin = Math.Sin(vectorAngle);
        double vectorAngleCos = Math.Cos(vectorAngle);
        //ArrayList<Double> XY = new ArrayList<Double>();
        finalPoint.lon = startPoint.lon + distance * vectorAngleCos;
        finalPoint.lat = startPoint.lat + distance * vectorAngleSin;
        return finalPoint;
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="plane"></param>
    /// <param name="RegionPoints"></param>
    /// <param name="sweepWidth">扫视宽度（需要换算成经纬度）sweepWidth = saruSweepWidth/(1.852 * 60) ;</param>
    /// <returns></returns>
    public static List<Point> parallellineSearch(Point plane, List<Point> RegionPoints, double sweepWidth)
    {
        Point[] points = new Point[4];
        points[0].lat = RegionPoints[0].lat;
        points[0].lon = RegionPoints[0].lon;
        points[1].lat = RegionPoints[1].lat;
        points[1].lon = RegionPoints[1].lon;
        points[2].lat = RegionPoints[2].lat;
        points[2].lon = RegionPoints[2].lon;
        points[3].lat = RegionPoints[3].lat;
        points[3].lon = RegionPoints[3].lon;

        List<Point> outputWaypoints = new List<Point>();

        int npIndex = 0; //最近点的索引nearestPoint
        double npDistance = Double.PositiveInfinity; //初始化最近距离为正无穷
        double midDistance = 0;
        for (int i = 0; i < points.Length; i++)
        {
            midDistance = Math.Sqrt(Math.Pow(points[i].lat - plane.lat, 2) + Math.Pow(points[i].lon - plane.lon, 2));
            if (midDistance < npDistance)
            {
                npDistance = midDistance;
                npIndex = i;
            }
        }


        //***找到最近点的领边哪个为长边,并定义长边方向与短边方向
        int lspIndex = 0; //longSidePointIndex
        int sspIndex = 0; //shortSidePointIndex
        //初始化长边索引与短边索引
        if (npIndex + 1 < 4)
            lspIndex = npIndex + 1;
        else
            lspIndex = npIndex + 1 - 4;
        if (npIndex - 1 < 0)
            sspIndex = npIndex - 1 + 4;
        else
            sspIndex = npIndex - 1;
        //计算长边的距离longSideDistance
        double lspDistance = Math.Sqrt(Math.Pow(points[npIndex].lat - points[lspIndex].lat, 2)
                                       + Math.Pow(points[npIndex].lon - points[lspIndex].lon, 2));
        //计算短边的距离shortSideDistance
        double sspDistance = Math.Sqrt(Math.Pow(points[npIndex].lat - points[sspIndex].lat, 2)
                                       + Math.Pow(points[npIndex].lon - points[sspIndex].lon, 2));
        //最终确定长边和短边的索引和长度
        int midIndex = 0;
        if (lspDistance < sspDistance)
        {
            //互换长边索引与短边索引的值
            midIndex = lspIndex;
            lspIndex = sspIndex;
            sspIndex = midIndex;
            //互换长边距离与短边距离
            midDistance = lspDistance;
            lspDistance = sspDistance;
            sspDistance = midDistance;
        }

        double[] lspDirection =
            { points[lspIndex].lon - points[npIndex].lon, points[lspIndex].lat - points[npIndex].lat };
        double[] sspDirection =
            { points[sspIndex].lon - points[npIndex].lon, points[sspIndex].lat - points[npIndex].lat };


        Point midWaypoint = new Point();
        //寻找路径规划的起点
        midWaypoint.lat = points[npIndex].lat;
        midWaypoint.lon = points[npIndex].lon;
        double pointnum = 0;
        //找到搜寻区域内路径的起点
        Point startWaypoint = new Point();
        startWaypoint.lat = movePointByVandA(0, sweepWidth / 2, midWaypoint, lspDirection).lat;
        startWaypoint.lon = movePointByVandA(0, sweepWidth / 2, midWaypoint, lspDirection).lon;
        midWaypoint.lat = startWaypoint.lat;
        midWaypoint.lon = startWaypoint.lon;
        startWaypoint.lat = movePointByVandA(0, sweepWidth / 2, midWaypoint, sspDirection).lat;
        startWaypoint.lon = movePointByVandA(0, sweepWidth / 2, midWaypoint, sspDirection).lon;


        //Console.WriteLine(startWaypoint.lon);
        //Console.WriteLine(startWaypoint.lat);
        //Console.WriteLine(sspDistance);
        //Console.WriteLine(lspDistance);

        outputWaypoints.Add(startWaypoint);
        pointnum++;
        midWaypoint.lat = startWaypoint.lat;
        midWaypoint.lon = startWaypoint.lon;

        //迭代计算搜寻区域内路径点
        double forwardDistance = sweepWidth / 2;
        double moveDistance;

        while (forwardDistance <= sspDistance)
        {
            //沿长边正方向移动longSideDistance-sweepWidth后的点
            moveDistance = lspDistance - sweepWidth;
            Point loopWaypoint1 = movePointByVandA(0, moveDistance, midWaypoint, lspDirection);
            outputWaypoints.Add(loopWaypoint1);
            pointnum++;
            midWaypoint.lat = loopWaypoint1.lat;
            midWaypoint.lon = loopWaypoint1.lon;
            //沿短边移动sweepWidth后的点
            moveDistance = sweepWidth;
            Point loopWaypoint2 = movePointByVandA(0, moveDistance, midWaypoint, sspDirection);
            outputWaypoints.Add(loopWaypoint2);
            pointnum++;
            midWaypoint.lat = loopWaypoint2.lat;
            midWaypoint.lon = loopWaypoint2.lon;
            forwardDistance += sweepWidth;
            if (forwardDistance > sspDistance)
            {
                break;
            }

            //沿长边负方向移动longSideDistance-sweepWidth后的点
            moveDistance = lspDistance - sweepWidth;
            Point loopWaypoint3 = movePointByVandA(Math.PI, moveDistance, midWaypoint, lspDirection);
            outputWaypoints.Add(loopWaypoint3);
            pointnum++;
            midWaypoint.lat = loopWaypoint3.lat;
            midWaypoint.lon = loopWaypoint3.lon;
            //沿短边移动sweepWidth后的点
            moveDistance = sweepWidth;
            Point loopWaypoint4 = movePointByVandA(0, moveDistance, midWaypoint, sspDirection);
            outputWaypoints.Add(loopWaypoint4);
            pointnum++;
            midWaypoint.lat = loopWaypoint4.lat;
            midWaypoint.lon = loopWaypoint4.lon;
            forwardDistance += sweepWidth;
            if (forwardDistance > sspDistance)
            {
                break;
            }
        }

        return outputWaypoints;
    }
}