Plane.FormationCreator/Plane.FormationCreator/Formation/Extensions.cs

using Plane.Copters;
using Plane.Geography;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;



namespace Plane.FormationCreator.Formation
{
    static class Extensions
    {
        public const float GpsArrivedDis = 2.0f; //GPS模式下航点到达精度
        public const float RTKArrivedDis = 0.8f;  //RTK模式航点达到精度
        public const float GpsCloseDis = 2.0f;    //GPS模式下碰撞检测最近距离
        public const float RTKClosedDis = 0.5f;   //RTK模式下碰撞检测最近距离


        public static LatLng? GetCenter(this IEnumerable<ICopter> copters)
        {
            int count = 0;
            double sumLat = 0;
            double sumLng = 0;
            foreach (var copter in copters)
            {
                sumLat += copter.Latitude;
                sumLng += copter.Longitude;
                ++count;
            }
            if (count == 0)
            {
                return null;
            }
            return new LatLng
            {
                Lat = sumLat / count,
                Lng = sumLng / count
            };
        }

        public static double InPlaneDirectionToDeg(this ICopter copter, double targetLat, double targetLng)
        {
            return GeographyUtils.RadToDeg(GeographyUtils.CalcDirection2D(copter.Latitude, copter.Longitude, targetLat, targetLng));
        }

        public static double DistanceTo(this ICopter copter, double targetLat, double targetLng, float targetAlt)
        {
            return GeographyUtils.CalcDistance(copter.Latitude, copter.Longitude, copter.Altitude, targetLat, targetLng, targetAlt);
        }

        public static double DistanceTo(this ICopter copter, ICopter copter2)
        {
            return GeographyUtils.CalcDistance(copter.Latitude, copter.Longitude, copter.Altitude, copter2.Latitude, copter2.Longitude, copter2.Altitude);
        }

        public static double InPlaneDistanceTo(this ICopter copter, double targetLat, double targetLng)
        {
            return GeographyUtils.CalcDistance2D(copter.Latitude, copter.Longitude, targetLat, targetLng);
        }
        //是否达到目标航点
        public static bool ArrivedTarget(this ICopter copter, double targetLat, double targetLng, float targetAlt)
        {
            if (copter.GpsFixType == GpsFixType.RTKFIXED)
                return copter.DistanceTo(targetLat, targetLng, targetAlt) < RTKArrivedDis;   //RTK到达航点精度0.6米
            else
                return copter.DistanceTo(targetLat, targetLng, targetAlt) < GpsArrivedDis;   //GPS到达航点精度2.0米
       }
        //是否已达到目标朝向
        public static bool ArrivedHeading(this ICopter copter, short targetHeading)
        {
            return Math.Abs(copter.Heading - targetHeading) < 2;
        }

        //是否距离太近
        public static bool IsTooCloseTo(this ICopter copter, ICopter copter2)
        {
            if ((copter.GpsFixType == GpsFixType.RTKFIXED)&&(copter2.GpsFixType == GpsFixType.RTKFIXED))
                return copter.DistanceTo(copter2) < RTKClosedDis;  //都是RTKFIXED则最近距离0.5米
            else
                return copter.DistanceTo(copter2) < GpsCloseDis;  //其他任何情况最近距离2米
        }



    }
}