diff --git a/FlightRouteV2/.vs/FlyBase/v16/.suo b/FlightRouteV2/.vs/FlyBase/v16/.suo new file mode 100644 index 0000000..893da3a Binary files /dev/null and b/FlightRouteV2/.vs/FlyBase/v16/.suo differ diff --git a/FlightRouteV2/FlightRouteV2.cs b/FlightRouteV2/FlightRouteV2.cs new file mode 100644 index 0000000..6dad670 --- /dev/null +++ b/FlightRouteV2/FlightRouteV2.cs @@ -0,0 +1,1997 @@ +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; + /// + /// 构造函数，接受四个 Vector3 对象 + /// + /// 向量1 + /// 向量2 + /// 向量3 + /// 向量4 + 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; + } + /// + /// 通过方法实现通过索引的访问 + /// + /// + 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"); + } + } + } + /// + /// // 重写 ToString 方法，以便能够直接打印矩阵 + /// + 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; } + /// + /// [数组下标]方式 访问XYZ属性 + /// + /// + /// [0]X [1]Y [2]Z + /// 访问下标0-2 + 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); } } + /// + /// 构造 初始化 + /// + /// x坐标 + /// y坐标 + /// z坐标 + public Vector3(double x, double y, double z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + /// + /// 重载二元坐标加法+ 向量+向量 + /// + /// 向量加数 + /// 向量加数 + /// + public static Vector3 operator +(Vector3 v1, Vector3 v2) + { + return new Vector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); + } + /// + /// 重载一元坐标加法+ 向量+小数 + /// + /// 向量 + /// 小数 + /// + public static Vector3 operator +(Vector3 v1, double i) + { + return new Vector3(v1.X + i, v1.Y + i, v1.Z + i); + } + /// + /// 重载一元坐标加法+ 向量+整数 + /// + /// 向量 + /// 整数 + /// + public static Vector3 operator +(Vector3 v1, int i) + { + return new Vector3(v1.X + (double)i, v1.Y + (double)i, v1.Z + (double)i); + } + /// + /// 重载二元坐标减法- 向量-向量 + /// + /// 向量被减数 + /// 向量减数 + /// + public static Vector3 operator -(Vector3 v1, Vector3 v2) + { + return new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + } + /// + /// 重载一元坐标加法- 向量-小数 + /// + /// 向量被减数 + /// 小数减数 + /// + public static Vector3 operator -(Vector3 v1, double i) + { + return new Vector3(v1.X - i, v1.Y - i, v1.Z - i); + } + /// + /// 重载一元坐标加法- 向量-整数 + /// + /// 向量被减数 + /// 整数减数 + /// + public static Vector3 operator -(Vector3 v1, int i) + { + return new Vector3(v1.X - (double)i, v1.Y - (double)i, v1.Z - (double)i); + } + /// + /// 重载一元坐标乘法* 向量*小数 + /// + /// 向量 + /// 小数乘数 + /// 得到向量的倍数向量 + public static Vector3 operator *(Vector3 v1, double i) + { + return new Vector3(v1.X * i, v1.Y * i, v1.Z * i); + } + /// + /// 重载一元坐标乘法* 向量*整数 + /// + /// 向量 + /// 整数乘数 + /// 得到向量的倍数向量 + public static Vector3 operator *(Vector3 v1, int i) + { + return new Vector3(v1.X * (double)i, v1.Y * (double)i, v1.Z * (double)i); + } + /// + /// 重载一元坐标除法/ 向量/小数 + /// + /// 向量 + /// 小数除数 + /// 得到向量的商向量 + public static Vector3 operator /(Vector3 v1, double i) + { + return new Vector3(v1.X / i, v1.Y / i, v1.Z / i); + } + /// + /// 重载一元坐标除法/ 向量/整数 + /// + /// 向量 + /// 整数除数 + /// 得到向量的商向量 + public static Vector3 operator /(Vector3 v1, int i) + { + return new Vector3(v1.X / (double)i, v1.Y / (double)i, v1.Z / (double)i); + } + /// + /// 重载== 向量==向量 + /// + /// 向量1 + /// 向量2 + /// 布尔值 判断向量是否相等 + 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; + } + /// + /// 重载！= 向量！=向量 + /// + /// 向量1 + /// 向量2 + /// 布尔值 判断向量是否不相等 + 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; + } + /// + /// 确定指定的对象是否等于当前向量。 + /// + /// 要与当前向量比较的对象。 + /// 如果指定的对象等于当前向量，则为 true；否则为 false。 + 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; + } + /// + /// 向量按矩阵旋转和偏移 + /// + /// 矩阵 + /// 返回一个新的向量 + 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; + } + /// + /// 求模长 + /// + /// 向量到原点的模长 + public double GetMag() + { + return Math.Sqrt(Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2)); + } + /// + /// 求模长 平方 + /// + /// 向量到原点的模长的平方值 + public double GetMagSquared() + { + return Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2); + } + /// + /// 标准化坐标 无返回值 直接改变愿坐标 + /// + /// 标准化单位 + 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; + } + } + /// + /// 标准化 返回一个标准化之后的值 不改变自身 + /// + /// 标准化单位 + /// 标准化之后的值 + 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; + } + /// + /// 归零 改变自身数值 一般配合归位使用 + /// + /// + public void SetZero(Vector3 v2) + { + this.X -= v2.X; + this.Y -= v2.Y; + this.Z -= v2.Z; + } + /// + /// 归零 返回一个归零值 不改变自身 + /// + /// + /// + public Vector3 SetZeroEd(Vector3 v2) + { + Vector3 re = new Vector3(this.X - v2.X, this.Y - v2.Y, this.Z - v2.Z); + return re; + } + /// + /// 归位 + /// + /// + public void SetFormerly(Vector3 v2) + { + this.X += v2.X; + this.Y += v2.Y; + this.Z += v2.Z; + } + /// + /// 重写ToString 打印坐标 + /// + /// 坐标字符串 + 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}"); + } + /// + /// 哈希码是一个整数值，用于对对象进行快速比较和索引 + /// + /// 哈希码 + 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 + { + /// + /// 航线 线间距 平方值 + /// + public static double LineDistanceSquare { get; set; } = 32400; + /// + /// 飞行过程中间距 平方值 + /// + public static double SpaceBetweenSquare { get; set; } = 90000; + /// + /// 输出日志回调函数 + /// + /// 日志内容 + public delegate void SomeCalculateWay(string str); + /// + /// 输出进度日志回调函数 + /// + /// 进度值ps:0-100 + public delegate void Schedule(int val); + /// + /// Arraylist 转 Vector3[] 坐标集 + /// + /// Arraylist 坐标集 + /// Vector3[] 坐标集 + 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; + } + /// + /// 取数组最大 或者最小值 + /// + /// 数组 + /// true返回最大值 false返回最小值 + /// 根据参数返回 最大或者最小值 + private static double GetMaxOrMin(double[] arr, bool isMax = true) + { + Array.Sort(arr);//给数组arr排序 + if (isMax) + return arr[arr.Length - 1]; + else + return arr[0]; + } + /// + /// 取数组最大 或者最小值 的数组下标 + /// + /// 数组 + /// true返回最大值 false返回最小值 + /// 数组下标 + 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]; + } + /// + /// 获取列表中最小值的随机下标 + /// + /// 输入的列表 + /// 最小值的随机下标 + private static int GetRandomMinIndex(List list) + { + // 检查输入的列表是否为 null 或为空 + if (list == null || list.Count == 0) + { + throw new ArgumentException("列表不能为 null 或为空"); + } + + int minValue = int.MaxValue; // 初始化为 int 类型的最大值 + List minIndices = new List(); // 存储最小值的下标列表 + + 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)]; + } + /// + /// 二维数组转一维数组 并去重 + /// + /// 二维数组 + /// 去重一维数组 + private static List TwoArrToArr(List twoArr) + { + // 创建一个用于存储去重后的一维数组的列表 + List arr = new List(); + // 遍历二维数组的每个子数组 + 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; + } + /// + /// 获取交叉序列 ps:处理二维数组 把有关联的子数组合并 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]] + /// + /// 需要处理的二维数组 + /// 交叉序列 + private static List> FindConnected(List arr) + { + Dictionary> graph = new Dictionary>(); + Dictionary visited = new Dictionary(); + List> result = new List>(); + + // 构建图 + foreach (var edge in arr) + { + foreach (var node in edge) + { + if (!graph.ContainsKey(node)) + { + graph[node] = new List(); + 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 connected = new List(); + DFS(node, connected, graph, visited); + result.Add(connected); + } + } + return result; + } + private static void DFS(int node, List connected, Dictionary> graph, Dictionary visited) + { + visited[node] = true; + connected.Add(node); + + foreach (var neighbor in graph[node]) + { + if (!visited[neighbor]) + { + DFS(neighbor, connected, graph, visited); + } + } + } + /// + /// 获取一组序列的所有排列方式 ps:[0,1,2] 结果[[0, 1, 2],[0, 2, 1],[1, 0, 2],[1, 2, 0],[2, 0, 1],[2, 1, 0]] + /// + /// 一组序列 + /// 所有序列的排列方式 + private static List> Permutations(List array) + { + List> result = new List>(); + GeneratePermutations(array, 0, array.Count - 1, result); + return result; + } + private static void GeneratePermutations(List array, int start, int end, List> result) + { + if (start == end) + { + result.Add(new List(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; + } + } + } + /// + /// 按照对应关系 生成新的b坐标集合 + /// + /// a坐标集合 + /// b坐标集合 + /// a b集合的对应关系 + /// 坐标集合 + private static Vector3[] CreateNewBVecs(Vector3[] bVecs, List match) + { + Vector3[] new_bVecs = new Vector3[bVecs.Length]; + foreach (int[] m in match) + { + new_bVecs[m[0]] = bVecs[m[1]]; + } + return new_bVecs; + } + /// + /// 从一组向量集合中 不重复的随机选择4个向量为一组 最多100组 组合成二维数组 如:[[1,2,3,4][5,6,7,8]...] + /// + /// 坐标集和 + /// re空数组则代表不够4个坐标 + private static List RandomSel4Vec(Vector3[] vecs) + { + List result = new List(); + int len = vecs.Length; + // 如果坐标数组少于4个，或者为空，则返回空列表 + if (len < 4) + { + return result; + } + // 确定最大可选择的组数，最多为100组 + int numGroups = Math.Min(len / 4, 100); + List flattenedList = vecs.ToList(); + Random random = new Random(); + // 遍历每一组 + for (int i = 0; i < numGroups; i++) + { + List selectedGroup = new List(); + + // 随机选择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; + } + /// + /// 设置中间航点 + /// + /// 起点 + /// 目标点 + /// 比例 + /// + private static Vector3 SetMiddleVec(Vector3 aVec, Vector3 bVec, double middlePos = 0.5) + { + return (bVec - aVec) * middlePos + aVec; + } + /// + /// 两点距离 + /// + /// 坐标1 + /// 坐标2 + /// 两点之间距离 + private static double GageLength(Vector3 v1, Vector3 v2) + { + return Math.Sqrt(GageLengthSquare(v1, v2)); + } + /// + /// 两点距离的平方 + /// + /// 坐标1 + /// 坐标2 + /// 两点距离的平方 + 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); + } + /// + /// 点积 + /// + /// 向量1 + /// 向量2 + /// 点积 + private static double DotPro(Vector3 v1, Vector3 v2) + { + return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z; + } + /// + /// 叉积 + /// + /// 向量1 + /// 向量2 + /// 两个向量叉积 + 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); + } + /// + /// 计算两个向量之间的夹角 角度 + /// + /// 第一个向量 + /// 第二个向量 + /// 角度 + public static double AngleBetween(Vector3 v1, Vector3 v2) + { + // 计算点积 + double dotProduct = DotPro(v1, v2); + + // 计算向量长度 + double magnitude1 = v1.GetMag(); + double magnitude2 = v2.GetMag(); + + // 使用 Atan2 计算弧度 + double thetaRadians = Math.Atan2(CrossPro(v1, v2).GetMag(), dotProduct); + + // 弧度转角度 + double thetaDegrees = thetaRadians * (180 / Math.PI); + + return thetaDegrees; + } + /// + /// 检查4个点是否在一个平面上 + /// + /// 点1 + /// 点2 + /// 点3 + /// 点4 + /// true在一个平面 false不在一个平面 + public static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4) + { + //计算三个向量 + Vector3 v1v2 = vec2 - vec1; + Vector3 v1v3 = vec3 - vec1; + Vector3 v1v4 = vec4 - vec1; + //计算法线向量 + Vector3 normal_vector = CrossPro(v1v2,v1v3); + //计算点到平面的距离 + double distance = DotPro(normal_vector, v1v4) / normal_vector.GetMag(); + //设置一个阈值，判断是否共面 + double epsilon = 10; //单位厘米 + return Math.Abs(distance) < epsilon; + } + /// + /// 判断3个点是否在同一条直线上 + /// + /// 点1 + /// 点2 + /// 点3 + /// true在一条直线上 false不在一条直线上 + 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; + } + /// + /// 从顶视图 判断点是否在两条内之间 + /// + /// 线段1端点 + /// 线段1端点 + /// 线段2端点 + /// 线段2端点 + /// 被判断点 + /// true点在两条线内部 false点不在两条线内部 + 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; + } + /// + /// 辅助方法，用于检查向量是否为零向量 + /// + /// 向量 + /// + private static bool IsZero(this Vector3 vector) + { + return vector.X == 0 && vector.Y == 0 && vector.Z == 0; + } + /// + /// 获取两条线段 的最近位置的距离和占比 + /// + /// 线段1起始点 + /// 线段1起终点 + /// 线段2起始点 + /// 线段2起终点 + /// [在线段1占比，在线段2占比，最近距离] + 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; + } + /// + /// 按比例在两条线段上截取对应点间的最小距离 平方 + /// + /// 线段1起始点 + /// 线段1起终点 + /// 线段2起始点 + /// 线段2起终点 + /// 最小距离的平方值 + 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(); + } + /// + /// 获取坐标集合的重心或中心 + /// + /// 坐标集合 + /// 默认返回为true重心 false则为中心 + /// 重心或中心坐标 + 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); + } + } + /// + /// 获取坐标集合的重心或中心 + /// + /// 坐标集合 + /// 默认返回为true重心 false则为中心 + /// 重心或中心坐标 + private static Vector3 GetPosCenter(List 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); + } + } + /// + /// 获取坐标集合 的总宽度 高度 长度 + /// + /// 坐标集合 + /// 返回数组[0]宽度[1]高度[2]长度 + public static double[] GetVecsWithHighLength(Vector3[] pos) + { + List w = new List(); + List h = new List(); + List l = new List(); + 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; + } + /// + /// 碰撞检测 + /// + /// 始点坐标集合 + /// 终点坐标集合 + /// + private static List AirImitation(Vector3[] aVecs, Vector3[] bVecs) + { + List planesCollision = new List(); //所有碰撞的组 + 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; + } + /// + /// 碰撞检测 + /// + /// 始点坐标集合 + /// 终点坐标集合 + /// + private static List AirImitation(List aVecs, List bVecs) + { + List planesCollision = new List(); //所有碰撞的组 + 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; + } + /// + /// 碰撞检测 + /// + /// 始点坐标集合 + /// 终点坐标集合 + /// + private static List AirImitation(Vector3[] aVecs , List bVecs) + { + List planesCollision = new List(); //所有碰撞的组 + 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; + } + /// + /// 碰撞检测 + /// + /// 始点坐标集合 + /// 终点坐标集合 + /// + private static List AirImitation( List aVecs, Vector3[] bVecs) + { + List planesCollision = new List(); //所有碰撞的组 + 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; + } + /// + /// 单机碰撞检测 + /// + /// 飞机的id PS:id从0开始 + /// 飞机起始坐标集合 + /// 飞机终点坐标集合 + /// true:有碰撞 false:无碰撞 + 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;//返回没有碰撞; + } + /// + /// 单机碰撞检测 + /// + /// 飞机的id PS:id从0开始 + /// 飞机起始坐标集合 + /// 飞机终点坐标集合 + /// true:有碰撞 false:无碰撞 + private static bool OnlyImitation(int onlyPlaneId, List aVecs, List 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;//返回没有碰撞; + } + /// + /// 单机碰撞检测 + /// + /// 飞机的id PS:id从0开始 + /// 飞机起始坐标集合 + /// 飞机终点坐标集合 + /// true:有碰撞 false:无碰撞 + private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, List 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;//返回没有碰撞; + } + /// + /// 单机碰撞检测 + /// + /// 飞机的id PS:id从0开始 + /// 飞机起始坐标集合 + /// 飞机终点坐标集合 + /// true:有碰撞 false:无碰撞 + private static bool OnlyImitation(int onlyPlaneId, List 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;//返回没有碰撞; + } + /// + /// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换 + /// + /// 起始点坐标组 + /// 目标点坐标组 + /// 日志输出 回调函数 + /// 交叉航线是否进行交换 + /// 交换次数 + /// 交叉线路数量上限 ps:超过这个数量则不进行交换 + /// 新的目标点 + public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, bool isSwap = true, int swapCount = 5, int crossingLimit = 6) + { + long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds(); + StrPrint("-------智能选择路径计算，开始-------"); + int planeCou = aVecs.Length; // 飞机总数 + List match = new List(); // ab对应关系 + ///记录a b集合索引 + List aIndex = new List(); + List bIndex = new List(); + 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 allVecs = new List(); + for (int i = 0; i < remainCou; i++) + { + allVecs.Add(aVecs[aIndex[i]]); + allVecs.Add(bVecs[bIndex[i]]); + } + Vector3 centerVec = GetPosCenter(allVecs);//重心点 + // 遍历所有ab点距离重心点的距离 + double[] aLens = new double[remainCou]; + double[] bLens = new double[remainCou]; + for (int i = 0; i < remainCou; i++) + { + aLens[i] = GageLengthSquare(aVecs[aIndex[i]], centerVec); + bLens[i] = GageLengthSquare(bVecs[bIndex[i]], centerVec); + } + // 找出ab集合最外层坐标的下标 即离重心点最远的点 + int aMaxIndex = GetIndexOfMaxOrMin(aLens); // a集合到重心点最长的距离 数组的下标 + int bMaxIndex = GetIndexOfMaxOrMin(bLens); // b集合到重心点最长的距离 数组的下标 + if (aLens[aMaxIndex] > bLens[bMaxIndex])//找出 最外层如果为A集合点 对应B集合最近点 的下标 + { + double[] outAtoBLen = new double[remainCou];//最外层A点到 B集合所有点的距离 + for (int i = 0; i < remainCou; i++) + { + outAtoBLen[i] = GageLengthSquare(aVecs[aIndex[aMaxIndex]], bVecs[bIndex[i]]); + } + int bMinIndex = GetIndexOfMaxOrMin(outAtoBLen, false);// 最短距离 + match.Add(new int[] { aIndex[aMaxIndex], bIndex[bMinIndex] });// 映射到配对 + aIndex.RemoveAt(aMaxIndex); // 删除已经配对的a集合 ID + bIndex.RemoveAt(bMinIndex); // 删除已经配对的b集合 ID + } + else//找出 最外层如果为B集合点 对应A集合最近点 的下标 + { + double[] outBtoALen = new double[remainCou];//最外层B点到 A集合所有点的距离 + for (int i = 0; i < remainCou; i++) + { + outBtoALen[i] = GageLengthSquare(aVecs[aIndex[i]], bVecs[bIndex[bMaxIndex]]); + } + int aMinIndex = GetIndexOfMaxOrMin(outBtoALen, false);// 最短距离 + match.Add(new int[] { aIndex[aMinIndex], bIndex[bMaxIndex] });// 映射到配对 + aIndex.RemoveAt(aMinIndex); // 删除已经配对的a集合 ID + bIndex.RemoveAt(bMaxIndex); // 删除已经配对的b集合 ID + } + } + Vector3[] new_bVecs = CreateNewBVecs(bVecs, match);// 按照映射 获取a 对应的 新的b集合 + if (!isSwap) + { + return new_bVecs; + } + ///交叉 交换 + for (int i = 0; i < swapCount; i++) + { + List planesCollision = AirImitation(aVecs, new_bVecs);// 获取碰撞组 + List> formatCollision = FindConnected(planesCollision);// 获取交叉序列 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]] + List> filteredCollision = formatCollision.Where(sublist => sublist.Count <= crossingLimit).ToList();// 过滤 只保留交叉数量小于等于crossingLimit的序列 + if (filteredCollision.Count == 0) break; + ///日志输出 + string log = ""; + foreach (List item in filteredCollision) + { + log += "["; + foreach (int itemInt in item) + { + log += $"{itemInt},"; + } + log += "]"; + } + StrPrint($"共迭代{swapCount}次交换，第{i}次。共{filteredCollision.Count}组，交换组为：{log}。"); + /// 遍历所有交叉组 分组做交换 + int cou = 0; + foreach (List swap_indices in filteredCollision) + { + cou++; + StrPrint($"进度：{cou}/{filteredCollision.Count}。交换组：[{string.Join(", ", swap_indices)}]。"); + ///交叉 生成所有排列 + List> all_permutations = Permutations(swap_indices);//所有排列组合 + List original = all_permutations[0];//原始排列 + all_permutations.RemoveAt(0);//删掉第一个 既原始排列 + /// 按所有的排列 互换航线 并检测出最佳的对应目标点 + List tempLen = new List(); //记录最少碰撞的 排列 + List tempNew_bVecsS = new List();//记录最少碰撞的 排列交换之后的目标坐标集 + foreach (List indices in all_permutations) + { + Vector3[] current_array = new Vector3[planeCou]; + Array.Copy(new_bVecs, current_array, planeCou);//复制一个new_bVecs 副本 + for (int k = 0; k < indices.Count; k++) + { + current_array[original[k]] = new_bVecs[indices[k]]; + } + ///把最少碰撞的排列 录入到数组 + int collisionsCou = (AirImitation(aVecs, current_array)).Count;//此排列的碰撞次数 + if (tempLen.Count == 0 || tempLen[0] == collisionsCou)//如果第一次添加 或者 碰撞次数等于最少碰撞 + { + ///录入数组 + tempLen.Add(collisionsCou); + tempNew_bVecsS.Add(current_array); + } + else if (tempLen[0] > collisionsCou) + { + /// 如果最小值 大于 当前碰撞次数，清空之前记录 + tempLen.Clear(); + tempNew_bVecsS.Clear(); + ///录入数组 + tempLen.Add(collisionsCou); + tempNew_bVecsS.Add(current_array); + } + } + new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)]; + } + } + + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------智能选择路径计算，结束-------"); + return new_bVecs; + } + /// + /// 智能错层 + /// + /// 起始坐标集合 + /// 终点做标集合 + /// 起始坐标航点名称 + /// 终点坐标航点名称 + /// 日志输出 回调函数 + /// 错层层高 + /// 返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点 + public static List> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,string aName,string bName ,SomeCalculateWay StrPrint, double layHight = 220) + { + long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds(); + StrPrint("-------错层，开始-------"); + List> re = new List>(); + ///判断有没有碰撞 有碰撞继续 没有直接返回 + List planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组 + if (planesCollision.Count == 0) + { + StrPrint("没有检测到碰撞，故不用添加中间航点"); + StrPrint($"-------错层结束-------"); + return re;//直接返回 + } + //获取飞机总数 + int planeCou = aVecs.Length; + if (planeCou < 2)//至少不少于2架飞机 才开始错层 + { + StrPrint("图案至少要两个点阵！"); + return re; + } + + ///从ab图案中取最外层3个点 确定一个面 ps:方便后续叉积算法线标量 + Vector3 vec0 = new Vector3(0, 0, 0); //记录最外圈一个坐标 + Vector3 vec1 = new Vector3(0, 0, 0); //记录离最外圈坐标最远的一个点坐标 + Vector3 vec2 = new Vector3(0, 0, 0); //最外圈距离 和最远距离 之和最远的一个点 + // 求重心 + List allVecs = new List(); + for (int i = 0; i < planeCou; i++) + { + allVecs.Add(aVecs[i]); + allVecs.Add(bVecs[i]); + } + Vector3 centerVec = GetPosCenter(allVecs);//重心点 + // 遍历所有ab点 + double tempLong = 0; + double currentLong; + for (int i = 0; i < planeCou * 2; i++) //取最外圈一个坐标 + { + currentLong = GageLength(allVecs[i], centerVec); + if (tempLong < currentLong) + { + vec0 = allVecs[i]; + tempLong = currentLong; + } + } + tempLong = 0; + for (int i = 0; i < planeCou * 2; i++) //取最外圈点 + { + currentLong = GageLength(allVecs[i], vec0); + if (tempLong < currentLong) + { + vec1 = allVecs[i]; + tempLong = currentLong; + } + } + tempLong = 0; + for (int i = 0; i < planeCou * 2; i++) //取最外圈点 + { + currentLong =(GageLength(allVecs[i], vec0) + GageLength(allVecs[i], vec1)); + if (tempLong < currentLong) + { + vec2 = allVecs[i]; + tempLong = currentLong; + } + } + ///遍历两个图形所有点 是否在一个平面上 + string aNoPlaneSN= "";//记录a图案所有不在一个平面上的点 + string bNoPlaneSN = "";//记录b图案所有不在一个平面上的点 + for (int i = 0; i < planeCou; i++) + { + if (!IsVecsOnPlane(vec0, vec1, vec2, aVecs[i])) aNoPlaneSN += $"{i+1},"; + if (!IsVecsOnPlane(vec0, vec1, vec2, bVecs[i])) bNoPlaneSN += $"{i+1},"; + } + if (aNoPlaneSN != "" || bNoPlaneSN != "") + { + if (aNoPlaneSN != "") StrPrint($"“{aName}”:{aNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!"); + if (bNoPlaneSN != "") StrPrint($"“{bName}”:{bNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!"); + StrPrint($"-------错层,结束-------"); + return re;//两个图形不在一个平面上 返回 空数组 + } + ///检查完毕后 + //计算法线向量 + Vector3 side1 = vec1 - vec0; + Vector3 side2 = vec2 - vec0; + Vector3 normal = CrossPro(side1, side2); + Vector3 normalScalar = normal.NormalizEd();//法线标量 + //开始错层 + for (int i = 0; i < planeCou; i++) + { + int shiftCou = 1; //记录循环次数 即层数 + Vector3 aOrigin = aVecs[i]; //原点位置 + Vector3 bOrigin = bVecs[i]; //原点位置 + while (OnlyImitation(i, aVecs, bVecs)) + { + Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight); + if (shiftCou % 2 == 1) + { + aVecs[i] = aOrigin + shiftVec; + bVecs[i] = bOrigin + shiftVec; + } + else + { + aVecs[i] = aOrigin - shiftVec; + bVecs[i] = bOrigin - shiftVec; + } + shiftCou += 1; + } + } + re.Add(aVecs.ToList()); + re.Add(bVecs.ToList()); + StrPrint($"错层成功"); + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------错层结束-------"); + return re; + } + /// + /// 路径绕行 + /// + /// 起始坐标集合 + /// 终点做标集合 + /// 日志输出 回调函数 + /// 返回一个二维数组 返回值长度0没有检测到碰撞或绕行失败 长度1为一个中间航点 长度为3为三个中间航点顺序(前中后) + public static List> ABypassB(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, Schedule GetVal) + { + long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds(); + StrPrint("-------3D绕行，开始-------"); + List> re = new List>(); + ///判断有没有碰撞 有碰撞继续 没有直接返回 + List planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组 + if (planesCollision.Count == 0) + { + StrPrint("没有检测到碰撞，故不用添加中间航点"); + StrPrint($"-------3D绕行，结束-------"); + return re;//直接返回 + } + ///飞机总数 + int planeCou = aVecs.Length; + ///第一次绕行 中间1航点 + List collisionGroup = TwoArrToArr(planesCollision); //整合数组 + List middleVecs = new List(); //中心航点坐标组 + for (int i = 0; i < planeCou; i++) + { + middleVecs.Add(SetMiddleVec(aVecs[i], bVecs[i])); //添加默认中间航点 + } + for (int c = 0; c < 2; c++) + { + foreach (int i in collisionGroup)//开始绕碰撞组 + { + List grv = GetRingVec(aVecs[i], bVecs[i], 0.5, 40, 4);//中间可绕行航点列表 + foreach (Vector3 v in grv) + { + middleVecs[i] = v; + if (!OnlyImitation(i, aVecs, middleVecs) && !(OnlyImitation(i, middleVecs, bVecs))) + { + break; + } + } + } + planesCollision = AirImitation(aVecs, middleVecs).Concat(AirImitation(middleVecs, bVecs)).ToList(); //获取碰撞组 + collisionGroup = TwoArrToArr(planesCollision); //整合数组 + } + //没有碰撞 返回一个中间航点 并返回 + if (collisionGroup.Count == 0) + { + StrPrint("第一次绕行成功！"); + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------3D绕行，结束-------"); + re.Add(middleVecs); + return re; + } + else + { + StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！"); + StrPrint("第一次绕行未成功，准备开始第二次绕行!"); + } + + ///第二次绕行 两头 两航点 + bool isPassMark = false; + planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组 + collisionGroup = TwoArrToArr(planesCollision); //整合数组 + List secondMiddleVecsOne = new List(); //中心航点坐标组1 + List secondMiddleVecsTwo = new List(); //中心航点坐标组2 + for (int i = 0; i < planeCou; i++) + { + secondMiddleVecsOne.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.1)); //添加中间航点1 + secondMiddleVecsTwo.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.9)); //添加中间航点2 + } + for (int c = 0; c < 2; c++) + { + int progress = 0;//进度 + foreach (int i in collisionGroup)//开始绕碰撞组 + { + progress++; + //StrPrint($"迭代{c}次{i}号绕行"); + List sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0.1, 40, 8);//中间可绕行航点列表 + List sgrv2 = GetRingVec(aVecs[i], bVecs[i], 0.9, 40, 8);//中间可绕行航点列表 + //if (sgrv1.Count > 250) sgrv1.RemoveRange(250, sgrv1.Count - 250); + //if (sgrv2.Count > 250) sgrv2.RemoveRange(250, sgrv2.Count - 250); + StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{Math.Pow(sgrv2.Count,2)}次"); + foreach (Vector3 v1 in sgrv1) + { + secondMiddleVecsOne[i] = v1; + foreach (Vector3 v2 in sgrv2) + { + secondMiddleVecsTwo[i] = v2; + if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs)) + { + isPassMark = true; + break; + } + isPassMark = false; + } + if (isPassMark) + { + break; //不撞则跳出 进行下一航线的绕行 + } + } + } + planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, secondMiddleVecsTwo)).ToList(); //获取碰撞组 + planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList(); + collisionGroup = TwoArrToArr(planesCollision); //整合数组 + } + //没有碰撞 返回两个中间航点 并返回 + if (collisionGroup.Count == 0) + { + StrPrint("第二次绕行成功！"); + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------3D绕行，结束-------"); + re.Add(secondMiddleVecsOne); + re.Add(secondMiddleVecsTwo); + return re; + } + else + { + StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！"); + StrPrint("第二次绕行未通过，准备开始第三次绕行!"); + } + + ///第三次绕行 两头 两航点 中间一行点 沿用第二次绕行 + isPassMark = false; + List thirdMiddleVecs = new List(); //中心航点坐标组 + for (int i = 0; i < planeCou; i++) + { + thirdMiddleVecs.Add(SetMiddleVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5)); //添加中间航点(保持二次绕行的两端航点 在两端航点中间添加) + } + for (int c = 0; c < 2; c++) + { + int progress = 0;//进度 + foreach (int i in collisionGroup)//开始绕碰撞组 + { + progress++; + //StrPrint($"迭代{c}次{i}号绕行"); + List sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0.1, 220, 8);//中间可绕行航点列表 + List sgrv2 = GetRingVec(aVecs[i], bVecs[i], 0.9, 220, 8);//中间可绕行航点列表 + List grv = GetRingVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5, 220, 4);//中间可绕行航点列表 + //if (sgrv1.Count > 300) sgrv1.RemoveRange(300, sgrv1.Count - 300); + //if (sgrv2.Count > 300) sgrv2.RemoveRange(300, sgrv2.Count - 300); + //if (grv.Count > 300) grv.RemoveRange(300, grv.Count - 300); + StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{Math.Pow(sgrv2.Count, 3)}次"); + foreach (Vector3 vm in grv) + { + thirdMiddleVecs[i] = vm; + foreach (Vector3 v1 in sgrv1) + { + secondMiddleVecsOne[i] = v1; + foreach (Vector3 v2 in sgrv2) + { + secondMiddleVecsTwo[i] = v2; + if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, thirdMiddleVecs ) && !OnlyImitation(i, thirdMiddleVecs, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs)) + { + isPassMark = true; + break; + } + isPassMark = false; + } + if (isPassMark) + { + break; //不撞则跳出 进行下一航线的绕行 + } + } + if (isPassMark) + { + break; //不撞则跳出 进行下一航线的绕行 + } + } + } + planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, thirdMiddleVecs)).ToList(); //获取碰撞组 + planesCollision = planesCollision.Concat(AirImitation(thirdMiddleVecs, secondMiddleVecsTwo)).ToList(); + planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList(); + collisionGroup = TwoArrToArr(planesCollision); //整合数组 + } + //没有碰撞 返回三个中间航点 并返回 + if (collisionGroup.Count == 0) + { + StrPrint("第三次绕行成功！"); + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------3D绕行，结束-------"); + re.Add(secondMiddleVecsOne); + re.Add(thirdMiddleVecs); + re.Add(secondMiddleVecsTwo); + return re; + } + else + { + StrPrint($"{string.Join("号， ", collisionGroup)}号，有碰撞，共{collisionGroup.Count}架！"); + StrPrint("第三次绕行未成功，准备开始第四次绕行!"); + } + + ///end + t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t; + StrPrint($"用时：{t}秒"); + StrPrint($"-------3D绕行，结束-------"); + return re; + } + /// + /// 在圆圈上 用固定弦长手拉手 分割点 + /// + /// 大圆半径 + /// 固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集 + /// + private static List GetVecsOnCircle(double radius, double chordLength) + { + List vecs = new List(); + // 计算圆的周长 + 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; + } + /// + /// 获取a指向b的向量矩阵 ps:偏移次数 加1 会在终点前后往返偏移 + /// + /// a向量 + /// b向量 + /// 默认中点位置比例 + /// 偏移次数 从0开始,每+1在中点前后往返偏移 + /// 偏移层高 + /// 层排布方向 "retrun"前后堆叠 "forward"向前排列（如：起点向目标点方向） "backward"向后排列 + /// 矩阵 + private static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion, int offCount, double layHight,string direction) + { + /// 计算前向向量 k帽 + Vector3 k_hat = (bVec - aVec).NormalizEd(); + /// 计算右向量，使用 Vector3.UnitY 作为上向量 i帽 + //Vector3 i_hat = CrossPro(Vector3.UnitY, k_hat).NormalizEd(); //固定方向i帽 + Random random = new Random();// 生成一个随机的单位向量 + Vector3 randomVector = new Vector3((double)random.NextDouble(), (double)random.NextDouble(), (double)random.NextDouble()); + Vector3 i_hat = CrossPro(k_hat, randomVector).NormalizEd();// 随机方向i帽 + /// 计算上向量 j帽 + Vector3 j_hat = CrossPro(k_hat, i_hat); + ///计算 对应的偏移比例 + double length = (bVec - aVec).GetMag(); + double offShift = middleProportion;//偏移比例 + if (direction == "retrun") + { + if (offCount % 2 == 1) //奇数 + offShift = middleProportion + (offCount + 2) / 2 * layHight / length; + else //偶数 + offShift = middleProportion - (offCount + 1) / 2 * layHight / length; + } + else if (direction == "forward") + { + offShift = middleProportion + offCount * layHight / length; + } + else if(direction == "backward") + { + offShift = middleProportion - offCount * layHight / length; + } + + /// 计算偏向量 + Vector3 off = aVec + (bVec - aVec) * offShift; + + /// 构建矩阵 + Matrix matrix = new Matrix + { + M11 = k_hat.X, + M12 = k_hat.Y, + M13 = k_hat.Z, + M21 = i_hat.X, + M22 = i_hat.Y, + M23 = i_hat.Z, + M31 = j_hat.X, + M32 = j_hat.Y, + M33 = j_hat.Z, + M41 = off.X, + M42 = off.Y, + M43 = off.Z + }; + return matrix; + } + /// + /// a b点中间的绕行航点列表 + /// + /// 起点 + /// 目标点 + /// 中间航点位置比例 + /// 绕行航点密度(值越小密度越大) ps:传递圈函数的弦长 向量矩阵函数的层高 + /// 绕行航点范围(值越小范围越大) ps:决定层的厚度 和 圈的直径 为 paunch/航线长度 + /// 绕行航点列表 + private static List GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer, double paunch,bool singleCircle =true, string direction = "retrun") + { + List ringVec = new List(); //记录所有绕行中间航点坐标 + /// 根据a到b的长度 算出中间绕行几圈 + int ringCou = (int)Math.Ceiling(GageLength(aVec, bVec) / paunch / transfer); //算层数和圈数 ps:层的厚度 和 圈的直径 为 paunch/航线长度 + if (ringCou < 2) ringCou = 2; //最少两圈 两层 + /// 不是单圈的话 设置层数跟圈数相等 + int layCou = ringCou; + if (singleCircle) layCou = 1; + ///遍历出所有绕行航点 + for (int z = 0; z < layCou; z++) //迭代层 + { + Matrix mat = GetBasisMatrix(aVec, bVec, middleProportion, z, transfer, direction); //根据圈数越多 偏移层数也越多 即每层矩阵off位置 + for (int i = 0; i < ringCou; i++) //迭代圈 + { + if (i != 0) + { + List tempCi = GetVecsOnCircle(i * transfer, transfer); + foreach (Vector3 vec in tempCi) + { + ringVec.Add(vec.Multiply(mat));//按照矩阵旋转之后 添加到中间航点列表 + } + } + else + { + ringVec.Add(mat[3]); //第一次循环 并非圈 只在航线上 按层比例取点 即可 + } + } + } + return ringVec; + } + /// + /// 按照矩阵 拉散图案 + /// + /// 平面图案坐标组 + /// 回归矩阵坐标组 + /// 日志输出 回调函数 + /// 拉散图案的坐标组 + public static Vector3[] NormalPull(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint) + { + int planeCou = aVecs.Length; //获取飞机总数 + ///a图b图 中心 + Vector3 aCenterPos = GetPosCenter(aVecs, false); + Vector3 bCenterPos = GetPosCenter(bVecs, false); + ///判断bVec 矩阵的数量长宽 + for (int i = 0; i < planeCou; i++)//把矩阵高度压平 + { + bVecs[i] = new Vector3(bVecs[i].X, 0, bVecs[i].Z); + } + int row = 1 ; + for (int i = 0; i < planeCou-2; i++) + { + if (!(IsVecsOnLine(bVecs[i], bVecs[i + 1], bVecs[i + 2]))) + { + row = i+2;//列 + break; + } + } + int ran = (int)Math.Ceiling((double)planeCou/(double)row);//行 + ///计算a图的法线标量 + Vector3 side1 = aVecs[1] - aVecs[0]; + Vector3 side2 = aVecs[2] - aVecs[0]; + Vector3 normal = CrossPro(side1, side2); + Vector3 normalScalar = normal.NormalizEd();//法线标量 + normalScalar.Y = 0;//高度上压平 + ///判断a图 法线朝向 和 矩阵“平行”方向 获取此方向层数 ps:用于a图的拉散 + if (GageLength(normalScalar, bVecs[0]) < GageLength(normalScalar * -1, bVecs[0])) + { + normalScalar *= -1;//法线选择方向为 靠近矩阵1号机的方向 ps:由于取的三个点位置随机 按照右手定则 法线方向也随机 + } + if (Math.Abs(AngleBetween(bVecs[row] - bVecs[0], normalScalar) - 90) < Math.Abs(AngleBetween(bVecs[1] - bVecs[0], normalScalar) - 90)) + { + /// 图案“如平行于0 21 41..” 平行于列 + for (int k = 0; k < row; k++) + { + for (int i = 0; i < ran; i++) + { + int cou = i * row + k; + if (cou > planeCou) break;// 溢出跳出 + ///判断图在矩阵的左方 还是右方 + if (GageLength(aCenterPos, bVecs[0]) > GageLength(aCenterPos, bVecs[row - 1])) + { + aVecs[cou] -= normalScalar * (row - k) * 300;//左方 + } + else + { + aVecs[cou] += normalScalar * k * 300;//右方 + } + } + } + ///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散 + if (IsPointBetweenLines(bVecs[(int)Math.Ceiling(((double)row/5))-1], bVecs[(int)Math.Ceiling(((double)row / 5)) - 1+row], bVecs[row-((int)Math.Ceiling(((double)row / 5)) - 1)], bVecs[(row - ((int)Math.Ceiling(((double)row / 5)) - 1))+row], aCenterPos)) + { + aCenterPos = GetPosCenter(aVecs, false);//重新算下中心点 + Vector3 offPos = aCenterPos - bCenterPos;//偏移量 + for (int i = 0; i < planeCou; i++) + { + aVecs[i] -= offPos; + } + } + + } + else + { + /// 图案“如平行于 0-20” 平行于行 + for (int k = 0; k < ran; k++) + { + for (int i = 0; i < row; i++) + { + int cou = k * row + i; + if (cou > planeCou) break;// 溢出跳出 + ///判断图在矩阵的上方 还是下方 + if (GageLength(aCenterPos, bVecs[0]) > GageLength(aCenterPos, bVecs[row * (ran - 1)])) + { + aVecs[cou] -= normalScalar * (ran - k) * 300;//上方 + } + else + { + aVecs[cou] += normalScalar * k * 300;//下方 + } + } + } + ///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散 + if (IsPointBetweenLines(bVecs[(int)Math.Ceiling((double)ran / 5) * row - row], bVecs[(int)Math.Ceiling((double)ran / 5) * row - 1], bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row], bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row + row - 1], aCenterPos)) + { + aCenterPos = GetPosCenter(aVecs, false);//重新算下中心点 + Vector3 offPos = aCenterPos - bCenterPos;//偏移量 + for (int i = 0; i < planeCou; i++) + { + aVecs[i] -= offPos; + } + } + } + return aVecs; + } + } +} \ No newline at end of file diff --git a/FlightRouteV2/FlightRouteV2.csproj b/FlightRouteV2/FlightRouteV2.csproj new file mode 100644 index 0000000..da4f76c --- /dev/null +++ b/FlightRouteV2/FlightRouteV2.csproj @@ -0,0 +1,50 @@ + + + + + Debug + AnyCPU + {626A9BFA-07DE-4063-A178-360EB7057ED6} + Library + Properties + FlightRouteV2 + FlightRouteV2 + v4.8 + 512 + true + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FlightRouteV2/Properties/AssemblyInfo.cs b/FlightRouteV2/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..331cefd --- /dev/null +++ b/FlightRouteV2/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 有关程序集的一般信息由以下 +// 控制。更改这些特性值可修改 +// 与程序集关联的信息。 +[assembly: AssemblyTitle("FlyBase")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("FlyBase")] +[assembly: AssemblyCopyright("Copyright © 2022")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// 将 ComVisible 设置为 false 会使此程序集中的类型 +//对 COM 组件不可见。如果需要从 COM 访问此程序集中的类型 +//请将此类型的 ComVisible 特性设置为 true。 +[assembly: ComVisible(false)] + +// 如果此项目向 COM 公开，则下列 GUID 用于类型库的 ID +[assembly: Guid("626a9bfa-07de-4063-a178-360eb7057ed6")] + +// 程序集的版本信息由下列四个值组成: +// +// 主版本 +// 次版本 +// 生成号 +// 修订号 +// +//可以指定所有这些值，也可以使用“生成号”和“修订号”的默认值 +//通过使用 "*"，如下所示: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/FlightRouteV2/Vector3.cs b/FlightRouteV2/Vector3.cs new file mode 100644 index 0000000..407a1ca --- /dev/null +++ b/FlightRouteV2/Vector3.cs @@ -0,0 +1,154 @@ +using System; + +namespace FlyBase +{ + public struct Vector3 + { + public double X { get; set; } + public double Y { get; set; } + public double Z { get; set; } + /// + /// 构造 初始化 + /// + /// x坐标 + /// y坐标 + /// z坐标 + public Vector3(double x, double y, double z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + //重载二元坐标加法+ + public static Vector3 operator +(Vector3 v1, Vector3 v2) + { + return new Vector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); + } + //重载一元坐标加法+ + public static Vector3 operator +(Vector3 v1, double i) + { + return new Vector3(v1.X + i, v1.Y + i, v1.Z + i); + } + //重载一元坐标加法+ + public static Vector3 operator +(Vector3 v1, int i) + { + return new Vector3(v1.X + (double)i, v1.Y + (double)i, v1.Z + (double)i); + } + //重载二元坐标减法- + public static Vector3 operator -(Vector3 v1, Vector3 v2) + { + return new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + } + //重载一元坐标加法- + public static Vector3 operator -(Vector3 v1, double i) + { + return new Vector3(v1.X - i, v1.Y - i, v1.Z - i); + } + //重载一元坐标加法- + public static Vector3 operator -(Vector3 v1, int i) + { + return new Vector3(v1.X - (double)i, v1.Y - (double)i, v1.Z - (double)i); + } + //重载一元坐标乘法* + public static Vector3 operator *(Vector3 v1, double i) + { + return new Vector3(v1.X * i, v1.Y * i, v1.Z * i); + } + //重载一元坐标乘法* + public static Vector3 operator *(Vector3 v1, int i) + { + return new Vector3(v1.X * (double)i, v1.Y * (double)i, v1.Z * (double)i); + } + //重载一元坐标除法/ + public static Vector3 operator /(Vector3 v1, double i) + { + return new Vector3(v1.X / i, v1.Y / i, v1.Z / i); + } + //重载一元坐标除法/ + public static Vector3 operator /(Vector3 v1, int i) + { + return new Vector3(v1.X / (double)i, v1.Y / (double)i, v1.Z / (double)i); + } + //重载== + 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; + } + //重载!= + 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; + } + //模长 + public double GetMag() + { + return Math.Sqrt(Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2)); + } + /// + /// 标准化坐标 + /// + /// 标准化单位 + 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; + } + } + /// + /// 标准化 返回一个标准化之后的值 不改变自身 + /// + /// 标准化单位 + /// 标准化之后的值 + 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; + } + //归零 改变自身数值 一般配合归位使用 + public void SetZero(Vector3 v2) + { + this.X -= v2.X; + this.Y -= v2.Y; + this.Z -= v2.Z; + } + //归零 返回一个归零值 不改变自身 + public Vector3 SetZeroEd(Vector3 v2) + { + Vector3 re = new Vector3(this.X - v2.X, this.Y - v2.Y, this.Z - v2.Z); + return re; + } + //归位 + public void SetFormerly(Vector3 v2) + { + this.X += v2.X; + this.Y += v2.Y; + this.Z += v2.Z; + } + /// + /// 打印坐标 + /// + /// 坐标字符串 + public string PosToString() + { + 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}"); + } + } +} \ No newline at end of file diff --git a/FlightRouteV2/bin/Debug/FlightRouteV2.dll b/FlightRouteV2/bin/Debug/FlightRouteV2.dll new file mode 100644 index 0000000..0cf5bbe Binary files /dev/null and b/FlightRouteV2/bin/Debug/FlightRouteV2.dll differ diff --git a/FlightRouteV2/bin/Debug/FlightRouteV2.pdb b/FlightRouteV2/bin/Debug/FlightRouteV2.pdb new file mode 100644 index 0000000..9808e34 Binary files /dev/null and b/FlightRouteV2/bin/Debug/FlightRouteV2.pdb differ diff --git a/FlightRouteV2/bin/Debug/FlyBase.dll b/FlightRouteV2/bin/Debug/FlyBase.dll new file mode 100644 index 0000000..88aa09c Binary files /dev/null and b/FlightRouteV2/bin/Debug/FlyBase.dll differ diff --git a/FlightRouteV2/bin/Debug/FlyBase.pdb b/FlightRouteV2/bin/Debug/FlyBase.pdb new file mode 100644 index 0000000..d63ecbc Binary files /dev/null and b/FlightRouteV2/bin/Debug/FlyBase.pdb differ diff --git a/FlightRouteV2/bin/Release/FlightRouteV2.dll b/FlightRouteV2/bin/Release/FlightRouteV2.dll new file mode 100644 index 0000000..b86bb34 Binary files /dev/null and b/FlightRouteV2/bin/Release/FlightRouteV2.dll differ diff --git a/FlightRouteV2/bin/Release/FlightRouteV2.pdb b/FlightRouteV2/bin/Release/FlightRouteV2.pdb new file mode 100644 index 0000000..d7856ee Binary files /dev/null and b/FlightRouteV2/bin/Release/FlightRouteV2.pdb differ diff --git a/FlightRouteV2/bin/Release/FlyBase.dll b/FlightRouteV2/bin/Release/FlyBase.dll new file mode 100644 index 0000000..9229221 Binary files /dev/null and b/FlightRouteV2/bin/Release/FlyBase.dll differ diff --git a/FlightRouteV2/bin/Release/FlyBase.pdb b/FlightRouteV2/bin/Release/FlyBase.pdb new file mode 100644 index 0000000..6adb3ca Binary files /dev/null and b/FlightRouteV2/bin/Release/FlyBase.pdb differ diff --git a/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.6.2.AssemblyAttributes.cs b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.6.2.AssemblyAttributes.cs new file mode 100644 index 0000000..edc4647 --- /dev/null +++ b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.6.2.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETFramework,Version=v4.6.2", FrameworkDisplayName = ".NET Framework 4.6.2")] diff --git a/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs new file mode 100644 index 0000000..3871b18 --- /dev/null +++ b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETFramework,Version=v4.7.2", FrameworkDisplayName = ".NET Framework 4.7.2")] diff --git a/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.8.AssemblyAttributes.cs b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.8.AssemblyAttributes.cs new file mode 100644 index 0000000..15efebf --- /dev/null +++ b/FlightRouteV2/obj/Debug/.NETFramework,Version=v4.8.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETFramework,Version=v4.8", FrameworkDisplayName = ".NET Framework 4.8")] diff --git a/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferences.cache b/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferences.cache new file mode 100644 index 0000000..f5e894a Binary files /dev/null and b/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferences.cache differ diff --git a/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache b/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache new file mode 100644 index 0000000..2939541 Binary files /dev/null and b/FlightRouteV2/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache differ diff --git a/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.AssemblyReference.cache b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.AssemblyReference.cache new file mode 100644 index 0000000..c58ab16 Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.AssemblyReference.cache differ diff --git a/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.CoreCompileInputs.cache b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.CoreCompileInputs.cache new file mode 100644 index 0000000..638701b --- /dev/null +++ b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.CoreCompileInputs.cache @@ -0,0 +1 @@ +254d0916f0a94457ded9d42398d6e5648a952309 diff --git a/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.FileListAbsolute.txt b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.FileListAbsolute.txt new file mode 100644 index 0000000..c41e345 --- /dev/null +++ b/FlightRouteV2/obj/Debug/FlightRouteV2.csproj.FileListAbsolute.txt @@ -0,0 +1,12 @@ +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Debug\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Debug\FlightRouteV2.pdb +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlightRouteV2.csproj.AssemblyReference.cache +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlightRouteV2.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlightRouteV2.pdb +F:\company\flyCube\repos\FlyCube\FlightRouteV2\bin\Debug\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlightRouteV2\bin\Debug\FlightRouteV2.pdb +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Debug\FlightRouteV2.csproj.AssemblyReference.cache +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Debug\FlightRouteV2.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Debug\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Debug\FlightRouteV2.pdb diff --git a/FlightRouteV2/obj/Debug/FlightRouteV2.dll b/FlightRouteV2/obj/Debug/FlightRouteV2.dll new file mode 100644 index 0000000..0cf5bbe Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlightRouteV2.dll differ diff --git a/FlightRouteV2/obj/Debug/FlightRouteV2.pdb b/FlightRouteV2/obj/Debug/FlightRouteV2.pdb new file mode 100644 index 0000000..9808e34 Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlightRouteV2.pdb differ diff --git a/FlightRouteV2/obj/Debug/FlyBase.csproj.AssemblyReference.cache b/FlightRouteV2/obj/Debug/FlyBase.csproj.AssemblyReference.cache new file mode 100644 index 0000000..528262c Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlyBase.csproj.AssemblyReference.cache differ diff --git a/FlightRouteV2/obj/Debug/FlyBase.csproj.CoreCompileInputs.cache b/FlightRouteV2/obj/Debug/FlyBase.csproj.CoreCompileInputs.cache new file mode 100644 index 0000000..1bc7f9a --- /dev/null +++ b/FlightRouteV2/obj/Debug/FlyBase.csproj.CoreCompileInputs.cache @@ -0,0 +1 @@ +65dcdc626d7d72bd83a6dbfb060115196969f69e diff --git a/FlightRouteV2/obj/Debug/FlyBase.csproj.FileListAbsolute.txt b/FlightRouteV2/obj/Debug/FlyBase.csproj.FileListAbsolute.txt new file mode 100644 index 0000000..5ed0bc4 --- /dev/null +++ b/FlightRouteV2/obj/Debug/FlyBase.csproj.FileListAbsolute.txt @@ -0,0 +1,12 @@ +F:\company\flyCube\repos\fly\FlyBase\bin\Debug\FlyBase.dll +F:\company\flyCube\repos\fly\FlyBase\bin\Debug\FlyBase.pdb +F:\company\flyCube\repos\fly\FlyBase\obj\Debug\FlyBase.csproj.AssemblyReference.cache +F:\company\flyCube\repos\fly\FlyBase\obj\Debug\FlyBase.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\fly\FlyBase\obj\Debug\FlyBase.dll +F:\company\flyCube\repos\fly\FlyBase\obj\Debug\FlyBase.pdb +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Debug\FlyBase.dll +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Debug\FlyBase.pdb +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlyBase.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlyBase.dll +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlyBase.pdb +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Debug\FlyBase.csproj.AssemblyReference.cache diff --git a/FlightRouteV2/obj/Debug/FlyBase.dll b/FlightRouteV2/obj/Debug/FlyBase.dll new file mode 100644 index 0000000..88aa09c Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlyBase.dll differ diff --git a/FlightRouteV2/obj/Debug/FlyBase.pdb b/FlightRouteV2/obj/Debug/FlyBase.pdb new file mode 100644 index 0000000..d63ecbc Binary files /dev/null and b/FlightRouteV2/obj/Debug/FlyBase.pdb differ diff --git a/FlightRouteV2/obj/Release/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs b/FlightRouteV2/obj/Release/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs new file mode 100644 index 0000000..3871b18 --- /dev/null +++ b/FlightRouteV2/obj/Release/.NETFramework,Version=v4.7.2.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETFramework,Version=v4.7.2", FrameworkDisplayName = ".NET Framework 4.7.2")] diff --git a/FlightRouteV2/obj/Release/.NETFramework,Version=v4.8.AssemblyAttributes.cs b/FlightRouteV2/obj/Release/.NETFramework,Version=v4.8.AssemblyAttributes.cs new file mode 100644 index 0000000..15efebf --- /dev/null +++ b/FlightRouteV2/obj/Release/.NETFramework,Version=v4.8.AssemblyAttributes.cs @@ -0,0 +1,4 @@ +// +using System; +using System.Reflection; +[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETFramework,Version=v4.8", FrameworkDisplayName = ".NET Framework 4.8")] diff --git a/FlightRouteV2/obj/Release/DesignTimeResolveAssemblyReferencesInput.cache b/FlightRouteV2/obj/Release/DesignTimeResolveAssemblyReferencesInput.cache new file mode 100644 index 0000000..41b170c Binary files /dev/null and b/FlightRouteV2/obj/Release/DesignTimeResolveAssemblyReferencesInput.cache differ diff --git a/FlightRouteV2/obj/Release/FlightRouteV2.csproj.AssemblyReference.cache b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.AssemblyReference.cache new file mode 100644 index 0000000..c58ab16 Binary files /dev/null and b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.AssemblyReference.cache differ diff --git a/FlightRouteV2/obj/Release/FlightRouteV2.csproj.CoreCompileInputs.cache b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.CoreCompileInputs.cache new file mode 100644 index 0000000..461b945 --- /dev/null +++ b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.CoreCompileInputs.cache @@ -0,0 +1 @@ +1aaa80ef5aa2c66232cf0fd45d64bc898b5ea44e diff --git a/FlightRouteV2/obj/Release/FlightRouteV2.csproj.FileListAbsolute.txt b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.FileListAbsolute.txt new file mode 100644 index 0000000..59aa564 --- /dev/null +++ b/FlightRouteV2/obj/Release/FlightRouteV2.csproj.FileListAbsolute.txt @@ -0,0 +1,6 @@ +F:\company\flyCube\repos\FlyCube\FlightRouteV2\bin\Release\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlightRouteV2\bin\Release\FlightRouteV2.pdb +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Release\FlightRouteV2.csproj.AssemblyReference.cache +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Release\FlightRouteV2.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Release\FlightRouteV2.dll +F:\company\flyCube\repos\FlyCube\FlightRouteV2\obj\Release\FlightRouteV2.pdb diff --git a/FlightRouteV2/obj/Release/FlightRouteV2.dll b/FlightRouteV2/obj/Release/FlightRouteV2.dll new file mode 100644 index 0000000..b86bb34 Binary files /dev/null and b/FlightRouteV2/obj/Release/FlightRouteV2.dll differ diff --git a/FlightRouteV2/obj/Release/FlightRouteV2.pdb b/FlightRouteV2/obj/Release/FlightRouteV2.pdb new file mode 100644 index 0000000..d7856ee Binary files /dev/null and b/FlightRouteV2/obj/Release/FlightRouteV2.pdb differ diff --git a/FlightRouteV2/obj/Release/FlyBase.csproj.AssemblyReference.cache b/FlightRouteV2/obj/Release/FlyBase.csproj.AssemblyReference.cache new file mode 100644 index 0000000..528262c Binary files /dev/null and b/FlightRouteV2/obj/Release/FlyBase.csproj.AssemblyReference.cache differ diff --git a/FlightRouteV2/obj/Release/FlyBase.csproj.CoreCompileInputs.cache b/FlightRouteV2/obj/Release/FlyBase.csproj.CoreCompileInputs.cache new file mode 100644 index 0000000..760a1fe --- /dev/null +++ b/FlightRouteV2/obj/Release/FlyBase.csproj.CoreCompileInputs.cache @@ -0,0 +1 @@ +a7be80f0590ce7e3d3292f0020daba81f87a9940 diff --git a/FlightRouteV2/obj/Release/FlyBase.csproj.FileListAbsolute.txt b/FlightRouteV2/obj/Release/FlyBase.csproj.FileListAbsolute.txt new file mode 100644 index 0000000..81f2923 --- /dev/null +++ b/FlightRouteV2/obj/Release/FlyBase.csproj.FileListAbsolute.txt @@ -0,0 +1,6 @@ +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Release\FlyBase.dll +F:\company\flyCube\repos\FlyCube\FlyBase\bin\Release\FlyBase.pdb +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Release\FlyBase.csproj.AssemblyReference.cache +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Release\FlyBase.csproj.CoreCompileInputs.cache +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Release\FlyBase.dll +F:\company\flyCube\repos\FlyCube\FlyBase\obj\Release\FlyBase.pdb diff --git a/FlightRouteV2/obj/Release/FlyBase.csprojAssemblyReference.cache b/FlightRouteV2/obj/Release/FlyBase.csprojAssemblyReference.cache new file mode 100644 index 0000000..d41e237 Binary files /dev/null and b/FlightRouteV2/obj/Release/FlyBase.csprojAssemblyReference.cache differ diff --git a/FlightRouteV2/obj/Release/FlyBase.dll b/FlightRouteV2/obj/Release/FlyBase.dll new file mode 100644 index 0000000..9229221 Binary files /dev/null and b/FlightRouteV2/obj/Release/FlyBase.dll differ diff --git a/FlightRouteV2/obj/Release/FlyBase.pdb b/FlightRouteV2/obj/Release/FlyBase.pdb new file mode 100644 index 0000000..6adb3ca Binary files /dev/null and b/FlightRouteV2/obj/Release/FlyBase.pdb differ diff --git a/FlyCube/MainWindow.xaml.cs b/FlyCube/MainWindow.xaml.cs index 80f11b4..f216fcc 100644 --- a/FlyCube/MainWindow.xaml.cs +++ b/FlyCube/MainWindow.xaml.cs @@ -35,6 +35,12 @@ namespace FlyCube InitializeComponent(); //创建一个 主画布对象 TopCanvas = new MainCanvas(this.LayerPlane); + Vector3 A = new Vector3(0,0,0); + Vector3 B = new Vector3(0, 0, 10); + Vector3 C = new Vector3(10, 0, 0); + Vector3 D = new Vector3(10, 0, 10); + Vector3 P = new Vector3(10, 0, 10); + //MessageBox.Show(FlyVecFun.IsPointBetweenLines(A, B, C, D, P).ToString()); } /// /// 3D绕行 @@ -51,7 +57,7 @@ namespace FlyCube Vector3[] aVecs = abVecs[0].ToArray(); Vector3[] bVecs = abVecs[1].ToArray(); //Vector3[] new_bVecs = FlyVecFun.ContactABOut(aVecs, bVecs, StrPrint); - Vector3[] new_bVecs = FlyVecFun.NormalPull(aVecs, bVecs); + Vector3[] new_bVecs = FlyVecFun.NormalPull(aVecs, bVecs, StrPrintAsync); //Task>> reTask = Task.Run(() => FlyVecFun.ABypassB(aVecs, bVecs, StrPrintAsync, GetVal)); //List> re = await reTask;