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 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)
        {
            return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2;
            // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 1.5;
            // return copter.DistanceTo(targetLat, targetLng, targetAlt) < 2;  // added by ZJF
        }

        public static bool ArrivedTargetInPlane(this ICopter copter, double targetLat, double targetLng)
        {
            return copter.InPlaneDistanceTo(targetLat, targetLng) < 1;
        }

        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)
        {
            return copter.DistanceTo(copter2) < 2;
        }
    }
}