1662 lines
71 KiB
C#
1662 lines
71 KiB
C#
using Microsoft.SqlServer.Server;
|
||
using System;
|
||
using System.Collections;
|
||
using System.Collections.Generic;
|
||
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>
|
||
/// <param name="str">日志内容</param>
|
||
public delegate void SomeCalculateWay(string str);
|
||
/// <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 ArrayList TwoArrToArr(ArrayList twoArr)
|
||
{
|
||
ArrayList arr = new ArrayList();
|
||
foreach (int[] item in twoArr)
|
||
{
|
||
int i = arr.IndexOf(item[0]);
|
||
if (i < 1) arr.Add(item[0]);
|
||
int x = arr.IndexOf(item[1]);
|
||
if (x < 1) arr.Add(item[1]);
|
||
}
|
||
return arr;
|
||
}
|
||
/// <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]);
|
||
if (i < 1) arr.Add(item[0]);
|
||
int x = arr.IndexOf(item[1]);
|
||
if (x < 1) arr.Add(item[1]);
|
||
}
|
||
return arr;
|
||
}
|
||
/// <summary>
|
||
/// 处理二维数组 把有关联的子数组合并 例如：[[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>
|
||
/// 检查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 = 0.1;
|
||
return Math.Abs(distance) < epsilon;
|
||
}
|
||
/// <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 IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||
{
|
||
// 计算相邻点之间的方向向量
|
||
Vector3 dir1 = new Vector3(vec2.X - vec1.X, vec2.Y - vec1.Y, vec2.Z - vec1.Z);
|
||
Vector3 dir2 = new Vector3(vec3.X - vec2.X, vec3.Y - vec2.Y, vec3.Z - vec2.Z);
|
||
Vector3 dir3 = new Vector3(vec4.X - vec3.X, vec4.Y - vec3.Y, vec4.Z - vec3.Z);
|
||
|
||
// 检查任意两个相邻方向向量的叉积是否为零
|
||
// 这是在三维空间中判断共线的条件
|
||
return CrossPro(dir1, dir2).GetMag() <0.1 && CrossPro(dir2, dir3).GetMag() <0.1;
|
||
}
|
||
/// <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>
|
||
/// <param name="lineDistanceSquare">航线最小间距平方值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation(Vector3[] aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 650000)
|
||
{
|
||
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>
|
||
/// <param name="lineDistanceSquare">航线最小间距平方值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
|
||
/// <returns></returns>
|
||
private static List<int[]> AirImitation(List<Vector3> aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 650000)
|
||
{
|
||
List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
|
||
int planeCou = 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>
|
||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
|
||
{
|
||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||
// 判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||
if (distanceSquare < lineDistanceSquare)
|
||
{
|
||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||
if (minDistanceSquare < spaceBetweenSquare)
|
||
{
|
||
return true;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 单机碰撞检测
|
||
/// </summary>
|
||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
|
||
{
|
||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||
// 判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||
if (distanceSquare < lineDistanceSquare)
|
||
{
|
||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||
if (minDistanceSquare < spaceBetweenSquare)
|
||
{
|
||
return true;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 单机碰撞检测
|
||
/// </summary>
|
||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, List<Vector3> bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
|
||
{
|
||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||
// 判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||
if (distanceSquare < lineDistanceSquare)
|
||
{
|
||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||
if (minDistanceSquare < spaceBetweenSquare)
|
||
{
|
||
return true;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 单机碰撞检测
|
||
/// </summary>
|
||
/// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
|
||
/// <param name="aVecs">飞机起始坐标集合</param>
|
||
/// <param name="bVecs">飞机终点坐标集合</param>
|
||
/// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
|
||
/// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
|
||
/// <returns>true:有碰撞 false:无碰撞</returns>
|
||
private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, Vector3[] bVecs, double lineDistanceSquare = 36100, double spaceBetweenSquare = 250000)
|
||
{
|
||
//选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
|
||
for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
|
||
{
|
||
if (onlyPlaneId == contrastId) continue;//不和自己比较
|
||
// 判断两条轨迹 之间的最小距离
|
||
double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
|
||
if (distanceSquare < lineDistanceSquare)
|
||
{
|
||
double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
|
||
if (minDistanceSquare < spaceBetweenSquare)
|
||
{
|
||
return true;//返回有碰撞
|
||
}
|
||
}
|
||
}
|
||
return false;//返回没有碰撞;
|
||
}
|
||
/// <summary>
|
||
/// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换
|
||
/// </summary>
|
||
/// <param name="aVecs">起始点坐标组</param>
|
||
/// <param name="bVecs">目标点坐标组</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="swapCount">交换次数</param>
|
||
/// <param name="crossingLimit">交叉线路数量上限 ps:超过这个数量则不进行交换</param>
|
||
/// <returns>新的目标点</returns>
|
||
public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, int swapCount = 5, int crossingLimit = 6)
|
||
{
|
||
int planeCou = aVecs.Length; // 飞机总数
|
||
List<int[]> match = new List<int[]>(); // ab对应关系
|
||
//记录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集合
|
||
|
||
//交叉 交换
|
||
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]]
|
||
// 遍历所有交叉组 分组做交换
|
||
foreach (List<int> swap_indices in formatCollision)
|
||
{
|
||
if (swap_indices.Count <= crossingLimit) // 只尝试5组交叉碰撞以下的航线互换
|
||
{
|
||
//交叉 生成所有排列
|
||
List<List<int>> all_permutations = Permutations(swap_indices);
|
||
//删掉第一个 既原始排列
|
||
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_bVecs.ToArray();//复制一个new_bVecs 副本
|
||
for (int k = 0; k < indices.Count - 1; k++)
|
||
{
|
||
int index1 = indices[k];
|
||
int index2 = indices[k + 1];
|
||
// 交换元素
|
||
Vector3 temp = current_array[index1];
|
||
current_array[index1] = current_array[index2];
|
||
current_array[index2] = temp;
|
||
}
|
||
tempLen.Add(AirImitation(aVecs, current_array).Count);// 迭代 记录所有排列的碰撞次数
|
||
tempNew_bVecsS.Add(current_array); //迭代 记录所有排列交换之后的目标坐标集
|
||
}
|
||
new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)].ToArray();
|
||
}
|
||
}
|
||
}
|
||
return new_bVecs;
|
||
}
|
||
/// <summary>
|
||
/// 智能挫层
|
||
/// </summary>
|
||
/// <param name="aVecs">起始坐标集合</param>
|
||
/// <param name="bVecs">终点做标集合</param>
|
||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||
/// <param name="layHight">挫层层高</param>
|
||
/// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点</returns>
|
||
public static List<Vector3[]> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, double layHight = 220)
|
||
{
|
||
List<Vector3[]> re = new List<Vector3[]>();
|
||
//获取飞机总数
|
||
int planeCou = aVecs.Length;
|
||
if (planeCou < 2)//至少不少于2架飞机 才开始挫层
|
||
{
|
||
StrPrint("图案至少要两个点阵！");
|
||
return re;
|
||
}
|
||
//检测两个图形是否在一个平面上
|
||
Vector3 vec0 = new Vector3(0, 0, 0);
|
||
Vector3 vec1 = new Vector3(0, 0, 0);
|
||
Vector3 vec2 = new Vector3(0, 0, 0);
|
||
Vector3 vec3 = new Vector3(0, 0, 0);
|
||
|
||
//遍历所有点 查看是不是都再一个平面内
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
vec0 = aVecs[i]; vec1 = bVecs[planeCou - i - 1]; vec2 = aVecs[planeCou - i - 1]; vec3 = bVecs[i];
|
||
if (!(IsVecsOnPlane(vec0, vec1, vec2, vec3)))
|
||
{
|
||
StrPrint("图案不在一个平面!");
|
||
return re;//两个图形不在一个平面上 返回 空数组
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
//遍历选出4个不共线的点 ps:方便后续叉积算法线标量
|
||
bool isVecsOnLine = false;
|
||
for (int i = 0; i < planeCou; i++) //遍历所有点 找出不在一条直线上的四个点
|
||
{
|
||
vec0 = aVecs[i]; vec1 = bVecs[planeCou - i - 1]; vec2 = aVecs[planeCou - i - 1]; vec3 = bVecs[i];
|
||
isVecsOnLine = IsVecsOnLine(vec0, vec1, vec2, vec3);
|
||
if (!isVecsOnLine)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
if (isVecsOnLine)
|
||
{
|
||
StrPrint("所有点都在一条直线上!");
|
||
return re;//两个图案的所有点都在一条直线上 返回 空数组
|
||
}
|
||
//检查完毕后
|
||
//计算法线向量
|
||
Vector3 side1 = vec0 - vec1;
|
||
Vector3 side2 = vec2 - vec3;
|
||
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;
|
||
}
|
||
}
|
||
StrPrint("挫层完成！");
|
||
re.Add(aVecs);
|
||
re.Add(bVecs);
|
||
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)
|
||
{
|
||
List<List<Vector3>> re = new List<List<Vector3>>();
|
||
int planeCou = aVecs.Length;
|
||
//第一次绕行
|
||
//起始坐标组重心点 与 目标坐标组重心点
|
||
Vector3 aCenterPoint = GetPosCenter(aVecs);
|
||
Vector3 bCenterPoint = GetPosCenter(bVecs);
|
||
//记录所有航线中心距 ps:起点离起点组中心点距离 + 目标点离目标点组中心点距离
|
||
double[] centerDistanceS = new double[planeCou];
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
centerDistanceS[i] = GageLengthSquare(aVecs[i], aCenterPoint) + GageLengthSquare(bVecs[i], bCenterPoint);
|
||
}
|
||
//记录从中心开始排序
|
||
int[] sortedIndices = Enumerable.Range(0, centerDistanceS.Length).ToArray();
|
||
Array.Sort(sortedIndices, (x, y) => centerDistanceS[x].CompareTo(centerDistanceS[y]));// 对索引数组按原始数组的值进行排序
|
||
//从中心航线 逐条绕行 ps:映射从中心内部到此到外部顺序
|
||
List<Vector3> tempAVecs = new List<Vector3>(); // 逐条起点坐标组
|
||
List<Vector3> tempBVecs = new List<Vector3>(); // 逐条目标点坐标组
|
||
List<Vector3> middleVecs = new List<Vector3>(); //中心航点坐标组
|
||
List<int> firstCollisionGroup = new List<int>(); //有碰撞飞机列表(第一次绕行)
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
StrPrint($"第一次绕行进度：{(int)((double)i/planeCou*100)}%");
|
||
bool isPassMark = false; //初始标记
|
||
int index = sortedIndices[i];//从内到外排序的索引
|
||
tempAVecs.Add(aVecs[index]);
|
||
tempBVecs.Add(bVecs[index]);
|
||
if (i == 0) //如果是第一条航线 直接添加中间航点
|
||
{
|
||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); //添加中间航点
|
||
continue; //跳出 继续第二条航线计算
|
||
}
|
||
else //从第二条开始绕
|
||
{
|
||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); // 添加默认中间航点
|
||
// 首次默认中间航点检测
|
||
if (!(OnlyImitation(i, tempAVecs, middleVecs)) && !(OnlyImitation(i, middleVecs, tempBVecs))) //前半段碰 碰撞检测
|
||
{
|
||
continue; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
else
|
||
{
|
||
middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
|
||
}
|
||
// 绕行列表遍历 检测
|
||
List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
|
||
foreach (Vector3 v in ringVecs)
|
||
{
|
||
middleVecs.Add(v); //添加临时绕行航点
|
||
//检测
|
||
if (!(OnlyImitation(i, tempAVecs, middleVecs)) && !(OnlyImitation(i, middleVecs, tempBVecs))) //前半段碰 碰撞检测
|
||
{
|
||
isPassMark = true; //中间航点 碰撞检测通过标记
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
else
|
||
{
|
||
middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
|
||
}
|
||
}
|
||
}
|
||
// 如果绕行失败
|
||
if (!isPassMark)
|
||
{
|
||
middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); //添加一个默认中间航点
|
||
firstCollisionGroup.Add(index);
|
||
}
|
||
}
|
||
//中间航点坐标集 按飞机id顺序重新映射 ps:恢复原id映射 排序
|
||
Vector3[] tempMiddleVecs = new Vector3[planeCou];
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
tempMiddleVecs[sortedIndices[i]] = middleVecs[i];
|
||
}
|
||
middleVecs = new List<Vector3>(tempMiddleVecs);
|
||
//没有碰撞 返回一个中间航点 并返回
|
||
if (firstCollisionGroup.Count == 0)
|
||
{
|
||
StrPrint("第一次绕行通过！");
|
||
re.Add(middleVecs);
|
||
return re;
|
||
}
|
||
else
|
||
{
|
||
string str = "";
|
||
foreach (int v in firstCollisionGroup) {
|
||
str += v.ToString() + "号，";
|
||
}
|
||
str += "有碰撞,共" + firstCollisionGroup.Count.ToString() + "架！";
|
||
StrPrint(str);
|
||
StrPrint("第一次绕行未通过，准备开始第二次绕行!");
|
||
}
|
||
|
||
//第二次绕行 ps:有碰撞进行第二次 前后再各加一航点 嵌套迭代
|
||
List<Vector3> firstMiddleVecs = new List<Vector3>(); //前中间航点
|
||
List<Vector3> secondMiddleVecs = new List<Vector3>(); //后中间航点
|
||
List<int> secondCollisionGroup = new List<int>(); //最终有碰撞飞机列表
|
||
for (int i = 0; i < planeCou; i++)
|
||
{
|
||
firstMiddleVecs.Add(SetMiddleVec(aVecs[i], middleVecs[i], 0));
|
||
secondMiddleVecs.Add(SetMiddleVec(middleVecs[i], bVecs[i], 1));
|
||
}
|
||
int z = 0;//计算进度
|
||
foreach (int index in firstCollisionGroup)
|
||
{
|
||
z++;//计算进度
|
||
StrPrint($"第二次绕行进度：{(int)((double)z / firstCollisionGroup.Count * 100)}%");
|
||
bool isPassMark = false;
|
||
// 正中 绕行列表
|
||
List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
|
||
foreach (Vector3 r in ringVecs)
|
||
{
|
||
middleVecs[index] = r; //中点逐点
|
||
List<Vector3> firstRingVecs = GetRingVec(aVecs[index], middleVecs[index], 0, 220, 6); //所有可绕行的 前中间航点集合
|
||
foreach (Vector3 fr in firstRingVecs)
|
||
{
|
||
firstMiddleVecs[index] = fr; //前中点逐点
|
||
List<Vector3> secondRingVecs = GetRingVec(middleVecs[index], bVecs[index], 1, 220, 6); //所有可绕行的 后中间航点集合
|
||
foreach (Vector3 sr in secondRingVecs)
|
||
{
|
||
secondMiddleVecs[index] = sr; //后中点逐点
|
||
// 检测 起点到 第一中间航点
|
||
if (!(OnlyImitation(index, aVecs, firstMiddleVecs)) && !(OnlyImitation(index, firstMiddleVecs, middleVecs)) && !(OnlyImitation(index, middleVecs, secondMiddleVecs)) && !(OnlyImitation(index, secondMiddleVecs, bVecs)))
|
||
{
|
||
isPassMark = true; //碰撞检测通过标记
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
if (isPassMark)
|
||
{
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
if (isPassMark)
|
||
{
|
||
break; //不撞则跳出 进行下一航线的绕行
|
||
}
|
||
}
|
||
if (!isPassMark) //如果绕行失败 添加一个默认中间航点
|
||
{
|
||
middleVecs[index] = SetMiddleVec(aVecs[index], bVecs[index]); //添加中间航点
|
||
firstMiddleVecs[index] = SetMiddleVec(aVecs[index], middleVecs[index], 0); // 添加中间航点
|
||
secondMiddleVecs[index] = SetMiddleVec(middleVecs[index], bVecs[index], 1); // 添加中间航点
|
||
secondCollisionGroup.Add(index);// 二次绕行仍不通过 添加到列表 （暂时无用）
|
||
}
|
||
}
|
||
if (secondCollisionGroup.Count == 0)//如果二次绕行通过 添加中间三航点
|
||
{
|
||
re.Add(firstMiddleVecs);
|
||
re.Add(middleVecs);
|
||
re.Add(secondMiddleVecs);
|
||
StrPrint("第二次绕行通过！");
|
||
}
|
||
else
|
||
{
|
||
string str = "";
|
||
foreach (int v in secondCollisionGroup)
|
||
{
|
||
str += v.ToString() + "号，";
|
||
}
|
||
str += "仍有碰撞,共" + secondCollisionGroup.Count.ToString() + "架！";
|
||
StrPrint(str);
|
||
StrPrint("第二次绕行仍未通过！");
|
||
}
|
||
return re;
|
||
}
|
||
/// <summary>
|
||
/// 在圆圈上 用固定弦长手拉手 分割点
|
||
/// </summary>
|
||
/// <param name="radius">大圆半径</param>
|
||
/// <param name="chordLength">固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集</param>
|
||
/// <returns></returns>
|
||
private static List<Vector3> GetVecsOnCircle(double radius, double chordLength = 220)
|
||
{
|
||
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.0f; // 如果是在平面上，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>
|
||
/// <returns>矩阵</returns>
|
||
private static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion = 0.5, int offCount = 0, double layHight = 220)
|
||
{
|
||
// 计算前向向量 k帽
|
||
Vector3 k_hat = (bVec - aVec).NormalizEd();
|
||
// 计算右向量，使用 Vector3.UnitY 作为上向量 i帽
|
||
Vector3 i_hat = CrossPro(Vector3.UnitY, k_hat).NormalizEd();
|
||
// 计算上向量 j帽
|
||
Vector3 j_hat = CrossPro(k_hat, i_hat);
|
||
//计算 对应的偏移比例
|
||
double length = (bVec - aVec).GetMag();
|
||
double offShift;//偏移比例
|
||
if (offCount % 2 == 1) //奇数
|
||
offShift = middleProportion + (offCount + 2) / 2 * layHight / length;
|
||
else //偶数
|
||
offShift = middleProportion - (offCount + 1) / 2 * 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">传递圈函数的弦长 向量矩阵函数的层高</param>
|
||
/// <param name="paunch">绕行航点范围 ps:绕行中间航点肚子(值越大肚子越小)</param>
|
||
/// <returns></returns>
|
||
private static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer = 220, double paunch = 3)
|
||
{
|
||
List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
|
||
// 根据a到b的长度 算出中间绕行几圈
|
||
int ringCou = (int)Math.Ceiling(GageLength(aVec, bVec) / paunch / transfer); //绝对中心绕行的圈数 即中间航点的'肚子'大小
|
||
if (ringCou < 2) ringCou = 2; //最少两圈 两层
|
||
if (ringCou > 15) ringCou = 15; //最多15圈 15层
|
||
for (int z = 0; z < ringCou; z++) //迭代层
|
||
{
|
||
Matrix mat = GetBasisMatrix(aVec, bVec, middleProportion, z, transfer); //根据圈数越多 偏移层数也越多 即每层矩阵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;
|
||
}
|
||
}
|
||
} |