using Microsoft.SqlServer.Server;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Net.Http;
using System.Net.NetworkInformation;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
using System.Security.Policy;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Documents;
using System.Windows.Shapes;
using static System.Net.WebRequestMethods;

namespace FlightRouteV2
{
    //矩阵类
    public struct Matrix
    {
        public double M11, M12, M13;
        public double M21, M22, M23;
        public double M31, M32, M33;
        public double M41, M42, M43;
        /// <summary>
        /// 构造函数，接受四个 Vector3 对象
        /// </summary>
        /// <param name="row1">向量1</param>
        /// <param name="row2">向量2</param>
        /// <param name="row3">向量3</param>
        /// <param name="row4">向量4</param>
        public Matrix(Vector3 row1, Vector3 row2, Vector3 row3, Vector3 row4)
        {
            M11 = row1.X; M12 = row1.Y; M13 = row1.Z;
            M21 = row2.X; M22 = row2.Y; M23 = row2.Z;
            M31 = row3.X; M32 = row3.Y; M33 = row3.Z;
            M41 = row4.X; M42 = row4.Y; M43 = row4.Z;
        }
        /// <summary>
        /// 通过方法实现通过索引的访问
        /// </summary>
        /// <exception cref="IndexOutOfRangeException"></exception>
        public Vector3 this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0: return new Vector3(M11, M12, M13);
                    case 1: return new Vector3(M21, M22, M23);
                    case 2: return new Vector3(M31, M32, M33);
                    case 3: return new Vector3(M41, M42, M43);
                    default:
                        throw new IndexOutOfRangeException("Index out of range for Matrix");
                }
            }
        }
        /// <summary>
        /// // 重写 ToString 方法，以便能够直接打印矩阵
        /// </summary>
        public override string ToString()
        {
            return $"v1:{M11}, {M12}, {M13}\n" +
                   $"v2:{M21}, {M22}, {M23}\n" +
                   $"v3:{M31}, {M32}, {M33}\n" +
                   $"off:{M41}, {M42}, {M43}";
        }
    }
    //向量类
    public struct Vector3
    {
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
        /// <summary>
        /// [数组下标]方式 访问XYZ属性
        /// </summary>
        /// <param name="index"></param>
        /// <returns>[0]X [1]Y [2]Z</returns>
        /// <exception cref="IndexOutOfRangeException">访问下标0-2</exception>
        public double this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0:
                        return X;
                    case 1:
                        return Y;
                    case 2:
                        return Z;
                    default:
                        throw new IndexOutOfRangeException($"Index {index} is out of range for Vector3");
                }
            }
            set
            {
                switch (index)
                {
                    case 0:
                        X = value;
                        break;
                    case 1:
                        Y = value;
                        break;
                    case 2:
                        Z = value;
                        break;
                    default:
                        throw new IndexOutOfRangeException($"Index {index} is out of range for Vector3");
                }
            }
        }
        // 静态属性表示单位向量
        public static Vector3 UnitX { get { return new Vector3(1.0, 0.0, 0.0); } }
        public static Vector3 UnitY { get { return new Vector3(0.0, 1.0, 0.0); } }
        public static Vector3 UnitZ { get { return new Vector3(0.0, 0.0, 1.0); } }
        /// <summary>
        /// 构造 初始化
        /// </summary>
        /// <param name="x">x坐标</param>
        /// <param name="y">y坐标</param>
        /// <param name="z">z坐标</param>
        public Vector3(double x, double y, double z)
        {
            this.X = x;
            this.Y = y;
            this.Z = z;
        }
        /// <summary>
        /// 重载二元坐标加法+ 向量+向量
        /// </summary>
        /// <param name="v1">向量加数</param>
        /// <param name="v2">向量加数</param>
        /// <returns></returns>
        public static Vector3 operator +(Vector3 v1, Vector3 v2)
        {
            return new Vector3(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z);
        }
        /// <summary>
        /// 重载一元坐标加法+ 向量+小数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">小数</param>
        /// <returns></returns>
        public static Vector3 operator +(Vector3 v1, double i)
        {
            return new Vector3(v1.X + i, v1.Y + i, v1.Z + i);
        }
        /// <summary>
        /// 重载一元坐标加法+ 向量+整数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">整数</param>
        /// <returns></returns>
        public static Vector3 operator +(Vector3 v1, int i)
        {
            return new Vector3(v1.X + (double)i, v1.Y + (double)i, v1.Z + (double)i);
        }
        /// <summary>
        /// 重载二元坐标减法- 向量-向量
        /// </summary>
        /// <param name="v1">向量被减数</param>
        /// <param name="v2">向量减数</param>
        /// <returns></returns>
        public static Vector3 operator -(Vector3 v1, Vector3 v2)
        {
            return new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z);
        }
        /// <summary>
        /// 重载一元坐标加法- 向量-小数
        /// </summary>
        /// <param name="v1">向量被减数</param>
        /// <param name="i">小数减数</param>
        /// <returns></returns>
        public static Vector3 operator -(Vector3 v1, double i)
        {
            return new Vector3(v1.X - i, v1.Y - i, v1.Z - i);
        }
        /// <summary>
        /// 重载一元坐标加法- 向量-整数
        /// </summary>
        /// <param name="v1">向量被减数</param>
        /// <param name="i">整数减数</param>
        /// <returns></returns>
        public static Vector3 operator -(Vector3 v1, int i)
        {
            return new Vector3(v1.X - (double)i, v1.Y - (double)i, v1.Z - (double)i);
        }
        /// <summary>
        /// 重载一元坐标乘法* 向量*小数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">小数乘数</param>
        /// <returns>得到向量的倍数向量</returns>
        public static Vector3 operator *(Vector3 v1, double i)
        {
            return new Vector3(v1.X * i, v1.Y * i, v1.Z * i);
        }
        /// <summary>
        /// 重载一元坐标乘法* 向量*整数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">整数乘数</param>
        /// <returns>得到向量的倍数向量</returns>
        public static Vector3 operator *(Vector3 v1, int i)
        {
            return new Vector3(v1.X * (double)i, v1.Y * (double)i, v1.Z * (double)i);
        }
        /// <summary>
        /// 重载一元坐标除法/ 向量/小数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">小数除数</param>
        /// <returns>得到向量的商向量</returns>
        public static Vector3 operator /(Vector3 v1, double i)
        {
            return new Vector3(v1.X / i, v1.Y / i, v1.Z / i);
        }
        /// <summary>
        /// 重载一元坐标除法/ 向量/整数
        /// </summary>
        /// <param name="v1">向量</param>
        /// <param name="i">整数除数</param>
        /// <returns>得到向量的商向量</returns>
        public static Vector3 operator /(Vector3 v1, int i)
        {
            return new Vector3(v1.X / (double)i, v1.Y / (double)i, v1.Z / (double)i);
        }
        /// <summary>
        /// 重载== 向量==向量
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>布尔值 判断向量是否相等</returns>
        public static bool operator ==(Vector3 v1, Vector3 v2)
        {
            if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) return true;
            return false;
        }
        /// <summary>
        /// 重载！= 向量！=向量
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>布尔值 判断向量是否不相等</returns>
        public static bool operator !=(Vector3 v1, Vector3 v2)
        {
            if (v1.X != v2.X || v1.Y != v2.Y || v1.Z != v2.Z) return true;
            return false;
        }
        /// <summary>
        /// 确定指定的对象是否等于当前向量。
        /// </summary>
        /// <param name="obj">要与当前向量比较的对象。</param>
        /// <returns>如果指定的对象等于当前向量，则为 <c>true</c>；否则为 <c>false</c>。</returns>
        public override bool Equals(object obj)
        {
            // 检查对象是否与当前向量具有相同的类型
            if (obj is Vector3)
            {
                Vector3 vector = (Vector3)obj;
                // 比较向量的每个分量
                return X == vector.X &&
                       Y == vector.Y &&
                       Z == vector.Z;
            }

            // 如果对象不是 Vector3 类型，则它们不相等
            return false;
        }
        /// <summary>
        /// 向量按矩阵旋转和偏移
        /// </summary>
        /// <param name="mat">矩阵</param>
        /// <returns>返回一个新的向量</returns>
        public Vector3 Multiply(Matrix mat)
        {
            Vector3 re = new Vector3();
            //矩阵相乘 ps:旋转角度
            re.X = this.X * mat[1].X + this.Y * mat[2].X + this.Z * mat[0].X;
            re.Y = this.X * mat[1].Y + this.Y * mat[2].Y + this.Z * mat[0].Y;
            re.Z = this.X * mat[1].Z + this.Y * mat[2].Z + this.Z * mat[0].Z;
            //off偏移
            re = re + mat[3];
            return re;
        }
        /// <summary>
        /// 求模长
        /// </summary>
        /// <returns>向量到原点的模长</returns>
        public double GetMag()
        {
            return Math.Sqrt(Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2));
        }
        /// <summary>
        /// 求模长 平方
        /// </summary>
        /// <returns>向量到原点的模长的平方值</returns>
        public double GetMagSquared()
        {
            return Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
        }
        /// <summary>
        /// 标准化坐标 无返回值 直接改变愿坐标
        /// </summary>
        /// <param name="multiple">标准化单位</param>
        public void Normalize(double multiple = 1.0)
        {
            double magSq = Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
            if (magSq > 0)
            {
                double oneOverMag = multiple / Math.Sqrt(magSq);
                this.X *= oneOverMag;
                this.Y *= oneOverMag;
                this.Z *= oneOverMag;
            }
        }
        /// <summary>
        /// 标准化 返回一个标准化之后的值 不改变自身
        /// </summary>
        /// <param name="multiple">标准化单位</param>
        /// <returns>标准化之后的值</returns>
        public Vector3 NormalizEd(double multiple = 1.0)
        {
            Vector3 re = new Vector3();
            double magSq = Math.Pow(this.X, 2) + Math.Pow(this.Y, 2) + Math.Pow(this.Z, 2);
            if (magSq > 0)
            {
                double oneOverMag = multiple / Math.Sqrt(magSq);
                re.X = this.X * oneOverMag;
                re.Y = this.Y * oneOverMag;
                re.Z = this.Z * oneOverMag;
            }
            return re;
        }
        /// <summary>
        /// 归零 改变自身数值 一般配合归位使用
        /// </summary>
        /// <param name="v2"></param>
        public void SetZero(Vector3 v2)
        {
            this.X -= v2.X;
            this.Y -= v2.Y;
            this.Z -= v2.Z;
        }
        /// <summary>
        /// 归零 返回一个归零值 不改变自身
        /// </summary>
        /// <param name="v2"></param>
        /// <returns></returns>
        public Vector3 SetZeroEd(Vector3 v2)
        {
            Vector3 re = new Vector3(this.X - v2.X, this.Y - v2.Y, this.Z - v2.Z);
            return re;
        }
        /// <summary>
        /// 归位
        /// </summary>
        /// <param name="v2"></param>
        public void SetFormerly(Vector3 v2)
        {
            this.X += v2.X;
            this.Y += v2.Y;
            this.Z += v2.Z;
        }
        /// <summary>
        /// 重写ToString 打印坐标
        /// </summary>
        /// <returns>坐标字符串</returns>
        public override string ToString()
        {
            string x = Convert.ToString(this.X);
            string y = Convert.ToString(this.Y);
            string z = Convert.ToString(this.Z);
            return string.Format($"X轴:{x} Y轴:{y} Z轴:{z}");
        }
        /// <summary>
        /// 哈希码是一个整数值，用于对对象进行快速比较和索引
        /// </summary>
        /// <returns>哈希码</returns>
        public override int GetHashCode()
        {
            int hashCode = -307843816;
            hashCode = hashCode * -1521134295 + X.GetHashCode();
            hashCode = hashCode * -1521134295 + Y.GetHashCode();
            hashCode = hashCode * -1521134295 + Z.GetHashCode();
            return hashCode;
        }
    }
    //坐标操作与验证
    public static class FlyVecFun
    {
        /// <summary>
        /// 随机种子
        /// </summary>
        public static int RandomSeed { get; set; } = 1;
        /// <summary>
        /// 航线 线间距 平方值
        /// </summary>
        public static double LineDistanceSquare { get; set; } = 32400;
        /// <summary>
        /// 飞行过程中间距 平方值
        /// </summary>
        public static double SpaceBetweenSquare { get; set; } = 250000;
        /// <summary>
        /// 算绕行时 中间取点 true在正中间取点即 一个圆盘 false在一个圆柱体内取点
        /// </summary>
        public static bool singleCircle = true;
        /// <summary>
        /// 输出日志回调函数
        /// </summary>
        /// <param name="str">日志内容</param>
        public delegate void SomeCalculateWay(string str);
        /// <summary>
        /// 输出进度日志回调函数
        /// </summary>
        /// <param name="val">进度值ps:0-100</param>
        public delegate void Schedule(int val);
        /// <summary>
        /// Arraylist 转 Vector3[] 坐标集
        /// </summary>
        /// <param name="arr">Arraylist 坐标集</param>
        /// <returns>Vector3[] 坐标集</returns>
        public static Vector3[] ArrToVec(ArrayList arr)
        {
            int cou = arr.Count;
            Vector3[] re = new Vector3[cou];
            int key = 0;
            foreach (Vector3 item in arr)
            {
                re[key] = item;
                key++;
            }
            return re;
        }
        /// <summary>
        /// 取数组最大 或者最小值
        /// </summary>
        /// <param name="arr">数组</param>
        /// <param name="isMax">true返回最大值 false返回最小值</param>
        /// <returns>根据参数返回 最大或者最小值</returns>
        private static double GetMaxOrMin(double[] arr, bool isMax = true)
        {
            Array.Sort(arr);//给数组arr排序
            if (isMax)
                return arr[arr.Length - 1];
            else
                return arr[0];
        }
        /// <summary>
        /// 取数组最大 或者最小值 的数组下标
        /// </summary>
        /// <param name="arr">数组</param>
        /// <param name="isMax">true返回最大值 false返回最小值</param>
        /// <returns>数组下标</returns>
        private static int GetIndexOfMaxOrMin(double[] arr, bool isMax = true)
        {
            int[] indexes = new int[arr.Length];
            for (int i = 0; i < arr.Length; i++)
            {
                indexes[i] = i; // 保存每个元素的原始索引
            }

            Array.Sort(arr, indexes); // 按值排序同时更新索引数组

            // 根据 isMax 参数返回相应的索引
            return isMax ? indexes[arr.Length - 1] : indexes[0];
        }
        /// <summary>
        /// 获取列表中最小值的随机下标
        /// </summary>
        /// <param name="list">输入的列表</param>
        /// <returns>最小值的随机下标</returns>
        private static int GetRandomMinIndex(List<int> list)
        {
            // 检查输入的列表是否为 null 或为空
            if (list == null || list.Count == 0)
            {
                throw new ArgumentException("列表不能为 null 或为空");
            }

            int minValue = int.MaxValue; // 初始化为 int 类型的最大值
            List<int> minIndices = new List<int>(); // 存储最小值的下标列表

            for (int i = 0; i < list.Count; i++)
            {
                // 如果当前元素比最小值小，更新最小值和清空之前的下标列表
                if (list[i] < minValue)
                {
                    minValue = list[i];
                    minIndices.Clear(); // 清空之前的下标列表
                    minIndices.Add(i); // 添加当前下标
                }
                // 如果当前元素等于最小值，添加当前下标到列表
                else if (list[i] == minValue)
                {
                    minIndices.Add(i);
                }
            }

            // 如果最小值下标列表为空，表示没有找到最小值
            if (minIndices.Count == 0)
            {
                throw new InvalidOperationException("列表中没有找到最小值");
            }

            // 生成一个随机数，用于从最小值下标列表中随机选择一个下标
            Random random = new Random(RandomSeed);
            return minIndices[random.Next(minIndices.Count)];
        }
        /// <summary>
        /// 二维数组转一维数组 并去重
        /// </summary>
        /// <param name="twoArr">二维数组</param>
        /// <returns>去重一维数组</returns>
        private static List<int> TwoArrToArr(List<int[]> twoArr)
        {
            // 创建一个用于存储去重后的一维数组的列表
            List<int> arr = new List<int>();
            // 遍历二维数组的每个子数组
            foreach (int[] item in twoArr)
            {
                // 查找第一个元素在一维数组中的索引
                int i = arr.IndexOf(item[0]);
                // 如果索引小于0，表示该元素在一维数组中不存在，将其添加到一维数组
                if (i < 0)
                {
                    arr.Add(item[0]);
                }
                // 查找第二个元素在一维数组中的索引
                int x = arr.IndexOf(item[1]);
                // 如果索引小于0，表示该元素在一维数组中不存在，将其添加到一维数组
                if (x < 0)
                {
                    arr.Add(item[1]);
                }
            }
            // 返回去重后的一维数组
            return arr;
        }
        /// <summary>
        /// 获取交叉序列 ps:处理二维数组 把有关联的子数组合并 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
        /// </summary>
        /// <param name="arr">需要处理的二维数组 </param>
        /// <returns>交叉序列</returns>
        private static List<List<int>> FindConnected(List<int[]> arr)
        {
            Dictionary<int, List<int>> graph = new Dictionary<int, List<int>>();
            Dictionary<int, bool> visited = new Dictionary<int, bool>();
            List<List<int>> result = new List<List<int>>();

            // 构建图
            foreach (var edge in arr)
            {
                foreach (var node in edge)
                {
                    if (!graph.ContainsKey(node))
                    {
                        graph[node] = new List<int>();
                        visited[node] = false;
                    }
                }
            }
            foreach (var edge in arr)
            {
                graph[edge[0]].Add(edge[1]);
                graph[edge[1]].Add(edge[0]);
            }
            foreach (var node in graph.Keys)
            {
                if (!visited[node])
                {
                    List<int> connected = new List<int>();
                    DFS(node, connected, graph, visited);
                    result.Add(connected);
                }
            }
            return result;
        }
        private static void DFS(int node, List<int> connected, Dictionary<int, List<int>> graph, Dictionary<int, bool> visited)
        {
            visited[node] = true;
            connected.Add(node);

            foreach (var neighbor in graph[node])
            {
                if (!visited[neighbor])
                {
                    DFS(neighbor, connected, graph, visited);
                }
            }
        }
        /// <summary>
        /// 获取一组序列的所有排列方式 ps:[0,1,2] 结果[[0, 1, 2],[0, 2, 1],[1, 0, 2],[1, 2, 0],[2, 0, 1],[2, 1, 0]]
        /// </summary>
        /// <param name="array">一组序列</param>
        /// <returns>所有序列的排列方式</returns>
        private static List<List<int>> Permutations(List<int> array)
        {
            List<List<int>> result = new List<List<int>>();
            GeneratePermutations(array, 0, array.Count - 1, result);
            return result;
        }
        private static void GeneratePermutations(List<int> array, int start, int end, List<List<int>> result)
        {
            if (start == end)
            {
                result.Add(new List<int>(array));
            }
            else
            {
                for (int i = start; i <= end; i++)
                {
                    int temp = array[start];
                    array[start] = array[i];
                    array[i] = temp;
                    GeneratePermutations(array, start + 1, end, result);
                    temp = array[start];
                    array[start] = array[i];
                    array[i] = temp;
                }
            }
        }
        /// <summary>
        /// 按照对应关系 生成新的b坐标集合
        /// </summary>
        /// <param name="aVecs">a坐标集合</param>
        /// <param name="bVecs">b坐标集合</param>
        /// <param name="match">a b集合的对应关系</param>
        /// <returns>坐标集合</returns>
        private static Vector3[] CreateNewBVecs(Vector3[] bVecs, List<int[]> match)
        {
            Vector3[] new_bVecs = new Vector3[bVecs.Length];
            foreach (int[] m in match)
            {
                new_bVecs[m[0]] = bVecs[m[1]];
            }
            return new_bVecs;
        }
        /// <summary>
        /// 从数组中删除指定索引处的元素
        /// </summary>
        /// <typeparam name="T">数组元素类型</typeparam>
        /// <param name="array">要操作的数组</param>
        /// <param name="indicesToRemove">要删除的元素的索引列表</param>
        /// <returns>删除元素后的新数组</returns>
        private static T[] RemoveElementsAtIndices<T>(T[] array, List<int> indicesToRemove)
        {
            // 检查索引是否有效
            if (indicesToRemove == null || indicesToRemove.Count == 0)
            {
                // 没有要删除的索引，返回原数组
                return array;
            }
            // 创建一个新数组，长度为原数组长度减去要删除的元素数量
            T[] newArray = new T[array.Length - indicesToRemove.Count];
            int newIndex = 0;
            // 复制不包括指定索引的元素到新数组中
            for (int i = 0; i < array.Length; i++)
            {
                if (!indicesToRemove.Contains(i))
                {
                    newArray[newIndex] = array[i];
                    newIndex++;
                }
            }
            // 返回新数组
            return newArray;
        }
        /// <summary>
        /// 设置中间航点
        /// </summary>
        /// <param name="aVec">起点</param>
        /// <param name="bVec">目标点</param>
        /// <param name="middlePos">比例</param>
        /// <returns></returns>
        private static Vector3 SetMiddleVec(Vector3 aVec, Vector3 bVec, double middlePos = 0.5)
        {
            return (bVec - aVec) * middlePos + aVec;
        }
        /// <summary>
        /// 两点距离
        /// </summary>
        /// <param name="v1">坐标1</param>
        /// <param name="v2">坐标2</param>
        /// <returns>两点之间距离</returns>
        private static double GageLength(Vector3 v1, Vector3 v2)
        {
            return Math.Sqrt(GageLengthSquare(v1, v2));
        }
        /// <summary>
        /// 两点距离的平方
        /// </summary>
        /// <param name="v1">坐标1</param>
        /// <param name="v2">坐标2</param>
        /// <returns>两点距离的平方</returns>
        private static double GageLengthSquare(Vector3 v1, Vector3 v2)
        {
            return Math.Pow(v1.X - v2.X, 2) + Math.Pow(v1.Y - v2.Y, 2) + Math.Pow(v1.Z - v2.Z, 2);
        }
        /// <summary>
        /// 点积
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>点积</returns>
        private static double DotPro(Vector3 v1, Vector3 v2)
        {
            return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
        }
        /// <summary>
        /// 叉积 ps：法线向量
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>两个向量叉积</returns>
        private static Vector3 CrossPro(Vector3 v1, Vector3 v2)
        {
            double x = v1.Y * v2.Z - v1.Z * v2.Y;
            double y = v1.Z * v2.X - v1.X * v2.Z;
            double z = v1.X * v2.Y - v1.Y * v2.X;
            return new Vector3(x, y, z);
        }
        /// <summary>
        /// 计算两个向量之间的夹角 角度
        /// </summary>
        /// <param name="v1">第一个向量</param>
        /// <param name="v2">第二个向量</param>
        /// <returns>角度</returns>
        private static double AngleBetween(Vector3 v1, Vector3 v2)
        {
            // 计算点积
            double dotProduct = DotPro(v1, v2);

            // 计算向量长度
            double magnitude1 = v1.GetMag();
            double magnitude2 = v2.GetMag();

            // 使用 Atan2 计算弧度
            double thetaRadians = Math.Atan2(CrossPro(v1, v2).GetMag(), dotProduct);

            // 弧度转角度
            double thetaDegrees = thetaRadians * (180 / Math.PI);

            return thetaDegrees;
        }
        /// <summary>
        /// 计算某个点到平面垂线与平面的交点
        /// </summary>
        /// <param name="vec1">平面上的点1</param>
        /// <param name="vec2">平面上的点2</param>
        /// <param name="vec3">平面上的点3</param>
        /// <param name="vec4">被求点</param>
        /// <returns>交点坐标</returns>
        private static Vector3 CalculateIntersectionPoint(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
        {
            // 计算平面的法线向量
            Vector3 normal = CrossPro(vec2 - vec1, vec3 - vec1);
            normal.Normalize();
            // 计算第4个点到平面的距离
            double distance = DotPro(normal, vec1);
            // 计算第4个点到平面的投影点坐标
            double projection = DotPro(normal, vec4) - distance;
            // 计算交点坐标
            Vector3 intersectionPoint = vec4 - normal * projection;
            return intersectionPoint;
        }
        /// <summary>
        /// 找到能组成平面的点的最大数量，并返回组成最大平面的点的索引。
        /// </summary>
        /// <param name="vecs">Vector3 点的列表。</param>
        /// <returns>组成最大平面的点的索引。</returns>
        private static List<int> FindMaxPlaneIndices(Vector3[] vecs)
        {
            int maxPointsOnPlane = 0;
            List<int> maxPointsIndices = new List<int>(); //记录返回值
            int planeCou = vecs.Length; // 飞机总数
            for (int i = 0; i < planeCou; i++)
            {
                for (int j = i + 1; j < planeCou; j++)
                {
                    for (int k = j + 1; k < planeCou; k++)
                    {
                        int currentPointsOnPlane = 3; // 当前遍历的三个点肯定在同一平面上

                        for (int l = k + 1; l < planeCou; l++)
                        {
                            if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
                            {
                                // 当前的 l 也在同一平面上
                                currentPointsOnPlane++;
                            }
                        }

                        // 检查当前平面是否比之前找到的平面更大
                        if (currentPointsOnPlane > maxPointsOnPlane)
                        {
                            maxPointsOnPlane = currentPointsOnPlane;
                            maxPointsIndices.Clear();
                            maxPointsIndices.Add(i);
                            maxPointsIndices.Add(j);
                            maxPointsIndices.Add(k);

                            // 添加当前平面的 l 索引
                            for (int l = k + 1; l < planeCou; l++)
                            {
                                if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
                                {
                                    maxPointsIndices.Add(l);
                                }
                            }
                        }
                    }
                }
            }
            return maxPointsIndices;
        }
        /// <summary>
        /// 检查4个点是否在一个平面上
        /// </summary>
        /// <param name="vector1">点1</param>
        /// <param name="vector2">点2</param>
        /// <param name="vector3">点3</param>
        /// <param name="vector4">点4</param>
        /// <returns>true在一个平面 false不在一个平面</returns>
        private static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
        {
            //计算三个向量
            Vector3 v1v2 = vec2 - vec1;
            Vector3 v1v3 = vec3 - vec1;
            Vector3 v1v4 = vec4 - vec1;
            //计算法线向量
            Vector3 normal_vector = CrossPro(v1v2, v1v3);
            //计算点到平面的距离
            double distance = DotPro(normal_vector, v1v4) / normal_vector.GetMag();
            //设置一个阈值，判断是否共面
            double epsilon = 10; //单位厘米
            return Math.Abs(distance) < epsilon;
        }
        /// <summary>
        /// 判断3个点是否在同一条直线上
        /// </summary>
        /// <param name="vector1">点1</param>
        /// <param name="vector2">点2</param>
        /// <param name="vector3">点3</param>
        /// <returns>true在一条直线上 false不在一条直线上</returns>
        private static bool IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3)
        {
            Vector3 v1 = vec2 - vec1;
            Vector3 v2 = vec3 - vec1;
            // 计算点积
            double dotProduct = DotPro(v1, v2);
            // 计算向量长度
            double magnitude1 = v1.GetMag();
            double magnitude2 = v2.GetMag();
            // 使用 Atan2 计算弧度
            double thetaRadians = Math.Atan2(CrossPro(v1, v2).GetMag(), dotProduct);
            // 弧度转角度
            double thetaDegrees = thetaRadians * (180 / Math.PI);
            if (Math.Abs(thetaDegrees) < 10) return true;
            else return false;
        }
        /// <summary>
        /// 从顶视图 判断点是否在两条线内之间
        /// </summary>
        /// <param name="A">线段1端点</param>
        /// <param name="B">线段1端点</param>
        /// <param name="C">线段2端点</param>
        /// <param name="D">线段2端点</param>
        /// <param name="P">被判断点</param>
        /// <returns>true点在两条线内部 false点不在两条线内部</returns>
        private static bool IsPointBetweenLines(Vector3 A, Vector3 B, Vector3 C, Vector3 D, Vector3 P)
        {
            /// Y轴亚平  即顶视图
            A = new Vector3(A.X, 0, A.Z);
            B = new Vector3(B.X, 0, B.Z);
            C = new Vector3(C.X, 0, C.Z);
            D = new Vector3(D.X, 0, D.Z);
            P = new Vector3(P.X, 0, P.Z);
            Vector3 dirAB = new Vector3(B.X - A.X, B.Y - A.Y, B.Z - A.Z);
            Vector3 dirCD = new Vector3(D.X - C.X, D.Y - C.Y, D.Z - C.Z);
            Vector3 vecAP = new Vector3(P.X - A.X, P.Y - A.Y, P.Z - A.Z);
            Vector3 vecCP = new Vector3(P.X - C.X, P.Y - C.Y, P.Z - C.Z);

            double cross1 = dirAB.X * vecAP.Z - dirAB.Z * vecAP.X;
            double cross2 = dirCD.X * vecCP.Z - dirCD.Z * vecCP.X;

            return cross1 * cross2 < 0;
        }
        /// <summary>
        /// 辅助方法，用于检查向量是否为零向量
        /// </summary>
        /// <param name="vector">向量</param>
        /// <returns></returns>
        private static bool IsZero(this Vector3 vector)
        {
            return vector.X == 0 && vector.Y == 0 && vector.Z == 0;
        }
        /// <summary>
        /// 获取两条线段 的最近位置的距离和占比
        /// </summary>
        /// <param name="a1">线段1起始点</param>
        /// <param name="a2">线段1起终点</param>
        /// <param name="b1">线段2起始点</param>
        /// <param name="b2">线段2起终点</param>
        /// <returns>[在线段1占比，在线段2占比，最近距离]</returns>
        private static double RecentlySquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            if (a1 == a2) a2 += 1;// 防止线段长度为0
            if (b1 == b2) b2 += 1;
            double ux = a2.X - a1.X;
            double uy = a2.Y - a1.Y;
            double uz = a2.Z - a1.Z;
            double vx = b2.X - b1.X;
            double vy = b2.Y - b1.Y;
            double vz = b2.Z - b1.Z;
            double wx = a1.X - b1.X;
            double wy = a1.Y - b1.Y;
            double wz = a1.Z - b1.Z;
            double a = (ux * ux + uy * uy + uz * uz);
            double b = (ux * vx + uy * vy + uz * vz);
            double c = (vx * vx + vy * vy + vz * vz);
            double d = (ux * wx + uy * wy + uz * wz);
            double e = (vx * wx + vy * wy + vz * wz);
            double dt = a * c - b * b;
            double sd = dt;
            double td = dt;
            double sn = 0.0;
            double tn = 0.0;
            if (IsEqual(dt, 0.0))
            {
                sn = 0.0;
                sd = 1.00;
                tn = e;
                td = c;
            }
            else
            {
                sn = (b * e - c * d);
                tn = (a * e - b * d);
                if (sn < 0.0)
                {
                    sn = 0.0;
                    tn = e;
                    td = c;
                }
                else if (sn > sd)
                {
                    sn = sd;
                    tn = e + b;
                    td = c;
                }
            }
            if (tn < 0.0)
            {
                tn = 0.0;
                if (-d < 0.0) sn = 0.0;
                else if (-d > a) sn = sd;
                else
                {
                    sn = -d;
                    sd = a;
                }
            }
            else if (tn > td)
            {
                tn = td;
                if ((-d + b) < 0.0) sn = 0.0;
                else if ((-d + b) > a) sn = sd;
                else
                {
                    sn = (-d + b);
                    sd = a;
                }

            }
            double sc = 0.0;
            double tc = 0.0;
            if (IsEqual(sn, 0.0)) sc = 0.0;
            else sc = sn / sd;//最近点在第一条线占比
            if (IsEqual(tn, 0.0)) tc = 0.0;
            else tc = tn / td;//最近点在第二条线占比 
            double dx = wx + (sc * ux) - (tc * vx);
            double dy = wy + (sc * uy) - (tc * vy);
            double dz = wz + (sc * uz) - (tc * vz);
            return dx * dx + dy * dy + dz * dz;//两线最近距离的平方
        }
        private static bool IsEqual(double x, double y)
        {
            if (Math.Abs(x - y) < 1e-7)
            {
                return true;
            }
            return false;
        }
        /// <summary>
        /// 按比例在两条线段上截取对应点间的最小距离 平方
        /// </summary>
        /// <param name="a1">线段1起始点</param>
        /// <param name="a2">线段1起终点</param>
        /// <param name="b1">线段2起始点</param>
        /// <param name="b2">线段2起终点</param>
        /// <returns>最小距离的平方值</returns>
        private static double MinDistanceSquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            //相对位置和相对速度
            Vector3 d0 = b1 - a1;
            Vector3 v_rel = (b2 - b1) - (a2 - a1);
            //计算最小距离位置 占比
            double vd = DotPro(v_rel, v_rel);
            double proportion;//最近距离占比
            if (vd != 0)
            {
                proportion = Math.Max(0, Math.Min(1, -DotPro(d0, v_rel) / vd));//max min函数把比例限制在0-1 保证比例在线段上
            }
            else
            {
                proportion = 0;
            }
            //计算最小距离平方
            Vector3 d_min = d0 + v_rel * proportion;
            return d_min.GetMagSquared();
        }
        /// <summary>
        /// 获取坐标集合的重心或中心
        /// </summary>
        /// <param name="pos">坐标集合</param>
        /// <param name="isCentroid">默认返回为true重心 false则为中心</param>
        /// <returns>重心或中心坐标</returns>
        public static Vector3 GetPosCenter(Vector3[] pos, bool isCentroid = true)
        {
            int cou = pos.Length;
            if (isCentroid)//重心
            {
                double x = 0;
                double y = 0;
                double z = 0;
                foreach (var item in pos)
                {
                    x += item.X;
                    y += item.Y;
                    z += item.Z;
                }
                x = x / cou;
                y = y / cou;
                z = z / cou;
                return new Vector3(x, y, z);
            }
            else//中心
            {
                double[] x = new double[cou];
                double[] y = new double[cou];
                double[] z = new double[cou];
                int key = 0;
                foreach (var item in pos)
                {
                    x[key] = item.X;
                    y[key] = item.Y;
                    z[key] = item.Z;
                    key++;
                }
                double xc = (GetMaxOrMin(x) + GetMaxOrMin(x, false)) * .5;
                double yc = (GetMaxOrMin(y) + GetMaxOrMin(y, false)) * .5;
                double zc = (GetMaxOrMin(z) + GetMaxOrMin(z, false)) * .5;
                return new Vector3(xc, yc, zc);
            }
        }
        /// <summary>
        /// 获取坐标集合的重心或中心
        /// </summary>
        /// <param name="pos">坐标集合</param>
        /// <param name="isCentroid">默认返回为true重心 false则为中心</param>
        /// <returns>重心或中心坐标</returns>
        private static Vector3 GetPosCenter(List<Vector3> pos, bool isCentroid = true)
        {
            int cou = pos.Count;
            if (isCentroid)//重心
            {
                double x = 0;
                double y = 0;
                double z = 0;
                foreach (var item in pos)
                {
                    x += item.X;
                    y += item.Y;
                    z += item.Z;
                }
                x = x / cou;
                y = y / cou;
                z = z / cou;
                return new Vector3(x, y, z);
            }
            else//中心
            {
                double[] x = new double[cou];
                double[] y = new double[cou];
                double[] z = new double[cou];
                int key = 0;
                foreach (var item in pos)
                {
                    x[key] = item.X;
                    y[key] = item.Y;
                    z[key] = item.Z;
                    key++;
                }
                double xc = (GetMaxOrMin(x) + GetMaxOrMin(x, false)) * .5;
                double yc = (GetMaxOrMin(y) + GetMaxOrMin(y, false)) * .5;
                double zc = (GetMaxOrMin(z) + GetMaxOrMin(z, false)) * .5;
                return new Vector3(xc, yc, zc);
            }
        }
        /// <summary>
        /// 获取坐标集合 的总宽度 高度 长度
        /// </summary>
        /// <param name="pos">坐标集合</param>
        /// <returns>返回数组[0]宽度[1]高度[2]长度</returns>
        public static double[] GetVecsWithHighLength(Vector3[] pos)
        {
            List<double> w = new List<double>();
            List<double> h = new List<double>();
            List<double> l = new List<double>();
            foreach (var item in pos)
            {
                w.Add(item.X);
                h.Add(item.Y);
                l.Add(item.Z);
            }
            double[] re = new double[3];
            re[0] = w.Max() - w.Min();
            re[1] = h.Max() - h.Min();
            re[2] = l.Max() - l.Min();
            return re;
        }
        /// <summary>
        /// 碰撞检测
        /// </summary>
        /// <param name="aVecs">始点坐标集合</param>
        /// <param name="bVecs">终点坐标集合</param>
        /// <returns></returns>
        private static List<int[]> AirImitation(Vector3[] aVecs, Vector3[] bVecs)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = aVecs.Length; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                    if (distanceSquare < LineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                        if (planeLenSquare < SpaceBetweenSquare)
                        {
                            planesCollision.Add(new int[] { a, a + 1 + b });
                        }
                    }
                }
            }
            return planesCollision;
        }
        /// <summary>
        /// 碰撞检测
        /// </summary>
        /// <param name="aVecs">始点坐标集合</param>
        /// <param name="bVecs">终点坐标集合</param>
        /// <returns></returns>
        private static List<int[]> AirImitation(List<Vector3> aVecs, List<Vector3> bVecs)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = aVecs.Count; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                    if (distanceSquare < LineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                        if (planeLenSquare < SpaceBetweenSquare)
                        {
                            planesCollision.Add(new int[] { a, a + 1 + b });
                        }
                    }
                }
            }
            return planesCollision;
        }
        /// <summary>
        /// 碰撞检测
        /// </summary>
        /// <param name="aVecs">始点坐标集合</param>
        /// <param name="bVecs">终点坐标集合</param>
        /// <returns></returns>
        private static List<int[]> AirImitation(Vector3[] aVecs, List<Vector3> bVecs)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = aVecs.Length; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                    if (distanceSquare < LineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                        if (planeLenSquare < SpaceBetweenSquare)
                        {
                            planesCollision.Add(new int[] { a, a + 1 + b });
                        }
                    }
                }
            }
            return planesCollision;
        }
        /// <summary>
        /// 碰撞检测
        /// </summary>
        /// <param name="aVecs">始点坐标集合</param>
        /// <param name="bVecs">终点坐标集合</param>
        /// <returns></returns>
        private static List<int[]> AirImitation(List<Vector3> aVecs, Vector3[] bVecs)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = aVecs.Count; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                    if (distanceSquare < LineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(aVecs[a], bVecs[a], aVecs[a + 1 + b], bVecs[a + 1 + b]);
                        if (planeLenSquare < SpaceBetweenSquare)
                        {
                            planesCollision.Add(new int[] { a, a + 1 + b });
                        }
                    }
                }
            }
            return planesCollision;
        }
        /// <summary>
        /// 单机碰撞检测
        /// </summary>
        /// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
        /// <param name="aVecs">飞机起始坐标集合</param>
        /// <param name="bVecs">飞机终点坐标集合</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, Vector3[] bVecs)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
                if (distanceSquare < LineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < SpaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 单机碰撞检测
        /// </summary>
        /// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
        /// <param name="aVecs">飞机起始坐标集合</param>
        /// <param name="bVecs">飞机终点坐标集合</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, List<Vector3> bVecs)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
                if (distanceSquare < LineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < SpaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 单机碰撞检测
        /// </summary>
        /// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
        /// <param name="aVecs">飞机起始坐标集合</param>
        /// <param name="bVecs">飞机终点坐标集合</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        private static bool OnlyImitation(int onlyPlaneId, Vector3[] aVecs, List<Vector3> bVecs)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < aVecs.Length; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
                if (distanceSquare < LineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < SpaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 单机碰撞检测
        /// </summary>
        /// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
        /// <param name="aVecs">飞机起始坐标集合</param>
        /// <param name="bVecs">飞机终点坐标集合</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        private static bool OnlyImitation(int onlyPlaneId, List<Vector3> aVecs, Vector3[] bVecs)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < aVecs.Count; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//航线最小距离
                if (distanceSquare < LineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(aVecs[onlyPlaneId], bVecs[onlyPlaneId], aVecs[contrastId], bVecs[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < SpaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换
        /// </summary>
        /// <param name="aVecs">起始点坐标组</param>
        /// <param name="bVecs">目标点坐标组</param>
        /// <param name="StrPrint">日志输出 回调函数</param>
        /// <param name="isStaticSkip">静态跳过 true跳过即保持原地不动 false不跳过参与“最近和交换”计算</param>
        /// <param name="staticThresholdSquare">静态距离判断 小于阈值判定为静态 注意是个平方值</param>
        /// <param name="isSwap">交叉航线是否进行交换</param>
        /// <param name="swapCount">交换次数</param>
        /// <param name="crossingLimit">交叉线路数量上限 ps:超过这个数量则不进行交换</param>
        /// <returns>新的目标点</returns>
        public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint,bool isStaticSkip = false, double staticThresholdSquare = 25 , bool isSwap = true, int swapCount = 5, int crossingLimit = 6)
        {
            long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
            StrPrint("-------智能选择路径计算，开始-------");
            int planeCou = aVecs.Length; // 飞机总数
            List<int> staticAindex = new List<int>(); // a静态对应关系（ab两图同一位置）
            List<int> staticBindex = new List<int>(); // b静态对应关系（ab两图同一位置）
            Vector3[] new_aVecs = aVecs.ToArray();  //a图副本
            Vector3[] new_bVecs = bVecs.ToArray(); //b图副本
            ///如果启动 静态航点跳过 把ab静态对应关系找出来 预存放到staticMatch
            if (isStaticSkip)
            {
                for (int i = 0; i < planeCou; i++)
                {
                    for (int j = 0; j < planeCou; j++)
                    {
                        if (GageLengthSquare(aVecs[i], bVecs[j]) <= staticThresholdSquare)
                        {
                            staticAindex.Add(i);
                            staticBindex.Add(j);
                            break;
                        }
                    }
                }
                //把静态的航点从列表里面清除
                new_aVecs = RemoveElementsAtIndices(new_aVecs, staticAindex); 
                new_bVecs = RemoveElementsAtIndices(new_bVecs, staticBindex);
                //刷新飞机总数 以下计算最近 和 交换航线时候 只有非静态飞机参与
                planeCou = new_aVecs.Length;
            }

            List<int[]> match = new List<int[]>(); // ab对应关系
            ///记录a b集合索引
            List<int> aIndex = new List<int>();
            List<int> bIndex = new List<int>();
            for (int k = 0; k < planeCou; k++)
            {
                aIndex.Add(k);
                bIndex.Add(k);
            }
            /// 遍历找出 AB集合对应关系
            for (int k = 0; k < planeCou; k++)
            {

                // 遍历每次刷新 剩下为未匹配总数
                int remainCou = aIndex.Count;
                // 遍历每次刷新 重心
                List<Vector3> allVecs = new List<Vector3>();
                for (int i = 0; i < remainCou; i++)
                {
                    allVecs.Add(new_aVecs[aIndex[i]]);
                    allVecs.Add(new_bVecs[bIndex[i]]);
                }
                Vector3 centerVec = GetPosCenter(allVecs);//重心点
                // 遍历所有ab点距离重心点的距离
                double[] aLens = new double[remainCou];
                double[] bLens = new double[remainCou];
                for (int i = 0; i < remainCou; i++)
                {
                    aLens[i] = GageLengthSquare(new_aVecs[aIndex[i]], centerVec);
                    bLens[i] = GageLengthSquare(new_bVecs[bIndex[i]], centerVec);
                }
                // 找出ab集合最外层坐标的下标 即离重心点最远的点
                int aMaxIndex = GetIndexOfMaxOrMin(aLens); // a集合到重心点最长的距离 数组的下标
                int bMaxIndex = GetIndexOfMaxOrMin(bLens); // b集合到重心点最长的距离 数组的下标
                if (aLens[aMaxIndex] > bLens[bMaxIndex])//找出 最外层如果为A集合点 对应B集合最近点 的下标
                {
                    double[] outAtoBLen = new double[remainCou];//最外层A点到 B集合所有点的距离
                    for (int i = 0; i < remainCou; i++)
                    {
                        outAtoBLen[i] = GageLengthSquare(new_aVecs[aIndex[aMaxIndex]], new_bVecs[bIndex[i]]);
                    }
                    int bMinIndex = GetIndexOfMaxOrMin(outAtoBLen, false);// 最短距离
                    match.Add(new int[] { aIndex[aMaxIndex], bIndex[bMinIndex] });// 映射到配对
                    aIndex.RemoveAt(aMaxIndex); // 删除已经配对的a集合 ID
                    bIndex.RemoveAt(bMinIndex); // 删除已经配对的b集合 ID
                }
                else//找出 最外层如果为B集合点 对应A集合最近点 的下标
                {
                    double[] outBtoALen = new double[remainCou];//最外层B点到 A集合所有点的距离
                    for (int i = 0; i < remainCou; i++)
                    {
                        outBtoALen[i] = GageLengthSquare(new_aVecs[aIndex[i]], new_bVecs[bIndex[bMaxIndex]]);
                    }
                    int aMinIndex = GetIndexOfMaxOrMin(outBtoALen, false);// 最短距离
                    match.Add(new int[] { aIndex[aMinIndex], bIndex[bMaxIndex] });// 映射到配对
                    aIndex.RemoveAt(aMinIndex); // 删除已经配对的a集合 ID
                    bIndex.RemoveAt(bMaxIndex); // 删除已经配对的b集合 ID
                }
            }
            Vector3[] re_bVecs = CreateNewBVecs(new_bVecs, match);// 按照映射 获取a 对应的 新的b集合
            ///交叉 交换
            if (isSwap)
            {
                for (int i = 0; i < swapCount; i++)
                {
                    List<int[]> planesCollision = AirImitation(new_aVecs, re_bVecs);// 获取碰撞组
                    List<List<int>> formatCollision = FindConnected(planesCollision);// 获取交叉序列 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
                    List<List<int>> filteredCollision = formatCollision.Where(sublist => sublist.Count <= crossingLimit).ToList();// 过滤 只保留交叉数量小于等于crossingLimit的序列
                    if (filteredCollision.Count == 0) break;
                    ///日志输出
                    string log = "";
                    foreach (List<int> item in filteredCollision)
                    {
                        log += "[";
                        foreach (int itemInt in item)
                        {
                            log += $"{itemInt},";
                        }
                        log += "]";
                    }
                    StrPrint($"共迭代{swapCount}次交换，第{i}次。共{filteredCollision.Count}组，交换组为：{log}。");
                    /// 遍历所有交叉组 分组做交换
                    int cou = 0;
                    foreach (List<int> swap_indices in filteredCollision)
                    {
                        cou++;
                        StrPrint($"进度：{cou}/{filteredCollision.Count}。交换组：[{string.Join(", ", swap_indices)}]。");
                        ///交叉 生成所有排列
                        List<List<int>> all_permutations = Permutations(swap_indices);//所有排列组合
                        List<int> original = all_permutations[0];//原始排列
                        all_permutations.RemoveAt(0);//删掉第一个 既原始排列
                        /// 按所有的排列 互换航线 并检测出最佳的对应目标点
                        List<int> tempLen = new List<int>(); //记录最少碰撞的 排列
                        List<Vector3[]> tempNew_bVecsS = new List<Vector3[]>();//记录最少碰撞的 排列交换之后的目标坐标集
                        foreach (List<int> indices in all_permutations)
                        {
                            Vector3[] current_array = new Vector3[planeCou];
                            Array.Copy(re_bVecs, current_array, planeCou);//复制一个re_bVecs 副本
                            for (int k = 0; k < indices.Count; k++)
                            {
                                current_array[original[k]] = re_bVecs[indices[k]];
                            }
                            ///把最少碰撞的排列 录入到数组
                            int collisionsCou = (AirImitation(new_aVecs, current_array)).Count;//此排列的碰撞次数
                            if (tempLen.Count == 0 || tempLen[0] == collisionsCou)//如果第一次添加 或者 碰撞次数等于最少碰撞
                            {
                                ///录入数组
                                tempLen.Add(collisionsCou);
                                tempNew_bVecsS.Add(current_array);
                            }
                            else if (tempLen[0] > collisionsCou)
                            {
                                /// 如果最小值 大于 当前碰撞次数，清空之前记录
                                tempLen.Clear();
                                tempNew_bVecsS.Clear();
                                ///录入数组
                                tempLen.Add(collisionsCou);
                                tempNew_bVecsS.Add(current_array);
                            }
                        }
                        re_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)];
                    }
                }
            }
            ///静态飞机 赋值回 返回航点组 ps:航点赋值的位置是对应的a组的ID位置 映射到b组的位置
            if (isStaticSkip)
            {
                planeCou = aVecs.Length;
                List<Vector3> re_bVecsCopy = new List<Vector3>(re_bVecs);
                List<Vector3> re_bVecsList = new List<Vector3>();
                for (int i = 0; i < planeCou; i++)
                {
                    if (staticAindex.IndexOf(i) != -1)
                    {
                        re_bVecsList.Add(aVecs[i]);
                    }
                    else 
                    {
                        re_bVecsList.Add(re_bVecsCopy[0]);
                        re_bVecsCopy.RemoveAt(0);
                    }
                }
                re_bVecs = re_bVecsList.ToArray();
            }
            t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
            StrPrint($"用时：{t}秒");
            StrPrint($"-------智能选择路径计算，结束-------");
            return re_bVecs;
        }
        /// <summary>
        /// 智能错层  说明：从A图上找出一个面（最多点组成的共面），以此面为错层基准，做错层计算。PS:A图 B图 都可以为多个平面图形组成，但这些图形都必须平行
        /// </summary>
        /// <param name="aVecs">起始坐标集合</param>
        /// <param name="bVecs">终点做标集合</param>
        /// <param name="StrPrint">日志输出 回调函数</param>
        /// <param name="layHight">错层层高</param>
        /// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组有几种情况 1.A图直接飞B图无碰撞 2.A图未能找到4个点以上的共面 3.有碰撞可能是AB图初始就过近 4.AB图并不平行</returns>
        public static List<List<Vector3>> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,SomeCalculateWay StrPrint, double layHight = 300)
        {
            long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
            //StrPrint("-------错层，开始-------");
            List<List<Vector3>> re = new List<List<Vector3>>();
            ///判断有没有碰撞 有碰撞继续 没有直接返回
            List<int[]> planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
            if (planesCollision.Count == 0)
            {
                StrPrint("执行成功：没有检测到碰撞，故不用添加中间航点");
                //StrPrint($"-------错层结束-------");
                return re;//直接返回
            }
            //获取飞机总数
            int planeCou = aVecs.Length;
            
            Vector3[] new_aVecs = aVecs.ToArray();  //a图副本
            Vector3[] new_bVecs = bVecs.ToArray(); //b图副本

            ///把所有点压在 主面“共面”上
            List<int> maxVecsOfCoplane = FindMaxPlaneIndices(aVecs);// 找出A图共面最多点的索引
            StrPrint("提示：正在进行“共面”检测，需要一些时间请耐心等待。。。");
            if (maxVecsOfCoplane.Count < 4) //a图至少要有4个点 共面
            {
                StrPrint("执行失败：起始图案至少有4个点以上共面");
                //StrPrint($"-------错层结束-------");
                return re;
            }
            ///共面上取三个点
            Vector3 vec0 = new_aVecs[maxVecsOfCoplane[0]];
            Vector3 vec1 = new_aVecs[maxVecsOfCoplane[1]];
            Vector3 vec2 = new_aVecs[maxVecsOfCoplane[2]];
            ///遍历 把a图和b图点压到 “共面”上
            for (int i = 0; i < planeCou; i++) 
            {
                if (!(maxVecsOfCoplane.Contains(i))) //除去在共面内的面
                {
                    new_aVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_aVecs[i]); //压平到共面上
                }
                new_bVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_bVecs[i]); //压平到共面上
            }
            
            ///计算法线向量
            Vector3 side1 = vec1 - vec0;
            Vector3 side2 = vec2 - vec0;
            Vector3 normal = CrossPro(side1, side2);
            Vector3 normalScalar = normal.NormalizEd();//法线标量
            ///开始错层
            for (int i = 0; i < planeCou; i++)
            {
                int shiftCou = 1; //记录循环次数 即层数
                Vector3 aOrigin = new_aVecs[i]; //原点位置
                Vector3 bOrigin = new_bVecs[i]; //原点位置
                while (OnlyImitation(i, new_aVecs, new_bVecs))
                {
                    Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
                    if (shiftCou % 2 == 1)
                    {
                        new_aVecs[i] = aOrigin + shiftVec;
                        new_bVecs[i] = bOrigin + shiftVec;
                    }
                    else
                    {
                        new_aVecs[i] = aOrigin - shiftVec;
                        new_bVecs[i] = bOrigin - shiftVec;
                    }
                    shiftCou += 1;
                }
            }
            ///计算碰撞
            planesCollision = AirImitation(aVecs, new_aVecs).Concat(AirImitation(new_aVecs, new_bVecs)).ToList(); //获取碰撞组
            planesCollision = planesCollision.Concat(AirImitation(new_bVecs, bVecs)).ToList();
            if (planesCollision.Count == 0)
            {
                re.Add(new_aVecs.ToList());
                re.Add(new_bVecs.ToList());
                StrPrint($"执行成功。");
            }
            else
            {
                StrPrint($"执行失败：计算完成后，检测有碰撞。可能原因：1.起始图形或结束图形点阵距离有过近情况。2.起始图形和结束图形点阵所在面不平行");
            }
            t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
            StrPrint($"用时：{t}秒");
            //StrPrint($"-------错层结束-------");
            return re;
        }
        /// <summary>
        /// 路径绕行
        /// </summary>
        /// <param name="aVecs">起始坐标集合</param>
        /// <param name="bVecs">终点做标集合</param>
        /// <param name="StrPrint">日志输出 回调函数</param>
        /// <param name="GetVal">进度日志输出 回调函数</param>
        /// <param name="cancellationToken">函数“取消执行”ps:new一个CancellationTokenSource类型 把实例.Token属性传进来，函数外部用实例.Cancel()函数控制实参值(bool)</param>
        /// <param name="isPass">out参数 返回true不碰撞程序直接返回 false有碰撞程序向下执行</param>
        /// <param name="mappingId">飞机真实序号的映射关系</param>
        /// <returns>返回一个二维数组 返回值长度0没有检测到碰撞或绕行失败 长度1为一个中间航点 长度为3为三个中间航点顺序(前中后)</returns>
        public static List<List<Vector3>> ABypassB(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint, Schedule GetVal, CancellationToken cancellationToken, out bool isPass, List<int> mappingId = null)
        {
            isPass = false;
            long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
            //StrPrint("-------3D绕行，开始-------");
            List<List<Vector3>> re = new List<List<Vector3>>();
            ///判断有没有碰撞 有碰撞继续 没有直接返回
            List<int[]> planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
            if (planesCollision.Count == 0)
            {
                StrPrint("执行成功：没有检测到碰撞，故不用添加中间航点。");
                //StrPrint($"-------3D绕行，结束-------");
                isPass = true;
                return re;//直接返回
            }
            ///飞机总数
            int planeCou = aVecs.Length;
            ///判断有没有给 映射的ID 没有就按顺序设置ID序号
            if (mappingId == null)
            {
                mappingId = new List<int>();
                for (int i = 1; i <= planeCou; i++)
                {
                    mappingId.Add(i);
                }
            }
            ///碰撞数
            int collisionCou;
            ///第一次绕行 中间1航点
            StrPrint($"3D航线第一次计算开始。");
            List<int> collisionGroup = TwoArrToArr(planesCollision); //整合数组
            List<Vector3> middleVecs = new List<Vector3>(); //中心航点坐标组
            for (int i = 0; i < planeCou; i++)
            {
                middleVecs.Add(SetMiddleVec(aVecs[i], bVecs[i])); //添加默认中间航点
            }
            collisionCou = collisionGroup.Count;
            while (true)
            {
                if (cancellationToken.IsCancellationRequested)//外部法取消指令
                {
                    StrPrint("取消3D航线操作。");
                    return null; // 退出函数
                }
                int progress = 0;//进度
                foreach (int i in collisionGroup)//开始绕碰撞组
                {
                    progress++;
                    GetVal(progress / collisionGroup.Count * 100);
                    List<Vector3> grv = GetRingVec(aVecs[i], bVecs[i], 0.5, 5, 4, 1500, 300);//中间可绕行航点列表
                    StrPrint($"3D航线，第一次计算进度：{progress}/{collisionGroup.Count}，本次计算{grv.Count}次");
                    foreach (Vector3 v in grv)
                    {
                        middleVecs[i] = v;
                        if (!OnlyImitation(i, aVecs, middleVecs) && !(OnlyImitation(i, middleVecs, bVecs)))
                        {
                            break;
                        }
                    }
                }
                planesCollision = AirImitation(aVecs, middleVecs).Concat(AirImitation(middleVecs, bVecs)).ToList(); //获取碰撞组
                collisionGroup = TwoArrToArr(planesCollision); //整合数组
                //如果绕行成功  或者 绕行结果和上次一样没有变化甚碰撞变多 则都退出循环
                if (collisionGroup.Count == 0 || collisionCou <= collisionGroup.Count)
                {
                    collisionCou = collisionGroup.Count;
                    break;
                }
                collisionCou = collisionGroup.Count;
            }
            //没有碰撞 返回一个中间航点 并返回
            if (collisionGroup.Count == 0)
            {
                StrPrint("执行成功：第一次计算即成功！");
                t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
                StrPrint($"用时：{t}秒");
                //StrPrint($"-------3D绕行，结束-------");
                re.Add(middleVecs);
                return re;
            }
            else
            {
                string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
                StrPrint($"3D航线第一次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
                //StrPrint("第一次绕行未成功!");
            }

            ///第二次绕行 两头 两航点
            StrPrint($"3D航线第二次计算开始。");
            bool isPassMark = false;
            planesCollision = AirImitation(aVecs, bVecs); //获取碰撞组
            collisionGroup = TwoArrToArr(planesCollision); //整合数组
            List<Vector3> secondMiddleVecsOne = new List<Vector3>(); //中心航点坐标组1
            List<Vector3> secondMiddleVecsTwo = new List<Vector3>(); //中心航点坐标组2
            for (int i = 0; i < planeCou; i++)
            {
                secondMiddleVecsOne.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.1)); //添加中间航点1
                secondMiddleVecsTwo.Add(SetMiddleVec(aVecs[i], bVecs[i], 0.9)); //添加中间航点2
            }
            collisionCou = collisionGroup.Count;
            while (true)
            {
                if (cancellationToken.IsCancellationRequested)//外部法取消指令
                {
                    StrPrint("取消3D航线操作。");
                    return null; // 退出函数
                }
                int progress = 0;//进度
                foreach (int i in collisionGroup)//开始绕碰撞组
                {
                    progress++;
                    GetVal(progress / collisionGroup.Count * 100);
                    //StrPrint($"迭代{c}次{i}号绕行");
                    List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0, 30, 10, 600, 300);//中间可绕行航点列表
                    sgrv1.Insert(0, secondMiddleVecsOne[i]);
                    List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 1, 30, 10, 600, 300);//中间可绕行航点列表
                    sgrv2.Insert(0, secondMiddleVecsTwo[i]);
                    StrPrint($"3D航线，第二次计算进度：{progress}/{collisionGroup.Count}，本次绕行{sgrv1.Count * sgrv2.Count}次");
                    foreach (Vector3 v1 in sgrv1)
                    {
                        secondMiddleVecsOne[i] = v1;
                        foreach (Vector3 v2 in sgrv2)
                        {
                            secondMiddleVecsTwo[i] = v2;
                            if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs))
                            {
                                isPassMark = true;
                                break;
                            }
                            isPassMark = false;
                        }
                        if (isPassMark)
                        {
                            break; //不撞则跳出 进行下一航线的绕行
                        }
                    }
                }
                planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, secondMiddleVecsTwo)).ToList(); //获取碰撞组
                planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList();
                collisionGroup = TwoArrToArr(planesCollision); //整合数组
                //如果绕行成功  或者 绕行结果和上次一样没有变化甚碰撞变多 则都退出循环
                if (collisionGroup.Count == 0 || collisionCou <= collisionGroup.Count)
                {
                    collisionCou = collisionGroup.Count;
                    break;
                }
                collisionCou = collisionGroup.Count;
            }
            //没有碰撞 返回两个中间航点 并返回
            if (collisionGroup.Count == 0)
            {
                StrPrint("执行成功：第二次计算成功！");
                t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
                StrPrint($"用时：{t}秒");
                //StrPrint($"-------3D绕行，结束-------");
                re.Add(secondMiddleVecsOne);
                re.Add(secondMiddleVecsTwo);
                return re;
            }
            else
            {
                string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
                StrPrint($"3D航线第二次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
                //StrPrint("第二次绕行未成功!");
            }

            ///第三次绕行 两头 两航点 中间一行点（实际添加两航点 但是0.6位置航点暂不启用留给第四次绕行） 沿用第二次绕行
            StrPrint($"3D航线第二次计算开始。");
            isPassMark = false;
            List<Vector3> thirdMiddleVecs = new List<Vector3>(); //中心航点坐标组1
            for (int i = 0; i < planeCou; i++)
            {
                thirdMiddleVecs.Add(SetMiddleVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5)); //添加中间航点(保持二次绕行的两端航点 在两端航点中间添加)
            }
            while (true)
            {
                if (cancellationToken.IsCancellationRequested)//外部法取消指令
                {
                    StrPrint("取消3D航线操作。");
                    return null; // 退出函数
                }
                int progress = 0;//进度
                foreach (int i in collisionGroup)//开始绕碰撞组
                {
                    GetVal(progress / collisionGroup.Count * 100);
                    progress++;
                    //StrPrint($"迭代{c}次{i}号绕行");
                    List<Vector3> sgrv1 = GetRingVec(aVecs[i], bVecs[i], 0, 100, 10, 600, 300);//中间可绕行航点列表
                    sgrv1.Insert(0, secondMiddleVecsOne[i]);
                    List<Vector3> sgrv2 = GetRingVec(aVecs[i], bVecs[i], 1, 100, 10, 600, 300);//中间可绕行航点列表
                    sgrv2.Insert(0, secondMiddleVecsTwo[i]);
                    List<Vector3> grv = GetRingVec(secondMiddleVecsOne[i], secondMiddleVecsTwo[i], 0.5, 80, 4, 1500, 300);//中间可绕行航点列表
                    StrPrint($"进度：{progress}/{collisionGroup.Count}，本次绕行{sgrv1.Count * sgrv2.Count * grv.Count}次");
                    foreach (Vector3 vm in grv)
                    {
                        thirdMiddleVecs[i] = vm;
                        foreach (Vector3 v1 in sgrv1)
                        {
                            secondMiddleVecsOne[i] = v1;
                            foreach (Vector3 v2 in sgrv2)
                            {
                                secondMiddleVecsTwo[i] = v2;
                                if (!OnlyImitation(i, aVecs, secondMiddleVecsOne) && !OnlyImitation(i, secondMiddleVecsOne, thirdMiddleVecs) && !OnlyImitation(i, thirdMiddleVecs, secondMiddleVecsTwo) && !OnlyImitation(i, secondMiddleVecsTwo, bVecs))
                                {
                                    isPassMark = true;
                                    break;
                                }
                                isPassMark = false;
                            }
                            if (isPassMark)
                            {
                                break; //不撞则跳出 进行下一航线的绕行
                            }
                        }
                        if (isPassMark)
                        {
                            break; //不撞则跳出 进行下一航线的绕行
                        }
                    }
                }
                planesCollision = AirImitation(aVecs, secondMiddleVecsOne).Concat(AirImitation(secondMiddleVecsOne, thirdMiddleVecs)).ToList(); //获取碰撞组
                planesCollision = planesCollision.Concat(AirImitation(thirdMiddleVecs, secondMiddleVecsTwo)).ToList();
                planesCollision = planesCollision.Concat(AirImitation(secondMiddleVecsTwo, bVecs)).ToList();
                collisionGroup = TwoArrToArr(planesCollision); //整合数组
                //如果绕行成功  或者 绕行结果和上次一样没有变化甚碰撞变多 则都退出循环
                if (collisionGroup.Count == 0 || collisionCou <= collisionGroup.Count)
                {
                    collisionCou = collisionGroup.Count;
                    break;
                }
                collisionCou = collisionGroup.Count;
            }
            //没有碰撞 返回三个中间航点 并返回
            if (collisionGroup.Count == 0)
            {
                StrPrint("执行成功：第三次计算成功！");
                t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
                StrPrint($"用时：{t}秒");
                re.Add(secondMiddleVecsOne);
                re.Add(thirdMiddleVecs);
                re.Add(secondMiddleVecsTwo);
                return re;
            }
            else
            {
                string mappingOutput = string.Join(", ", collisionGroup.Select(index => $"{mappingId[index]}号")); // 构建映射关系字符串
                StrPrint($"3D航线第三次计算之后，仍有{collisionGroup.Count}架有碰撞：{mappingOutput}！");
                StrPrint("执行失败：3D航线经过三次计算，仍有碰撞。");
            }

            ///end
            t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
            StrPrint($"用时：{t}秒");
            //StrPrint($"-------3D绕行，结束-------");
            return re;
        }
        /// <summary>
        /// 在圆圈上 用固定弦长手拉手 分割点
        /// </summary>
        /// <param name="radius">大圆半径</param>
        /// <param name="chordLength">固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集</param>
        /// <returns></returns>
        private static List<Vector3> GetVecsOnCircle(double radius, double chordLength)
        {
            List<Vector3> vecs = new List<Vector3>();
            // 计算圆的周长
            double circumference = (double)(2 * Math.PI * radius);
            // 计算弧上的点数
            int numberOfPoints = (int)(circumference / chordLength);
            // 计算每个点的弧度
            double angleIncrement = (double)(2 * Math.PI / numberOfPoints);
            // 生成点的坐标
            for (int i = 0; i < numberOfPoints; i++)
            {
                double angle = i * angleIncrement;
                double x = radius * (double)Math.Cos(angle);
                double y = radius * (double)Math.Sin(angle);
                double z = 0.0; // 如果是在平面上，Z 可能是 0
                vecs.Add(new Vector3(x, y, z));
            }
            return vecs;
        }
        /// <summary>
        /// 获取a指向b的向量矩阵 ps:偏移次数 加1 会在终点前后往返偏移
        /// </summary>
        /// <param name="aVec">a向量</param>
        /// <param name="bVec">b向量</param>
        /// <param name="middleProportion">默认中点位置比例</param>
        /// <param name="offCount">偏移次数 从0开始,每+1在中点前后往返偏移</param>
        /// <param name="layHight">偏移层高</param>
        /// <param name="direction">层排布方向 "retrun"前后堆叠 "forward"向前排列（如：起点向目标点方向） "backward"向后排列</param>
        /// <returns>矩阵</returns>
        private static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion, int offCount, double layHight, string direction)
        {
            /// 计算前向向量 k帽
            Vector3 k_hat = (bVec - aVec).NormalizEd();
            /// 计算右向量，使用 Vector3.UnitY 作为上向量 i帽
            //Vector3 i_hat = CrossPro(Vector3.UnitY, k_hat).NormalizEd(); //固定方向i帽
            Random random = new Random(RandomSeed);// 生成一个随机的单位向量
            Vector3 randomVector = new Vector3((double)random.NextDouble(), (double)random.NextDouble(), (double)random.NextDouble());
            Vector3 i_hat = CrossPro(k_hat, randomVector).NormalizEd();// 随机方向i帽
            /// 计算上向量 j帽
            Vector3 j_hat = CrossPro(k_hat, i_hat);
            ///计算 对应的偏移比例
            double length = (bVec - aVec).GetMag();
            double offShift = middleProportion; //偏移比例
            if (direction == "retrun")
            {
                if (offCount % 2 == 1) //奇数
                    offShift = middleProportion + (offCount + 2) / 2 * layHight / length;
                else //偶数
                    offShift = middleProportion - (offCount + 1) / 2 * layHight / length;
            }
            else if (direction == "forward")
            {
                offShift = middleProportion + offCount * layHight / length;
            }
            else if (direction == "backward")
            {
                offShift = middleProportion - offCount * layHight / length;
            }

            /// 计算偏向量
            Vector3 off = aVec + (bVec - aVec) * offShift;

            /// 构建矩阵
            Matrix matrix = new Matrix
            {
                M11 = k_hat.X,
                M12 = k_hat.Y,
                M13 = k_hat.Z,
                M21 = i_hat.X,
                M22 = i_hat.Y,
                M23 = i_hat.Z,
                M31 = j_hat.X,
                M32 = j_hat.Y,
                M33 = j_hat.Z,
                M41 = off.X,
                M42 = off.Y,
                M43 = off.Z
            };
            return matrix;
        }
        /// <summary>
        /// a b点中间的绕行航点列表
        /// </summary>
        /// <param name="aVec">起点</param>
        /// <param name="bVec">目标点</param>
        /// <param name="middleProportion">中间航点位置比例</param>
        /// <param name="transfer">绕行航点密度(值越小密度越大)  ps:传递圈函数的弦长 向量矩阵函数的层高</param>
        /// <param name="paunch">绕行航点范围(值越小范围越大 如:参数给4 圆盘的半径是a到b距离的1/4)   ps:决定层的厚度 和 圈的直径 为 paunch/航线长度</param>
        /// <param name="maxPaunchRadius">设定圆盘半径 的最大值 单位是厘米</param>
        /// <param name="direction">层排布方向 "retrun"前后堆叠 "forward"向前排列（如：起点向目标点方向） "backward"向后排列</param>
        /// <returns>绕行航点列表</returns>
        public static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer, double paunch, double maxPaunchRadius, double minPaunchRadius, string direction = "retrun")
        {
            List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
            /// 根据a到b的长度 算出中间绕行几圈
            double discRadius = GageLength(aVec, bVec) / paunch;//圆盘半径
            if (discRadius > maxPaunchRadius)
            {
                discRadius = maxPaunchRadius;//设定圆盘直径上限
            }
            if (discRadius < minPaunchRadius)
            {
                discRadius = minPaunchRadius;//设定圆盘直径下限
            }
            int ringCou = (int)Math.Ceiling(discRadius / transfer); //算层数和圈数 ps:层的厚度 和 圈的直径 为 paunch/航线长度
            /// 不是单圈的话 设置层数跟圈数相等
            int layCou = ringCou;
            if (singleCircle) layCou = 1;
            ///遍历出所有绕行航点
            for (int z = 0; z < layCou; z++) //迭代层
            {
                Matrix mat = GetBasisMatrix(aVec, bVec, middleProportion, z, transfer, direction); //根据圈数越多 偏移层数也越多 即每层矩阵off位置
                for (int i = 0; i < ringCou; i++) //迭代圈
                {
                    if (i != 0)
                    {
                        List<Vector3> tempCi = GetVecsOnCircle(i * transfer, transfer);
                        foreach (Vector3 vec in tempCi)
                        {
                            ringVec.Add(vec.Multiply(mat));//按照矩阵旋转之后 添加到中间航点列表
                        }
                    }
                    else
                    {
                        ringVec.Add(mat[3]); //第一次循环 并非圈 只在航线上 按层比例取点 即可
                    }
                }
            }
            return ringVec;
        }
        /// <summary>
        /// 按照矩阵 拉散图案
        /// </summary>
        /// <param name="aVecs">平面图案坐标组</param>
        /// <param name="bVecs">回归矩阵坐标组</param>
        /// <param name="strPrint">日志输出 回调函数</param>
        /// <param name="pullingDistance">拉散层距</param>
        /// <returns>拉散图案的坐标组</returns>
        public static Vector3[] NormalPull(Vector3[] aVecs, Vector3[] bVecs, SomeCalculateWay StrPrint,double pullingDistance = 300)
        {
            Vector3[] new_aVecs = aVecs.ToArray();  //a图副本
            Vector3[] new_bVecs = bVecs.ToArray(); //矩阵副本
            int planeCou = new_aVecs.Length; //获取飞机总数
            ///判断a图是不是平面
            if (!(planeCou == FindMaxPlaneIndices(aVecs).Count))
            {
                StrPrint("-------前图航点非平面图形,故不能做拉散图案操作-------");
                return null;
            }
            ///a图b图 中心
            Vector3 aCenterPos = GetPosCenter(new_aVecs, false);
            ///判断bVec 矩阵的数量长宽
            for (int i = 0; i < planeCou; i++)//把矩阵高度压平
            {
                new_bVecs[i] = new Vector3(new_bVecs[i].X, 0, new_bVecs[i].Z);
            }
            int row = 1;
            for (int i = 0; i < planeCou - 2; i++)
            {
                if (!(IsVecsOnLine(new_bVecs[i], new_bVecs[i + 1], new_bVecs[i + 2])))
                {
                    row = i + 2;//列
                    break;
                }
            }
            int ran = (int)Math.Ceiling((double)planeCou / (double)row);//行
            StrPrint($"{ran}行{row}列");
            if (ran > 2)
            {
                for (int i = 0; i < ran - 2; i++)
                {
                    if (!(IsVecsOnLine(new_bVecs[i * row], new_bVecs[(i + 1) * row], new_bVecs[(i + 2) * row])))
                    {
                        StrPrint("-------降落航点非常规矩阵,故不能做拉散图案操作-------");
                        return null;
                    }
                }
            }
            ///计算a图的法线标量
            Vector3 side1 = new_aVecs[1] - new_aVecs[0];
            Vector3 side2 = new_aVecs[2] - new_aVecs[0];
            Vector3 normal = CrossPro(side1, side2);
            Vector3 normalScalar = normal.NormalizEd();//法线标量
            normalScalar.Y = 0;//高度上压平
            ///判断a图 法线朝向 和 矩阵“平行”方向  获取此方向层数 ps:用于a图的拉散
            if (GageLength(normalScalar, new_bVecs[0]) < GageLength(normalScalar * -1, new_bVecs[0]))
            {
                normalScalar *= -1;//法线选择方向为 靠近矩阵1号机的方向 ps:由于取的三个点位置随机 按照右手定则 法线方向也随机 
            }
            if (Math.Abs(AngleBetween(new_bVecs[row] - new_bVecs[0], normalScalar) - 90) < Math.Abs(AngleBetween(new_bVecs[1] - new_bVecs[0], normalScalar) - 90))
            {
                /// 图案“如平行于0 21 41..” 平行于列
                for (int k = 0; k < row; k++)
                {
                    for (int i = 0; i < ran; i++)
                    {
                        int cou = i * row + k;
                        if (cou >= planeCou) break;// 溢出跳出
                        ///判断图在矩阵的左方 还是右方 
                        if (GageLength(aCenterPos, new_bVecs[0]) > GageLength(aCenterPos, new_bVecs[row - 1]))
                        {
                            new_aVecs[cou] -= normalScalar * (row - k) * pullingDistance;//左方
                        }
                        else
                        {
                            new_aVecs[cou] += normalScalar * k * pullingDistance;//右方
                        }
                    }
                }
                ///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
                if (IsPointBetweenLines(new_bVecs[(int)Math.Ceiling(((double)row / 5)) - 1], new_bVecs[(int)Math.Ceiling(((double)row / 5)) - 1 + row], new_bVecs[row - ((int)Math.Ceiling(((double)row / 5)) - 1)], new_bVecs[(row - ((int)Math.Ceiling(((double)row / 5)) - 1)) + row], aCenterPos))
                {
                    if (GageLength(new_aVecs[0], aVecs[0]) > GageLength(new_aVecs[planeCou - 1], aVecs[planeCou - 1]))//判断最大偏移量 是第一排 还是最后一排
                    {
                        Vector3 offPos = (new_aVecs[0] - aVecs[0]) * 0.5;//偏移量 的一半
                        for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
                        {
                            new_aVecs[i] -= offPos;
                        }
                    }
                    else
                    {
                        Vector3 offPos = (new_aVecs[planeCou - 1] - aVecs[planeCou - 1]) * 0.5;//偏移量 的一半
                        for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
                        {
                            new_aVecs[i] -= offPos;
                        }
                    }
                }

            }
            else
            {
                /// 图案“如平行于 0-20” 平行于行
                for (int k = 0; k < ran; k++)
                {
                    for (int i = 0; i < row; i++)
                    {
                        int cou = k * row + i;
                        //StrPrint($"{cou}");
                        if (cou >= planeCou) break;// 溢出跳出
                        ///判断图在矩阵的上方 还是下方
                        if (GageLength(aCenterPos, new_bVecs[0]) > GageLength(aCenterPos, new_bVecs[row * (ran - 1)]))
                        {
                            new_aVecs[cou] -= normalScalar * (ran - k) * pullingDistance;//上方
                        }
                        else
                        {
                            new_aVecs[cou] += normalScalar * k * pullingDistance;//下方
                        }
                    }
                }
                ///判断a图的中心点 是否在矩阵内部 在矩阵内部 做一个a b两组中心点对其 ps:相当于朝两边拉散
                if (IsPointBetweenLines(new_bVecs[(int)Math.Ceiling((double)ran / 5) * row - row], new_bVecs[(int)Math.Ceiling((double)ran / 5) * row - 1], new_bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row], new_bVecs[(ran - (int)Math.Ceiling((double)ran / 5)) * row + row - 1], aCenterPos))
                {
                    if (GageLength(new_aVecs[0], aVecs[0]) > GageLength(new_aVecs[planeCou - 1], aVecs[planeCou - 1]))//判断最大偏移量 是第一排 还是最后一排
                    {
                        Vector3 offPos = (new_aVecs[0] - aVecs[0]) * 0.5;//偏移量 的一半
                        for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
                        {
                            new_aVecs[i] -= offPos;
                        }
                    }
                    else
                    {
                        Vector3 offPos = (new_aVecs[planeCou - 1] - aVecs[planeCou - 1]) * 0.5;//偏移量 的一半
                        for (int i = 0; i < planeCou; i++)//所有飞机重新计算偏移量 ps:向两侧拉开
                        {
                            new_aVecs[i] -= offPos;
                        }
                    }
                }
            }
            return new_aVecs;
        }
    }
}