完成 对接

Models/SimulationCommon/ParallellineSearch.cs

@@ -0,0 +1,197 @@
+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;
+    }
+}

Models/SimulationCommon/Rectangular_Area_Search_Function.cs

@@ -103,92 +103,91 @@ public class Rectangular_Area_Search_Function
         return hull;
     }
 
-    public static Tuple<Point2f[], double> MinEnclosingRectangle(Point2f[] hull)
+    public static List<double[]> MinEnclosingRectangle(Point2f[] hull)
     {
-        double min_rectangular_area = double.PositiveInfinity;
-        Point2f[] Point_muster = new Point2f[4];
+         double min_rectangular_area = double.PositiveInfinity;
+            List<double[]> Point_muster = new List<double[]>{};
 
-        for (int num = 0; num < hull.Length - 1; num++)
-        {
-            double[] Base_line = GetLinearEquation(hull[num], hull[num + 1]);
-            double max_Ditance = 0;
-            Point2f Point = hull[num + 1];
-
-            foreach (Point2f i_point in hull)
+            for (int num = 0; num < hull.Length - 1; num++)
             {
-                if ((i_point.X == hull[num].X && i_point.Y == hull[num].Y) ||
-                    (i_point.X == hull[num + 1].X && i_point.Y == hull[num + 1].Y))
+                double[] Base_line = GetLinearEquation(hull[num], hull[num + 1]);
+                double max_Ditance = 0;
+                Point2f Point = hull[num + 1];
+
+                foreach (Point2f i_point in hull)
                 {
-                    continue;
-                }
+                    if ((i_point.X == hull[num].X && i_point.Y == hull[num].Y) ||
+                        (i_point.X == hull[num + 1].X && i_point.Y == hull[num + 1].Y))
+                    {
+                        continue;
+                    }
 
-                double distance = GetDistance(i_point, Base_line);
+                    double distance = GetDistance(i_point, Base_line);
 
-                if (distance > max_Ditance)
-                {
-                    max_Ditance = distance;
-                    Point = i_point;
+                    if (distance > max_Ditance)
+                    {
+                        max_Ditance = distance;
+                        Point = i_point;
+                    }
                 }
-            }
-
-            double[] line_Parallel_Base = GetParallelLine(Point, Base_line);
 
-            double[] line_Perpendicular_Base_right = null;
-            double[] line_Perpendicular_Base_left = null;
+                double[] line_Parallel_Base = GetParallelLine(Point, Base_line);
 
-            foreach (Point2f j_point in hull)
-            {
-                double[] line_Preparatory = GetpPerpendicular(j_point, Base_line);
-                int point_right = 0;
-                int point_left = 0;
+                double[] line_Perpendicular_Base_right = null;
+                double[] line_Perpendicular_Base_left = null;
 
-                foreach (Point2f k_point in hull)
+                foreach (Point2f j_point in hull)
                 {
-                    if (k_point.X == j_point.X && k_point.Y == j_point.Y)
+                    double[] line_Preparatory = GetpPerpendicular(j_point, Base_line);
+                    int point_right = 0;
+                    int point_left = 0;
+
+                    foreach (Point2f k_point in hull)
                     {
-                        continue;
+                        if (k_point.X == j_point.X && k_point.Y == j_point.Y)
+                        {
+                            continue;
+                        }
+
+                        if (CheckDirection(k_point, line_Preparatory) < 0)
+                        {
+                            point_left++;
+                        }
+                        else
+                        {
+                            point_right++;
+                        }
                     }
 
-                    if (CheckDirection(k_point, line_Preparatory) < 0)
+                    if (point_right == hull.Length - 1)
                     {
-                        point_left++;
+                        line_Perpendicular_Base_right = line_Preparatory;
                     }
-                    else
+
+                    if (point_left == hull.Length - 1)
                     {
-                        point_right++;
+                        line_Perpendicular_Base_left = line_Preparatory;
                     }
                 }
 
-                if (point_right == hull.Length - 1)
-                {
-                    line_Perpendicular_Base_right = line_Preparatory;
-                }
+                double[] Point0 = GetCrossPoint(Base_line, line_Perpendicular_Base_left);
+                double[] Point1 = GetCrossPoint(Base_line, line_Perpendicular_Base_right);
+                double[] Point2 = GetCrossPoint(line_Parallel_Base, line_Perpendicular_Base_right);
+                double[] Point3 = GetCrossPoint(line_Parallel_Base, line_Perpendicular_Base_left);
+                double rectangular_area = GetRectangularArea(Point0, Point1, Point2, Point3);
 
-                if (point_left == hull.Length - 1)
+                if (rectangular_area < min_rectangular_area)
                 {
-                    line_Perpendicular_Base_left = line_Preparatory;
+                    min_rectangular_area = rectangular_area;    
+                    Point_muster = new List<double[]> {
+                        Point0,
+                        Point1,
+                        Point2,
+                        Point3
+                    };
                 }
             }
 
-            double[] Point0 = GetCrossPoint(Base_line, line_Perpendicular_Base_left);
-            double[] Point1 = GetCrossPoint(Base_line, line_Perpendicular_Base_right);
-            double[] Point2 = GetCrossPoint(line_Parallel_Base, line_Perpendicular_Base_right);
-            double[] Point3 = GetCrossPoint(line_Parallel_Base, line_Perpendicular_Base_left);
-            double rectangular_area = GetRectangularArea(Point0, Point1, Point2, Point3);
-
-            if (rectangular_area < min_rectangular_area)
-            {
-                min_rectangular_area = rectangular_area;
-                Point_muster = new Point2f[]
-                {
-                    new Point2f((float)Point0[0], (float)Point0[1]),
-                    new Point2f((float)Point1[0], (float)Point1[1]),
-                    new Point2f((float)Point2[0], (float)Point2[1]),
-                    new Point2f((float)Point3[0], (float)Point3[1])
-                };
-            }
-        }
-
-        return Tuple.Create(Point_muster, min_rectangular_area);
+            return Point_muster;
     }
 }

