判断平面（bug修改） 智能挫层判断4点平面 智能路径添加交叉上限参数 3D绕行修改（日志添加）
This commit is contained in:
szdot
parent
194b12335e
commit
825384813f
@ -1,4 +1,4 @@
|
||||
using FlyBase;
|
||||
using FlightRouteV2;
|
||||
using System;
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
@ -12,7 +12,7 @@ namespace FlieOperate
|
||||
/// <summary>
|
||||
/// 关于文件的 操作类
|
||||
/// </summary>
|
||||
public class FileBase
|
||||
public static class FileBase
|
||||
{
|
||||
/// <summary>
|
||||
/// 逐行读取 文档
|
||||
|
||||
@ -31,9 +31,6 @@
|
||||
<WarningLevel>4</WarningLevel>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<Reference Include="FlyBase">
|
||||
<HintPath>..\FlyCube\bin\Release\FlyBase.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="Newtonsoft.Json, Version=13.0.0.0, Culture=neutral, PublicKeyToken=30ad4fe6b2a6aeed, processorArchitecture=MSIL">
|
||||
<HintPath>..\FlyCube\packages\Newtonsoft.Json.13.0.1\lib\net45\Newtonsoft.Json.dll</HintPath>
|
||||
</Reference>
|
||||
@ -59,5 +56,11 @@
|
||||
<ItemGroup>
|
||||
<WCFMetadata Include="Connected Services\" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\FlyBase\FlyBase.csproj">
|
||||
<Project>{626a9bfa-07de-4063-a178-360eb7057ed6}</Project>
|
||||
<Name>FlyBase</Name>
|
||||
</ProjectReference>
|
||||
</ItemGroup>
|
||||
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
|
||||
</Project>
|
||||
@ -8,17 +8,19 @@ using System.Net.Http;
|
||||
using System.Net.NetworkInformation;
|
||||
using System.Reflection;
|
||||
using System.Runtime.CompilerServices;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Security.Cryptography;
|
||||
using System.Security.Policy;
|
||||
using System.Text;
|
||||
using System.Text.RegularExpressions;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows;
|
||||
using System.Windows.Documents;
|
||||
using System.Windows.Shapes;
|
||||
using static System.Net.WebRequestMethods;
|
||||
|
||||
namespace FlyBase
|
||||
namespace FlightRouteV2
|
||||
{
|
||||
//矩阵类
|
||||
public struct Matrix
|
||||
@ -263,8 +265,9 @@ namespace FlyBase
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
// 检查对象是否与当前向量具有相同的类型
|
||||
if (obj is Vector3 vector)
|
||||
if (obj is Vector3)
|
||||
{
|
||||
Vector3 vector = (Vector3)obj;
|
||||
// 比较向量的每个分量
|
||||
return X == vector.X &&
|
||||
Y == vector.Y &&
|
||||
@ -537,19 +540,7 @@ namespace FlyBase
|
||||
Dictionary<int, List<int>> graph = new Dictionary<int, List<int>>();
|
||||
Dictionary<int, bool> visited = new Dictionary<int, bool>();
|
||||
List<List<int>> result = new List<List<int>>();
|
||||
void DFS(int node, List<int> connected)
|
||||
{
|
||||
visited[node] = true;
|
||||
connected.Add(node);
|
||||
|
||||
foreach (var neighbor in graph[node])
|
||||
{
|
||||
if (!visited[neighbor])
|
||||
{
|
||||
DFS(neighbor, connected);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 构建图
|
||||
foreach (var edge in arr)
|
||||
{
|
||||
@ -572,18 +563,31 @@ namespace FlyBase
|
||||
if (!visited[node])
|
||||
{
|
||||
List<int> connected = new List<int>();
|
||||
DFS(node, connected);
|
||||
DFS(node, connected, graph, visited);
|
||||
result.Add(connected);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
private static void DFS(int node, List<int> connected, Dictionary<int, List<int>> graph, Dictionary<int, bool> visited)
|
||||
{
|
||||
visited[node] = true;
|
||||
connected.Add(node);
|
||||
|
||||
foreach (var neighbor in graph[node])
|
||||
{
|
||||
if (!visited[neighbor])
|
||||
{
|
||||
DFS(neighbor, connected, graph, visited);
|
||||
}
|
||||
}
|
||||
}
|
||||
/// <summary>
|
||||
/// 获取一组序列的所有排列方式 ps:[0,1,2] 结果[[0, 1, 2],[0, 2, 1],[1, 0, 2],[1, 2, 0],[2, 0, 1],[2, 1, 0]]
|
||||
/// </summary>
|
||||
/// <param name="array">一组序列</param>
|
||||
/// <returns>所有序列的排列方式</returns>
|
||||
public static List<List<int>> Permutations(List<int> array)
|
||||
private static List<List<int>> Permutations(List<int> array)
|
||||
{
|
||||
List<List<int>> result = new List<List<int>>();
|
||||
GeneratePermutations(array, 0, array.Count - 1, result);
|
||||
@ -616,20 +620,12 @@ namespace FlyBase
|
||||
/// <param name="bVecs">b坐标集合</param>
|
||||
/// <param name="match">a b集合的对应关系</param>
|
||||
/// <returns>坐标集合</returns>
|
||||
private static Vector3[] CreateNewBVecs(Vector3[] aVecs, Vector3[] bVecs, List<int[]> match)
|
||||
private static Vector3[] CreateNewBVecs(Vector3[] bVecs, List<int[]> match)
|
||||
{
|
||||
Dictionary<int, int> indexMap = new Dictionary<int, int>();
|
||||
// 构建索引映射
|
||||
foreach (var pair in match)
|
||||
{
|
||||
indexMap[pair[0]] = pair[1];
|
||||
}
|
||||
// 根据映射构建新的bb数组
|
||||
Vector3[] new_bVecs = new Vector3[bVecs.Length];
|
||||
foreach (var entry in indexMap)
|
||||
foreach (int[] m in match)
|
||||
{
|
||||
int newIndex = entry.Value;
|
||||
new_bVecs[newIndex] = bVecs[entry.Key];
|
||||
new_bVecs[m[0]] = bVecs[m[1]];
|
||||
}
|
||||
return new_bVecs;
|
||||
}
|
||||
@ -733,17 +729,17 @@ namespace FlyBase
|
||||
/// <returns>true在一个平面 false不在一个平面</returns>
|
||||
public static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
{
|
||||
// 计算两条边
|
||||
Vector3 side1 = vec2 - vec1;
|
||||
Vector3 side2 = vec3 - vec2;
|
||||
// 计算法线向量
|
||||
Vector3 normal = CrossPro(side1, side2);
|
||||
// 计算第四个点到第一个点的向量
|
||||
Vector3 toPoint4 = vec4 - vec1;
|
||||
// 设置容差值，根据具体情况调整
|
||||
float tolerance = 0.01f;
|
||||
// 判断四个点是否在同一个平面上
|
||||
return Math.Abs(DotPro(toPoint4, normal)) < tolerance;
|
||||
//计算三个向量
|
||||
Vector3 v1v2 = vec2 - vec1;
|
||||
Vector3 v1v3 = vec3 - vec1;
|
||||
Vector3 v1v4 = vec4 - vec1;
|
||||
//计算法线向量
|
||||
Vector3 normal_vector = CrossPro(v1v2,v1v3);
|
||||
//计算点到平面的距离
|
||||
double distance = DotPro(normal_vector, v1v4) / normal_vector.GetMag();
|
||||
//设置一个阈值，判断是否共面
|
||||
double epsilon = 0.1;
|
||||
return Math.Abs(distance) < epsilon;
|
||||
}
|
||||
/// <summary>
|
||||
/// 判断4个点是否在同一条直线上
|
||||
@ -753,7 +749,7 @@ namespace FlyBase
|
||||
/// <param name="vector3">点3</param>
|
||||
/// <param name="vector4">点4</param>
|
||||
/// <returns>true在一条直线上 false不在一条直线上</returns>
|
||||
public static bool IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
private static bool IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
{
|
||||
// 计算相邻点之间的方向向量
|
||||
Vector3 dir1 = new Vector3(vec2.X - vec1.X, vec2.Y - vec1.Y, vec2.Z - vec1.Z);
|
||||
@ -762,7 +758,7 @@ namespace FlyBase
|
||||
|
||||
// 检查任意两个相邻方向向量的叉积是否为零
|
||||
// 这是在三维空间中判断共线的条件
|
||||
return CrossPro(dir1, dir2).IsZero() && CrossPro(dir2, dir3).IsZero();
|
||||
return CrossPro(dir1, dir2).GetMag() <0.1 && CrossPro(dir2, dir3).GetMag() <0.1;
|
||||
}
|
||||
/// <summary>
|
||||
/// 辅助方法，用于检查向量是否为零向量
|
||||
@ -774,20 +770,6 @@ namespace FlyBase
|
||||
return vector.X == 0 && vector.Y == 0 && vector.Z == 0;
|
||||
}
|
||||
/// <summary>
|
||||
/// 坐标置换 向量乘以矩阵(i帽j帽k帽)得到置换后的向量坐标 ps:x指向i帽 y轴指向j帽 z轴指向k帽
|
||||
/// </summary>
|
||||
/// <param name="vec">要变换的向量</param>
|
||||
/// <param name="mat">矩阵标量 既i帽 j帽 k帽 三点的坐标组成的矩阵 “可以想象成臂长为1的操作轴图标”</param>
|
||||
/// <returns></returns>
|
||||
private static Vector3 MatrixMul(Vector3 vec, Vector3[] mat)
|
||||
{
|
||||
Vector3 re = new Vector3();//声明返回值
|
||||
re.X = vec.X * mat[0].X + vec.Y * mat[1].X + vec.Z * mat[2].X;
|
||||
re.Y = vec.X * mat[0].Y + vec.Y * mat[1].Y + vec.Z * mat[2].Y;
|
||||
re.Z = vec.X * mat[0].Z + vec.Y * mat[1].Z + vec.Z * mat[2].Z;
|
||||
return re;
|
||||
}
|
||||
/// <summary>
|
||||
/// 获取两条线段 的最近位置的距离和占比
|
||||
/// </summary>
|
||||
/// <param name="a1">线段1起始点</param>
|
||||
@ -795,16 +777,8 @@ namespace FlyBase
|
||||
/// <param name="b1">线段2起始点</param>
|
||||
/// <param name="b2">线段2起终点</param>
|
||||
/// <returns>[在线段1占比，在线段2占比，最近距离]</returns>
|
||||
public static double RecentlySquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||||
private static double RecentlySquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||||
{
|
||||
bool IsEqual(double x, double y)
|
||||
{
|
||||
if (Math.Abs(x - y) < 1e-7)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (a1 == a2) a2 += 1;// 防止线段长度为0
|
||||
if (b1 == b2) b2 += 1;
|
||||
double ux = a2.X - a1.X;
|
||||
@ -884,6 +858,14 @@ namespace FlyBase
|
||||
double dz = wz + (sc * uz) - (tc * vz);
|
||||
return dx * dx + dy * dy + dz * dz;//两线最近距离的平方
|
||||
}
|
||||
private static bool IsEqual(double x, double y)
|
||||
{
|
||||
if (Math.Abs(x - y) < 1e-7)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
/// <summary>
|
||||
/// 按比例在两条线段上截取对应点间的最小距离 平方
|
||||
/// </summary>
|
||||
@ -892,7 +874,7 @@ namespace FlyBase
|
||||
/// <param name="b1">线段2起始点</param>
|
||||
/// <param name="b2">线段2起终点</param>
|
||||
/// <returns>最小距离的平方值</returns>
|
||||
public static double MinDistanceSquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||||
private static double MinDistanceSquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||||
{
|
||||
//相对位置和相对速度
|
||||
Vector3 d0 = b1 - a1;
|
||||
@ -962,7 +944,7 @@ namespace FlyBase
|
||||
/// <param name="pos">坐标集合</param>
|
||||
/// <param name="isCentroid">默认返回为true重心 false则为中心</param>
|
||||
/// <returns>重心或中心坐标</returns>
|
||||
public static Vector3 GetPosCenter(List<Vector3> pos, bool isCentroid = true)
|
||||
private static Vector3 GetPosCenter(List<Vector3> pos, bool isCentroid = true)
|
||||
{
|
||||
int cou = pos.Count;
|
||||
if (isCentroid)//重心
|
||||
@ -1025,25 +1007,25 @@ namespace FlyBase
|
||||
/// <summary>
|
||||
/// 碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="startPos">始点坐标集合</param>
|
||||
/// <param name="endPos">终点坐标集合</param>
|
||||
/// <param name="aVecs">始点坐标集合</param>
|
||||
/// <param name="bVecs">终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线最小间距平方值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
|
||||
/// <returns></returns>
|
||||
public static List<int[]> AirImitation(Vector3[] startPos, Vector3[] endPos, double lineDistanceSquare = 36100, double spaceBetweenSquare = 48400)
|
||||
private static List<int[]> AirImitation(Vector3[] aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 650000)
|
||||
{
|
||||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||||
int planeCou = startPos.Length; //获取飞机总数
|
||||
int planeCou = aVecs.Length; //获取飞机总数
|
||||
for (int a = 0; a < planeCou; a++)
|
||||
{
|
||||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||||
{
|
||||
//判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
//判断飞机距离是否过近
|
||||
double planeLenSquare = MinDistanceSquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
|
||||
double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||||
if (planeLenSquare < spaceBetweenSquare)
|
||||
{
|
||||
planesCollision.Add(new int[] { a, a + 1 + b });
|
||||
@ -1056,25 +1038,25 @@ namespace FlyBase
|
||||
/// <summary>
|
||||
/// 碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="startPos">始点坐标集合</param>
|
||||
/// <param name="endPos">终点坐标集合</param>
|
||||
/// <param name="aVecs">始点坐标集合</param>
|
||||
/// <param name="bVecs">终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线最小间距平方值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
|
||||
/// <returns></returns>
|
||||
public static List<int[]> AirImitation(List<Vector3> startPos, List<Vector3> endPos, double lineDistanceSquare = 36100, double spaceBetweenSquare = 48400)
|
||||
private static List<int[]> AirImitation(List<Vector3> aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 650000)
|
||||
{
|
||||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||||
int planeCou = startPos.Count; //获取飞机总数
|
||||
int planeCou = aVecs.Count; //获取飞机总数
|
||||
for (int a = 0; a < planeCou; a++)
|
||||
{
|
||||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||||
{
|
||||
//判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
//判断飞机距离是否过近
|
||||
double planeLenSquare = MinDistanceSquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
|
||||
double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||||
if (planeLenSquare < spaceBetweenSquare)
|
||||
{
|
||||
planesCollision.Add(new int[] { a, a + 1 + b });
|
||||
@ -1088,22 +1070,22 @@ namespace FlyBase
|
||||
/// 单机碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||||
/// <param name="startPos">飞机起始坐标集合</param>
|
||||
/// <param name="endPos">飞机终点坐标集合</param>
|
||||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||||
public static bool OnlyImitation(int onlyPlaneId, Vector3[] startPos, Vector3[] endPos, double lineDistanceSquare = 32400, double spaceBetweenSquare = 90000)
|
||||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||||
{
|
||||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||||
for (int contrastId = 0; contrastId < startPos.Length; contrastId++)
|
||||
for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
|
||||
{
|
||||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||||
// 判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//航线最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//按比例飞行最小间距
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||||
if (minDistanceSquare < spaceBetweenSquare)
|
||||
{
|
||||
return true;//返回有碰撞
|
||||
@ -1116,22 +1098,78 @@ namespace FlyBase
|
||||
/// 单机碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||||
/// <param name="startPos">飞机起始坐标集合</param>
|
||||
/// <param name="endPos">飞机终点坐标集合</param>
|
||||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||||
public static bool OnlyImitation(int onlyPlaneId, List<Vector3> startPos, List<Vector3> endPos, double lineDistanceSquare = 32400, double spaceBetweenSquare = 90000)
|
||||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||||
{
|
||||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||||
for (int contrastId = 0; contrastId < startPos.Count; contrastId++)
|
||||
for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
|
||||
{
|
||||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||||
// 判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//航线最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//按比例飞行最小间距
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||||
if (minDistanceSquare < spaceBetweenSquare)
|
||||
{
|
||||
return true;//返回有碰撞
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;//返回没有碰撞;
|
||||
}
|
||||
/// <summary>
|
||||
/// 单机碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||||
{
|
||||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||||
for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
|
||||
{
|
||||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||||
// 判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||||
if (minDistanceSquare < spaceBetweenSquare)
|
||||
{
|
||||
return true;//返回有碰撞
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;//返回没有碰撞;
|
||||
}
|
||||
/// <summary>
|
||||
/// 单机碰撞检测
|
||||
/// </summary>
|
||||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||||
{
|
||||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||||
for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
|
||||
{
|
||||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||||
// 判断两条轨迹 之间的最小距离
|
||||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||||
if (distanceSquare < lineDistanceSquare)
|
||||
{
|
||||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||||
if (minDistanceSquare < spaceBetweenSquare)
|
||||
{
|
||||
return true;//返回有碰撞
|
||||
@ -1143,11 +1181,13 @@ namespace FlyBase
|
||||
/// <summary>
|
||||
/// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换
|
||||
/// </summary>
|
||||
/// <param name="startPos">起始点坐标组</param>
|
||||
/// <param name="endPos">目标点坐标组</param>
|
||||
/// <param name="aVecs">起始点坐标组</param>
|
||||
/// <param name="bVecs">目标点坐标组</param>
|
||||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||||
/// <param name="swapCount">交换次数</param>
|
||||
/// <returns></returns>
|
||||
public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, int swapCount = 5)
|
||||
/// <param name="crossingLimit">交叉线路数量上限 ps:超过这个数量则不进行交换</param>
|
||||
/// <returns>新的目标点</returns>
|
||||
public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, int swapCount = 5, int crossingLimit = 6)
|
||||
{
|
||||
int planeCou = aVecs.Length; // 飞机总数
|
||||
List<int[]> match = new List<int[]>(); // ab对应关系
|
||||
@ -1162,6 +1202,7 @@ namespace FlyBase
|
||||
// 遍历找出 AB集合对应关系
|
||||
for (int k = 0; k < planeCou; k++)
|
||||
{
|
||||
|
||||
// 遍历每次刷新 剩下为未匹配总数
|
||||
int remainCou = aIndex.Count;
|
||||
// 遍历每次刷新 重心
|
||||
@ -1185,7 +1226,6 @@ namespace FlyBase
|
||||
int bMaxIndex = GetIndexOfMaxOrMin(bLens); // b集合到重心点最长的距离 数组的下标
|
||||
if (aLens[aMaxIndex] > bLens[bMaxIndex])//找出 最外层如果为A集合点 对应B集合最近点 的下标
|
||||
{
|
||||
//MessageBox.Show(aIndex[aMaxIndex].ToString());
|
||||
double[] outAtoBLen = new double[remainCou];//最外层A点到 B集合所有点的距离
|
||||
for (int i = 0; i < remainCou; i++)
|
||||
{
|
||||
@ -1209,8 +1249,7 @@ namespace FlyBase
|
||||
bIndex.RemoveAt(bMaxIndex); // 删除已经配对的b集合 ID
|
||||
}
|
||||
}
|
||||
Vector3[] new_bVecs = CreateNewBVecs(aVecs, bVecs, match);// 按照映射 获取a 对应的 新的b集合
|
||||
return new_bVecs;
|
||||
Vector3[] new_bVecs = CreateNewBVecs(bVecs, match);// 按照映射 获取a 对应的 新的b集合
|
||||
|
||||
//交叉 交换
|
||||
for (int i = 0; i < swapCount; i++)
|
||||
@ -1220,7 +1259,7 @@ namespace FlyBase
|
||||
// 遍历所有交叉组 分组做交换
|
||||
foreach (List<int> swap_indices in formatCollision)
|
||||
{
|
||||
if (swap_indices.Count <= 6) // 只尝试5组交叉碰撞以下的航线互换
|
||||
if (swap_indices.Count <= crossingLimit) // 只尝试5组交叉碰撞以下的航线互换
|
||||
{
|
||||
//交叉 生成所有排列
|
||||
List<List<int>> all_permutations = Permutations(swap_indices);
|
||||
@ -1244,7 +1283,7 @@ namespace FlyBase
|
||||
tempLen.Add(AirImitation(aVecs, current_array).Count);// 迭代 记录所有排列的碰撞次数
|
||||
tempNew_bVecsS.Add(current_array); //迭代 记录所有排列交换之后的目标坐标集
|
||||
}
|
||||
new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)];
|
||||
new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)].ToArray();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1253,44 +1292,55 @@ namespace FlyBase
|
||||
/// <summary>
|
||||
/// 智能挫层
|
||||
/// </summary>
|
||||
/// <param name="startPos">起始坐标集合</param>
|
||||
/// <param name="endPos">终点做标集合</param>
|
||||
/// <param name="aVecs">起始坐标集合</param>
|
||||
/// <param name="bVecs">终点做标集合</param>
|
||||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||||
/// <param name="layHight">挫层层高</param>
|
||||
/// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点</returns>
|
||||
public static List<Vector3[]> CollisionLayer(Vector3[] startPos, Vector3[] endPos, double layHight = 220)
|
||||
public static List<Vector3[]> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, double layHight = 220)
|
||||
{
|
||||
List<Vector3[]> re = new List<Vector3[]>();
|
||||
//获取飞机总数
|
||||
int planeCou = startPos.Length;
|
||||
//检测两个图形是否在一个平面上
|
||||
Vector3[] allPos = startPos.Concat(endPos).ToArray();//合并两个集合
|
||||
List<Vector3[]> ppppVecArr = RandomSel4Vec(allPos);//随机选择
|
||||
if (ppppVecArr.Count == 0)//两个图形不够四个点 返回 空数组
|
||||
int planeCou = aVecs.Length;
|
||||
if (planeCou < 2)//至少不少于2架飞机 才开始挫层
|
||||
{
|
||||
StrPrint("图案至少要两个点阵！");
|
||||
return re;
|
||||
}
|
||||
// 使用 foreach 遍历 List<Vector3[]> 中的元素
|
||||
foreach (Vector3[] pppVec in ppppVecArr)
|
||||
//检测两个图形是否在一个平面上
|
||||
Vector3 vec0 = new Vector3(0, 0, 0);
|
||||
Vector3 vec1 = new Vector3(0, 0, 0);
|
||||
Vector3 vec2 = new Vector3(0, 0, 0);
|
||||
Vector3 vec3 = new Vector3(0, 0, 0);
|
||||
|
||||
//遍历所有点 查看是不是都再一个平面内
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
if (!(IsVecsOnPlane(pppVec[0], pppVec[1], pppVec[2], pppVec[3])))
|
||||
vec0 = aVecs[i]; vec1 = bVecs[planeCou - i - 1]; vec2 = aVecs[planeCou - i - 1]; vec3 = bVecs[i];
|
||||
if (!(IsVecsOnPlane(vec0, vec1, vec2, vec3)))
|
||||
{
|
||||
StrPrint("图案不在一个平面!");
|
||||
return re;//两个图形不在一个平面上 返回 空数组
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
//遍历选出4个不共线的点 ps:方便后续叉积算法线标量
|
||||
bool isVecsOnLine = false; Vector3 vec0 = new Vector3(0, 0, 0); Vector3 vec1 = new Vector3(0, 0, 0); Vector3 vec2 = new Vector3(0, 0, 0); Vector3 vec3 = new Vector3(0, 0, 0);
|
||||
bool isVecsOnLine = false;
|
||||
for (int i = 0; i < planeCou; i++) //遍历所有点 找出不在一条直线上的四个点
|
||||
{
|
||||
vec0 = startPos[i]; vec1 = endPos[planeCou - i - 1]; vec2 = startPos[planeCou - i - 1]; vec3 = endPos[i];
|
||||
vec0 = aVecs[i]; vec1 = bVecs[planeCou - i - 1]; vec2 = aVecs[planeCou - i - 1]; vec3 = bVecs[i];
|
||||
isVecsOnLine = IsVecsOnLine(vec0, vec1, vec2, vec3);
|
||||
if (isVecsOnLine)
|
||||
if (!isVecsOnLine)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (isVecsOnLine)
|
||||
{
|
||||
StrPrint("所有点都在一条直线上!");
|
||||
return re;//两个图案的所有点都在一条直线上 返回 空数组
|
||||
}
|
||||
//检查完毕后
|
||||
@ -1303,35 +1353,37 @@ namespace FlyBase
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
int shiftCou = 1; //记录循环次数 即层数
|
||||
Vector3 aOrigin = startPos[i]; //原点位置
|
||||
Vector3 bOrigin = endPos[i]; //原点位置
|
||||
while (OnlyImitation(i, startPos, endPos))
|
||||
Vector3 aOrigin = aVecs[i]; //原点位置
|
||||
Vector3 bOrigin = bVecs[i]; //原点位置
|
||||
while (OnlyImitation(i, aVecs, bVecs))
|
||||
{
|
||||
Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
|
||||
if (shiftCou % 2 == 1)
|
||||
{
|
||||
startPos[i] = aOrigin + shiftVec;
|
||||
endPos[i] = bOrigin + shiftVec;
|
||||
aVecs[i] = aOrigin + shiftVec;
|
||||
bVecs[i] = bOrigin + shiftVec;
|
||||
}
|
||||
else
|
||||
{
|
||||
startPos[i] = aOrigin - shiftVec;
|
||||
endPos[i] = bOrigin - shiftVec;
|
||||
aVecs[i] = aOrigin - shiftVec;
|
||||
bVecs[i] = bOrigin - shiftVec;
|
||||
}
|
||||
shiftCou += 1;
|
||||
}
|
||||
}
|
||||
re.Add(startPos);
|
||||
re.Add(endPos);
|
||||
StrPrint("挫层完成！");
|
||||
re.Add(aVecs);
|
||||
re.Add(bVecs);
|
||||
return re;
|
||||
}
|
||||
/// <summary>
|
||||
/// 路径绕行
|
||||
/// </summary>
|
||||
/// <param name="startPos">起始坐标集合</param>
|
||||
/// <param name="endPos">终点做标集合</param>
|
||||
/// <returns>返回一个二维数组 长度为1即1个中间航点 长度为3即前中后三个中间航点</returns>
|
||||
public static List<List<Vector3>> ABypassB(Vector3[] aVecs, Vector3[] bVecs)
|
||||
/// <param name="aVecs">起始坐标集合</param>
|
||||
/// <param name="bVecs">终点做标集合</param>
|
||||
/// <param name="strPrint">日志输出 回调函数</param>
|
||||
/// <returns>返回一个二维数组 返回值长度0绕行失败 长度1为一个中间航点 长度为3为三个中间航点顺序(前中后)</returns>
|
||||
public static List<List<Vector3>> ABypassB(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint)
|
||||
{
|
||||
List<List<Vector3>> re = new List<List<Vector3>>();
|
||||
int planeCou = aVecs.Length;
|
||||
@ -1355,29 +1407,23 @@ namespace FlyBase
|
||||
List<int> firstCollisionGroup = new List<int>(); //有碰撞飞机列表(第一次绕行)
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
bool isPassMark = false;//初始标记
|
||||
StrPrint($"第一次绕行进度：{(int)((double)i/planeCou*100)}%");
|
||||
bool isPassMark = false; //初始标记
|
||||
int index = sortedIndices[i];//从内到外排序的索引
|
||||
tempAVecs.Add(aVecs[index]);
|
||||
tempBVecs.Add(bVecs[index]);
|
||||
if (i == 0) //如果是第一条航线 直接添加中间航点
|
||||
{
|
||||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); //添加中间航点
|
||||
continue; //跳出 继续第二条航线计算
|
||||
}
|
||||
else //从第二条开始绕
|
||||
{
|
||||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); // 添加默认中间航点
|
||||
// 首次默认中间航点检测
|
||||
if (!(OnlyImitation(i, tempAVecs, middleVecs))) //前半段碰 碰撞检测
|
||||
if (!(OnlyImitation(i, tempAVecs, middleVecs)) && !(OnlyImitation(i, middleVecs, tempBVecs))) //前半段碰 碰撞检测
|
||||
{
|
||||
if (!(OnlyImitation(i, middleVecs, tempBVecs))) //后半段碰 碰撞检测
|
||||
{
|
||||
isPassMark = true; //中间航点
|
||||
continue; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
else
|
||||
{
|
||||
middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
|
||||
}
|
||||
continue; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -1387,19 +1433,12 @@ namespace FlyBase
|
||||
List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
|
||||
foreach (Vector3 v in ringVecs)
|
||||
{
|
||||
middleVecs.Add(v);
|
||||
middleVecs.Add(v); //添加临时绕行航点
|
||||
//检测
|
||||
if (!(OnlyImitation(i, tempAVecs, middleVecs))) //前半段碰 碰撞检测
|
||||
if (!(OnlyImitation(i, tempAVecs, middleVecs)) && !(OnlyImitation(i, middleVecs, tempBVecs))) //前半段碰 碰撞检测
|
||||
{
|
||||
if (!(OnlyImitation(i, middleVecs, tempBVecs))) //后半段碰 碰撞检测
|
||||
{
|
||||
isPassMark = true; //中间航点 碰撞检测通过标记
|
||||
break; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
else
|
||||
{
|
||||
middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
|
||||
}
|
||||
isPassMark = true; //中间航点 碰撞检测通过标记
|
||||
break; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -1407,22 +1446,39 @@ namespace FlyBase
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!(isPassMark)) // 如果绕行失败 添加一个默认中间航点
|
||||
// 如果绕行失败
|
||||
if (!isPassMark)
|
||||
{
|
||||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index]));
|
||||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); //添加一个默认中间航点
|
||||
firstCollisionGroup.Add(index);
|
||||
}
|
||||
}
|
||||
//中间航点坐标集 按飞机id顺序重新映射 ps:恢复原id映射
|
||||
middleVecs = sortedIndices.Select(index => middleVecs[index]).ToList();
|
||||
//中间航点坐标集 按飞机id顺序重新映射 ps:恢复原id映射 排序
|
||||
Vector3[] tempMiddleVecs = new Vector3[planeCou];
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
tempMiddleVecs[sortedIndices[i]] = middleVecs[i];
|
||||
}
|
||||
middleVecs = new List<Vector3>(tempMiddleVecs);
|
||||
//没有碰撞 返回一个中间航点 并返回
|
||||
if (firstCollisionGroup.Count == 0)
|
||||
{
|
||||
StrPrint("第一次绕行通过！");
|
||||
re.Add(middleVecs);
|
||||
return re;
|
||||
}
|
||||
else
|
||||
{
|
||||
string str = "";
|
||||
foreach (int v in firstCollisionGroup) {
|
||||
str += v.ToString() + "号，";
|
||||
}
|
||||
str += "有碰撞,共" + firstCollisionGroup.Count.ToString() + "架！";
|
||||
StrPrint(str);
|
||||
StrPrint("第一次绕行未通过，准备开始第二次绕行!");
|
||||
}
|
||||
|
||||
//第二次绕行 ps:有碰撞进行第二次 前后再各加以航点 嵌套迭代
|
||||
//第二次绕行 ps:有碰撞进行第二次 前后再各加一航点 嵌套迭代
|
||||
List<Vector3> firstMiddleVecs = new List<Vector3>(); //前中间航点
|
||||
List<Vector3> secondMiddleVecs = new List<Vector3>(); //后中间航点
|
||||
List<int> secondCollisionGroup = new List<int>(); //最终有碰撞飞机列表
|
||||
@ -1431,8 +1487,11 @@ namespace FlyBase
|
||||
firstMiddleVecs.Add(SetMiddleVec(aVecs[i], middleVecs[i], 0));
|
||||
secondMiddleVecs.Add(SetMiddleVec(middleVecs[i], bVecs[i], 1));
|
||||
}
|
||||
int z = 0;//计算进度
|
||||
foreach (int index in firstCollisionGroup)
|
||||
{
|
||||
z++;//计算进度
|
||||
StrPrint($"第二次绕行进度：{(int)((double)z / firstCollisionGroup.Count * 100)}%");
|
||||
bool isPassMark = false;
|
||||
// 正中 绕行列表
|
||||
List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
|
||||
@ -1447,20 +1506,49 @@ namespace FlyBase
|
||||
foreach (Vector3 sr in secondRingVecs)
|
||||
{
|
||||
secondMiddleVecs[index] = sr; //后中点逐点
|
||||
// 检测
|
||||
if (!(OnlyImitation(index, aVecs, firstMiddleVecs))){
|
||||
// 检测 起点到 第一中间航点
|
||||
if (!(OnlyImitation(index, aVecs, firstMiddleVecs)) && !(OnlyImitation(index, firstMiddleVecs, middleVecs)) && !(OnlyImitation(index, middleVecs, secondMiddleVecs)) && !(OnlyImitation(index, secondMiddleVecs, bVecs)))
|
||||
{
|
||||
isPassMark = true; //碰撞检测通过标记
|
||||
break; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
//if not(self.onlyImitation(v, aVecs, firstMiddleVecs)): #前半段碰 碰撞检测
|
||||
// if not(self.onlyImitation(v, firstMiddleVecs, middleVecs)):#后半段碰 碰撞检测
|
||||
// if not(self.onlyImitation(v, middleVecs, secondMiddleVecs)): #前半段碰 碰撞检测
|
||||
// if not(self.onlyImitation(v, secondMiddleVecs, bVecs)):#后半段碰 碰撞检测
|
||||
// isPassMark = True #碰撞检测通过标记
|
||||
// break #不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
if (isPassMark)
|
||||
{
|
||||
break; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
}
|
||||
if (isPassMark)
|
||||
{
|
||||
break; //不撞则跳出 进行下一航线的绕行
|
||||
}
|
||||
}
|
||||
if (!isPassMark) //如果绕行失败 添加一个默认中间航点
|
||||
{
|
||||
middleVecs[index] = SetMiddleVec(aVecs[index], bVecs[index]); //添加中间航点
|
||||
firstMiddleVecs[index] = SetMiddleVec(aVecs[index], middleVecs[index], 0); // 添加中间航点
|
||||
secondMiddleVecs[index] = SetMiddleVec(middleVecs[index], bVecs[index], 1); // 添加中间航点
|
||||
secondCollisionGroup.Add(index);// 二次绕行仍不通过 添加到列表 （暂时无用）
|
||||
}
|
||||
}
|
||||
|
||||
if (secondCollisionGroup.Count == 0)//如果二次绕行通过 添加中间三航点
|
||||
{
|
||||
re.Add(firstMiddleVecs);
|
||||
re.Add(middleVecs);
|
||||
re.Add(secondMiddleVecs);
|
||||
StrPrint("第二次绕行通过！");
|
||||
}
|
||||
else
|
||||
{
|
||||
string str = "";
|
||||
foreach (int v in secondCollisionGroup)
|
||||
{
|
||||
str += v.ToString() + "号，";
|
||||
}
|
||||
str += "仍有碰撞,共" + secondCollisionGroup.Count.ToString() + "架！";
|
||||
StrPrint(str);
|
||||
StrPrint("第二次绕行仍未通过！");
|
||||
}
|
||||
return re;
|
||||
}
|
||||
/// <summary>
|
||||
@ -1469,7 +1557,7 @@ namespace FlyBase
|
||||
/// <param name="radius">大圆半径</param>
|
||||
/// <param name="chordLength">固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集</param>
|
||||
/// <returns></returns>
|
||||
public static List<Vector3> GetVecsOnCircle(double radius, double chordLength = 220)
|
||||
private static List<Vector3> GetVecsOnCircle(double radius, double chordLength = 220)
|
||||
{
|
||||
List<Vector3> vecs = new List<Vector3>();
|
||||
// 计算圆的周长
|
||||
@ -1498,7 +1586,7 @@ namespace FlyBase
|
||||
/// <param name="offCount">偏移次数 从0开始,每+1在中点前后往返偏移</param>
|
||||
/// <param name="layHight">偏移层高</param>
|
||||
/// <returns>矩阵</returns>
|
||||
public static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion = 0.5, int offCount = 0, double layHight = 220)
|
||||
private static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion = 0.5, int offCount = 0, double layHight = 220)
|
||||
{
|
||||
// 计算前向向量 k帽
|
||||
Vector3 k_hat = (bVec - aVec).NormalizEd();
|
||||
@ -1542,7 +1630,7 @@ namespace FlyBase
|
||||
/// <param name="transfer">传递圈函数的弦长 向量矩阵函数的层高</param>
|
||||
/// <param name="paunch">绕行航点范围 ps:绕行中间航点肚子(值越大肚子越小)</param>
|
||||
/// <returns></returns>
|
||||
public static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer = 220, double paunch = 3)
|
||||
private static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer = 220, double paunch = 3)
|
||||
{
|
||||
List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
|
||||
// 根据a到b的长度 算出中间绕行几圈
|
||||
|
||||
@ -98,6 +98,7 @@
|
||||
<Generator>ResXFileCodeGenerator</Generator>
|
||||
<LastGenOutput>Resources.Designer.cs</LastGenOutput>
|
||||
</EmbeddedResource>
|
||||
<None Include=".editorconfig" />
|
||||
<None Include="packages.config" />
|
||||
<None Include="Properties\Settings.settings">
|
||||
<Generator>SettingsSingleFileGenerator</Generator>
|
||||
|
||||
@ -8,9 +8,6 @@ EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "FlyBase", "..\FlyBase\FlyBase.csproj", "{626A9BFA-07DE-4063-A178-360EB7057ED6}"
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "FlieOperate", "..\FlieOperate\FlieOperate.csproj", "{C354FD6B-5863-4246-BB48-6DF14A85C161}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{626A9BFA-07DE-4063-A178-360EB7057ED6} = {626A9BFA-07DE-4063-A178-360EB7057ED6}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
|
||||
@ -21,7 +21,8 @@
|
||||
<Button Content="导入映射图" HorizontalAlignment="Left" VerticalAlignment="Top" Width="220" Height="49" Margin="0,54,0,0" Click="ImportImg_Click"/>
|
||||
<Button Content="保存航点" HorizontalAlignment="Left" VerticalAlignment="Top" Width="220" Height="49" Margin="0,108,0,0" Click="ExportFcgm_Click"/>
|
||||
<Button Content="渲染" HorizontalAlignment="Left" VerticalAlignment="Top" Width="220" Height="49" Margin="0,162,0,0" Click="Render_Click"/>
|
||||
<Button Content="测试" HorizontalAlignment="Left" VerticalAlignment="Top" Width="220" Height="49" Margin="0,220,0,0" Click="Del_Click"/>
|
||||
<Button Content="3D绕行" HorizontalAlignment="Left" VerticalAlignment="Top" Width="220" Height="49" Margin="0,270,0,0" Click="ByPass_Click"/>
|
||||
<TextBox Name="vLogBox" HorizontalAlignment="Center" VerticalAlignment="Top" Width="220" Height="284" Margin="0,333,0,0"/>
|
||||
</Grid>
|
||||
</WrapPanel>
|
||||
</Window>
|
||||
@ -13,18 +13,20 @@ using System.Windows.Input;
|
||||
using System.Windows.Media;
|
||||
using System.Windows.Media.Imaging;
|
||||
using System.Windows.Shapes;
|
||||
using FlyBase;
|
||||
using FlightRouteV2;
|
||||
using FlieOperate;
|
||||
using static FlieOperate.FcgmJsonModel.Tasks.SingleCopterInfos;
|
||||
using FlyCube.Models;
|
||||
using ControlLibrary;
|
||||
using WpfAnimatedGif;//播放GIF
|
||||
using System.Security.Cryptography;
|
||||
using System.Runtime.CompilerServices;
|
||||
|
||||
namespace FlyCube
|
||||
{
|
||||
public partial class MainWindow : Window
|
||||
{
|
||||
private TextBox LOG;
|
||||
/// <summary>
|
||||
/// 入口函数
|
||||
/// </summary>
|
||||
@ -33,53 +35,64 @@ namespace FlyCube
|
||||
InitializeComponent();
|
||||
//创建一个 主画布对象
|
||||
TopCanvas = new MainCanvas(this.LayerPlane);
|
||||
|
||||
}
|
||||
private void Del_Click(object sender, RoutedEventArgs e)
|
||||
/// <summary>
|
||||
/// 3D绕行
|
||||
/// </summary>
|
||||
private void ByPass_Click(object sender, RoutedEventArgs e)
|
||||
{
|
||||
Vector3 a1 = new Vector3(0, 50, 220);
|
||||
Vector3 a2 = new Vector3(0, 0, 0);
|
||||
Vector3 a3 = new Vector3(0, 1000, 1000);
|
||||
Vector3 a4 = new Vector3(1020, 0, 1000);
|
||||
Vector3 a5 = new Vector3(1020, 0, 2240);
|
||||
Vector3 a6 = new Vector3(1400, 2000, 4500);
|
||||
|
||||
Vector3 b1 = new Vector3(420, 0, 330);
|
||||
Vector3 b2 = new Vector3(310, 0, 1400);
|
||||
Vector3 b3 = new Vector3(120, 0, 4200);
|
||||
Vector3 b4 = new Vector3(2330, 0, 7300);
|
||||
Vector3 b5 = new Vector3(110, 0, 1400);
|
||||
Vector3 b6 = new Vector3(1200, 0, 5200);
|
||||
Vector3[] aa = new Vector3[] { a1, a2, a3, a4, a5, a6 };
|
||||
Vector3[] bb = new Vector3[] { b1, b2, b3, b4, b5, b6 };
|
||||
|
||||
//FlyVecFun.GetRingVec(a1, a6, 0.5);
|
||||
//return;
|
||||
List<Vector3> a = FlyVecFun.GetRingVec(a1, a6, 0.5);
|
||||
//foreach (int[] v in cc)
|
||||
//{
|
||||
// MessageBox.Show($"{ v[0].ToString()}----{v[1].ToString()}");
|
||||
//}
|
||||
|
||||
//Vector3[] cc = FlyVecFun.ContactABOut(aa,bb);
|
||||
//foreach (var item in a)
|
||||
//{
|
||||
// MessageBox.Show(item.PosToString());
|
||||
//}
|
||||
//MessageBox.Show($"{a[0].ToString()}---{a[1].ToString()}---{a[2].ToString()}---{a[3].ToString()}");
|
||||
string str = "";
|
||||
int id = 1;
|
||||
foreach (Vector3 item in a)
|
||||
//资源管理器 获取航点文件路径
|
||||
FileBase.FileClass = new string[] { "航点文件" };
|
||||
FileBase.Pascal = new string[] { "*.txt" };
|
||||
string FliePath = FileBase.OpenExplorer("航点文件导入", out bool isSelect);
|
||||
if (isSelect)
|
||||
{
|
||||
str += id + " 0" + " " + item.X + " " + item.Y + " " + item.Z + "\r\n";
|
||||
id++;
|
||||
List<Vector3[]> abVecs = FileBase.TxtToPos(FliePath, out string[] fightNames);//从txt文件里面读取航点 信息
|
||||
Vector3[] aVecs = abVecs[0].ToArray();
|
||||
Vector3[] bVecs = abVecs[1].ToArray();
|
||||
Vector3[] new_bVecs = FlyVecFun.ContactABOut(aVecs, bVecs , StrPrint);
|
||||
List<List<Vector3>> re = FlyVecFun.ABypassB(aVecs, new_bVecs, StrPrint);
|
||||
if (re.Count == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
string txta = "";
|
||||
string txtb = "";
|
||||
string txtc = "";
|
||||
|
||||
for (int i = 0; i < abVecs[0].Length; i++)
|
||||
{
|
||||
txta += i + " 0" + " " + re[0][i].X + " " + re[0][i].Y + " " + re[0][i].Z + "\r\n";
|
||||
if (re.Count > 1)
|
||||
{
|
||||
txtb += i + " 0" + " " + re[1][i].X + " " + re[1][i].Y + " " + re[1][i].Z + "\r\n";
|
||||
txtc += i + " 0" + " " + re[2][i].X + " " + re[2][i].Y + " " + re[2][i].Z + "\r\n";
|
||||
}
|
||||
|
||||
}
|
||||
SaveFile("C:/Users/szdot/Desktop/a.txt", txta);
|
||||
if (re.Count > 1)
|
||||
{
|
||||
SaveFile("C:/Users/szdot/Desktop/b.txt", txtb);
|
||||
SaveFile("C:/Users/szdot/Desktop/c.txt", txtc);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
//for (int i = 0; i < 4; i++)
|
||||
//{
|
||||
// str += i + " 0" + " " + a[i].X + " " + a[i].Y + " " + a[i].Z + "\r\n";
|
||||
//}
|
||||
SaveFile("C:/Users/szdot/Desktop/del.txt", str);
|
||||
}
|
||||
//保存输出文本
|
||||
//回调函数 输出日志
|
||||
private void StrPrint(string str)
|
||||
{
|
||||
//MessageBox.Show(str);
|
||||
vLogBox.Text += "\r\n" + str;
|
||||
}
|
||||
/// <summary>
|
||||
/// 保存输出文本
|
||||
/// </summary>
|
||||
private static void SaveFile(string filePath, string txtCon)
|
||||
{
|
||||
txtCon = txtCon.TrimEnd((char[])"\n\r".ToCharArray());//去除最后的回车符
|
||||
@ -123,7 +136,7 @@ namespace FlyCube
|
||||
//资源管理器 获取航点文件路径
|
||||
FileBase.FileClass = new string[] { "航点文件" };
|
||||
FileBase.Pascal = new string[] { "*.fcgm" };
|
||||
string FliePath = FileBase.OpenExplorer("航点文件导入",out bool isSelect);
|
||||
string FliePath = FileBase.OpenExplorer("航点文件导入", out bool isSelect);
|
||||
if (!isSelect)
|
||||
{
|
||||
return;//未打开文件跳出.
|
||||
@ -137,10 +150,10 @@ namespace FlyCube
|
||||
int id = 0;
|
||||
foreach (var item in this.FcgmInfo.tasks[2].singleCopterInfos)
|
||||
{
|
||||
planesPos[id] = new Vector3(item.x*100, item.targetAlt*100, -item.y*100);//转换厘米单位
|
||||
planesPos[id] = new Vector3(item.x * 100, item.targetAlt * 100, -item.y * 100);//转换厘米单位
|
||||
id++;
|
||||
}
|
||||
Vector3 planesCenterPos=FlyVecFun.GetPosCenter(planesPos);//获取航点的中心点
|
||||
Vector3 planesCenterPos = FlyVecFun.GetPosCenter(planesPos);//获取航点的中心点
|
||||
//坐标集合 自适应画布 缩放比例因子
|
||||
double[] planesPosWhl = FlyVecFun.GetVecsWithHighLength(planesPos);
|
||||
double scale;//比例因子
|
||||
@ -183,8 +196,8 @@ namespace FlyCube
|
||||
private void ImportImg_Click(object sender, RoutedEventArgs e)
|
||||
{
|
||||
//资源管理器 获取航点文件路径
|
||||
FileBase.FileClass = new string[] { "动态图片","静态图片" };
|
||||
FileBase.Pascal = new string[] { "*.gif","*.jpeg;*.jpg;*.png"};
|
||||
FileBase.FileClass = new string[] { "动态图片", "静态图片" };
|
||||
FileBase.Pascal = new string[] { "*.gif", "*.jpeg;*.jpg;*.png" };
|
||||
ImagePath = FileBase.OpenExplorer("映射图片导入", out bool isSelect);
|
||||
if (!isSelect)
|
||||
{
|
||||
@ -194,7 +207,7 @@ namespace FlyCube
|
||||
{
|
||||
TopCanvas.ImportGif(ImagePath);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/// <summary>
|
||||
/// 渲染输出航点
|
||||
@ -239,6 +252,7 @@ namespace FlyCube
|
||||
MessageBox.Show("成功");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/// <summary>
|
||||
/// 灯光 组
|
||||
@ -262,10 +276,9 @@ namespace FlyCube
|
||||
异步变色随机 = 14,
|
||||
拉烟 = 15
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 画布 渲染
|
||||
/// </summary>
|
||||
/// <summary>
|
||||
/// 画布 渲染
|
||||
/// </summary>
|
||||
public class MainCanvas
|
||||
{
|
||||
public Canvas MyCanvas { get; set; }
|
||||
@ -296,7 +309,7 @@ namespace FlyCube
|
||||
string extendName = string.Copy(path);
|
||||
string[] extendNameArr = extendName.Split('.');//获取扩展名
|
||||
Image img = new Image();
|
||||
if (extendNameArr[1]=="gif")
|
||||
if (extendNameArr[1] == "gif")
|
||||
{
|
||||
var image = new BitmapImage();
|
||||
image.BeginInit();
|
||||
@ -305,7 +318,7 @@ namespace FlyCube
|
||||
ImageBehavior.SetAnimatedSource(img, image);
|
||||
MyCanvas.Children.Add(img);
|
||||
}
|
||||
else
|
||||
else
|
||||
{
|
||||
img.Source = new BitmapImage(new Uri(path));
|
||||
MyCanvas.Children.Add(img);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user