248fe77995
【主 题】：3d绕行 ABypassB函数 注释 日志修改 【描 述】： [原因]：日志输出不明确 [过程]： [影响]： 【结 束】 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
2201 lines
98 KiB
C#
2201 lines
98 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 int RandomSeed { get; set; } = 1;
|
||
/// <summary>
|
||
/// 航线 线间距 平方值
|
||
/// </summary>
|
||
public static double LineDistanceSquare { get; set; } = 32400;
|
||
/// <summary>
|
||
/// 飞行过程中间距 平方值
|
||
/// </summary>
|
||
public static double SpaceBetweenSquare { get; set; } = 250000;
|
||
/// <summary>
|
||
/// 算绕行时 中间取点 true在正中间取点即 一个圆盘 false在一个圆柱体内取点
|
||
/// </summary>
|
||
public static bool singleCircle = true;
|
||
/// <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(RandomSeed);
|
||
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>
|
||
/// 从数组中删除指定索引处的元素
|
||
/// </summary>
|
||
/// <typeparam name="T">数组元素类型</typeparam>
|
||
/// <param name="array">要操作的数组</param>
|
||
/// <param name="indicesToRemove">要删除的元素的索引列表</param>
|
||
/// <returns>删除元素后的新数组</returns>
|
||
private static T[] RemoveElementsAtIndices<T>(T[] array, List<int> indicesToRemove)
|
||
{
|
||
// 检查索引是否有效
|
||
if (indicesToRemove == null || indicesToRemove.Count == 0)
|
||
{
|
||
// 没有要删除的索引，返回原数组
|
||
return array;
|
||
}
|
||
// 创建一个新数组，长度为原数组长度减去要删除的元素数量
|
||
T[] newArray = new T[array.Length - indicesToRemove.Count];
|
||
int newIndex = 0;
|
||
// 复制不包括指定索引的元素到新数组中
|
||
for (int i = 0; i < array.Length; i++)
|
||
{
|
||
if (!indicesToRemove.Contains(i))
|
||
{
|
||
newArray[newIndex] = array[i];
|
||
newIndex++;
|
||
}
|
||
}
|
||
// 返回新数组
|
||
return newArray;
|
||
}
|
||
/// <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>
|
||
/// 叉积 ps：法线向量
|
||
/// </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>
|
||
private 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>
|
||
/// 计算某个点到平面垂线与平面的交点
|
||
/// </summary>
|
||
/// <param name="vec1">平面上的点1</param>
|
||
/// <param name="vec2">平面上的点2</param>
|
||
/// <param name="vec3">平面上的点3</param>
|
||
/// <param name="vec4">被求点</param>
|
||
/// <returns>交点坐标</returns>
|
||
private static Vector3 CalculateIntersectionPoint(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||
{
|
||
// 计算平面的法线向量
|
||
Vector3 normal = CrossPro(vec2 - vec1, vec3 - vec1);
|
||
normal.Normalize();
|
||
// 计算第4个点到平面的距离
|
||
double distance = DotPro(normal, vec1);
|
||
// 计算第4个点到平面的投影点坐标
|
||
double projection = DotPro(normal, vec4) - distance;
|
||
// 计算交点坐标
|
||
Vector3 intersectionPoint = vec4 - normal * projection;
|
||
return intersectionPoint;
|
||
}
|
||
/// <summary>
|
||
/// 找到能组成平面的点的最大数量，并返回组成最大平面的点的索引。
|
||
/// </summary>
|
||
/// <param name="vecs">Vector3 点的列表。</param>
|
||
/// <returns>组成最大平面的点的索引。</returns>
|
||
private static List<int> FindMaxPlaneIndices(Vector3[] vecs)
|
||
{
|
||
int maxPointsOnPlane = 0;
|
||
List<int> maxPointsIndices = new List<int>(); //记录返回值
|
||
int planeCou = vecs.Length; // 飞机总数
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
for (int j = i + 1; j < planeCou; j++)
|
||
{
|
||
for (int k = j + 1; k < planeCou; k++)
|
||
{
|
||
int currentPointsOnPlane = 3; // 当前遍历的三个点肯定在同一平面上
|
||
|
||
for (int l = k + 1; l < planeCou; l++)
|
||
{
|
||
if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
|
||
{
|
||
// 当前的 l 也在同一平面上
|
||
currentPointsOnPlane++;
|
||
}
|
||
}
|
||
|
||
// 检查当前平面是否比之前找到的平面更大
|
||
if (currentPointsOnPlane > maxPointsOnPlane)
|
||
{
|
||
maxPointsOnPlane = currentPointsOnPlane;
|
||
maxPointsIndices.Clear();
|
||
maxPointsIndices.Add(i);
|
||
maxPointsIndices.Add(j);
|
||
maxPointsIndices.Add(k);
|
||
|
||
// 添加当前平面的 l 索引
|
||
for (int l = k + 1; l < planeCou; l++)
|
||
{
|
||
if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
|
||
{
|
||
maxPointsIndices.Add(l);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return maxPointsIndices;
|
||
}
|
||
/// <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>
|
||
private 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="isStaticSkip">静态跳过 true跳过即保持原地不动 false不跳过参与“最近和交换”计算</param>
|
||
/// <param name="staticThresholdSquare">静态距离判断 小于阈值判定为静态 注意是个平方值</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 isStaticSkip = false, double staticThresholdSquare = 25 , bool isSwap = true, int swapCount = 5, int crossingLimit = 6)
|
||
{
|
||
long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
|
||
StrPrint("-------智能选择路径计算，开始-------");
|
||
int planeCou = aVecs.Length; // 飞机总数
|
||
List<int> staticAindex = new List<int>(); // a静态对应关系（ab两图同一位置）
|
||
List<int> staticBindex = new List<int>(); // b静态对应关系（ab两图同一位置）
|
||
Vector3[] new_aVecs = aVecs.ToArray(); //a图副本
|
||
Vector3[] new_bVecs = bVecs.ToArray(); //b图副本
|
||
///如果启动 静态航点跳过 把ab静态对应关系找出来 预存放到staticMatch
|
||
if (isStaticSkip)
|
||
{
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
for (int j = 0; j < planeCou; j++)
|
||
{
|
||
if (GageLengthSquare(aVecs[i], bVecs[j]) <= staticThresholdSquare)
|
||
{
|
||
staticAindex.Add(i);
|
||
staticBindex.Add(j);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
//把静态的航点从列表里面清除
|
||
new_aVecs = RemoveElementsAtIndices(new_aVecs, staticAindex);
|
||
new_bVecs = RemoveElementsAtIndices(new_bVecs, staticBindex);
|
||
//刷新飞机总数 以下计算最近 和 交换航线时候 只有非静态飞机参与
|
||
planeCou = new_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(new_aVecs[aIndex[i]]);
|
||
allVecs.Add(new_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(new_aVecs[aIndex[i]], centerVec);
|
||
bLens[i] = GageLengthSquare(new_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(new_aVecs[aIndex[aMaxIndex]], new_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(new_aVecs[aIndex[i]], new_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[] re_bVecs = CreateNewBVecs(new_bVecs, match);// 按照映射 获取a 对应的 新的b集合
|
||
///交叉 交换
|
||
if (isSwap)
|
||
{
|
||
for (int i = 0; i < swapCount; i++)
|
||
{
|
||
List<int[]> planesCollision = AirImitation(new_aVecs, re_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(re_bVecs, current_array, planeCou);//复制一个re_bVecs 副本
|
||
for (int k = 0; k < indices.Count; k++)
|
||
{
|
||
current_array[original[k]] = re_bVecs[indices[k]];
|
||
}
|
||
///把最少碰撞的排列 录入到数组
|
||
int collisionsCou = (AirImitation(new_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);
|
||
}
|
||
}
|
||
re_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)];
|
||
}
|
||
}
|
||
}
|
||
///静态飞机 赋值回 返回航点组 ps:航点赋值的位置是对应的a组的ID位置 映射到b组的位置
|
||
if (isStaticSkip)
|
||
{
|
||
planeCou = aVecs.Length;
|
||
List<Vector3> re_bVecsCopy = new List<Vector3>(re_bVecs);
|
||
List<Vector3> re_bVecsList = new List<Vector3>();
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
if (staticAindex.IndexOf(i) != -1)
|
||
{
|
||
re_bVecsList.Add(aVecs[i]);
|
||
}
|
||
else
|
||
{
|
||
re_bVecsList.Add(re_bVecsCopy[0]);
|
||
re_bVecsCopy.RemoveAt(0);
|
||
}
|
||
}
|
||
re_bVecs = re_bVecsList.ToArray();
|
||
}
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
StrPrint($"-------智能选择路径计算，结束-------");
|
||
return re_bVecs;
|
||
}
|
||
/// <summary>
|
||
/// 智能错层 说明：从A图上找出一个面（最多点组成的共面），以此面为错层基准，做错层计算。PS:A图 B图 都可以为多个平面图形组成，但这些图形都必须平行
|
||
/// </summary>
|
||
/// <param name="aVecs">起始坐标集合</param>
|
||
/// <param name="bVecs">终点做标集合</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="layHight">错层层高</param>
|
||
/// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组有几种情况 1.A图直接飞B图无碰撞 2.A图未能找到4个点以上的共面 3.有碰撞可能是AB图初始就过近 4.AB图并不平行</returns>
|
||
public static List<List<Vector3>> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,SomeCalculateWay StrPrint, double layHight = 300)
|
||
{
|
||
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;
|
||
|
||
Vector3[] new_aVecs = aVecs.ToArray(); //a图副本
|
||
Vector3[] new_bVecs = bVecs.ToArray(); //b图副本
|
||
|
||
///把所有点压在 主面“共面”上
|
||
List<int> maxVecsOfCoplane = FindMaxPlaneIndices(aVecs);// 找出A图共面最多点的索引
|
||
StrPrint("提示：正在进行“共面”检测，需要一些时间请耐心等待。。。");
|
||
if (maxVecsOfCoplane.Count < 4) //a图至少要有4个点 共面
|
||
{
|
||
StrPrint("执行失败：起始图案至少有4个点以上共面");
|
||
//StrPrint($"-------错层结束-------");
|
||
return re;
|
||
}
|
||
///共面上取三个点
|
||
Vector3 vec0 = new_aVecs[maxVecsOfCoplane[0]];
|
||
Vector3 vec1 = new_aVecs[maxVecsOfCoplane[1]];
|
||
Vector3 vec2 = new_aVecs[maxVecsOfCoplane[2]];
|
||
///遍历 把a图和b图点压到 “共面”上
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
if (!(maxVecsOfCoplane.Contains(i))) //除去在共面内的面
|
||
{
|
||
new_aVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_aVecs[i]); //压平到共面上
|
||
}
|
||
new_bVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_bVecs[i]); //压平到共面上
|
||
}
|
||
|
||
///计算法线向量
|
||
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 = new_aVecs[i]; //原点位置
|
||
Vector3 bOrigin = new_bVecs[i]; //原点位置
|
||
while (OnlyImitation(i, new_aVecs, new_bVecs))
|
||
{
|
||
Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
|
||
if (shiftCou % 2 == 1)
|
||
{
|
||
new_aVecs[i] = aOrigin + shiftVec;
|
||
new_bVecs[i] = bOrigin + shiftVec;
|
||
}
|
||
else
|
||
{
|
||
new_aVecs[i] = aOrigin - shiftVec;
|
||
new_bVecs[i] = bOrigin - shiftVec;
|
||
}
|
||
shiftCou += 1;
|
||
}
|
||
}
|
||
///计算碰撞
|
||
planesCollision = AirImitation(aVecs, new_aVecs).Concat(AirImitation(new_aVecs, new_bVecs)).ToList(); //获取碰撞组
|
||
planesCollision = planesCollision.Concat(AirImitation(new_bVecs, bVecs)).ToList();
|
||
if (planesCollision.Count == 0)
|
||
{
|
||
re.Add(new_aVecs.ToList());
|
||
re.Add(new_bVecs.ToList());
|
||
StrPrint($"执行成功。");
|
||
}
|
||
else
|
||
{
|
||
StrPrint($"执行失败：计算完成后，检测有碰撞。可能原因：1.起始图形或结束图形点阵距离有过近情况。2.起始图形和结束图形点阵所在面不平行");
|
||
}
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
//StrPrint($"-------错层结束-------");
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 路径绕行
|
||
/// </summary>
|
||
/// <param name="aVecs">起始坐标集合</param>
|
||
/// <param name="bVecs">终点做标集合</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="GetVal">进度日志输出 回调函数</param>
|
||
/// <param name="cancellationToken">函数“取消执行”ps:new一个CancellationTokenSource类型 把实例.Token属性传进来，函数外部用实例.Cancel()函数控制实参值(bool)</param>
|
||
/// <param name="isPass">out参数 返回true不碰撞程序直接返回 false有碰撞程序向下执行</param>
|
||
/// <param name="mappingId">飞机真实序号的映射关系</param>
|
||
/// <returns>返回一个二维数组 返回值长度0没有检测到碰撞或绕行失败 长度1为一个中间航点 长度为3为三个中间航点顺序(前中后)</returns>
|
||
public static List<List<Vector3>> ABypassB(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, Schedule GetVal, CancellationToken cancellationToken, out bool isPass, List<int> mappingId = null)
|
||
{
|
||
isPass = false;
|
||
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绕行，结束-------");
|
||
isPass = true;
|
||
return re;//直接返回
|
||
}
|
||
///飞机总数
|
||
int planeCou = aVecs.Length;
|
||
///判断有没有给 映射的ID 没有就按顺序设置ID序号
|
||
if (mappingId == null)
|
||
{
|
||
mappingId = new List<int>();
|
||
for (int i = 1; i <= planeCou; i++)
|
||
{
|
||
mappingId.Add(i);
|
||
}
|
||
}
|
||
///碰撞数
|
||
int collisionCou;
|
||
///第一次绕行 中间1航点
|
||
StrPrint($"3D航线第一次计算开始。");
|
||
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])); //添加默认中间航点
|
||
}
|
||
collisionCou = collisionGroup.Count;
|
||
while (true)
|
||
{
|
||
if (cancellationToken.IsCancellationRequested)//外部法取消指令
|
||
{
|
||
StrPrint("取消3D航线操作。");
|
||
return null; // 退出函数
|
||
}
|
||
int progress = 0;//进度
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
progress++;
|
||
GetVal(progress / collisionGroup.Count * 100);
|
||
List<Vector3> grv = GetRingVec(aVecs[i], bVecs[i], 0.5, 5, 4, 1500, 300);//中间可绕行航点列表
|
||
StrPrint($"3D航线，第一次计算进度：{progress}/{collisionGroup.Count}，本次计算{grv.Count}次");
|
||
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 || collisionCou <= collisionGroup.Count)
|
||
{
|
||
collisionCou = collisionGroup.Count;
|
||
break;
|
||
}
|
||
collisionCou = collisionGroup.Count;
|
||
}
|
||
//没有碰撞 返回一个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("执行成功：第一次计算即成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
//StrPrint($"-------3D绕行，结束-------");
|
||
re.Add(middleVecs);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
|
||
StrPrint($"3D航线第一次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
|
||
//StrPrint("第一次绕行未成功!");
|
||
}
|
||
|
||
///第二次绕行 两头 两航点
|
||
StrPrint($"3D航线第二次计算开始。");
|
||
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
|
||
}
|
||
collisionCou = collisionGroup.Count;
|
||
while (true)
|
||
{
|
||
if (cancellationToken.IsCancellationRequested)//外部法取消指令
|
||
{
|
||
StrPrint("取消3D航线操作。");
|
||
return null; // 退出函数
|
||
}
|
||
int progress = 0;//进度
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
progress++;
|
||
GetVal(progress / collisionGroup.Count * 100);
|
||
//StrPrint($"迭代{c}次{i}号绕行");
|
||
List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0, 30, 10, 600, 300);//中间可绕行航点列表
|
||
sgrv1.Insert(0, secondMiddleVecsOne[i]);
|
||
List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 1, 30, 10, 600, 300);//中间可绕行航点列表
|
||
sgrv2.Insert(0, secondMiddleVecsTwo[i]);
|
||
StrPrint($"3D航线，第二次计算进度：{progress}/{collisionGroup.Count}，本次绕行{sgrv1.Count * sgrv2.Count}次");
|
||
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 || collisionCou <= collisionGroup.Count)
|
||
{
|
||
collisionCou = collisionGroup.Count;
|
||
break;
|
||
}
|
||
collisionCou = collisionGroup.Count;
|
||
}
|
||
//没有碰撞 返回两个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("执行成功：第二次计算成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
//StrPrint($"-------3D绕行，结束-------");
|
||
re.Add(secondMiddleVecsOne);
|
||
re.Add(secondMiddleVecsTwo);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
|
||
StrPrint($"3D航线第二次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
|
||
//StrPrint("第二次绕行未成功!");
|
||
}
|
||
|
||
///第三次绕行 两头 两航点 中间一行点（实际添加两航点 但是0.6位置航点暂不启用留给第四次绕行） 沿用第二次绕行
|
||
StrPrint($"3D航线第二次计算开始。");
|
||
isPassMark = false;
|
||
List<Vector3> thirdMiddleVecs = new List<Vector3>(); //中心航点坐标组1
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
thirdMiddleVecs.Add(SetMiddleVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5)); //添加中间航点(保持二次绕行的两端航点 在两端航点中间添加)
|
||
}
|
||
while (true)
|
||
{
|
||
if (cancellationToken.IsCancellationRequested)//外部法取消指令
|
||
{
|
||
StrPrint("取消3D航线操作。");
|
||
return null; // 退出函数
|
||
}
|
||
int progress = 0;//进度
|
||
foreach (int i in collisionGroup)//开始绕碰撞组
|
||
{
|
||
GetVal(progress / collisionGroup.Count * 100);
|
||
progress++;
|
||
//StrPrint($"迭代{c}次{i}号绕行");
|
||
List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0, 100, 10, 600, 300);//中间可绕行航点列表
|
||
sgrv1.Insert(0, secondMiddleVecsOne[i]);
|
||
List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 1, 100, 10, 600, 300);//中间可绕行航点列表
|
||
sgrv2.Insert(0, secondMiddleVecsTwo[i]);
|
||
List<Vector3> grv = GetRingVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5, 80, 4, 1500, 300);//中间可绕行航点列表
|
||
StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{sgrv1.Count * sgrv2.Count * grv.Count}次");
|
||
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 || collisionCou <= collisionGroup.Count)
|
||
{
|
||
collisionCou = collisionGroup.Count;
|
||
break;
|
||
}
|
||
collisionCou = collisionGroup.Count;
|
||
}
|
||
//没有碰撞 返回三个中间航点 并返回
|
||
if (collisionGroup.Count == 0)
|
||
{
|
||
StrPrint("执行成功：第三次计算成功！");
|
||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||
StrPrint($"用时：{t}秒");
|
||
re.Add(secondMiddleVecsOne);
|
||
re.Add(thirdMiddleVecs);
|
||
re.Add(secondMiddleVecsTwo);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
|
||
StrPrint($"3D航线第三次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
|
||
StrPrint("执行失败：3D航线经过三次计算，仍有碰撞。");
|
||
}
|
||
|
||
///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(RandomSeed);// 生成一个随机的单位向量
|
||
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">绕行航点范围(值越小范围越大 如:参数给4 圆盘的半径是a到b距离的1/4) ps:决定层的厚度 和 圈的直径 为 paunch/航线长度</param>
|
||
/// <param name="maxPaunchRadius">设定圆盘半径 的最大值 单位是厘米</param>
|
||
/// <param name="direction">层排布方向 "retrun"前后堆叠 "forward"向前排列（如：起点向目标点方向） "backward"向后排列</param>
|
||
/// <returns>绕行航点列表</returns>
|
||
public static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer, double paunch, double maxPaunchRadius, double minPaunchRadius, string direction = "retrun")
|
||
{
|
||
List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
|
||
/// 根据a到b的长度 算出中间绕行几圈
|
||
double discRadius = GageLength(aVec, bVec) / paunch;//圆盘半径
|
||
if (discRadius > maxPaunchRadius)
|
||
{
|
||
discRadius = maxPaunchRadius;//设定圆盘直径上限
|
||
}
|
||
if (discRadius < minPaunchRadius)
|
||
{
|
||
discRadius = minPaunchRadius;//设定圆盘直径下限
|
||
}
|
||
int ringCou = (int)Math.Ceiling(discRadius / transfer); //算层数和圈数 ps:层的厚度 和 圈的直径 为 paunch/航线长度
|
||
/// 不是单圈的话 设置层数跟圈数相等
|
||
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>
|
||
/// <param name="pullingDistance">拉散层距</param>
|
||
/// <returns>拉散图案的坐标组</returns>
|
||
public static Vector3[] NormalPull(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint,double pullingDistance = 300)
|
||
{
|
||
Vector3[] new_aVecs = aVecs.ToArray(); //a图副本
|
||
Vector3[] new_bVecs = bVecs.ToArray(); //矩阵副本
|
||
int planeCou = new_aVecs.Length; //获取飞机总数
|
||
///判断a图是不是平面
|
||
if (!(planeCou == FindMaxPlaneIndices(aVecs).Count))
|
||
{
|
||
StrPrint("-------前图航点非平面图形,故不能做拉散图案操作-------");
|
||
return null;
|
||
}
|
||
///a图b图 中心
|
||
Vector3 aCenterPos = GetPosCenter(new_aVecs, false);
|
||
///判断bVec 矩阵的数量长宽
|
||
for (int i = 0; i < planeCou; i++)//把矩阵高度压平
|
||
{
|
||
new_bVecs[i] = new Vector3(new_bVecs[i].X, 0, new_bVecs[i].Z);
|
||
}
|
||
int row = 1;
|
||
for (int i = 0; i < planeCou - 2; i++)
|
||
{
|
||
if (!(IsVecsOnLine(new_bVecs[i], new_bVecs[i + 1], new_bVecs[i + 2])))
|
||
{
|
||
row = i + 2;//列
|
||
break;
|
||
}
|
||
}
|
||
int ran = (int)Math.Ceiling((double)planeCou / (double)row);//行
|
||
StrPrint($"{ran}行{row}列");
|
||
if (ran > 2)
|
||
{
|
||
for (int i = 0; i < ran - 2; i++)
|
||
{
|
||
if (!(IsVecsOnLine(new_bVecs[i * row], new_bVecs[(i + 1) * row], new_bVecs[(i + 2) * row])))
|
||
{
|
||
StrPrint("-------降落航点非常规矩阵,故不能做拉散图案操作-------");
|
||
return null;
|
||
}
|
||
}
|
||
}
|
||
///计算a图的法线标量
|
||
Vector3 side1 = new_aVecs[1] - new_aVecs[0];
|
||
Vector3 side2 = new_aVecs[2] - new_aVecs[0];
|
||
Vector3 normal = CrossPro(side1, side2);
|
||
Vector3 normalScalar = normal.NormalizEd();//法线标量
|
||
normalScalar.Y = 0;//高度上压平
|
||
///判断a图 法线朝向 和 矩阵“平行”方向 获取此方向层数 ps:用于a图的拉散
|
||
if (GageLength(normalScalar, new_bVecs[0]) < GageLength(normalScalar * -1, new_bVecs[0]))
|
||
{
|
||
normalScalar *= -1;//法线选择方向为 靠近矩阵1号机的方向 ps:由于取的三个点位置随机 按照右手定则 法线方向也随机
|
||
}
|
||
if (Math.Abs(AngleBetween(new_bVecs[row] - new_bVecs[0], normalScalar) - 90) < Math.Abs(AngleBetween(new_bVecs[1] - new_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, new_bVecs[0]) > GageLength(aCenterPos, new_bVecs[row - 1]))
|
||
{
|
||
new_aVecs[cou] -= normalScalar * (row - k) * pullingDistance;//左方
|
||
}
|
||
else
|
||
{
|
||
new_aVecs[cou] += normalScalar * k * pullingDistance;//右方
|
||
}
|
||
}
|
||
}
|
||
///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
|
||
if (IsPointBetweenLines(new_bVecs[(int)Math.Ceiling(((double)row / 5)) - 1], new_bVecs[(int)Math.Ceiling(((double)row / 5)) - 1 + row], new_bVecs[row - ((int)Math.Ceiling(((double)row / 5)) - 1)], new_bVecs[(row - ((int)Math.Ceiling(((double)row / 5)) - 1)) + row], aCenterPos))
|
||
{
|
||
if (GageLength(new_aVecs[0], aVecs[0]) > GageLength(new_aVecs[planeCou - 1], aVecs[planeCou - 1]))//判断最大偏移量 是第一排 还是最后一排
|
||
{
|
||
Vector3 offPos = (new_aVecs[0] - aVecs[0]) * 0.5;//偏移量 的一半
|
||
for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
|
||
{
|
||
new_aVecs[i] -= offPos;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Vector3 offPos = (new_aVecs[planeCou - 1] - aVecs[planeCou - 1]) * 0.5;//偏移量 的一半
|
||
for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
|
||
{
|
||
new_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;
|
||
//StrPrint($"{cou}");
|
||
if (cou >= planeCou) break;// 溢出跳出
|
||
///判断图在矩阵的上方 还是下方
|
||
if (GageLength(aCenterPos, new_bVecs[0]) > GageLength(aCenterPos, new_bVecs[row * (ran - 1)]))
|
||
{
|
||
new_aVecs[cou] -= normalScalar * (ran - k) * pullingDistance;//上方
|
||
}
|
||
else
|
||
{
|
||
new_aVecs[cou] += normalScalar * k * pullingDistance;//下方
|
||
}
|
||
}
|
||
}
|
||
///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
|
||
if (IsPointBetweenLines(new_bVecs[(int)Math.Ceiling((double)ran / 5) * row - row], new_bVecs[(int)Math.Ceiling((double)ran / 5) * row - 1], new_bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row], new_bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row + row - 1], aCenterPos))
|
||
{
|
||
if (GageLength(new_aVecs[0], aVecs[0]) > GageLength(new_aVecs[planeCou - 1], aVecs[planeCou - 1]))//判断最大偏移量 是第一排 还是最后一排
|
||
{
|
||
Vector3 offPos = (new_aVecs[0] - aVecs[0]) * 0.5;//偏移量 的一半
|
||
for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
|
||
{
|
||
new_aVecs[i] -= offPos;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Vector3 offPos = (new_aVecs[planeCou - 1] - aVecs[planeCou - 1]) * 0.5;//偏移量 的一半
|
||
for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
|
||
{
|
||
new_aVecs[i] -= offPos;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return new_aVecs;
|
||
}
|
||
}
|
||
} |