Models/SimulationCommon/SeaSJ.cs

@@ -168,7 +168,7 @@ public class SeaSJ
     }
     
     // 交付部分方法代码
-    public static Tuple<Point2f[], double> getminEnclosingRect(List<double[]> latLonList)
+    public static List<double[]> getminEnclosingRect(List<double[]> latLonList)
     {
         // 转换经纬度为墨卡托投影坐标，并添加到点集合
         List<Point2f> pointList = new List<Point2f>();
@@ -183,9 +183,18 @@ public class SeaSJ
         Point2f[] convexHull = Rectangular_Area_Search_Function.GetConvexHull(pointList);
 
         // 计算最小包围矩形
-        Tuple<Point2f[], double> minEnclosingRect = Rectangular_Area_Search_Function.MinEnclosingRectangle(convexHull);
+        List<double[]> minEnclosingRect = Rectangular_Area_Search_Function.MinEnclosingRectangle(convexHull);
 
-        return minEnclosingRect;
+        // 最小包围矩形顶点经纬度坐标
+        List<double[]> startPoint = new List<double[]>();
+        foreach (var minEnclosingRectPoint in minEnclosingRect)
+        {
+            double lat = Rectangular_Area_Search_Function.RMokatuoLat(minEnclosingRectPoint[0]);
+            double lon = Rectangular_Area_Search_Function.RMokatuoLon(minEnclosingRectPoint[1]);
+            startPoint.Add(new double[] { lat, lon });
+        }
+
+        return startPoint;
 
     }
 }

Models/SimulationCommon/SectorSearch.cs

@@ -0,0 +1,95 @@
+namespace SimulationCommon;
+
+public class SectorSearch
+{
+    public class LocationW
+    {
+        public double Lon { get; set; }
+        public double Lat { get; set; }
+    }
+
+    // 计算向量角度
+    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 void PerformSearch()
+    {
+        var boundaryPoints = new List<LocationW>();
+        var outputWaypoints = new List<LocationW>();
+
+        var basePoint = new LocationW();
+        var baseDir = new double[] { 0, 1 };
+
+        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 LocationW { Lat = boundaryPoints[index].Lat, Lon = boundaryPoints[index].Lon });
+        }
+
+        // Optionally display output
+        foreach (var waypoint in outputWaypoints)
+        {
+            Console.WriteLine($"Waypoint: Lat = {waypoint.Lat}, Lon = {waypoint.Lon}");
+        }
+    }
+}

SimulationServer/Entity/AircraftSJ.cs

@@ -1,11 +1,11 @@
 using KYFramework;
 using Model;
 using MuShiApp;
-using OpenCvSharp;
 using SimulationCommon;
 using SimulationSingleServer.Utils;
 using Unity.Mathematics;
 using Define = SimulationServer.Utils.Define;
+using Point = SimulationCommon.Point;
 using Random = System.Random;
 
 namespace SimulationServer;
@@ -36,13 +36,42 @@ public class AircraftSJ : AircraftEntity
         var nCread = text_ReadNC.windNCread;
         //漂移轨迹
         List<double[]> trajectory = SeaSJ.CalculateDriftTrajectory(nCread, initialPosition,dt, totalTime);
-        Tuple<Point2f[], double> temp = SeaSJ.getminEnclosingRect(trajectory);
+        
+        // 生成任务终点
+        MissionEndPoint = new MissionEndPoint
+        {
+            MissionEndPointLongitude = trajectory[trajectory.Count - 1][1],
+            MissionEndPointLatitude = trajectory[trajectory.Count - 1][0],
+            MissionEndPointHeight = 0
+        };
         
         
+        var temp = SeaSJ.getminEnclosingRect(trajectory);
+        // temp 转成 List<Point>
+        List<Point> points = new List<Point>();
+        foreach (var item in temp)
+        {
+            points.Add(new Point(item[0], item[1]));
+        }
         
+        Point basePoint = new Point(FlightPlanEditor.originbase.BaseLatitude, FlightPlanEditor.originbase.BaseLongitude);
+        double sweepWidth = 0.2;
+        // 生成航路点
+        List<Point> waypoints = ParallellineSearch.parallellineSearch(basePoint, points, sweepWidth);
         
         
+        // List<Point> 转成 List<AirRoute>
+        List<AirRoute> airRoutes = new List<AirRoute>();
+        foreach (var item in waypoints)
+        {
+            airRoutes.Add(new AirRoute
+            {
+                AirRouteLatitude = item.lat,
+                AirRouteLongitude = item.lon
+            });
+        }
         
+        FlightPlanEditor.airroute = airRoutes.ToArray();
         
         
 
@@ -114,8 +143,6 @@ public class AircraftSJ : AircraftEntity
                 };
                 FXJHGenerate.SeaSouJiu2(FlightPlanEditor, missionPoint, ref TurningPoints);
 
-                //*********
-                //更新任务终点
                 //发现的目标坐标，需要其他模型输入
                 FXJHGenerate.FromMissionToEnd(FlightPlanEditor, MissionEndPoint, ref TurningPoints);
             }