diff --git a/Plane.FormationCreator.sln b/Plane.FormationCreator.sln
index 5013cb2..dadee74 100644
--- a/Plane.FormationCreator.sln
+++ b/Plane.FormationCreator.sln
@@ -4,6 +4,9 @@ Microsoft Visual Studio Solution File, Format Version 12.00
VisualStudioVersion = 14.0.25420.1
MinimumVisualStudioVersion = 10.0.40219.1
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.FormationCreator", "Plane.FormationCreator\Plane.FormationCreator.csproj", "{61E2F31E-220A-4E3F-A64D-F7CDC2135008}"
+ ProjectSection(ProjectDependencies) = postProject
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB} = {BE3280E8-8C7F-4961-9685-1124C5B990CB}
+ EndProjectSection
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Plane.Windows", "..\Plane.Libraries\Plane.Windows\Plane.Windows.csproj", "{06848293-9B17-4068-9B35-44D0ED713CD4}"
EndProject
@@ -23,6 +26,8 @@ Project("{D954291E-2A0B-460D-934E-DC6B0785DB48}") = "PlaneGcsSdk_Shared", "..\Pl
EndProject
Project("{D954291E-2A0B-460D-934E-DC6B0785DB48}") = "PlaneGcsSdk.Contract_Shared", "..\Plane.Sdk3\PlaneGcsSdk.Contract_Shared\PlaneGcsSdk.Contract_Shared.shproj", "{695733D7-99FF-4707-8C89-474E949CADCB}"
EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "flyBase", "..\Tools\flyBase\flyBase.csproj", "{BE3280E8-8C7F-4961-9685-1124C5B990CB}"
+EndProject
Global
GlobalSection(SharedMSBuildProjectFiles) = preSolution
..\Plane.Sdk3\PlaneGcsSdk_Shared\PlaneGcsSdk_Shared.projitems*{0111eb6e-72e3-499c-a3ba-022f5bbc4caf}*SharedItemsImports = 4
@@ -101,6 +106,14 @@ Global
{47141894-ECE3-48CA-8DCF-CA751BDA231E}.v4.0-Debug|Any CPU.Build.0 = Debug|Any CPU
{47141894-ECE3-48CA-8DCF-CA751BDA231E}.v4.0-Release|Any CPU.ActiveCfg = Release|Any CPU
{47141894-ECE3-48CA-8DCF-CA751BDA231E}.v4.0-Release|Any CPU.Build.0 = Release|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.Debug|Any CPU.Build.0 = Debug|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.Release|Any CPU.ActiveCfg = Release|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.Release|Any CPU.Build.0 = Release|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.v4.0-Debug|Any CPU.ActiveCfg = Debug|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.v4.0-Debug|Any CPU.Build.0 = Debug|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.v4.0-Release|Any CPU.ActiveCfg = Release|Any CPU
+ {BE3280E8-8C7F-4961-9685-1124C5B990CB}.v4.0-Release|Any CPU.Build.0 = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
diff --git a/Plane.FormationCreator/App.xaml b/Plane.FormationCreator/App.xaml
index ca1595e..b3c077f 100644
--- a/Plane.FormationCreator/App.xaml
+++ b/Plane.FormationCreator/App.xaml
@@ -36,6 +36,25 @@
False="【飞行模式】"
True="【设计模式】" />
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Plane.FormationCreator/Formation/FlightTaskManager.cs b/Plane.FormationCreator/Formation/FlightTaskManager.cs
index 7628d6e..10007a2 100644
--- a/Plane.FormationCreator/Formation/FlightTaskManager.cs
+++ b/Plane.FormationCreator/Formation/FlightTaskManager.cs
@@ -13,6 +13,10 @@ using System.Threading.Tasks;
using Plane.CommunicationManagement;
using FlightRoute;
using System.IO;
+using System.Windows.Media.Media3D;
+using System.Diagnostics;
+using Plane.FormationCreator.Util;
+using System.Collections;
namespace Plane.FormationCreator.Formation
{
@@ -993,7 +997,6 @@ namespace Plane.FormationCreator.Formation
if (copters == null || !copters.Any()) return;
AppEx.Current.AppMode = AppMode.ModifyingTask;
var lastTask = Tasks.LastOrDefault();
-
FlightTask takeOffTask = Tasks[0];
takeOffTask.TakeOffTime = takeOffTime;
for (int i = 0; i < copters.Count; i++)
@@ -1307,11 +1310,250 @@ namespace Plane.FormationCreator.Formation
}
}
+
+ //计算中心点 (三维的)
+ private Point3D CenterFormMeter(List point1, List point2)
+ {
+ Point3D centerPoint = new Point3D(0, 0, 0);
+ if (point1.Count != point2.Count) return centerPoint;
+
+ double minX = point1[0].X;
+ double maxX = point1[0].X;
+
+ double minY = point1[0].Y;
+ double maxY = point1[0].Y;
+
+
+ double minZ = point1[0].Z;
+ double maxZ = point1[0].Z;
+
+
+
+ int count = point1.Count;
+ for (int i = 0; i < count; i++)
+ {
+ minX = Math.Min(minX, Math.Min(point1[i].X, point2[i].X));
+ maxX = Math.Max(maxX, Math.Min(point1[i].X, point2[i].X));
+
+ minY = Math.Min(minY, Math.Min(point1[i].Y, point2[i].Y));
+ maxY = Math.Max(maxY, Math.Max(point1[i].Y, point2[i].Y));
+
+ minZ = Math.Min(minZ, Math.Min(point1[i].Z, point2[i].Z));
+ maxZ = Math.Max(maxZ, Math.Max(point1[i].Z, point2[i].Z));
+
+
+ }
+ centerPoint.X = (minX + maxX) / 2;
+ centerPoint.Y = (minY + maxY) / 2;
+ centerPoint.Z = (minZ + maxZ) / 2;
+ return centerPoint;
+ }
+
+
+ public double SumFlyLines(List preinfos, List currinfos)
+ {
+ double sumdis = 0;
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ //当前任务
+ Point3D preLoc = new Point3D(preinfos[i].X, preinfos[i].Y, preinfos[i].TargetAlt);
+ Point3D curLoc = new Point3D(currinfos[i].X, currinfos[i].Y, currinfos[i].TargetAlt);
+ //算距离
+ sumdis += Point3D.Subtract(curLoc, preLoc).Length;
+ }
+ return sumdis;
+ }
+ public void SetAllTaskFlytime()
+ {
+ for (int taskIndex = 1; taskIndex < Tasks.Count; taskIndex++)
+ SetTaskFlytime(taskIndex);
+ }
+
+
+ public void SetTaskFlytime(int taskIndex,bool settime=true)
+ {
+ if ((taskIndex > 0 )&&(taskIndex < Tasks.Count) &&( Tasks[taskIndex].TaskType == FlightTaskType.FlyTo))
+ {
+ if (_copterManager.Copters.Count() > 0)
+ {
+ double maxDistance = 0.0f;
+ string copterName = "";
+ double speed = 0.0f;
+ foreach (var copter in _copterManager.Copters)
+ {
+ var prevWaypoint = Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
+ var curWaypoint = Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
+ if ((curWaypoint.IsChangeSpeed) && (curWaypoint.LevelSpeed == -1))
+ {
+ Message.Show($"任务{taskIndex + 1}飞机编号:{copter.Id}是自动速度，无法计算时间!");
+ return ;
+ }
+ double distance = GeographyUtils.CalcDistance(
+ prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
+ curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
+ if (distance > maxDistance)
+ {
+ maxDistance = distance;
+ copterName = copter.Name;
+ speed = curWaypoint.LevelSpeed;
+ }
+ }
+
+ double time = CalculateFlyIime(maxDistance, speed);
+ Message.Show($"任务{taskIndex+1}最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
+ if (settime)
+ Tasks[taskIndex].FlytoTime = (int)Math.Round(time, 2);
+ }
+ }
+
+ return ;
+ }
+
+
+ public static double CalculateFlyIime(double s, double v)
+ {
+ double t;
+ double a = 1; //加速度1米每秒
+ double at = v / a;
+ double a_s = 0.5f * a * at * at;
+ if (a_s > (s / 2)) //还没到特定速度就到了中点了
+ {
+ t = (float)System.Math.Sqrt(s / a) * 2;
+ }
+ else
+ {
+ t = (s - a_s * 2) / v + at * 2;
+ }
+
+ return t;
+ } //计算优化线路，采用米计算
+ public void OptimizeRouteMeter(bool Is3d=false)
+ {
+ Dictionary curTaskPoint = new Dictionary();
+ Dictionary prevTaskPoint = new Dictionary();
+ var stopWatch = new Stopwatch();
+ stopWatch.Start();
+ //获取当前航点与前一航点所有经纬高
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ //当前任务
+ var curinfo = SelectedTask.SingleCopterInfos[i];
+ Point3D curLoc = new Point3D(curinfo.X, curinfo.Y, curinfo.TargetAlt);
+ curTaskPoint.Add(i, curLoc);
+
+ //前一任务
+ var prevInfo = Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i];
+ Point3D prevLoc = new Point3D(prevInfo.X, prevInfo.Y, prevInfo.TargetAlt);
+ prevTaskPoint.Add(i, prevLoc);
+ }
+ //Dictionary RouteRes = Util.OptimizeRoute.GenRoute(curTaskPoint, prevTaskPoint);
+ if (Is3d)
+ {
+ ArrayList resarray = Util.OptimizeRoute.Gen3DRoute(curTaskPoint.Values.ToArray(), prevTaskPoint.Values.ToArray());
+ Vector3[] RouteRes;
+ //最终
+ RouteRes = (Vector3[])resarray[0];
+
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
+ SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
+ }
+
+ SetTaskFlytime(SelectedTaskIndex);
+
+
+ }
+ else
+ {
+ ArrayList resarray = Util.OptimizeRoute.Gen2DRoute(curTaskPoint.Values.ToArray(), prevTaskPoint.Values.ToArray());
+ Vector3[] RouteRes;
+ //有错层，需要插入2个错层任务
+ if (resarray.Count == 3)
+ {
+ //选中前一个任务，插入错层
+ SelectTask(SelectedTaskIndex - 1);
+ AddTask();
+
+ //第一个错层
+ RouteRes = (Vector3[])resarray[0];
+
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
+ SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
+ }
+ SetTaskFlytime(SelectedTaskIndex);
+
+
+ //第二个错层
+ AddTask();
+
+ RouteRes = (Vector3[])resarray[1];
+
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
+ SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
+ }
+
+ SetTaskFlytime(SelectedTaskIndex);
+
+
+ SelectTask(SelectedTaskIndex + 1);
+
+ //最终
+ RouteRes = (Vector3[])resarray[2];
+
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
+ SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
+ }
+ SetTaskFlytime(SelectedTaskIndex);
+
+
+ }
+ else
+ {
+ //最终
+ RouteRes = (Vector3[])resarray[0];
+
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+ {
+ SelectedTask.SingleCopterInfos[i].X = RouteRes[i].x;
+ SelectedTask.SingleCopterInfos[i].Y = RouteRes[i].y;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = (float)RouteRes[i].z;
+ }
+ SetTaskFlytime(SelectedTaskIndex);
+
+ }
+ }
+ stopWatch.Stop();
+
+ double sumlength = SumFlyLines(Tasks[SelectedTaskIndex - 1].SingleCopterInfos, SelectedTask.SingleCopterInfos);
+
+ Message.Show($"计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米");
+
+
+ return;
+ }
+
+
+
+
+ //计算优化线路，采用经纬度计算
public void OptimizeRouteNew()
{
Dictionary curTaskPoint = new Dictionary();
Dictionary prevTaskPoint = new Dictionary();
+ var stopWatch = new Stopwatch();
+ stopWatch.Start();
//获取当前航点与前一航点所有经纬高
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
@@ -1326,72 +1568,74 @@ namespace Plane.FormationCreator.Formation
prevTaskPoint.Add(i, prevLoc);
}
- // int sss = Plane.AutoLine.CalAutoLine(curTaskPoint, prevTaskPoint);
- // Message.Show($"中心点：{sss}");
+ // int sss = Plane.AutoLine.CalAutoLine(curTaskPoint, prevTaskPoint);
+ // Message.Show($"中心点：{sss}");
double farDistance;
double nearDistance;
int index;
- Dictionary recordLatLng = new Dictionary();
+ Dictionary recordLatLng = new Dictionary();
while (curTaskPoint.Count > 0)
{
farDistance = double.MinValue;
-
+ PLLocation centerLatLng = CenterLatLng(curTaskPoint.Values.ToList(), prevTaskPoint.Values.ToList());
index = 0;
- LatLng centerLatLng = CenterLatLng(curTaskPoint.Values.ToList(), prevTaskPoint.Values.ToList());
//计算两个列表距离中心最远距离
-
+ // Message.Show($"有{curTaskPoint.Count}个点，当前中心点：{centerLatLng.Longitude},{centerLatLng.Latitude},{centerLatLng.Altitude}");
double distance1;
bool farIsCurTaskPoint = true;
foreach (var item in curTaskPoint)
{
- distance1 = GeographyUtils.CalcDistance(centerLatLng.Lat, centerLatLng.Lng, 1,
- item.Value.Latitude, item.Value.Longitude, 1);
+ distance1 =Math.Round(GeographyUtils.CalcDistance(centerLatLng,item.Value),4);
if (distance1 > farDistance)
{
index = item.Key;
farDistance = distance1;
farIsCurTaskPoint = true;
+ // Message.Show($"调整距离{farDistance}是{farIsCurTaskPoint}的{index}");
+
}
}
foreach (var item in prevTaskPoint)
{
- distance1 = GeographyUtils.CalcDistance(centerLatLng.Lat, centerLatLng.Lng, 1,
- item.Value.Latitude, item.Value.Longitude, 1);
+ distance1 = Math.Round(GeographyUtils.CalcDistance(centerLatLng,item.Value),4);
if (distance1 > farDistance)
{
index = item.Key;
farDistance = distance1;
farIsCurTaskPoint = false;
+ // Message.Show($"调整距离{farDistance}是{farIsCurTaskPoint}的{index}");
+
}
}
+ // Message.Show($"最远{farDistance}是{farIsCurTaskPoint}的{index}");
+
+
double tempDistance1;
nearDistance = double.MaxValue;
int nearIndex = 0;
if (farIsCurTaskPoint)
{
- double curLat = curTaskPoint[index].Latitude;
- double curLng = curTaskPoint[index].Longitude;
//最远的航点在当前任务
-
foreach (var item in prevTaskPoint)
{
- tempDistance1 = GeographyUtils.CalcDistance(curLat, curLng, 1,
- item.Value.Latitude, item.Value.Longitude, 1);
+ tempDistance1 = Math.Round(GeographyUtils.CalcDistance(curTaskPoint[index],item.Value),4);
if (tempDistance1 < nearDistance)
{
nearDistance = tempDistance1;
nearIndex = item.Key;
}
}
+ //最远的航点在当前任务
+ // Message.Show($"最远是当前任务{index}距离它最近的前一任务点：ind={nearIndex},记录前一任务序号和当前任务位置");
- recordLatLng.Add(nearIndex, new LatLng(curLat, curLng));
+ recordLatLng.Add(nearIndex, curTaskPoint[index]);
curTaskPoint.Remove(index);
prevTaskPoint.Remove(nearIndex);
@@ -1401,20 +1645,20 @@ namespace Plane.FormationCreator.Formation
//最远的航点在前一任务
double prevLat = prevTaskPoint[index].Latitude;
double prevLng = prevTaskPoint[index].Longitude;
+ float prevAlt = prevTaskPoint[index].Altitude;
//最远的航点在当前任务
+ //Message.Show($"最远点在前一任务：ind={index},距离{farDistance}");
foreach (var item in curTaskPoint)
{
- tempDistance1 = GeographyUtils.CalcDistance(prevLat, prevLng, 1,
- item.Value.Latitude, item.Value.Longitude, 1);
+ tempDistance1 = Math.Round(GeographyUtils.CalcDistance(prevTaskPoint[index],item.Value),4);
if (tempDistance1 < nearDistance)
{
nearDistance = tempDistance1;
nearIndex = item.Key;
}
}
-
- recordLatLng.Add(index, new LatLng(curTaskPoint[nearIndex].Latitude, curTaskPoint[nearIndex].Longitude));
-
+ // Message.Show($"最远是前一任务{index}距离它最近的当前任务点：ind={nearIndex},记录前一任务序号和当前任务位置");
+ recordLatLng.Add(index, curTaskPoint[nearIndex]);
curTaskPoint.Remove(nearIndex);
prevTaskPoint.Remove(index);
}
@@ -1424,16 +1668,81 @@ namespace Plane.FormationCreator.Formation
for (int i = 0; i < _copterManager.Copters.Count; i++)
{
- SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Lat;
- SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Lng;
+ SelectedTask.SingleCopterInfos[i].TargetLat = recordLatLng[i].Latitude;
+ SelectedTask.SingleCopterInfos[i].TargetLng = recordLatLng[i].Longitude ;
+ SelectedTask.SingleCopterInfos[i].TargetAlt = recordLatLng[i].Altitude;
+ }
+ stopWatch.Stop();
+ double sumlength = SumFlyLines(Tasks[SelectedTaskIndex - 1].SingleCopterInfos, SelectedTask.SingleCopterInfos);
+
+
+
+
+ double Dist;
+ int crosscount = 0;
+ for (int i = 0; i < _copterManager.Copters.Count; i++)
+
+ {
+ for (int j = i + 1; j < _copterManager.Copters.Count; j++)
+ {
+
+
+ Dist = Util.OptimizeRoute.SqureDistanceSegmentToSegment(
+ new Point3D(Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].X,
+ Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].Y,
+ Tasks[SelectedTaskIndex - 1].SingleCopterInfos[i].TargetAlt),
+
+ new Point3D(Tasks[SelectedTaskIndex].SingleCopterInfos[i].X,
+ Tasks[SelectedTaskIndex].SingleCopterInfos[i].Y,
+ Tasks[SelectedTaskIndex].SingleCopterInfos[i].TargetAlt),
+
+ new Point3D(Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].X,
+ Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].Y,
+ Tasks[SelectedTaskIndex - 1].SingleCopterInfos[j].TargetAlt),
+
+ new Point3D(Tasks[SelectedTaskIndex].SingleCopterInfos[j].X,
+ Tasks[SelectedTaskIndex].SingleCopterInfos[j].Y,
+ Tasks[SelectedTaskIndex].SingleCopterInfos[j].TargetAlt)
+ );
+ //交叉
+ if (Dist < 0.01)
+ {
+ crosscount++;
+ // Message.Show($"飞机{i}和飞机{j}有交叉，距离{Dist}");
+
+
+ }
+ }
+
+
}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Message.Show($"经纬方式计算用时：{stopWatch.Elapsed.TotalMilliseconds}ms,总飞行距离{sumlength}米,有{crosscount}个交叉");
+
}
- //计算中心点 (二维的-只算水平面)
- private LatLng CenterLatLng(List point1, List point2)
+ //计算中心点 (三维的)
+ private PLLocation CenterLatLng(List point1, List point2)
{
- LatLng centerLatLng = new LatLng(0, 0);
+ PLLocation centerLatLng = new PLLocation(0,0,0);
if (point1.Count != point2.Count) return centerLatLng;
double minLat = point1[0].Latitude;
@@ -1442,18 +1751,29 @@ namespace Plane.FormationCreator.Formation
double minLng = point1[0].Longitude;
double maxLng = point1[0].Longitude;
+
+ double minAlt = point1[0].Altitude;
+ double maxAlt = point1[0].Altitude;
+
+
+
int count = point1.Count;
for (int i = 0; i < count; i++)
{
minLat = Math.Min(minLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
- maxLat = Math.Min(maxLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
+ maxLat = Math.Max(maxLat, Math.Min(point1[i].Latitude, point2[i].Latitude));
minLng = Math.Min(minLng, Math.Min(point1[i].Longitude, point2[i].Longitude));
maxLng = Math.Max(maxLng, Math.Max(point1[i].Longitude, point2[i].Longitude));
- }
- centerLatLng.Lat = (minLat + maxLat) / 2;
- centerLatLng.Lng = (minLng + maxLng) / 2;
+ minAlt = Math.Min(minAlt, Math.Min(point1[i].Altitude, point2[i].Altitude));
+ maxAlt = Math.Max(maxAlt, Math.Max(point1[i].Altitude, point2[i].Altitude));
+
+
+ }
+ centerLatLng.Latitude = (minLat + maxLat) / 2;
+ centerLatLng.Longitude = (minLng + maxLng) / 2;
+ centerLatLng.Altitude =(float)(minAlt + maxAlt) / 2;
return centerLatLng;
}
diff --git a/Plane.FormationCreator/MainWindow.xaml b/Plane.FormationCreator/MainWindow.xaml
index 97abeed..e98b081 100644
--- a/Plane.FormationCreator/MainWindow.xaml
+++ b/Plane.FormationCreator/MainWindow.xaml
@@ -48,11 +48,39 @@
+
+
+
+
+
+ Command="{Binding ShowOrHideModifyTaskViewCommand}" Visibility="Collapsed" />
+
+
+
+
+
+ Command="{Binding ChangeMapModeCommand}" Visibility="Collapsed"/>
+
+
+
@@ -236,7 +264,7 @@
+ Text="{Binding Message}" Width="480" MouseUp="LogShowHide"/>
diff --git a/Plane.FormationCreator/Plane.FormationCreator.csproj b/Plane.FormationCreator/Plane.FormationCreator.csproj
index 02f607e..a7fbaea 100644
--- a/Plane.FormationCreator/Plane.FormationCreator.csproj
+++ b/Plane.FormationCreator/Plane.FormationCreator.csproj
@@ -51,9 +51,6 @@
..\packages\EntityFramework.6.2.0\lib\net45\EntityFramework.SqlServer.dll
True
-
- ..\..\Tools\flyBase.dll
-
..\packages\MvvmLightLibs.5.2.0.0\lib\net45\GalaSoft.MvvmLight.dll
True
@@ -197,6 +194,7 @@
+
@@ -451,6 +449,10 @@
{0111eb6e-72e3-499c-a3ba-022f5bbc4caf}
PlaneGcsSdk_Private_NET46
+
+ {be3280e8-8c7f-4961-9685-1124c5b990cb}
+ flyBase
+
diff --git a/Plane.FormationCreator/ViewModels/MainViewModel.cs b/Plane.FormationCreator/ViewModels/MainViewModel.cs
index 11c68cc..45adc9b 100644
--- a/Plane.FormationCreator/ViewModels/MainViewModel.cs
+++ b/Plane.FormationCreator/ViewModels/MainViewModel.cs
@@ -18,6 +18,7 @@ using Plane.FormationCreator.Views;
using System.Windows;
using Plane.Collections;
using Plane.Copters;
+using Newtonsoft.Json.Linq;
namespace Plane.FormationCreator.ViewModels
{
@@ -219,6 +220,30 @@ namespace Plane.FormationCreator.ViewModels
}));
}
}
+ private ICommand _ShowModifyTaskViewCommand;
+ public ICommand ShowModifyTaskViewCommand
+ {
+ get
+ {
+ return _ShowModifyTaskViewCommand ?? (_ShowModifyTaskViewCommand = new RelayCommand(() =>
+ {
+ AppEx.Current.ShowModifyTaskView = true;
+ }));
+ }
+ }
+
+ private ICommand _HideModifyTaskViewCommand;
+ public ICommand HideModifyTaskViewCommand
+ {
+ get
+ {
+ return _HideModifyTaskViewCommand ?? (_HideModifyTaskViewCommand = new RelayCommand(() =>
+ {
+ AppEx.Current.ShowModifyTaskView =false;
+ }));
+ }
+ }
+
private ICommand _ShowOrHideModifyTaskViewCommand;
public ICommand ShowOrHideModifyTaskViewCommand
@@ -256,6 +281,54 @@ namespace Plane.FormationCreator.ViewModels
+ private ICommand _Map2DCommand;
+ public ICommand Map2DCommand
+ {
+ get
+ {
+ return _Map2DCommand ?? (_Map2DCommand = new RelayCommand(() =>
+ {
+ MainWindow mainw = (MainWindow)App.Current.MainWindow;
+
+ mainw.map.Visibility = System.Windows.Visibility.Visible;
+ mainw.map3D.Visibility = System.Windows.Visibility.Collapsed;
+ MapMode = 0;
+ b2DMapMode = true;
+ _flightTaskManager.TaskRun_2D = b2DMapMode; //2D模式模拟显示
+ //强制刷新飞机位置
+ _copterManager.Copters.ForEach(copter => copter.RefreashLoc());
+
+
+
+ }));
+ }
+ }
+
+ private ICommand _Map3DCommand;
+ public ICommand Map3DCommand
+ {
+ get
+ {
+ return _Map3DCommand ?? (_Map3DCommand = new RelayCommand(() =>
+ {
+ MainWindow mainw = (MainWindow)App.Current.MainWindow;
+ mainw.map.Visibility = System.Windows.Visibility.Collapsed;
+ mainw.map3D.Visibility = System.Windows.Visibility.Visible;
+ MapMode = 1;
+ b2DMapMode = false;
+ _flightTaskManager.TaskRun_2D = b2DMapMode; //3D模式模拟显示
+
+
+
+ }));
+ }
+ }
+
+
+
+
+
+
private ICommand _ChangeMapModeCommand;
public ICommand ChangeMapModeCommand
{
@@ -500,8 +573,22 @@ namespace Plane.FormationCreator.ViewModels
//int task Newtonsoft.Json.Linq.JArray
-
-
+
+ if (taskinfo.Count == 0)
+ {
+
+ Alert.Show($"导入的文件没有飞行任务！", "提示");
+ return;
+
+
+ }
+ if ((taskinfo[0].singleCopterInfos.Count != _copterManager.Copters.Count))
+ {
+ Alert.Show($"导入的飞机数量不符！导入飞机{taskinfo[0].singleCopterInfos.Count}，实际飞机{ _copterManager.Copters.Count}", "提示");
+ return;
+
+ }
+
if ((txtStarindex == 0) && (txtendindex == 0))
{
diff --git a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs
index ed37269..dda75fb 100644
--- a/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs
+++ b/Plane.FormationCreator/ViewModels/ModifyTaskViewModel.cs
@@ -191,6 +191,34 @@ namespace Plane.FormationCreator.ViewModels
set { Set(nameof(directionvalueall), ref _directionvalueall, value); }
}
+
+ private int _taskstartno = 0;
+ public int taskstartno
+ {
+ get { return _taskstartno; }
+ set { Set(nameof(taskstartno), ref _taskstartno, value); }
+ }
+
+ private int _taskendno = 0;
+ public int taskendno
+ {
+ get { return _taskendno; }
+ set { Set(nameof(taskendno), ref _taskendno, value); }
+ }
+
+
+ private float _taskdirection = 0;
+ public float taskdirection
+ {
+ get { return _taskdirection; }
+ set { Set(nameof(taskdirection), ref _taskdirection, value); }
+ }
+
+
+
+
+
+
private double _FlyToLat;
public double FlyToLat
{
@@ -226,9 +254,121 @@ namespace Plane.FormationCreator.ViewModels
set { Set(nameof(OnlySelected), ref _OnlySelected, value); }
}
-
-
+
+ ///
+ /// 垂直旋转多个任务
+ ///
+ private ICommand _MitTaskVrotationCommand;
+ public ICommand MitTaskVrotationCommand
+
+ {
+ get
+ {
+ return _MitTaskVrotationCommand ?? (_MitTaskVrotationCommand = new RelayCommand(async =>
+ {
+
+
+ double lngsum = 0;
+ double latsum = 0;
+ double altsum = 0;
+ int selectcount = 0;
+ // double centlng = 0;
+ double centlat = 0;
+ double centalt = 0;
+
+
+ if ((taskstartno<=1)||(taskendno> _flightTaskManager.Tasks.Count()))
+ {
+ Alert.Show("任务范围不正确", "提示", MessageBoxButton.OK, MessageBoxImage.Warning);
+ return;
+
+ }
+ //计算旋转中心轴
+ for (int i = taskstartno-1; i < taskendno; i++)
+ {
+ for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
+ {
+
+ lngsum += _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng;
+ latsum += _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
+ altsum += _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetAlt;
+ selectcount++;
+ }
+ }
+ if (selectcount > 0)
+ {
+ // centlng = lngsum / selectcount;
+ centlat = latsum / selectcount;
+ centalt = altsum / selectcount;
+ }
+
+
+
+ //////////计算旋转，经测试用下面的函数组合计算比较准确
+ double k = (double)taskdirection / 180 * Math.PI;
+ double dx = 0;
+ double dy = 0;
+ double ax = 0;
+ double ay = 0;
+ double tlng = 0;
+ double tlat = 0;
+
+
+ for (int i = taskstartno-1; i < taskendno; i++)
+ {
+ for (int j = 0; j < _flightTaskManager.Tasks[i].SingleCopterInfos.Count; j++)
+ {
+
+
+ tlng = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLng;
+ tlat = _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat;
+ //纬度方向距离（单位m）
+ ax = CalculationLogLatDistance.GetDistanceOne(tlng, centlat, tlng, tlat) * 1000;
+ //方向角用于正负,0为正，180为负
+ CalculationLogLatDistance.MyLatLng mypos1, mypos2;
+ mypos1 = new CalculationLogLatDistance.MyLatLng(tlng, centlat);
+ mypos2 = new CalculationLogLatDistance.MyLatLng(tlng, tlat);
+ double lpAzimuth = CalculationLogLatDistance.getAngle(mypos1, mypos2);
+ if (lpAzimuth > 90)
+ ax = -ax;
+
+
+ //高度方向距离（单位m）
+ ay = (_flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetAlt - centalt);
+
+ dx = ax * Math.Cos(k) + ay * Math.Sin(k);
+ dy = -ax * Math.Sin(k) + ay * Math.Cos(k);
+
+ //新高度（米）
+ _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetAlt = (float)(centalt + dy);
+ //计算新纬度
+ double lng2 = 0;
+ double lat2 = 0;
+ if (dx < 0)
+ lpAzimuth = 180;
+ else
+ lpAzimuth = 0;
+ dx = Math.Abs(dx);
+
+ CalculationLogLatDistance.ConvertDistanceToLogLat(
+ tlng,
+ centlat, //旋转中心纬度
+ dx / 1000, //新距离
+ lpAzimuth, //方向垂直
+ out lng2,
+ out lat2);
+ _flightTaskManager.Tasks[i].SingleCopterInfos[j].TargetLat = lat2;
+
+ }
+
+
+ }
+ // await Task.Delay(100); // 如果不等待一段时间，很可能会再触发 DataStreamReceived 事件导致飞行器重新出现在地图上。
+ }));
+ }
+ }
+
private ICommand _ShowallTaskpointCommand;
public ICommand ShowallTaskpointCommand
@@ -243,10 +383,6 @@ namespace Plane.FormationCreator.ViewModels
//_flightTaskManager.SelectedTask = null;
}));
}
-
-
-
-
}
//调整所有任务经度
private ICommand _ModiAllPosCommand;
@@ -559,7 +695,7 @@ namespace Plane.FormationCreator.ViewModels
{
var tasksText = File.ReadAllText(dialog.FileName);
_flightTaskManager.ImportC4DFlytoTask(tasksText);
- autoalltasktime();
+ _flightTaskManager.SetAllTaskFlytime();
}else
if ((extname == ".svg")|| (extname == ".obj"))
{
@@ -575,43 +711,8 @@ namespace Plane.FormationCreator.ViewModels
- public void autoalltasktime()
- {
- for (int taskIndex = 1; taskIndex < _flightTaskManager.Tasks.Count; taskIndex++)
- {
- if (taskIndex != 0 || _flightTaskManager.Tasks[taskIndex].TaskType == FlightTaskType.FlyTo)
- {
- if (_copterManager.Copters.Count() > 0)
- {
- double maxDistance = 0.0f;
- string copterName = "";
- double speed = 0.0f;
- foreach (var copter in _copterManager.Copters)
- {
- var prevWaypoint = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
- var curWaypoint = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
-
- double distance = GeographyUtils.CalcDistance(
- prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
- curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
- if (distance > maxDistance)
- {
- maxDistance = distance;
- copterName = copter.Name;
- speed = curWaypoint.LevelSpeed;
- }
- }
- double time = CalculateFlyIime(maxDistance, speed);
- Message.Show($"任务：{taskIndex},最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
- _flightTaskManager.Tasks[taskIndex].FlytoTime = (int)Math.Round(time, 2);
-
- }
- }
- }
- }
-
private ICommand _ExportWayPointCommand;
@@ -621,6 +722,9 @@ namespace Plane.FormationCreator.ViewModels
{
return _ExportWayPointCommand ?? (_ExportWayPointCommand = new RelayCommand(async =>
{
+
+ // _flightTaskManager.OptimizeRouteNew();
+ // return;
var dialog = new SaveFileDialog
{
DefaultExt = "txt",
@@ -645,58 +749,7 @@ namespace Plane.FormationCreator.ViewModels
{
return _AutoWayPointTmCommand ?? (_AutoWayPointTmCommand = new RelayCommand(async =>
{
-
-
-
-
- int taskIndex = _flightTaskManager.SelectedTaskIndex;
- if (taskIndex != 0 || _flightTaskManager.SelectedTask.TaskType == FlightTaskType.FlyTo)
- {
- if (_copterManager.Copters.Count() > 0)
- {
- double maxDistance = 0.0f;
- string copterName = "";
- double speed = 0.0f;
- foreach (var copter in _copterManager.Copters)
- {
- var prevWaypoint = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
- var curWaypoint = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
-
- double distance = GeographyUtils.CalcDistance(
- prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
- curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
- if (distance > maxDistance)
- {
- maxDistance = distance;
- copterName = copter.Name;
- speed = curWaypoint.LevelSpeed;
- }
- }
-
- double time = CalculateFlyIime(maxDistance, speed);
- Message.Show($"最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
- _flightTaskManager.SelectedTask.FlytoTime = (int)Math.Round(time, 2);
-
- }
- }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+ _flightTaskManager.SetTaskFlytime(_flightTaskManager.SelectedTaskIndex);
}));
}
}
@@ -714,16 +767,26 @@ namespace Plane.FormationCreator.ViewModels
if (Alert.Show("本操作将导致所有任务飞行时间重新计算，您确定要继续吗？", "提示", MessageBoxButton.OKCancel, MessageBoxImage.Warning)
== MessageBoxResult.OK)
{
- autoalltasktime();
-
-
+ _flightTaskManager.SetAllTaskFlytime();
}
}));
}
}
-
+ private ICommand _OptimizeRouteCommand3D;
+ public ICommand OptimizeRouteCommand3D
+ {
+ get
+ {
+ return _OptimizeRouteCommand3D ?? (_OptimizeRouteCommand3D = new RelayCommand(async =>
+ {
+ //_flightTaskManager.OptimizeRoute2();
+ // _flightTaskManager.OptimizeRouteNew (); //最后可用的，但是经纬度计算，
+ _flightTaskManager.OptimizeRouteMeter(true); //采用米计算逻辑和OptimizeRouteNew一样
+ }));
+ }
+ }
private ICommand _OptimizeRouteCommand;
public ICommand OptimizeRouteCommand
{
@@ -732,7 +795,8 @@ namespace Plane.FormationCreator.ViewModels
return _OptimizeRouteCommand ?? (_OptimizeRouteCommand = new RelayCommand(async =>
{
//_flightTaskManager.OptimizeRoute2();
- _flightTaskManager.OptimizeRouteNew();
+ // _flightTaskManager.OptimizeRouteNew (); //最后可用的，但是经纬度计算，
+ _flightTaskManager.OptimizeRouteMeter(); //采用米计算逻辑和OptimizeRouteNew一样
}));
}
}
@@ -919,23 +983,7 @@ public ICommand VerticlAlignmentCommand
}
- public static double CalculateFlyIime(double s, double v)
- {
- double t;
- double a = 1; //加速度1米每秒
- double at = v / a;
- double a_s = 0.5f * a * at * at;
- if (a_s > (s / 2)) //还没到特定速度就到了中点了
- {
- t = (float)System.Math.Sqrt(s / a) * 2;
- }
- else
- {
- t = (s - a_s * 2) / v + at * 2;
- }
- return t;
- }
@@ -1546,35 +1594,8 @@ public ICommand VerticlAlignmentCommand
{
return _MaxDistinceAndTimeCommand ?? (_MaxDistinceAndTimeCommand = new RelayCommand(async =>
{
- int taskIndex = _flightTaskManager.SelectedTaskIndex;
- if (taskIndex != 0 || _flightTaskManager.SelectedTask.TaskType == FlightTaskType.FlyTo)
- {
- if (_copterManager.AcceptingControlCopters.Count() > 0)
- {
- double maxDistance = 0.0f;
- string copterName = "";
- double speed = 0.0f;
- foreach (var copter in _copterManager.AcceptingControlCopters)
- {
- var prevWaypoint = _flightTaskManager.Tasks[taskIndex - 1].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
- var curWaypoint = _flightTaskManager.Tasks[taskIndex].SingleCopterInfos.FirstOrDefault(c => c.Copter == copter);
-
- double distance = GeographyUtils.CalcDistance(
- prevWaypoint.TargetLat, prevWaypoint.TargetLng, prevWaypoint.TargetAlt,
- curWaypoint.TargetLat, curWaypoint.TargetLng, curWaypoint.TargetAlt);
- if (distance > maxDistance)
- {
- maxDistance = distance;
- copterName = copter.Name;
- speed = curWaypoint.LevelSpeed;
- }
- }
-
- double time = CalculateFlyIime(maxDistance, speed);
- Message.Show($"最大航点间距 = {Math.Round(maxDistance, 2)}米, 水平速度={Math.Round(speed, 2)}, 飞行时间 = {Math.Round(time, 2)}秒, 飞机编号:{copterName}");
- }
- }
-
+ //不设置时间，只是计算
+ _flightTaskManager.SetTaskFlytime(_flightTaskManager.SelectedTaskIndex, false);
}));
}
}
diff --git a/Plane.FormationCreator/Views/ModifyTaskView.xaml b/Plane.FormationCreator/Views/ModifyTaskView.xaml
index 86fa3c7..8209560 100644
--- a/Plane.FormationCreator/Views/ModifyTaskView.xaml
+++ b/Plane.FormationCreator/Views/ModifyTaskView.xaml
@@ -70,7 +70,9 @@
-
+
@@ -569,8 +571,9 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -630,6 +662,7 @@
+