SimulationServer/Models/SimulationCommon/SectorSearch.cs

namespace SimulationCommon;
public class LocationW
{
    public double Lon { get; set; }
    public double Lat { get; set; }
}

public class SectorSearch
{
    // 计算向量角度
    private 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;
    }

    // 根据角度和距离移动点
    private static LocationW MovePointByVectorAndAngle(double angle, double distance, LocationW startPoint, double[] vector)
    {
        double vectorAngle = CalculateVectorAngle(vector) + angle;
        return new LocationW
        {
            Lon = startPoint.Lon + distance * Math.Cos(vectorAngle),
            Lat = startPoint.Lat + distance * Math.Sin(vectorAngle)
        };
    }

    // 主逻辑
    public static List<Point> PerformSearch( double sweepWidth)
    {
        var boundaryPoints = new List<LocationW>();
        var outputWaypoints = new List<Point>();

        var basePoint = new LocationW();
        var baseDir = new double[] { 0, 1 };
        double searchAngle = 30;
        // Console.WriteLine("输入搜索角度:");
        // double searchAngle = double.Parse(Console.ReadLine());
        //
        // Console.WriteLine("输入扫描宽度:");
        // double sweepWidth = double.Parse(Console.ReadLine());

        double rs = sweepWidth / Math.Sin(searchAngle / 2);

        int numPoints = (int)(360 / searchAngle);
        boundaryPoints.Add(basePoint);

        for (int i = 0; i < numPoints; i++)
        {
            var midpoint = MovePointByVectorAndAngle(i * searchAngle, rs, basePoint, baseDir);
            boundaryPoints.Add(midpoint);
            midpoint.Lat = basePoint.Lat;
            midpoint.Lon = basePoint.Lon;
        }

        var num = new int[(int)(3 * (180 / searchAngle) + 1)];
        num[0] = 0;
        for (int i = 1, n = 1, m = 1; n < 3 * (180 / searchAngle); i += 2)
        {
            m = i;
            num[n++] = m;
            m = i + 1;
            num[n++] = m;
            m = 0;
            num[n++] = m;
            m = (int)(i + 1 + 180 / searchAngle);
            num[n++] = m;
            m = (int)(i + 180 / searchAngle);
            num[n++] = m;
            m = 0;
            num[n++] = m;
        }

        foreach (var index in num)
        {
            outputWaypoints.Add(new Point() { lat = boundaryPoints[index].Lat, lon = boundaryPoints[index].Lon });
        }

        return outputWaypoints;
    }
}