From 07f5b39814186de6da716adbed950b3185737db7 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Sun, 12 Mar 2017 16:53:49 +0800 Subject: [PATCH] =?UTF-8?q?1.=E4=BF=AE=E6=94=B9=E7=95=8C=E9=9D=A2=EF=BC=8C?= =?UTF-8?q?=E5=88=86=E6=88=90=E6=98=8E=E6=98=BE=E7=9A=84=E4=B8=A4=E7=A7=8D?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F=EF=BC=8C=E9=A3=9E=E8=A1=8C=E6=A8=A1=E5=BC=8F?= =?UTF-8?q?=E5=92=8C=E7=BC=96=E8=BE=91=E6=A8=A1=E5=BC=8F=20=E8=AE=A9?= =?UTF-8?q?=E7=95=8C=E9=9D=A2=E7=AE=80=E6=B4=81=E5=92=8C=E6=9C=89=E7=A9=BA?= =?UTF-8?q?=E9=97=B4=E5=8A=A0=E6=9B=B4=E5=A4=9A=E5=8A=9F=E8=83=BD=202.?= =?UTF-8?q?=E5=8A=A0=E5=85=A5=E6=B0=B4=E5=B9=B3=E6=97=8B=E8=BD=AC=E5=9B=BE?= =?UTF-8?q?=E5=83=8F=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Plane.FormationCreator/App.xaml | 4 +- .../CalculationLogLatDistance.cs | 350 ++++++++++++ .../Formation/FlightTask.cs | 4 +- Plane.FormationCreator/MainWindow.xaml | 18 +- .../Plane.FormationCreator.csproj | 1 + .../ViewModels/CopterListViewModel.cs | 245 +-------- .../ViewModels/ModifyTaskViewModel.cs | 507 +++++++++++++++++- .../Views/ControlPanelView.xaml | 13 +- .../Views/CopterInfoView.xaml | 3 + .../Views/CopterListView.xaml | 18 +- .../Views/ModifyTaskView.xaml | 68 ++- .../Views/ModifyTaskView.xaml.cs | 7 +- 12 files changed, 953 insertions(+), 285 deletions(-) create mode 100644 Plane.FormationCreator/CalculationLogLatDistance.cs diff --git a/Plane.FormationCreator/App.xaml b/Plane.FormationCreator/App.xaml index ab593cc..736d16d 100644 --- a/Plane.FormationCreator/App.xaml +++ b/Plane.FormationCreator/App.xaml @@ -33,8 +33,8 @@ True="Collapsed" /> + False="【飞行模式】" + True="【编辑模式】" /> diff --git a/Plane.FormationCreator/CalculationLogLatDistance.cs b/Plane.FormationCreator/CalculationLogLatDistance.cs new file mode 100644 index 0000000..199b13d --- /dev/null +++ b/Plane.FormationCreator/CalculationLogLatDistance.cs @@ -0,0 +1,350 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Plane.FormationCreator +{ + /** + * + * 计算经纬度、距离、方位角 + * + * */ + public class CalculationLogLatDistance + { + /** + * 地球赤道半径(km) + * */ + static public double EARTH_RADIUS = 6378.137; + /** + * 地球每度的弧长(km) + * */ + static public double EARTH_ARC = 111.199; + + /** + * 转化为弧度(rad) + * */ + public static double rad(double d) + { + return d * Math.PI / 180.0; + } + + /** + * 求两经纬度距离 + * + * @param lon1 + * 第一点的经度 + * @param lat1 + * 第一点的纬度 + * @param lon2 + * 第二点的经度 + * @param lat2 + * 第二点的纬度 + * @return 两点距离，单位km + * */ + public static double GetDistanceOne(double lon1, double lat1, double lon2, + double lat2) + { + double r1 = rad(lat1); + double r2 = rad(lon1); + double a = rad(lat2); + double b = rad(lon2); + double s = Math.Acos(Math.Cos(r1) * Math.Cos(a) * Math.Cos(r2 - b) + + Math.Sin(r1) * Math.Sin(a)) + * EARTH_RADIUS; + return s; + } + + /** + * 求两经纬度距离(google maps源码中) + * + * @param lon1 + * 第一点的经度 + * @param lat1 + * 第一点的纬度 + * @param lon2 + * 第二点的经度 + * @param lat2 + * 第二点的纬度 + * @return 两点距离，单位km + * */ + public static double GetDistanceTwo(double lon1, double lat1, double lon2, + double lat2) + { + double radLat1 = rad(lat1); + double radLat2 = rad(lat2); + double a = radLat1 - radLat2; + double b = rad(lon1) - rad(lon2); + double s = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) + + Math.Cos(radLat1) * Math.Cos(radLat2) + * Math.Pow(Math.Sin(b / 2), 2))); + s = s * EARTH_RADIUS; + return s; + } + + /** + * 求两经纬度距离 + * + * @param lon1 + * 第一点的经度 + * @param lat1 + * 第一点的纬度 + * @param lon2 + * 第二点的经度 + * @param lat2 + * 第二点的纬度 + * @return 两点距离，单位km + * */ + public static double GetDistanceThree(double lon1, double lat1, + double lon2, double lat2) + { + double radLat1 = rad(lat1); + double radLat2 = rad(lat2); + double radLon1 = rad(lon1); + double radLon2 = rad(lon2); + if (radLat1 < 0) + radLat1 = Math.PI / 2 + Math.Abs(radLat1);// south + if (radLat1 > 0) + radLat1 = Math.PI / 2 - Math.Abs(radLat1);// north + if (radLon1 < 0) + radLon1 = Math.PI * 2 - Math.Abs(radLon1);// west + if (radLat2 < 0) + radLat2 = Math.PI / 2 + Math.Abs(radLat2);// south + if (radLat2 > 0) + radLat2 = Math.PI / 2 - Math.Abs(radLat2);// north + if (radLon2 < 0) + radLon2 = Math.PI * 2 - Math.Abs(radLon2);// west + double x1 = Math.Cos(radLon1) * Math.Sin(radLat1); + double y1 = Math.Sin(radLon1) * Math.Sin(radLat1); + double z1 = Math.Cos(radLat1); + + double x2 = Math.Cos(radLon2) * Math.Sin(radLat2); + double y2 = Math.Sin(radLon2) * Math.Sin(radLat2); + double z2 = Math.Cos(radLat2); + + double d = Math.Pow((x1 - x2), 2) + Math.Pow((y1 - y2), 2) + + Math.Pow((z1 - z2), 2); + // // 余弦定理求夹角 + // double theta = Math.Acos((2 - d) / 2); + + d = Math.Pow(EARTH_RADIUS, 2) * d; + // //余弦定理求夹角 + double theta = Math.Acos((2 * Math.Pow(EARTH_RADIUS, 2) - d) + / (2 * Math.Pow(EARTH_RADIUS, 2))); + + double dist = theta * EARTH_RADIUS; + return dist; + } + + /** + * 求两经纬度方向角 + * + * @param lon1 + * 第一点的经度 + * @param lat1 + * 第一点的纬度 + * @param lon2 + * 第二点的经度 + * @param lat2 + * 第二点的纬度 + * @return 方位角，角度（单位：°） + * */ + public static double GetAzimuth(double lon1, double lat1, double lon2, + double lat2) + { + lat1 = rad(lat1); + lat2 = rad(lat2); + lon1 = rad(lon1); + lon2 = rad(lon2); + double azimuth = Math.Sin(lat1) * Math.Sin(lat2) + Math.Cos(lat1) + * Math.Cos(lat2) * Math.Cos(lon2 - lon1); + azimuth = Math.Sqrt(1 - azimuth * azimuth); + azimuth = Math.Cos(lat2) * Math.Sin(lon2 - lon1) / azimuth; + azimuth = Math.Asin(azimuth) * 180 / Math.PI; + if (Double.IsNaN(azimuth)) + { + if (lon1 < lon2) + { + azimuth = 90.0; + } + else { + azimuth = 270.0; + } + } + return azimuth; + } + + /** + * 已知一点经纬度A，和与另一点B的距离和方位角，求B的经纬度(计算结果有误) + * + * @param lon1 + * A的经度 + * @param lat1 + * A的纬度 + * @param distance + * AB距离（单位：米） + * @param azimuth + * AB方位角 + * @return B的经纬度 + * */ + public static void GetOtherPoint(double lon1, double lat1, + double distance, double azimuth, out double lng2, out double lat2) + { + azimuth = rad(azimuth); + double ab = distance / EARTH_ARC;// AB间弧线长 + ab = rad(ab); + double Lat = Math.Asin(Math.Sin(lat1) * Math.Cos(ab) + Math.Cos(lat1) + * Math.Sin(ab) * Math.Cos(azimuth)); + double Lon = lon1 + + Math.Asin(Math.Sin(azimuth) * Math.Sin(ab) / Math.Cos(Lat)); + // System.out.println(Lon + "," + Lat); + + double a = Math.Acos(Math.Cos(90 - lon1) * Math.Cos(ab) + + Math.Sin(90 - lon1) * Math.Sin(ab) * Math.Cos(azimuth)); + double C = Math.Asin(Math.Sin(ab) * Math.Sin(azimuth) / Math.Sin(a)); + // System.out.println("c=" + C); + lng2 = lon1 + C; + lat2 = 90 - a; + return ; + } + + /** + * 已知一点经纬度A，和与另一点B的距离和方位角，求B的经纬度 + * + * @param lon1 + * A的经度 + * @param lat1 + * A的纬度 + * @param distance + * AB距离（单位：米） + * @param azimuth + * AB方位角 + * @return B的经纬度 + * */ + public static void ConvertDistanceToLogLat(double lng1, double lat1, + double distance, double azimuth, out double lng2, out double lat2) + { + azimuth = rad(azimuth); + // 将距离转换成经度的计算公式 + lng2 = lng1 + (distance * Math.Sin(azimuth)) + / (EARTH_ARC * Math.Cos(rad(lat1))); + // 将距离转换成纬度的计算公式 + lat2 = lat1 + (distance * Math.Cos(azimuth)) / EARTH_ARC; + return ; + } + + + + public static MyLatLng getMyLatLng(MyLatLng A, double distance, double angle) + { + + double dx = distance * 1000 * Math.Sin((Math.PI / 180) * angle); + double dy = distance * 1000 * Math.Cos((Math.PI / 180) * angle); + + double bjd = (dx / A.Ed + A.m_RadLo) * 180.0/ Math.PI; + double bwd = (dy / A.Ec + A.m_RadLa) * 180.0/ Math.PI; + return new MyLatLng(bjd, bwd); + } + + + /** + * 获取AB连线与正北方向的角度 + * @param A A点的经纬度 + * @param B B点的经纬度 + * @return AB连线与正北方向的角度（0~360） + */ + public static double getAngle(MyLatLng A, MyLatLng B) + { + double dx = (B.m_RadLo - A.m_RadLo) * A.Ed; + double dy = (B.m_RadLa - A.m_RadLa) * A.Ec; + double angle = 0.0; + angle = Math.Atan(Math.Abs(dx / dy)) * 180.0/ Math.PI; + double dLo = B.m_Longitude - A.m_Longitude; + double dLa = B.m_Latitude - A.m_Latitude; + if (dLo > 0 && dLa <= 0) + { + angle = (90.0- angle) + 90; + } + else if (dLo <= 0 && dLa < 0) + { + angle = angle + 180.0; + } + else if (dLo < 0 && dLa >= 0) + { + angle = (90.0- angle) + 270; + } + if (Double.IsNaN(angle)) angle = 0.0; + + return angle; + } + public class MyLatLng + { + static double Rc = 6378137; + static double Rj = 6356725; + public double m_LoDeg, m_LoMin, m_LoSec; + public double m_LaDeg, m_LaMin, m_LaSec; + public double m_Longitude, m_Latitude; + public double m_RadLo, m_RadLa; + public double Ec; + public double Ed; + public MyLatLng(double longitude, double latitude) + { + m_LoDeg = (int)longitude; + m_LoMin = (int)((longitude - m_LoDeg) * 60); + m_LoSec = (longitude - m_LoDeg - m_LoMin / 60.0) * 3600; + + m_LaDeg = (int)latitude; + m_LaMin = (int)((latitude - m_LaDeg) * 60); + m_LaSec = (latitude - m_LaDeg - m_LaMin / 60.0) * 3600; + + m_Longitude = longitude; + m_Latitude = latitude; + m_RadLo = longitude * Math.PI / 180.0; + m_RadLa = latitude * Math.PI / 180.0; + Ec = Rj + (Rc - Rj) * (90.0- m_Latitude) / 90.0; + Ed = Ec * Math.Cos(m_RadLa); + } + + } + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + } + + + + + + + + +} diff --git a/Plane.FormationCreator/Formation/FlightTask.cs b/Plane.FormationCreator/Formation/FlightTask.cs index 92b720c..1a3db2c 100644 --- a/Plane.FormationCreator/Formation/FlightTask.cs +++ b/Plane.FormationCreator/Formation/FlightTask.cs @@ -101,11 +101,11 @@ namespace Plane.FormationCreator.Formation break; case FlightTaskType.TakeOff: //多架同时起飞 - await MutilRunTakeOffTaskAsync(2).ConfigureAwait(false); + await MutilRunTakeOffTaskAsync(5).ConfigureAwait(false); break; case FlightTaskType.ReturnToLand: // Added by ZJF //多架同时返航 - await MutilRunReturnToLandTaskAsync(2).ConfigureAwait(false); + await MutilRunReturnToLandTaskAsync(5).ConfigureAwait(false); break; default: throw new InvalidOperationException(); diff --git a/Plane.FormationCreator/MainWindow.xaml b/Plane.FormationCreator/MainWindow.xaml index e9d6da2..f62652a 100644 --- a/Plane.FormationCreator/MainWindow.xaml +++ b/Plane.FormationCreator/MainWindow.xaml @@ -102,15 +102,27 @@ - - + + + + + + + + + + + - + + diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj index 8211d3e..e462752 100644 --- a/Plane.FormationCreator/Plane.FormationCreator.csproj +++ b/Plane.FormationCreator/Plane.FormationCreator.csproj @@ -141,6 +141,7 @@ + diff --git a/Plane.FormationCreator/ViewModels/CopterListViewModel.cs b/Plane.FormationCreator/ViewModels/CopterListViewModel.cs index fe5439e..76ca8ed 100644 --- a/Plane.FormationCreator/ViewModels/CopterListViewModel.cs +++ b/Plane.FormationCreator/ViewModels/CopterListViewModel.cs @@ -41,6 +41,8 @@ namespace Plane.FormationCreator.ViewModels private int _virtualCopterId = 1; private LatLng? _lastVirtualCopterLocation; + + private ICommand _AddVirtualCopterCommand; public ICommand AddVirtualCopterCommand { @@ -98,249 +100,6 @@ namespace Plane.FormationCreator.ViewModels } - private ICommand _AverageCommand; - public ICommand AverageCommand - { - get - { - return _AverageCommand ?? (_AverageCommand = new RelayCommand(async alignmentLine => - { - double maxlat = 0; - double tlat = 0; - double minlat = 0; - - double maxlng = 0; - double minlng = 0; - double tlng = 0; - double avgl=0; - - if (alignmentLine == 0) - { - - var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); - bool copterisselect; - int selectednumber = _copterManager.SelectedCopters.Count()-1; - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - tlng = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; - if (minlng == 0) - minlng = tlng; - if (tlng > maxlng) - maxlng = tlng; - else if (tlng < minlng) - minlng = tlng; - - } - } - avgl = (maxlng - minlng) / selectednumber; - int coptnum = 0; - - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng= minlng+ avgl*coptnum; - coptnum++; - - } - } - - } - - - - if (alignmentLine == 90) - { - - var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); - bool copterisselect; - int selectednumber = _copterManager.SelectedCopters.Count() - 1; - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - tlat = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; - if (minlat == 0) - minlat = tlat; - if (tlat > maxlat) - maxlat = tlat; - else if (tlat < minlat) - minlat = tlat; - - } - } - avgl = (maxlat - minlat) / selectednumber; - int coptnum = 0; - - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = minlat + avgl * coptnum; - coptnum++; - - } - } - - } - - await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 - - })); - } - } - - - - private ICommand _AlignmentCommand; - public ICommand AlignmentCommand - { - get - { - return _AlignmentCommand ?? (_AlignmentCommand = new RelayCommand(async alignmentLine => - { - double maxlat = 0; - double tlat = 0; - - double maxlng = 0; - double tlng = 0; - - if (alignmentLine == 0) - { - - var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); - bool copterisselect; - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - tlat = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; - if (tlat > maxlat) - maxlat = tlat; - - } - } - - - - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = maxlat; - - } - } - } - - - if (alignmentLine == 90) - { - - - var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); - bool copterisselect; - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - tlng = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; - if (tlng > maxlng) - maxlng = tlng; - - } - } - - - - for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) - { - copterisselect = false; - selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; - foreach (var capter in _copterManager.SelectedCopters) - { - if (capter == selectedCopter) - copterisselect = true; - - } - if (copterisselect) - { - - _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = maxlng; - - } - } - } - await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 - - })); - } - } } } diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs index 046f61b..8b629b6 100644 --- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs +++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs @@ -16,9 +16,10 @@ namespace Plane.FormationCreator.ViewModels { public class ModifyTaskViewModel : ViewModelBase { - public ModifyTaskViewModel(FlightTaskManager flightTaskManager) + public ModifyTaskViewModel(FlightTaskManager flightTaskManager, CopterManager copterManager) { _flightTaskManager = flightTaskManager; + _copterManager = copterManager; //_flightTaskManager.SingleCopterInfoChanged += (sender, e) => //{ // var changedSingleInfo = e.ChangedSingleCopterInfo; @@ -117,7 +118,10 @@ namespace Plane.FormationCreator.ViewModels } private FlightTaskManager _flightTaskManager; + private CopterManager _copterManager; public FlightTaskManager FlightTaskManager { get { return _flightTaskManager; } } + public CopterManager CopterManager { get { return _copterManager; } } + //private void InvokeForSelectedSingleCopterInfos(Action action) //{ // var modifyingTask = _flightTaskManager.SelectedTask as FlyToFlightTask; @@ -208,5 +212,506 @@ namespace Plane.FormationCreator.ViewModels })); } } + + + + private ICommand _LevelAverageCommand; + public ICommand LevelAverageCommand + { + get + { + return _LevelAverageCommand ?? (_LevelAverageCommand = new RelayCommand(async => + { + double maxlng = 0; + double minlng = 0; + double tlng = 0; + double avgl = 0; + + + + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + int selectednumber = _copterManager.SelectedCopters.Count() - 1; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + tlng = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + if (minlng == 0) + minlng = tlng; + if (tlng > maxlng) + maxlng = tlng; + else if (tlng < minlng) + minlng = tlng; + + } + } + avgl = (maxlng - minlng) / selectednumber; + int coptnum = 0; + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = minlng + avgl * coptnum; + coptnum++; + + } + } + + + + + /////////////////////// + + // await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + + private ICommand _VerticlAverageCommand; + public ICommand VerticlAverageCommand + { + get + { + return _VerticlAverageCommand ?? (_VerticlAverageCommand = new RelayCommand(async => + { + double maxlat = 0; + double tlat = 0; + double minlat = 0; + + double avgl = 0; + + + ////////////////////// + + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + int selectednumber = _copterManager.SelectedCopters.Count() - 1; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + tlat = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + if (minlat == 0) + minlat = tlat; + if (tlat > maxlat) + maxlat = tlat; + else if (tlat < minlat) + minlat = tlat; + + } + } + avgl = (maxlat - minlat) / selectednumber; + int coptnum = 0; + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = minlat + avgl * coptnum; + coptnum++; + + } + } + + /////////////////////// + + // await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + private ICommand _LevelAlignmentCommand; + public ICommand LevelAlignmentCommand + { + get + { + return _LevelAlignmentCommand ?? (_LevelAlignmentCommand = new RelayCommand(async => + { + double maxlat = 0; + double tlat = 0; + + + + + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + tlat = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + if (tlat > maxlat) + maxlat = tlat; + + } + } + + + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = maxlat; + + } + } + + + + + //await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + + + + + private ICommand _VerticlAlignmentCommand; +public ICommand VerticlAlignmentCommand + { + get + { + return _VerticlAlignmentCommand ?? (_VerticlAlignmentCommand = new RelayCommand(async => + { + + double maxlng = 0; + double tlng = 0; + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + tlng = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + if (tlng > maxlng) + maxlng = tlng; + + } + } + + + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = maxlng; + + } + } + + //await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + + + + + + + + + + + + + + + + + + + + + + + + + public static double DistanceOfTwoPoints(double lng1, double lat1, double lng2, double lat2, GaussSphere gs) + { + double radLat1 = Rad(lat1); + double radLat2 = Rad(lat2); + double a = radLat1 - radLat2; + double b = Rad(lng1) - Rad(lng2); + double s = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) + + Math.Cos(radLat1) * Math.Cos(radLat2) * Math.Pow(Math.Sin(b / 2), 2))); + s = s * (gs == GaussSphere.WGS84 ? 6378137.0 : (gs == GaussSphere.Xian80 ? 6378140.0 : 6378245.0)); + s = Math.Round(s * 10000) / 10000; + return s; + } + + private static double Rad(double d) + { + return d * Math.PI / 180.0; + } + + + /// + /// 高斯投影中所选用的参考椭球 + /// + public enum GaussSphere + { + Beijing54, + Xian80, + WGS84, + } + + //水平旋转 + private ICommand _LevelRotateCommand; + public ICommand LevelRotateCommand + { + get + { + return _LevelRotateCommand ?? (_LevelRotateCommand = new RelayCommand(async RotateLine => + { + + + + double lngsum = 0; + double latsum = 0; + int selectcount = 0; + double centlng = 0; + double centlat = 0; + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + lngsum+= _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + latsum+= _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + selectcount++; + + } + } + //计算旋转中心 + if (selectcount > 0) + { + centlng = lngsum / selectcount; + centlat = latsum / selectcount; + } + else return; + + + //////////计算旋转，经测试用下面的函数组合计算比较准确 + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + { + double lpDistance = CalculationLogLatDistance.GetDistanceOne(centlng, centlat, + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng, + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat); + + CalculationLogLatDistance.MyLatLng mypos1, mypos2; + mypos1 = new CalculationLogLatDistance.MyLatLng(centlng, centlat); + mypos2 = new CalculationLogLatDistance.MyLatLng(_flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng + , _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat); + double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2); + double lng2 = 0; + double lat2=0; + CalculationLogLatDistance.ConvertDistanceToLogLat(centlng, centlat, lpDistance, + lpAzimuth + (double)RotateLine,out lng2, out lat2); + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = lng2; + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = lat2; + + } + + + } + } + //// + await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + //垂直旋转 + private ICommand _VerticlRotateCommand; + public ICommand VerticlRotateCommand + { + get + { + return _VerticlRotateCommand ?? (_VerticlRotateCommand = new RelayCommand(async RotateLine => + { + double lngsum = 0; + double latsum = 0; + int selectcount = 0; + double centlng = 0; + double centlat = 0; + var selectedCopter = _copterManager.SelectedCopters.FirstOrDefault(); + bool copterisselect; + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + copterisselect = false; + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + copterisselect = true; + + } + if (copterisselect) + { + + lngsum += _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + latsum += _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + selectcount++; + + } + } + //计算旋转中心 + if (selectcount > 0) + { + centlng = lngsum / selectcount; + centlat = latsum / selectcount; + } + else return; + double dx = 0; + double dy = 0; + double ax = 0; + double ay = 0; + double x = centlng; + double y = centlat; + double k = (double)RotateLine / 180 * Math.PI; + + for (int i = 0; i < _flightTaskManager.SelectedTask.SingleCopterInfos.Count; i++) + { + selectedCopter = _flightTaskManager.SelectedTask.SingleCopterInfos[i].Copter; + foreach (var capter in _copterManager.SelectedCopters) + { + if (capter == selectedCopter) + { + ax = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng; + ay = _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat; + dx = (ax - x) * Math.Cos(k) + (ay - y) * Math.Sin(k) + x; //此为最后的横坐标 + dy = -(ax - x) * Math.Sin(k) + (ay - y) * Math.Cos(k) + y; //此为最后的纵坐标 + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLng = dx; + _flightTaskManager.SelectedTask.SingleCopterInfos[i].TargetLat = dy; + + } + + + } + } + + + + + await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。 + + })); + } + } + + + + } } diff --git a/Plane.FormationCreator/Views/ControlPanelView.xaml b/Plane.FormationCreator/Views/ControlPanelView.xaml index 68c9c00..c732d75 100644 --- a/Plane.FormationCreator/Views/ControlPanelView.xaml +++ b/Plane.FormationCreator/Views/ControlPanelView.xaml @@ -29,7 +29,8 @@ Command="{Binding DisconnectCommand}" /> --> - +