1997 lines
86 KiB
C#
1997 lines
86 KiB
C#
using Microsoft.SqlServer.Server;
|
||
using System;
|
||
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.IO;
|
||
using System.Linq;
|
||
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 FlightRouteV2
|
||
{
|
||
//矩阵类
|
||
public struct Matrix
|
||
{
|
||
public double M11, M12, M13;
|
||
public double M21, M22, M23;
|
||
public double M31, M32, M33;
|
||
public double M41, M42, M43;
|
||
/// <summary>
|
||
/// 构造函数，接受四个 Vector3 对象
|
||
/// </summary>
|
||
/// <param name="row1">向量1</param>
|
||
/// <param name="row2">向量2</param>
|
||
/// <param name="row3">向量3</param>
|
||
/// <param name="row4">向量4</param>
|
||
public Matrix(Vector3 row1, Vector3 row2, Vector3 row3, Vector3 row4)
|
||
{
|
||
M11 = row1.X; M12 = row1.Y; M13 = row1.Z;
|
||
M21 = row2.X; M22 = row2.Y; M23 = row2.Z;
|
||
M31 = row3.X; M32 = row3.Y; M33 = row3.Z;
|
||
M41 = row4.X; M42 = row4.Y; M43 = row4.Z;
|
||
}
|
||
/// <summary>
|
||
/// 通过方法实现通过索引的访问
|
||
/// </summary>
|
||
/// <exception cref="IndexOutOfRangeException"></exception>
|
||
public Vector3 this[int index]
|
||
{
|
||
get
|
||
{
|
||
switch (index)
|
||
{
|
||
case 0: return new Vector3(M11, M12, M13);
|
||
case 1: return new Vector3(M21, M22, M23);
|
||
case 2: return new Vector3(M31, M32, M33);
|
||
case 3: return new Vector3(M41, M42, M43);
|
||
default:
|
||
throw new IndexOutOfRangeException("Index out of range for Matrix");
|
||
}
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// // 重写 ToString 方法，以便能够直接打印矩阵
|
||
/// </summary>
|
||
public override string ToString()
|
||
{
|
||
return $"v1:{M11}, {M12}, {M13}\n" +
|
||
$"v2:{M21}, {M22}, {M23}\n" +
|
||
$"v3:{M31}, {M32}, {M33}\n" +
|
||
$"off:{M41}, {M42}, {M43}";
|
||
}
|
||
}
|
||
//向量类
|
||
public struct Vector3
|
||
{
|
||
public double X { get; set; }
|
||
public double Y { get; set; }
|
||
public double Z { get; set; }
|
||
/// <summary>
|
||
/// [数组下标]方式 访问XYZ属性
|
||
/// </summary>
|
||
/// <param name="index"></param>
|
||
/// <returns>[0]X [1]Y [2]Z</returns>
|
||
/// <exception cref="IndexOutOfRangeException">访问下标0-2</exception>
|
||
public double this[int index]
|
||
{
|
||
get
|
||
{
|
||
switch (index)
|
||
{
|
||
case 0:
|
||
return X;
|
||
case 1:
|
||
return Y;
|
||
case 2:
|
||
return Z;
|
||
default:
|
||
throw new IndexOutOfRangeException($"Index {index} is out of range for Vector3");
|
||
}
|
||
}
|
||
set
|
||
{
|
||
switch (index)
|
||
{
|
||
case 0:
|
||
X = value;
|
||
break;
|
||
case 1:
|
||
Y = value;
|
||
break;
|
||
case 2:
|
||
Z = value;
|
||
break;
|
||
default:
|
||
throw new IndexOutOfRangeException($"Index {index} is out of range for Vector3");
|
||
}
|
||
}
|
||
}
|
||
// 静态属性表示单位向量
|
||
public static Vector3 UnitX { get { return new Vector3(1.0, 0.0, 0.0); } }
|
||
public static Vector3 UnitY { get { return new Vector3(0.0, 1.0, 0.0); } }
|
||
public static Vector3 UnitZ { get { return new Vector3(0.0, 0.0, 1.0); } }
|
||
/// <summary>
|
||
/// 构造 初始化
|
||
/// </summary>
|
||
/// <param name="x">x坐标</param>
|
||
/// <param name="y">y坐标</param>
|
||
/// <param name="z">z坐标</param>
|
||
public Vector3(double x, double y, double z)
|
||
{
|
||
this.X = x;
|
||
this.Y = y;
|
||
this.Z = z;
|
||
}
|
||
/// <summary>
|
||
/// 重载二元坐标加法+ 向量+向量
|
||
/// </summary>
|
||
/// <param name="v1">向量加数</param>
|
||
/// <param name="v2">向量加数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator +(Vector3 v1, Vector3 v2)
|
||
{
|
||
return new Vector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标加法+ 向量+小数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">小数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator +(Vector3 v1, double i)
|
||
{
|
||
return new Vector3(v1.X + i, v1.Y + i, v1.Z + i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标加法+ 向量+整数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">整数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator +(Vector3 v1, int i)
|
||
{
|
||
return new Vector3(v1.X + (double)i, v1.Y + (double)i, v1.Z + (double)i);
|
||
}
|
||
/// <summary>
|
||
/// 重载二元坐标减法- 向量-向量
|
||
/// </summary>
|
||
/// <param name="v1">向量被减数</param>
|
||
/// <param name="v2">向量减数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator -(Vector3 v1, Vector3 v2)
|
||
{
|
||
return new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标加法- 向量-小数
|
||
/// </summary>
|
||
/// <param name="v1">向量被减数</param>
|
||
/// <param name="i">小数减数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator -(Vector3 v1, double i)
|
||
{
|
||
return new Vector3(v1.X - i, v1.Y - i, v1.Z - i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标加法- 向量-整数
|
||
/// </summary>
|
||
/// <param name="v1">向量被减数</param>
|
||
/// <param name="i">整数减数</param>
|
||
/// <returns></returns>
|
||
public static Vector3 operator -(Vector3 v1, int i)
|
||
{
|
||
return new Vector3(v1.X - (double)i, v1.Y - (double)i, v1.Z - (double)i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标乘法* 向量*小数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">小数乘数</param>
|
||
/// <returns>得到向量的倍数向量</returns>
|
||
public static Vector3 operator *(Vector3 v1, double i)
|
||
{
|
||
return new Vector3(v1.X * i, v1.Y * i, v1.Z * i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标乘法* 向量*整数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">整数乘数</param>
|
||
/// <returns>得到向量的倍数向量</returns>
|
||
public static Vector3 operator *(Vector3 v1, int i)
|
||
{
|
||
return new Vector3(v1.X * (double)i, v1.Y * (double)i, v1.Z * (double)i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标除法/ 向量/小数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">小数除数</param>
|
||
/// <returns>得到向量的商向量</returns>
|
||
public static Vector3 operator /(Vector3 v1, double i)
|
||
{
|
||
return new Vector3(v1.X / i, v1.Y / i, v1.Z / i);
|
||
}
|
||
/// <summary>
|
||
/// 重载一元坐标除法/ 向量/整数
|
||
/// </summary>
|
||
/// <param name="v1">向量</param>
|
||
/// <param name="i">整数除数</param>
|
||
/// <returns>得到向量的商向量</returns>
|
||
public static Vector3 operator /(Vector3 v1, int i)
|
||
{
|
||
return new Vector3(v1.X / (double)i, v1.Y / (double)i, v1.Z / (double)i);
|
||
}
|
||
/// <summary>
|
||
/// 重载== 向量==向量
|
||
/// </summary>
|
||
/// <param name="v1">向量1</param>
|
||
/// <param name="v2">向量2</param>
|
||
/// <returns>布尔值 判断向量是否相等</returns>
|
||
public static bool operator ==(Vector3 v1, Vector3 v2)
|
||
{
|
||
if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) return true;
|
||
return false;
|
||
}
|
||
/// <summary>
|
||
/// 重载！= 向量！=向量
|
||
/// </summary>
|
||
/// <param name="v1">向量1</param>
|
||
/// <param name="v2">向量2</param>
|
||
/// <returns>布尔值 判断向量是否不相等</returns>
|
||
public static bool operator !=(Vector3 v1, Vector3 v2)
|
||
{
|
||
if (v1.X != v2.X || v1.Y != v2.Y || v1.Z != v2.Z) return true;
|
||
return false;
|
||
}
|
||
/// <summary>
|
||
/// 确定指定的对象是否等于当前向量。
|
||
/// </summary>
|
||
/// <param name="obj">要与当前向量比较的对象。</param>
|
||
/// <returns>如果指定的对象等于当前向量，则为 <c>true</c>；否则为 <c>false</c>。</returns>
|
||
public override bool Equals(object obj)
|
||
{
|
||
// 检查对象是否与当前向量具有相同的类型
|
||
if (obj is Vector3)
|
||
{
|
||
Vector3 vector = (Vector3)obj;
|
||
// 比较向量的每个分量
|
||
return X == vector.X &&
|
||
Y == vector.Y &&
|
||
Z == vector.Z;
|
||
}
|
||
|
||
// 如果对象不是 Vector3 类型，则它们不相等
|
||
return false;
|
||
}
|
||
/// <summary>
|
||
/// 向量按矩阵旋转和偏移
|
||
/// </summary>
|
||
/// <param name="mat">矩阵</param>
|
||
/// <returns>返回一个新的向量</returns>
|
||
public Vector3 Multiply(Matrix mat)
|
||
{
|
||
Vector3 re = new Vector3();
|
||
//矩阵相乘 ps:旋转角度
|
||
re.X = this.X * mat[1].X + this.Y * mat[2].X + this.Z * mat[0].X;
|
||
re.Y = this.X * mat[1].Y + this.Y * mat[2].Y + this.Z * mat[0].Y;
|
||
re.Z = this.X * mat[1].Z + this.Y * mat[2].Z + this.Z * mat[0].Z;
|
||
//off偏移
|
||
re = re + mat[3];
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 求模长
|
||
/// </summary>
|
||
/// <returns>向量到原点的模长</returns>
|
||
public double GetMag()
|
||
{
|
||
return Math.Sqrt(Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2));
|
||
}
|
||
/// <summary>
|
||
/// 求模长 平方
|
||
/// </summary>
|
||
/// <returns>向量到原点的模长的平方值</returns>
|
||
public double GetMagSquared()
|
||
{
|
||
return Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
|
||
}
|
||
/// <summary>
|
||
/// 标准化坐标 无返回值 直接改变愿坐标
|
||
/// </summary>
|
||
/// <param name="multiple">标准化单位</param>
|
||
public void Normalize(double multiple = 1.0)
|
||
{
|
||
double magSq = Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
|
||
if (magSq > 0)
|
||
{
|
||
double oneOverMag = multiple / Math.Sqrt(magSq);
|
||
this.X *= oneOverMag;
|
||
this.Y *= oneOverMag;
|
||
this.Z *= oneOverMag;
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 标准化 返回一个标准化之后的值 不改变自身
|
||
/// </summary>
|
||
/// <param name="multiple">标准化单位</param>
|
||
/// <returns>标准化之后的值</returns>
|
||
public Vector3 NormalizEd(double multiple = 1.0)
|
||
{
|
||
Vector3 re = new Vector3();
|
||
double magSq = Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
|
||
if (magSq > 0)
|
||
{
|
||
double oneOverMag = multiple / Math.Sqrt(magSq);
|
||
re.X = this.X * oneOverMag;
|
||
re.Y = this.Y * oneOverMag;
|
||
re.Z = this.Z * oneOverMag;
|
||
}
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 归零 改变自身数值 一般配合归位使用
|
||
/// </summary>
|
||
/// <param name="v2"></param>
|
||
public void SetZero(Vector3 v2)
|
||
{
|
||
this.X -= v2.X;
|
||
this.Y -= v2.Y;
|
||
this.Z -= v2.Z;
|
||
}
|
||
/// <summary>
|
||
/// 归零 返回一个归零值 不改变自身
|
||
/// </summary>
|
||
/// <param name="v2"></param>
|
||
/// <returns></returns>
|
||
public Vector3 SetZeroEd(Vector3 v2)
|
||
{
|
||
Vector3 re = new Vector3(this.X - v2.X, this.Y - v2.Y, this.Z - v2.Z);
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 归位
|
||
/// </summary>
|
||
/// <param name="v2"></param>
|
||
public void SetFormerly(Vector3 v2)
|
||
{
|
||
this.X += v2.X;
|
||
this.Y += v2.Y;
|
||
this.Z += v2.Z;
|
||
}
|
||
/// <summary>
|
||
/// 重写ToString 打印坐标
|
||
/// </summary>
|
||
/// <returns>坐标字符串</returns>
|
||
public override string ToString()
|
||
{
|
||
string x = Convert.ToString(this.X);
|
||
string y = Convert.ToString(this.Y);
|
||
string z = Convert.ToString(this.Z);
|
||
return string.Format($"X轴:{x} Y轴:{y} Z轴:{z}");
|
||
}
|
||
/// <summary>
|
||
/// 哈希码是一个整数值，用于对对象进行快速比较和索引
|
||
/// </summary>
|
||
/// <returns>哈希码</returns>
|
||
public override int GetHashCode()
|
||
{
|
||
int hashCode = -307843816;
|
||
hashCode = hashCode * -1521134295 + X.GetHashCode();
|
||
hashCode = hashCode * -1521134295 + Y.GetHashCode();
|
||
hashCode = hashCode * -1521134295 + Z.GetHashCode();
|
||
return hashCode;
|
||
}
|
||
}
|
||
//坐标操作与验证
|
||
public static class FlyVecFun
|
||
{
|
||
/// <summary>
|
||
/// 航线 线间距 平方值
|
||
/// </summary>
|
||
public static double LineDistanceSquare { get; set; } = 32400;
|
||
/// <summary>
|
||
/// 飞行过程中间距 平方值
|
||
/// </summary>
|
||
public static double SpaceBetweenSquare { get; set; } = 90000;
|
||
/// <summary>
|
||
/// 输出日志回调函数
|
||
/// </summary>
|
||
/// <param name="str">日志内容</param>
|
||
public delegate void SomeCalculateWay(string str);
|
||
/// <summary>
|
||
/// 输出进度日志回调函数
|
||
/// </summary>
|
||
/// <param name="val">进度值ps:0-100</param>
|
||
public delegate void Schedule(int val);
|
||
/// <summary>
|
||
/// Arraylist 转 Vector3[] 坐标集
|
||
/// </summary>
|
||
/// <param name="arr">Arraylist 坐标集</param>
|
||
/// <returns>Vector3[] 坐标集</returns>
|
||
public static Vector3[] ArrToVec(ArrayList arr)
|
||
{
|
||
int cou = arr.Count;
|
||
Vector3[] re = new Vector3[cou];
|
||
int key = 0;
|
||
foreach (Vector3 item in arr)
|
||
{
|
||
re[key] = item;
|
||
key++;
|
||
}
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 取数组最大 或者最小值
|
||
/// </summary>
|
||
/// <param name="arr">数组</param>
|
||
/// <param name="isMax">true返回最大值 false返回最小值</param>
|
||
/// <returns>根据参数返回 最大或者最小值</returns>
|
||
private static double GetMaxOrMin(double[] arr, bool isMax = true)
|
||
{
|
||
Array.Sort(arr);//给数组arr排序
|
||
if (isMax)
|
||
return arr[arr.Length - 1];
|
||
else
|
||
return arr[0];
|
||
}
|
||
/// <summary>
|
||
/// 取数组最大 或者最小值 的数组下标
|
||
/// </summary>
|
||
/// <param name="arr">数组</param>
|
||
/// <param name="isMax">true返回最大值 false返回最小值</param>
|
||
/// <returns>数组下标</returns>
|
||
private static int GetIndexOfMaxOrMin(double[] arr, bool isMax = true)
|
||
{
|
||
int[] indexes = new int[arr.Length];
|
||
for (int i = 0; i < arr.Length; i++)
|
||
{
|
||
indexes[i] = i; // 保存每个元素的原始索引
|
||
}
|
||
|
||
Array.Sort(arr, indexes); // 按值排序同时更新索引数组
|
||
|
||
// 根据 isMax 参数返回相应的索引
|
||
return isMax ? indexes[arr.Length - 1] : indexes[0];
|
||
}
|
||
/// <summary>
|
||
/// 获取列表中最小值的随机下标
|
||
/// </summary>
|
||
/// <param name="list">输入的列表</param>
|
||
/// <returns>最小值的随机下标</returns>
|
||
private static int GetRandomMinIndex(List<int> list)
|
||
{
|
||
// 检查输入的列表是否为 null 或为空
|
||
if (list == null || list.Count == 0)
|
||
{
|
||
throw new ArgumentException("列表不能为 null 或为空");
|
||
}
|
||
|
||
int minValue = int.MaxValue; // 初始化为 int 类型的最大值
|
||
List<int> minIndices = new List<int>(); // 存储最小值的下标列表
|
||
|
||
for (int i = 0; i < list.Count; i++)
|
||
{
|
||
// 如果当前元素比最小值小，更新最小值和清空之前的下标列表
|
||
if (list[i] < minValue)
|
||
{
|
||
minValue = list[i];
|
||
minIndices.Clear(); // 清空之前的下标列表
|
||
minIndices.Add(i); // 添加当前下标
|
||
}
|
||
// 如果当前元素等于最小值，添加当前下标到列表
|
||
else if (list[i] == minValue)
|
||
{
|
||
minIndices.Add(i);
|
||
}
|
||
}
|
||
|
||
// 如果最小值下标列表为空，表示没有找到最小值
|
||
if (minIndices.Count == 0)
|
||
{
|
||
throw new InvalidOperationException("列表中没有找到最小值");
|
||
}
|
||
|
||
// 生成一个随机数，用于从最小值下标列表中随机选择一个下标
|
||
Random random = new Random();
|
||
return minIndices[random.Next(minIndices.Count)];
|
||
}
|
||
/// <summary>
|
||
/// 二维数组转一维数组 并去重
|
||
/// </summary>
|
||
/// <param name="twoArr">二维数组</param>
|
||
/// <returns>去重一维数组</returns>
|
||
private static List<int> TwoArrToArr(List<int[]> twoArr)
|
||
{
|
||
// 创建一个用于存储去重后的一维数组的列表
|
||
List<int> arr = new List<int>();
|
||
// 遍历二维数组的每个子数组
|
||
foreach (int[] item in twoArr)
|
||
{
|
||
// 查找第一个元素在一维数组中的索引
|
||
int i = arr.IndexOf(item[0]);
|
||
// 如果索引小于0，表示该元素在一维数组中不存在，将其添加到一维数组
|
||
if (i < 0)
|
||
{
|
||
arr.Add(item[0]);
|
||
}
|
||
// 查找第二个元素在一维数组中的索引
|
||
int x = arr.IndexOf(item[1]);
|
||
// 如果索引小于0，表示该元素在一维数组中不存在，将其添加到一维数组
|
||
if (x < 0)
|
||
{
|
||
arr.Add(item[1]);
|
||
}
|
||
}
|
||
// 返回去重后的一维数组
|
||
return arr;
|
||
}
|
||
/// <summary>
|
||
/// 获取交叉序列 ps:处理二维数组 把有关联的子数组合并 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
|
||
/// </summary>
|
||
/// <param name="arr">需要处理的二维数组 </param>
|
||
/// <returns>交叉序列</returns>
|
||
private static List<List<int>> FindConnected(List<int[]> arr)
|
||
{
|
||
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>>();
|
||
|
||
// 构建图
|
||
foreach (var edge in arr)
|
||
{
|
||
foreach (var node in edge)
|
||
{
|
||
if (!graph.ContainsKey(node))
|
||
{
|
||
graph[node] = new List<int>();
|
||
visited[node] = false;
|
||
}
|
||
}
|
||
}
|
||
foreach (var edge in arr)
|
||
{
|
||
graph[edge[0]].Add(edge[1]);
|
||
graph[edge[1]].Add(edge[0]);
|
||
}
|
||
foreach (var node in graph.Keys)
|
||
{
|
||
if (!visited[node])
|
||
{
|
||
List<int> connected = new List<int>();
|
||
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>
|
||
private static List<List<int>> Permutations(List<int> array)
|
||
{
|
||
List<List<int>> result = new List<List<int>>();
|
||
GeneratePermutations(array, 0, array.Count - 1, result);
|
||
return result;
|
||
}
|
||
private static void GeneratePermutations(List<int> array, int start, int end, List<List<int>> result)
|
||
{
|
||
if (start == end)
|
||
{
|
||
result.Add(new List<int>(array));
|
||
}
|
||
else
|
||
{
|
||
for (int i = start; i <= end; i++)
|
||
{
|
||
int temp = array[start];
|
||
array[start] = array[i];
|
||
array[i] = temp;
|
||
GeneratePermutations(array, start + 1, end, result);
|
||
temp = array[start];
|
||
array[start] = array[i];
|
||
array[i] = temp;
|
||
}
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 按照对应关系 生成新的b坐标集合
|
||
/// </summary>
|
||
/// <param name="aVecs">a坐标集合</param>
|
||
/// <param name="bVecs">b坐标集合</param>
|
||
/// <param name="match">a b集合的对应关系</param>
|
||
/// <returns>坐标集合</returns>
|
||
private static Vector3[] CreateNewBVecs(Vector3[] bVecs, List<int[]> match)
|
||
{
|
||
Vector3[] new_bVecs = new Vector3[bVecs.Length];
|
||
foreach (int[] m in match)
|
||
{
|
||
new_bVecs[m[0]] = bVecs[m[1]];
|
||
}
|
||
return new_bVecs;
|
||
}
|
||
/// <summary>
|
||
/// 从一组向量集合中 不重复的随机选择4个向量为一组 最多100组 组合成二维数组 如:[[1,2,3,4][5,6,7,8]...]
|
||
/// </summary>
|
||
/// <param name="vecs">坐标集和</param>
|
||
/// <returns>re空数组则代表不够4个坐标 </returns>
|
||
private static List<Vector3[]> RandomSel4Vec(Vector3[] vecs)
|
||
{
|
||
List<Vector3[]> result = new List<Vector3[]>();
|
||
int len = vecs.Length;
|
||
// 如果坐标数组少于4个，或者为空，则返回空列表
|
||
if (len < 4)
|
||
{
|
||
return result;
|
||
}
|
||
// 确定最大可选择的组数，最多为100组
|
||
int numGroups = Math.Min(len / 4, 100);
|
||
List<Vector3> flattenedList = vecs.ToList();
|
||
Random random = new Random();
|
||
// 遍历每一组
|
||
for (int i = 0; i < numGroups; i++)
|
||
{
|
||
List<Vector3> selectedGroup = new List<Vector3>();
|
||
|
||
// 随机选择4个不重复的坐标
|
||
for (int j = 0; j < 4; j++)
|
||
{
|
||
int index = random.Next(flattenedList.Count);
|
||
selectedGroup.Add(flattenedList[index]);
|
||
flattenedList.RemoveAt(index);
|
||
}
|
||
|
||
// 将选择的坐标组成的 List 转换为数组，并添加到结果中
|
||
result.Add(selectedGroup.ToArray());
|
||
}
|
||
return result;
|
||
}
|
||
/// <summary>
|
||
/// 设置中间航点
|
||
/// </summary>
|
||
/// <param name="aVec">起点</param>
|
||
/// <param name="bVec">目标点</param>
|
||
/// <param name="middlePos">比例</param>
|
||
/// <returns></returns>
|
||
private static Vector3 SetMiddleVec(Vector3 aVec, Vector3 bVec, double middlePos = 0.5)
|
||
{
|
||
return (bVec - aVec) * middlePos + aVec;
|
||
}
|
||
/// <summary>
|
||
/// 两点距离
|
||
/// </summary>
|
||
/// <param name="v1">坐标1</param>
|
||
/// <param name="v2">坐标2</param>
|
||
/// <returns>两点之间距离</returns>
|
||
private static double GageLength(Vector3 v1, Vector3 v2)
|
||
{
|
||
return Math.Sqrt(GageLengthSquare(v1, v2));
|
||
}
|
||
/// <summary>
|
||
/// 两点距离的平方
|
||
/// </summary>
|
||
/// <param name="v1">坐标1</param>
|
||
/// <param name="v2">坐标2</param>
|
||
/// <returns>两点距离的平方</returns>
|
||
private static double GageLengthSquare(Vector3 v1, Vector3 v2)
|
||
{
|
||
return Math.Pow(v1.X - v2.X, 2) + Math.Pow(v1.Y - v2.Y, 2) + Math.Pow(v1.Z - v2.Z, 2);
|
||
}
|
||
/// <summary>
|
||
/// 点积
|
||
/// </summary>
|
||
/// <param name="v1">向量1</param>
|
||
/// <param name="v2">向量2</param>
|
||
/// <returns>点积</returns>
|
||
private static double DotPro(Vector3 v1, Vector3 v2)
|
||
{
|
||
return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
|
||
}
|
||
/// <summary>
|
||
/// 叉积
|
||
/// </summary>
|
||
/// <param name="v1">向量1</param>
|
||
/// <param name="v2">向量2</param>
|
||
/// <returns>两个向量叉积</returns>
|
||
private static Vector3 CrossPro(Vector3 v1, Vector3 v2)
|
||
{
|
||
double x = v1.Y * v2.Z - v1.Z * v2.Y;
|
||
double y = v1.Z * v2.X - v1.X * v2.Z;
|
||
double z = v1.X * v2.Y - v1.Y * v2.X;
|
||
return new Vector3(x, y, z);
|
||
}
|
||
/// <summary>
|
||
/// 计算两个向量之间的夹角 角度
|
||
/// </summary>
|
||
/// <param name="v1">第一个向量</param>
|
||
/// <param name="v2">第二个向量</param>
|
||
/// <returns>角度</returns>
|
||
public static double AngleBetween(Vector3 v1, Vector3 v2)
|
||
{
|
||
// 计算点积
|
||
double dotProduct = DotPro(v1, v2);
|
||
|
||
// 计算向量长度
|
||
double magnitude1 = v1.GetMag();
|
||
double magnitude2 = v2.GetMag();
|
||
|
||
// 使用 Atan2 计算弧度
|
||
double thetaRadians = Math.Atan2(CrossPro(v1, v2).GetMag(), dotProduct);
|
||
|
||
// 弧度转角度
|
||
double thetaDegrees = thetaRadians * (180 / Math.PI);
|
||
|
||
return thetaDegrees;
|
||
}
|
||
/// <summary>
|
||
/// 检查4个点是否在一个平面上
|
||
/// </summary>
|
||
/// <param name="vector1">点1</param>
|
||
/// <param name="vector2">点2</param>
|
||
/// <param name="vector3">点3</param>
|
||
/// <param name="vector4">点4</param>
|
||
/// <returns>true在一个平面 false不在一个平面</returns>
|
||
public static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||
{
|
||
//计算三个向量
|
||
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 = 10; //单位厘米
|
||
return Math.Abs(distance) < epsilon;
|
||
}
|
||
/// <summary>
|
||
/// 判断3个点是否在同一条直线上
|
||
/// </summary>
|
||
/// <param name="vector1">点1</param>
|
||
/// <param name="vector2">点2</param>
|
||
/// <param name="vector3">点3</param>
|
||
/// <returns>true在一条直线上 false不在一条直线上</returns>
|
||
private static bool IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3)
|
||
{
|
||
Vector3 v1 = vec2 - vec1;
|
||
Vector3 v2 = vec3 - vec1;
|
||
// 计算点积
|
||
double dotProduct = DotPro(v1, v2);
|
||
// 计算向量长度
|
||
double magnitude1 = v1.GetMag();
|
||
double magnitude2 = v2.GetMag();
|
||
// 使用 Atan2 计算弧度
|
||
double thetaRadians = Math.Atan2(CrossPro(v1, v2).GetMag(), dotProduct);
|
||
// 弧度转角度
|
||
double thetaDegrees = thetaRadians * (180 / Math.PI);
|
||
if (Math.Abs(thetaDegrees) < 10) return true;
|
||
else return false;
|
||
}
|
||
/// <summary>
|
||
/// 从顶视图 判断点是否在两条内之间
|
||
/// </summary>
|
||
/// <param name="A">线段1端点</param>
|
||
/// <param name="B">线段1端点</param>
|
||
/// <param name="C">线段2端点</param>
|
||
/// <param name="D">线段2端点</param>
|
||
/// <param name="P">被判断点</param>
|
||
/// <returns>true点在两条线内部 false点不在两条线内部</returns>
|
||
private static bool IsPointBetweenLines(Vector3 A, Vector3 B, Vector3 C, Vector3 D, Vector3 P)
|
||
{
|
||
/// Y轴亚平 即顶视图
|
||
A = new Vector3(A.X,0,A.Z);
|
||
B = new Vector3(B.X, 0, B.Z);
|
||
C = new Vector3(C.X, 0, C.Z);
|
||
D = new Vector3(D.X, 0, D.Z);
|
||
P = new Vector3(P.X, 0, P.Z);
|
||
Vector3 dirAB = new Vector3(B.X - A.X, B.Y - A.Y, B.Z - A.Z);
|
||
Vector3 dirCD = new Vector3(D.X - C.X, D.Y - C.Y, D.Z - C.Z);
|
||
Vector3 vecAP = new Vector3(P.X - A.X, P.Y - A.Y, P.Z - A.Z);
|
||
Vector3 vecCP = new Vector3(P.X - C.X, P.Y - C.Y, P.Z - C.Z);
|
||
|
||
double cross1 = dirAB.X * vecAP.Z - dirAB.Z * vecAP.X;
|
||
double cross2 = dirCD.X * vecCP.Z - dirCD.Z * vecCP.X;
|
||
|
||
return cross1 * cross2 < 0;
|
||
}
|
||
/// <summary>
|
||
/// 辅助方法，用于检查向量是否为零向量
|
||
/// </summary>
|
||
/// <param name="vector">向量</param>
|
||
/// <returns></returns>
|
||
private static bool IsZero(this Vector3 vector)
|
||
{
|
||
return vector.X == 0 && vector.Y == 0 && vector.Z == 0;
|
||
}
|
||
/// <summary>
|
||
/// 获取两条线段 的最近位置的距离和占比
|
||
/// </summary>
|
||
/// <param name="a1">线段1起始点</param>
|
||
/// <param name="a2">线段1起终点</param>
|
||
/// <param name="b1">线段2起始点</param>
|
||
/// <param name="b2">线段2起终点</param>
|
||
/// <returns>[在线段1占比，在线段2占比，最近距离]</returns>
|
||
private static double RecentlySquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||
{
|
||
if (a1 == a2) a2 += 1;// 防止线段长度为0
|
||
if (b1 == b2) b2 += 1;
|
||
double ux = a2.X - a1.X;
|
||
double uy = a2.Y - a1.Y;
|
||
double uz = a2.Z - a1.Z;
|
||
double vx = b2.X - b1.X;
|
||
double vy = b2.Y - b1.Y;
|
||
double vz = b2.Z - b1.Z;
|
||
double wx = a1.X - b1.X;
|
||
double wy = a1.Y - b1.Y;
|
||
double wz = a1.Z - b1.Z;
|
||
double a = (ux * ux + uy * uy + uz * uz);
|
||
double b = (ux * vx + uy * vy + uz * vz);
|
||
double c = (vx * vx + vy * vy + vz * vz);
|
||
double d = (ux * wx + uy * wy + uz * wz);
|
||
double e = (vx * wx + vy * wy + vz * wz);
|
||
double dt = a * c - b * b;
|
||
double sd = dt;
|
||
double td = dt;
|
||
double sn = 0.0;
|
||
double tn = 0.0;
|
||
if (IsEqual(dt, 0.0))
|
||
{
|
||
sn = 0.0;
|
||
sd = 1.00;
|
||
tn = e;
|
||
td = c;
|
||
}
|
||
else
|
||
{
|
||
sn = (b * e - c * d);
|
||
tn = (a * e - b * d);
|
||
if (sn < 0.0)
|
||
{
|
||
sn = 0.0;
|
||
tn = e;
|
||
td = c;
|
||
}
|
||
else if (sn > sd)
|
||
{
|
||
sn = sd;
|
||
tn = e + b;
|
||
td = c;
|
||
}
|
||
}
|
||
if (tn < 0.0)
|
||
{
|
||
tn = 0.0;
|
||
if (-d < 0.0) sn = 0.0;
|
||
else if (-d > a) sn = sd;
|
||
else
|
||
{
|
||
sn = -d;
|
||
sd = a;
|
||
}
|
||
}
|
||
else if (tn > td)
|
||
{
|
||
tn = td;
|
||
if ((-d + b) < 0.0) sn = 0.0;
|
||
else if ((-d + b) > a) sn = sd;
|
||
else
|
||
{
|
||
sn = (-d + b);
|
||
sd = a;
|
||
}
|
||
|
||
}
|
||
double sc = 0.0;
|
||
double tc = 0.0;
|
||
if (IsEqual(sn, 0.0)) sc = 0.0;
|
||
else sc = sn / sd;//最近点在第一条线占比
|
||
if (IsEqual(tn, 0.0)) tc = 0.0;
|
||
else tc = tn / td;//最近点在第二条线占比
|
||
double dx = wx + (sc * ux) - (tc * vx);
|
||
double dy = wy + (sc * uy) - (tc * vy);
|
||
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>
|
||
/// <param name="a1">线段1起始点</param>
|
||
/// <param name="a2">线段1起终点</param>
|
||
/// <param name="b1">线段2起始点</param>
|
||
/// <param name="b2">线段2起终点</param>
|
||
/// <returns>最小距离的平方值</returns>
|
||
private static double MinDistanceSquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
|
||
{
|
||
//相对位置和相对速度
|
||
Vector3 d0 = b1 - a1;
|
||
Vector3 v_rel = (b2 - b1) - (a2 - a1);
|
||
//计算最小距离位置 占比
|
||
double vd = DotPro(v_rel, v_rel);
|
||
double proportion;//最近距离占比
|
||
if (vd != 0)
|
||
{
|
||
proportion = Math.Max(0, Math.Min(1, -DotPro(d0, v_rel) / vd));//max min函数把比例限制在0-1 保证比例在线段上
|
||
}
|
||
else
|
||
{
|
||
proportion = 0;
|
||
}
|
||
//计算最小距离平方
|
||
Vector3 d_min = d0 + v_rel * proportion;
|
||
return d_min.GetMagSquared();
|
||
}
|
||
/// <summary>
|
||
/// 获取坐标集合的重心或中心
|
||
/// </summary>
|
||
/// <param name="pos">坐标集合</param>
|
||
/// <param name="isCentroid">默认返回为true重心 false则为中心</param>
|
||
/// <returns>重心或中心坐标</returns>
|
||
public static Vector3 GetPosCenter(Vector3[] pos, bool isCentroid = true)
|
||
{
|
||
int cou = pos.Length;
|
||
if (isCentroid)//重心
|
||
{
|
||
double x = 0;
|
||
double y = 0;
|
||
double z = 0;
|
||
foreach (var item in pos)
|
||
{
|
||
x += item.X;
|
||
y += item.Y;
|
||
z += item.Z;
|
||
}
|
||
x = x / cou;
|
||
y = y / cou;
|
||
z = z / cou;
|
||
return new Vector3(x, y, z);
|
||
}
|
||
else//中心
|
||
{
|
||
double[] x = new double[cou];
|
||
double[] y = new double[cou];
|
||
double[] z = new double[cou];
|
||
int key = 0;
|
||
foreach (var item in pos)
|
||
{
|
||
x[key] = item.X;
|
||
y[key] = item.Y;
|
||
z[key] = item.Z;
|
||
key++;
|
||
}
|
||
double xc = (GetMaxOrMin(x) + GetMaxOrMin(x, false)) * .5;
|
||
double yc = (GetMaxOrMin(y) + GetMaxOrMin(y, false)) * .5;
|
||
double zc = (GetMaxOrMin(z) + GetMaxOrMin(z, false)) * .5;
|
||
return new Vector3(xc, yc, zc);
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 获取坐标集合的重心或中心
|
||
/// </summary>
|
||
/// <param name="pos">坐标集合</param>
|
||
/// <param name="isCentroid">默认返回为true重心 false则为中心</param>
|
||
/// <returns>重心或中心坐标</returns>
|
||
private static Vector3 GetPosCenter(List<Vector3> pos, bool isCentroid = true)
|
||
{
|
||
int cou = pos.Count;
|
||
if (isCentroid)//重心
|
||
{
|
||
double x = 0;
|
||
double y = 0;
|
||
double z = 0;
|
||
foreach (var item in pos)
|
||
{
|
||
x += item.X;
|
||
y += item.Y;
|
||
z += item.Z;
|
||
}
|
||
x = x / cou;
|
||
y = y / cou;
|
||
z = z / cou;
|
||
return new Vector3(x, y, z);
|
||
}
|
||
else//中心
|
||
{
|
||
double[] x = new double[cou];
|
||
double[] y = new double[cou];
|
||
double[] z = new double[cou];
|
||
int key = 0;
|
||
foreach (var item in pos)
|
||
{
|
||
x[key] = item.X;
|
||
y[key] = item.Y;
|
||
z[key] = item.Z;
|
||
key++;
|
||
}
|
||
double xc = (GetMaxOrMin(x) + GetMaxOrMin(x, false)) * .5;
|
||
double yc = (GetMaxOrMin(y) + GetMaxOrMin(y, false)) * .5;
|
||
double zc = (GetMaxOrMin(z) + GetMaxOrMin(z, false)) * .5;
|
||
return new Vector3(xc, yc, zc);
|
||
}
|
||
}
|
||
/// <summary>
|
||
/// 获取坐标集合 的总宽度 高度 长度
|
||
/// </summary>
|
||
/// <param name="pos">坐标集合</param>
|
||
/// <returns>返回数组[0]宽度[1]高度[2]长度</returns>
|
||
public static double[] GetVecsWithHighLength(Vector3[] pos)
|
||
{
|
||
List<double> w = new List<double>();
|
||
List<double> h = new List<double>();
|
||
List<double> l = new List<double>();
|
||
foreach (var item in pos)
|
||
{
|
||
w.Add(item.X);
|
||
h.Add(item.Y);
|
||
l.Add(item.Z);
|
||
}
|
||
double[] re = new double[3];
|
||
re[0] = w.Max() - w.Min();
|
||
re[1] = h.Max() - h.Min();
|
||
re[2] = l.Max() - l.Min();
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 碰撞检测
|
||
/// </summary>
|
||
/// <param name="aVecs">始点坐标集合</param>
|
||
/// <param name="bVecs">终点坐标集合</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation(Vector3[] aVecs, Vector3[] bVecs)
|
||
{
|
||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||
int planeCou = aVecs.Length; //获取飞机总数
|
||
for (int a = 0; a < planeCou; a++)
|
||
{
|
||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||
{
|
||
//判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||
if (distanceSquare < LineDistanceSquare)
|
||
{
|
||
//判断飞机距离是否过近
|
||
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 });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return planesCollision;
|
||
}
|
||
/// <summary>
|
||
/// 碰撞检测
|
||
/// </summary>
|
||
/// <param name="aVecs">始点坐标集合</param>
|
||
/// <param name="bVecs">终点坐标集合</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation(List<Vector3> aVecs, List<Vector3> bVecs)
|
||
{
|
||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||
int planeCou = aVecs.Count; //获取飞机总数
|
||
for (int a = 0; a < planeCou; a++)
|
||
{
|
||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||
{
|
||
//判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||
if (distanceSquare < LineDistanceSquare)
|
||
{
|
||
//判断飞机距离是否过近
|
||
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 });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return planesCollision;
|
||
}
|
||
/// <summary>
|
||
/// 碰撞检测
|
||
/// </summary>
|
||
/// <param name="aVecs">始点坐标集合</param>
|
||
/// <param name="bVecs">终点坐标集合</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation(Vector3[] aVecs , List<Vector3> bVecs)
|
||
{
|
||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||
int planeCou = aVecs.Length; //获取飞机总数
|
||
for (int a = 0; a < planeCou; a++)
|
||
{
|
||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||
{
|
||
//判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||
if (distanceSquare < LineDistanceSquare)
|
||
{
|
||
//判断飞机距离是否过近
|
||
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 });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return planesCollision;
|
||
}
|
||
/// <summary>
|
||
/// 碰撞检测
|
||
/// </summary>
|
||
/// <param name="aVecs">始点坐标集合</param>
|
||
/// <param name="bVecs">终点坐标集合</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation( List<Vector3> aVecs, Vector3[] bVecs)
|
||
{
|
||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||
int planeCou = aVecs.Count; //获取飞机总数
|
||
for (int a = 0; a < planeCou; a++)
|
||
{
|
||
for (int b = 0; a + 1 + b < planeCou; b++)
|
||
{
|
||
//判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
|
||
if (distanceSquare < LineDistanceSquare)
|
||
{
|
||
//判断飞机距离是否过近
|
||
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 });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return planesCollision;
|
||
}
|
||
/// <summary>
|
||
/// 单机碰撞检测
|
||
/// </summary>
|
||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, Vector3[] bVecs)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
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>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, List<Vector3> bVecs)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
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;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 单机碰撞检测
|
||
/// </summary>
|
||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, List<Vector3> bVecs)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
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>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, Vector3[] bVecs)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
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;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换
|
||
/// </summary>
|
||
/// <param name="aVecs">起始点坐标组</param>
|
||
/// <param name="bVecs">目标点坐标组</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="isSwap">交叉航线是否进行交换</param>
|
||
/// <param name="swapCount">交换次数</param>
|
||
/// <param name="crossingLimit">交叉线路数量上限 ps:超过这个数量则不进行交换</param>
|
||
/// <returns>新的目标点</returns>
|
||
public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, bool isSwap = true, int swapCount = 5, int crossingLimit = 6)
|
||
{
|
||
long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
|
||
StrPrint("-------智能选择路径计算，开始-------");
|
||
int planeCou = aVecs.Length; // 飞机总数
|
||
List<int[]> match = new List<int[]>(); // ab对应关系
|
||
///记录a b集合索引
|
||
List<int> aIndex = new List<int>();
|
||
List<int> bIndex = new List<int>();
|
||
for (int k = 0; k < planeCou; k++)
|
||
{
|
||
aIndex.Add(k);
|
||
bIndex.Add(k);
|
||
}
|
||
/// 遍历找出 AB集合对应关系
|
||
for (int k = 0; k < planeCou; k++)
|
||
{
|
||
|
||
// 遍历每次刷新 剩下为未匹配总数
|
||
int remainCou = aIndex.Count;
|
||
// 遍历每次刷新 重心
|
||
List<Vector3> allVecs = new List<Vector3>();
|
||
for (int i = 0; i < remainCou; i++)
|
||
{
|
||
allVecs.Add(aVecs[aIndex[i]]);
|
||
allVecs.Add(bVecs[bIndex[i]]);
|
||
}
|
||
Vector3 centerVec = GetPosCenter(allVecs);//重心点
|
||
// 遍历所有ab点距离重心点的距离
|
||
double[] aLens = new double[remainCou];
|
||
double[] bLens = new double[remainCou];
|
||
for (int i = 0; i < remainCou; i++)
|
||
{
|
||
aLens[i] = GageLengthSquare(aVecs[aIndex[i]], centerVec);
|
||
bLens[i] = GageLengthSquare(bVecs[bIndex[i]], centerVec);
|
||
}
|
||
// 找出ab集合最外层坐标的下标 即离重心点最远的点
|
||
int aMaxIndex = GetIndexOfMaxOrMin(aLens); // a集合到重心点最长的距离 数组的下标
|
||
int bMaxIndex = GetIndexOfMaxOrMin(bLens); // b集合到重心点最长的距离 数组的下标
|
||
if (aLens[aMaxIndex] > bLens[bMaxIndex])//找出 最外层如果为A集合点 对应B集合最近点 的下标
|
||
{
|
||
double[] outAtoBLen = new double[remainCou];//最外层A点到 B集合所有点的距离
|
||
for (int i = 0; i < remainCou; i++)
|
||
{
|
||
outAtoBLen[i] = GageLengthSquare(aVecs[aIndex[aMaxIndex]], bVecs[bIndex[i]]);
|
||
}
|
||
int bMinIndex = GetIndexOfMaxOrMin(outAtoBLen, false);// 最短距离
|
||
match.Add(new int[] { aIndex[aMaxIndex], bIndex[bMinIndex] });// 映射到配对
|
||
aIndex.RemoveAt(aMaxIndex); // 删除已经配对的a集合 ID
|
||
bIndex.RemoveAt(bMinIndex); // 删除已经配对的b集合 ID
|
||
}
|
||
else//找出 最外层如果为B集合点 对应A集合最近点 的下标
|
||
{
|
||
double[] outBtoALen = new double[remainCou];//最外层B点到 A集合所有点的距离
|
||
for (int i = 0; i < remainCou; i++)
|
||
{
|
||
outBtoALen[i] = GageLengthSquare(aVecs[aIndex[i]], bVecs[bIndex[bMaxIndex]]);
|
||
}
|
||
int aMinIndex = GetIndexOfMaxOrMin(outBtoALen, false);// 最短距离
|
||
match.Add(new int[] { aIndex[aMinIndex], bIndex[bMaxIndex] });// 映射到配对
|
||
aIndex.RemoveAt(aMinIndex); // 删除已经配对的a集合 ID
|
||
bIndex.RemoveAt(bMaxIndex); // 删除已经配对的b集合 ID
|
||
}
|
||
}
|
||
Vector3[] new_bVecs = CreateNewBVecs(bVecs, match);// 按照映射 获取a 对应的 新的b集合
|
||
if (!isSwap)
|
||
{
|
||
return new_bVecs;
|
||
}
|
||
///交叉 交换
|
||
for (int i = 0; i < swapCount; i++)
|
||
{
|
||
List<int[]> planesCollision = AirImitation(aVecs, new_bVecs);// 获取碰撞组
|
||
List<List<int>> formatCollision = FindConnected(planesCollision);// 获取交叉序列 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
|
||
List<List<int>> filteredCollision = formatCollision.Where(sublist => sublist.Count <= crossingLimit).ToList();// 过滤 只保留交叉数量小于等于crossingLimit的序列
|
||
if (filteredCollision.Count == 0) break;
|
||
///日志输出
|
||
string log = "";
|
||
foreach (List<int> item in filteredCollision)
|
||
{
|
||
log += "[";
|
||
foreach (int itemInt in item)
|
||
{
|
||
log += $"{itemInt},";
|
||
}
|
||
log += "]";
|
||
}
|
||
StrPrint($"共迭代{swapCount}次交换，第{i}次。共{filteredCollision.Count}组，交换组为：{log}。");
|
||
/// 遍历所有交叉组 分组做交换
|
||
int cou = 0;
|
||
foreach (List<int> swap_indices in filteredCollision)
|
||
{
|
||
cou++;
|
||
StrPrint($"进度：{cou}/{filteredCollision.Count}。交换组：[{string.Join(", ", swap_indices)}]。");
|
||
///交叉 生成所有排列
|
||
List<List<int>> all_permutations = Permutations(swap_indices);//所有排列组合
|
||
List<int> original = all_permutations[0];//原始排列
|
||
all_permutations.RemoveAt(0);//删掉第一个 既原始排列
|
||
/// 按所有的排列 互换航线 并检测出最佳的对应目标点
|
||
List<int> tempLen = new List<int>(); //记录最少碰撞的 排列
|
||
List<Vector3[]> tempNew_bVecsS = new List<Vector3[]>();//记录最少碰撞的 排列交换之后的目标坐标集
|
||
foreach (List<int> indices in all_permutations)
|
||
{
|
||
Vector3[] current_array = new Vector3[planeCou];
|
||
Array.Copy(new_bVecs, current_array, planeCou);//复制一个new_bVecs 副本
|
||
for (int k = 0; k < indices.Count; k++)
|
||
{
|
||
current_array[original[k]] = new_bVecs[indices[k]];
|
||
}
|
||
///把最少碰撞的排列 录入到数组
|
||
int collisionsCou = (AirImitation(aVecs, current_array)).Count;//此排列的碰撞次数
|
||
if (tempLen.Count == 0 || tempLen[0] == collisionsCou)//如果第一次添加 或者 碰撞次数等于最少碰撞
|
||
{
|
||
///录入数组
|
||
tempLen.Add(collisionsCou);
|
||
tempNew_bVecsS.Add(current_array);
|
||
}
|
||
else if (tempLen[0] > collisionsCou)
|
||
{
|
||
/// 如果最小值 大于 当前碰撞次数，清空之前记录
|
||
tempLen.Clear();
|
||
tempNew_bVecsS.Clear();
|
||
///录入数组
|
||
tempLen.Add(collisionsCou);
|
||
tempNew_bVecsS.Add(current_array);
|
||
}
|
||
}
|
||
new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)];
|
||
}
|
||
}
|
||
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------智能选择路径计算，结束-------");
|
||
return new_bVecs;
|
||
}
|
||
/// <summary>
|
||
/// 智能错层
|
||
/// </summary>
|
||
/// <param name="aVecs">起始坐标集合</param>
|
||
/// <param name="bVecs">终点做标集合</param>
|
||
/// <param name="aName">起始坐标航点名称</param>
|
||
/// <param name="bName">终点坐标航点名称</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="layHight">错层层高</param>
|
||
/// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点</returns>
|
||
public static List<List<Vector3>> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,string aName,string bName ,SomeCalculateWay StrPrint, double layHight = 220)
|
||
{
|
||
long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
|
||
StrPrint("-------错层，开始-------");
|
||
List<List<Vector3>> re = new List<List<Vector3>>();
|
||
///判断有没有碰撞 有碰撞继续 没有直接返回
|
||
List<int[]> planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
|
||
if (planesCollision.Count == 0)
|
||
{
|
||
StrPrint("没有检测到碰撞，故不用添加中间航点");
|
||
StrPrint($"-------错层结束-------");
|
||
return re;//直接返回
|
||
}
|
||
//获取飞机总数
|
||
int planeCou = aVecs.Length;
|
||
if (planeCou < 2)//至少不少于2架飞机 才开始错层
|
||
{
|
||
StrPrint("图案至少要两个点阵！");
|
||
return re;
|
||
}
|
||
|
||
///从ab图案中取最外层3个点 确定一个面 ps:方便后续叉积算法线标量
|
||
Vector3 vec0 = new Vector3(0, 0, 0); //记录最外圈一个坐标
|
||
Vector3 vec1 = new Vector3(0, 0, 0); //记录离最外圈坐标最远的一个点坐标
|
||
Vector3 vec2 = new Vector3(0, 0, 0); //最外圈距离 和最远距离 之和最远的一个点
|
||
// 求重心
|
||
List<Vector3> allVecs = new List<Vector3>();
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
allVecs.Add(aVecs[i]);
|
||
allVecs.Add(bVecs[i]);
|
||
}
|
||
Vector3 centerVec = GetPosCenter(allVecs);//重心点
|
||
// 遍历所有ab点
|
||
double tempLong = 0;
|
||
double currentLong;
|
||
for (int i = 0; i < planeCou * 2; i++) //取最外圈一个坐标
|
||
{
|
||
currentLong = GageLength(allVecs[i], centerVec);
|
||
if (tempLong < currentLong)
|
||
{
|
||
vec0 = allVecs[i];
|
||
tempLong = currentLong;
|
||
}
|
||
}
|
||
tempLong = 0;
|
||
for (int i = 0; i < planeCou * 2; i++) //取最外圈点
|
||
{
|
||
currentLong = GageLength(allVecs[i], vec0);
|
||
if (tempLong < currentLong)
|
||
{
|
||
vec1 = allVecs[i];
|
||
tempLong = currentLong;
|
||
}
|
||
}
|
||
tempLong = 0;
|
||
for (int i = 0; i < planeCou * 2; i++) //取最外圈点
|
||
{
|
||
currentLong =(GageLength(allVecs[i], vec0) + GageLength(allVecs[i], vec1));
|
||
if (tempLong < currentLong)
|
||
{
|
||
vec2 = allVecs[i];
|
||
tempLong = currentLong;
|
||
}
|
||
}
|
||
///遍历两个图形所有点 是否在一个平面上
|
||
string aNoPlaneSN= "";//记录a图案所有不在一个平面上的点
|
||
string bNoPlaneSN = "";//记录b图案所有不在一个平面上的点
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
if (!IsVecsOnPlane(vec0, vec1, vec2, aVecs[i])) aNoPlaneSN += $"{i+1},";
|
||
if (!IsVecsOnPlane(vec0, vec1, vec2, bVecs[i])) bNoPlaneSN += $"{i+1},";
|
||
}
|
||
if (aNoPlaneSN != "" || bNoPlaneSN != "")
|
||
{
|
||
if (aNoPlaneSN != "") StrPrint($"“{aName}”:{aNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!");
|
||
if (bNoPlaneSN != "") StrPrint($"“{bName}”:{bNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!");
|
||
StrPrint($"-------错层,结束-------");
|
||
return re;//两个图形不在一个平面上 返回 空数组
|
||
}
|
||
///检查完毕后
|
||
//计算法线向量
|
||
Vector3 side1 = vec1 - vec0;
|
||
Vector3 side2 = vec2 - vec0;
|
||
Vector3 normal = CrossPro(side1, side2);
|
||
Vector3 normalScalar = normal.NormalizEd();//法线标量
|
||
//开始错层
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
int shiftCou = 1; //记录循环次数 即层数
|
||
Vector3 aOrigin = aVecs[i]; //原点位置
|
||
Vector3 bOrigin = bVecs[i]; //原点位置
|
||
while (OnlyImitation(i, aVecs, bVecs))
|
||
{
|
||
Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
|
||
if (shiftCou % 2 == 1)
|
||
{
|
||
aVecs[i] = aOrigin + shiftVec;
|
||
bVecs[i] = bOrigin + shiftVec;
|
||
}
|
||
else
|
||
{
|
||
aVecs[i] = aOrigin - shiftVec;
|
||
bVecs[i] = bOrigin - shiftVec;
|
||
}
|
||
shiftCou += 1;
|
||
}
|
||
}
|
||
re.Add(aVecs.ToList());
|
||
re.Add(bVecs.ToList());
|
||
StrPrint($"错层成功");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------错层结束-------");
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 路径绕行
|
||
/// </summary>
|
||
/// <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, Schedule GetVal)
|
||
{
|
||
long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
|
||
StrPrint("-------3D绕行，开始-------");
|
||
List<List<Vector3>> re = new List<List<Vector3>>();
|
||
///判断有没有碰撞 有碰撞继续 没有直接返回
|
||
List<int[]> planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
|
||
if (planesCollision.Count == 0)
|
||
{
|
||
StrPrint("没有检测到碰撞，故不用添加中间航点");
|
||
StrPrint($"-------3D绕行，结束-------");
|
||
return re;//直接返回
|
||
}
|
||
///飞机总数
|
||
int planeCou = aVecs.Length;
|
||
///第一次绕行 中间1航点
|
||
List<int> collisionGroup = TwoArrToArr(planesCollision); //整合数组
|
||
List<Vector3> middleVecs = new List<Vector3>(); //中心航点坐标组
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
middleVecs.Add(SetMiddleVec(aVecs[i], bVecs[i])); //添加默认中间航点
|
||
}
|
||
for (int c = 0; c < 2; c++)
|
||
{
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
List<Vector3> grv = GetRingVec(aVecs[i], bVecs[i], 0.5, 40, 4);//中间可绕行航点列表
|
||
foreach (Vector3 v in grv)
|
||
{
|
||
middleVecs[i] = v;
|
||
if (!OnlyImitation(i, aVecs, middleVecs) && !(OnlyImitation(i, middleVecs, bVecs)))
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
planesCollision = AirImitation(aVecs, middleVecs).Concat(AirImitation(middleVecs, bVecs)).ToList(); //获取碰撞组
|
||
collisionGroup = TwoArrToArr(planesCollision); //整合数组
|
||
}
|
||
//没有碰撞 返回一个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("第一次绕行成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------3D绕行，结束-------");
|
||
re.Add(middleVecs);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！");
|
||
StrPrint("第一次绕行未成功，准备开始第二次绕行!");
|
||
}
|
||
|
||
///第二次绕行 两头 两航点
|
||
bool isPassMark = false;
|
||
planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
|
||
collisionGroup = TwoArrToArr(planesCollision); //整合数组
|
||
List<Vector3> secondMiddleVecsOne = new List<Vector3>(); //中心航点坐标组1
|
||
List<Vector3> secondMiddleVecsTwo = new List<Vector3>(); //中心航点坐标组2
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
secondMiddleVecsOne.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.1)); //添加中间航点1
|
||
secondMiddleVecsTwo.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.9)); //添加中间航点2
|
||
}
|
||
for (int c = 0; c < 2; c++)
|
||
{
|
||
int progress = 0;//进度
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
progress++;
|
||
//StrPrint($"迭代{c}次{i}号绕行");
|
||
List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0.1, 40, 8);//中间可绕行航点列表
|
||
List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 0.9, 40, 8);//中间可绕行航点列表
|
||
//if (sgrv1.Count > 250) sgrv1.RemoveRange(250, sgrv1.Count - 250);
|
||
//if (sgrv2.Count > 250) sgrv2.RemoveRange(250, sgrv2.Count - 250);
|
||
StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{Math.Pow(sgrv2.Count,2)}次");
|
||
foreach (Vector3 v1 in sgrv1)
|
||
{
|
||
secondMiddleVecsOne[i] = v1;
|
||
foreach (Vector3 v2 in sgrv2)
|
||
{
|
||
secondMiddleVecsTwo[i] = v2;
|
||
if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs))
|
||
{
|
||
isPassMark = true;
|
||
break;
|
||
}
|
||
isPassMark = false;
|
||
}
|
||
if (isPassMark)
|
||
{
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
}
|
||
planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, secondMiddleVecsTwo)).ToList(); //获取碰撞组
|
||
planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList();
|
||
collisionGroup = TwoArrToArr(planesCollision); //整合数组
|
||
}
|
||
//没有碰撞 返回两个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("第二次绕行成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------3D绕行，结束-------");
|
||
re.Add(secondMiddleVecsOne);
|
||
re.Add(secondMiddleVecsTwo);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！");
|
||
StrPrint("第二次绕行未通过，准备开始第三次绕行!");
|
||
}
|
||
|
||
///第三次绕行 两头 两航点 中间一行点 沿用第二次绕行
|
||
isPassMark = false;
|
||
List<Vector3> thirdMiddleVecs = new List<Vector3>(); //中心航点坐标组
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
thirdMiddleVecs.Add(SetMiddleVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5)); //添加中间航点(保持二次绕行的两端航点 在两端航点中间添加)
|
||
}
|
||
for (int c = 0; c < 2; c++)
|
||
{
|
||
int progress = 0;//进度
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
progress++;
|
||
//StrPrint($"迭代{c}次{i}号绕行");
|
||
List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0.1, 220, 8);//中间可绕行航点列表
|
||
List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 0.9, 220, 8);//中间可绕行航点列表
|
||
List<Vector3> grv = GetRingVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5, 220, 4);//中间可绕行航点列表
|
||
//if (sgrv1.Count > 300) sgrv1.RemoveRange(300, sgrv1.Count - 300);
|
||
//if (sgrv2.Count > 300) sgrv2.RemoveRange(300, sgrv2.Count - 300);
|
||
//if (grv.Count > 300) grv.RemoveRange(300, grv.Count - 300);
|
||
StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{Math.Pow(sgrv2.Count, 3)}次");
|
||
foreach (Vector3 vm in grv)
|
||
{
|
||
thirdMiddleVecs[i] = vm;
|
||
foreach (Vector3 v1 in sgrv1)
|
||
{
|
||
secondMiddleVecsOne[i] = v1;
|
||
foreach (Vector3 v2 in sgrv2)
|
||
{
|
||
secondMiddleVecsTwo[i] = v2;
|
||
if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, thirdMiddleVecs ) && !OnlyImitation(i, thirdMiddleVecs, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs))
|
||
{
|
||
isPassMark = true;
|
||
break;
|
||
}
|
||
isPassMark = false;
|
||
}
|
||
if (isPassMark)
|
||
{
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
if (isPassMark)
|
||
{
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
}
|
||
planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, thirdMiddleVecs)).ToList(); //获取碰撞组
|
||
planesCollision = planesCollision.Concat(AirImitation(thirdMiddleVecs, secondMiddleVecsTwo)).ToList();
|
||
planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList();
|
||
collisionGroup = TwoArrToArr(planesCollision); //整合数组
|
||
}
|
||
//没有碰撞 返回三个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("第三次绕行成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------3D绕行，结束-------");
|
||
re.Add(secondMiddleVecsOne);
|
||
re.Add(thirdMiddleVecs);
|
||
re.Add(secondMiddleVecsTwo);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！");
|
||
StrPrint("第三次绕行未成功，准备开始第四次绕行!");
|
||
}
|
||
|
||
///end
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------3D绕行，结束-------");
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 在圆圈上 用固定弦长手拉手 分割点
|
||
/// </summary>
|
||
/// <param name="radius">大圆半径</param>
|
||
/// <param name="chordLength">固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集</param>
|
||
/// <returns></returns>
|
||
private static List<Vector3> GetVecsOnCircle(double radius, double chordLength)
|
||
{
|
||
List<Vector3> vecs = new List<Vector3>();
|
||
// 计算圆的周长
|
||
double circumference = (double)(2 * Math.PI * radius);
|
||
// 计算弧上的点数
|
||
int numberOfPoints = (int)(circumference / chordLength);
|
||
// 计算每个点的弧度
|
||
double angleIncrement = (double)(2 * Math.PI / numberOfPoints);
|
||
// 生成点的坐标
|
||
for (int i = 0; i < numberOfPoints; i++)
|
||
{
|
||
double angle = i * angleIncrement;
|
||
double x = radius * (double)Math.Cos(angle);
|
||
double y = radius * (double)Math.Sin(angle);
|
||
double z = 0.0; // 如果是在平面上，Z 可能是 0
|
||
vecs.Add(new Vector3(x, y, z));
|
||
}
|
||
return vecs;
|
||
}
|
||
/// <summary>
|
||
/// 获取a指向b的向量矩阵 ps:偏移次数 加1 会在终点前后往返偏移
|
||
/// </summary>
|
||
/// <param name="aVec">a向量</param>
|
||
/// <param name="bVec">b向量</param>
|
||
/// <param name="middleProportion">默认中点位置比例</param>
|
||
/// <param name="offCount">偏移次数 从0开始,每+1在中点前后往返偏移</param>
|
||
/// <param name="layHight">偏移层高</param>
|
||
/// <param name="direction">层排布方向 "retrun"前后堆叠 "forward"向前排列（如：起点向目标点方向） "backward"向后排列</param>
|
||
/// <returns>矩阵</returns>
|
||
private static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion, int offCount, double layHight,string direction)
|
||
{
|
||
/// 计算前向向量 k帽
|
||
Vector3 k_hat = (bVec - aVec).NormalizEd();
|
||
/// 计算右向量，使用 Vector3.UnitY 作为上向量 i帽
|
||
//Vector3 i_hat = CrossPro(Vector3.UnitY, k_hat).NormalizEd(); //固定方向i帽
|
||
Random random = new Random();// 生成一个随机的单位向量
|
||
Vector3 randomVector = new Vector3((double)random.NextDouble(), (double)random.NextDouble(), (double)random.NextDouble());
|
||
Vector3 i_hat = CrossPro(k_hat, randomVector).NormalizEd();// 随机方向i帽
|
||
/// 计算上向量 j帽
|
||
Vector3 j_hat = CrossPro(k_hat, i_hat);
|
||
///计算 对应的偏移比例
|
||
double length = (bVec - aVec).GetMag();
|
||
double offShift = middleProportion;//偏移比例
|
||
if (direction == "retrun")
|
||
{
|
||
if (offCount % 2 == 1) //奇数
|
||
offShift = middleProportion + (offCount + 2) / 2 * layHight / length;
|
||
else //偶数
|
||
offShift = middleProportion - (offCount + 1) / 2 * layHight / length;
|
||
}
|
||
else if (direction == "forward")
|
||
{
|
||
offShift = middleProportion + offCount * layHight / length;
|
||
}
|
||
else if(direction == "backward")
|
||
{
|
||
offShift = middleProportion - offCount * layHight / length;
|
||
}
|
||
|
||
/// 计算偏向量
|
||
Vector3 off = aVec + (bVec - aVec) * offShift;
|
||
|
||
/// 构建矩阵
|
||
Matrix matrix = new Matrix
|
||
{
|
||
M11 = k_hat.X,
|
||
M12 = k_hat.Y,
|
||
M13 = k_hat.Z,
|
||
M21 = i_hat.X,
|
||
M22 = i_hat.Y,
|
||
M23 = i_hat.Z,
|
||
M31 = j_hat.X,
|
||
M32 = j_hat.Y,
|
||
M33 = j_hat.Z,
|
||
M41 = off.X,
|
||
M42 = off.Y,
|
||
M43 = off.Z
|
||
};
|
||
return matrix;
|
||
}
|
||
/// <summary>
|
||
/// a b点中间的绕行航点列表
|
||
/// </summary>
|
||
/// <param name="aVec">起点</param>
|
||
/// <param name="bVec">目标点</param>
|
||
/// <param name="middleProportion">中间航点位置比例</param>
|
||
/// <param name="transfer">绕行航点密度(值越小密度越大) ps:传递圈函数的弦长 向量矩阵函数的层高</param>
|
||
/// <param name="paunch">绕行航点范围(值越小范围越大) ps:决定层的厚度 和 圈的直径 为 paunch/航线长度</param>
|
||
/// <returns>绕行航点列表</returns>
|
||
private static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer, double paunch,bool singleCircle =true, string direction = "retrun")
|
||
{
|
||
List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
|
||
/// 根据a到b的长度 算出中间绕行几圈
|
||
int ringCou = (int)Math.Ceiling(GageLength(aVec, bVec) / paunch / transfer); //算层数和圈数 ps:层的厚度 和 圈的直径 为 paunch/航线长度
|
||
if (ringCou < 2) ringCou = 2; //最少两圈 两层
|
||
/// 不是单圈的话 设置层数跟圈数相等
|
||
int layCou = ringCou;
|
||
if (singleCircle) layCou = 1;
|
||
///遍历出所有绕行航点
|
||
for (int z = 0; z < layCou; z++) //迭代层
|
||
{
|
||
Matrix mat = GetBasisMatrix(aVec, bVec, middleProportion, z, transfer, direction); //根据圈数越多 偏移层数也越多 即每层矩阵off位置
|
||
for (int i = 0; i < ringCou; i++) //迭代圈
|
||
{
|
||
if (i != 0)
|
||
{
|
||
List<Vector3> tempCi = GetVecsOnCircle(i * transfer, transfer);
|
||
foreach (Vector3 vec in tempCi)
|
||
{
|
||
ringVec.Add(vec.Multiply(mat));//按照矩阵旋转之后 添加到中间航点列表
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ringVec.Add(mat[3]); //第一次循环 并非圈 只在航线上 按层比例取点 即可
|
||
}
|
||
}
|
||
}
|
||
return ringVec;
|
||
}
|
||
/// <summary>
|
||
/// 按照矩阵 拉散图案
|
||
/// </summary>
|
||
/// <param name="aVecs">平面图案坐标组</param>
|
||
/// <param name="bVecs">回归矩阵坐标组</param>
|
||
/// <param name="strPrint">日志输出 回调函数</param>
|
||
/// <returns>拉散图案的坐标组</returns>
|
||
public static Vector3[] NormalPull(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint)
|
||
{
|
||
int planeCou = aVecs.Length; //获取飞机总数
|
||
///a图b图 中心
|
||
Vector3 aCenterPos = GetPosCenter(aVecs, false);
|
||
Vector3 bCenterPos = GetPosCenter(bVecs, false);
|
||
///判断bVec 矩阵的数量长宽
|
||
for (int i = 0; i < planeCou; i++)//把矩阵高度压平
|
||
{
|
||
bVecs[i] = new Vector3(bVecs[i].X, 0, bVecs[i].Z);
|
||
}
|
||
int row = 1 ;
|
||
for (int i = 0; i < planeCou-2; i++)
|
||
{
|
||
if (!(IsVecsOnLine(bVecs[i], bVecs[i + 1], bVecs[i + 2])))
|
||
{
|
||
row = i+2;//列
|
||
break;
|
||
}
|
||
}
|
||
int ran = (int)Math.Ceiling((double)planeCou/(double)row);//行
|
||
///计算a图的法线标量
|
||
Vector3 side1 = aVecs[1] - aVecs[0];
|
||
Vector3 side2 = aVecs[2] - aVecs[0];
|
||
Vector3 normal = CrossPro(side1, side2);
|
||
Vector3 normalScalar = normal.NormalizEd();//法线标量
|
||
normalScalar.Y = 0;//高度上压平
|
||
///判断a图 法线朝向 和 矩阵“平行”方向 获取此方向层数 ps:用于a图的拉散
|
||
if (GageLength(normalScalar, bVecs[0]) < GageLength(normalScalar * -1, bVecs[0]))
|
||
{
|
||
normalScalar *= -1;//法线选择方向为 靠近矩阵1号机的方向 ps:由于取的三个点位置随机 按照右手定则 法线方向也随机
|
||
}
|
||
if (Math.Abs(AngleBetween(bVecs[row] - bVecs[0], normalScalar) - 90) < Math.Abs(AngleBetween(bVecs[1] - bVecs[0], normalScalar) - 90))
|
||
{
|
||
/// 图案“如平行于0 21 41..” 平行于列
|
||
for (int k = 0; k < row; k++)
|
||
{
|
||
for (int i = 0; i < ran; i++)
|
||
{
|
||
int cou = i * row + k;
|
||
if (cou > planeCou) break;// 溢出跳出
|
||
///判断图在矩阵的左方 还是右方
|
||
if (GageLength(aCenterPos, bVecs[0]) > GageLength(aCenterPos, bVecs[row - 1]))
|
||
{
|
||
aVecs[cou] -= normalScalar * (row - k) * 300;//左方
|
||
}
|
||
else
|
||
{
|
||
aVecs[cou] += normalScalar * k * 300;//右方
|
||
}
|
||
}
|
||
}
|
||
///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
|
||
if (IsPointBetweenLines(bVecs[(int)Math.Ceiling(((double)row/5))-1], bVecs[(int)Math.Ceiling(((double)row / 5)) - 1+row], bVecs[row-((int)Math.Ceiling(((double)row / 5)) - 1)], bVecs[(row - ((int)Math.Ceiling(((double)row / 5)) - 1))+row], aCenterPos))
|
||
{
|
||
aCenterPos = GetPosCenter(aVecs, false);//重新算下中心点
|
||
Vector3 offPos = aCenterPos - bCenterPos;//偏移量
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
aVecs[i] -= offPos;
|
||
}
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
/// 图案“如平行于 0-20” 平行于行
|
||
for (int k = 0; k < ran; k++)
|
||
{
|
||
for (int i = 0; i < row; i++)
|
||
{
|
||
int cou = k * row + i;
|
||
if (cou > planeCou) break;// 溢出跳出
|
||
///判断图在矩阵的上方 还是下方
|
||
if (GageLength(aCenterPos, bVecs[0]) > GageLength(aCenterPos, bVecs[row * (ran - 1)]))
|
||
{
|
||
aVecs[cou] -= normalScalar * (ran - k) * 300;//上方
|
||
}
|
||
else
|
||
{
|
||
aVecs[cou] += normalScalar * k * 300;//下方
|
||
}
|
||
}
|
||
}
|
||
///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
|
||
if (IsPointBetweenLines(bVecs[(int)Math.Ceiling((double)ran / 5) * row - row], bVecs[(int)Math.Ceiling((double)ran / 5) * row - 1], bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row], bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row + row - 1], aCenterPos))
|
||
{
|
||
aCenterPos = GetPosCenter(aVecs, false);//重新算下中心点
|
||
Vector3 offPos = aCenterPos - bCenterPos;//偏移量
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
aVecs[i] -= offPos;
|
||
}
|
||
}
|
||
}
|
||
return aVecs;
|
||
}
|
||
}
|
||
} |