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

namespace FlyBase
{
    //矩阵类
    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 vector)
            {
                // 比较向量的每个分量
                return X == vector.X &&
                       Y == vector.Y &&
                       Z == vector.Z;
            }

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

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

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

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

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

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

            // 生成一个随机数，用于从最小值下标列表中随机选择一个下标
            Random random = new Random();
            return minIndices[random.Next(minIndices.Count)];
        }
        /// <summary>
        /// 二维数组转一维数组 并去重
        /// </summary>
        /// <param name="twoArr">二维数组</param>
        /// <returns>去重一维数组</returns>
        private static ArrayList TwoArrToArr(ArrayList twoArr)
        {
            ArrayList arr = new ArrayList();
            foreach (int[] item in twoArr)
            {
                int i = arr.IndexOf(item[0]);
                if (i < 1) arr.Add(item[0]);
                int x = arr.IndexOf(item[1]);
                if (x < 1) arr.Add(item[1]);
            }
            return arr;
        }
        /// <summary>
        /// 二维数组转一维数组 并去重
        /// </summary>
        /// <param name="twoArr">二维数组</param>
        /// <returns>去重一维数组</returns>
        private static List<int> TwoArrToArr(List<int[]> twoArr)
        {
            List<int> arr = new List<int>();
            foreach (int[] item in twoArr)
            {
                int i = arr.IndexOf(item[0]);
                if (i < 1) arr.Add(item[0]);
                int x = arr.IndexOf(item[1]);
                if (x < 1) arr.Add(item[1]);
            }
            return arr;
        }
        /// <summary>
        /// 处理二维数组 把有关联的子数组合并 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
        /// </summary>
        /// <param name="arr">需要处理的二维数组 </param>
        /// <returns>处理完成的数组</returns>
        private static List<List<int>> FindConnected(List<int[]> arr)
        {
            Dictionary<int, List<int>> graph = new Dictionary<int, List<int>>();
            Dictionary<int, bool> visited = new Dictionary<int, bool>();
            List<List<int>> result = new List<List<int>>();
            void DFS(int node, List<int> connected)
            {
                visited[node] = true;
                connected.Add(node);

                foreach (var neighbor in graph[node])
                {
                    if (!visited[neighbor])
                    {
                        DFS(neighbor, connected);
                    }
                }
            }
            // 构建图
            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);
                    result.Add(connected);
                }
            }
            return result;
        }
        /// <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>
        public 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[] aVecs, Vector3[] bVecs, List<int[]> match)
        {
            Dictionary<int, int> indexMap = new Dictionary<int, int>();
            // 构建索引映射
            foreach (var pair in match)
            {
                indexMap[pair[0]] = pair[1];
            }
            // 根据映射构建新的bb数组
            Vector3[] new_bVecs = new Vector3[bVecs.Length];
            foreach (var entry in indexMap)
            {
                int newIndex = entry.Value;
                new_bVecs[newIndex] = bVecs[entry.Key];
            }
            return new_bVecs;
        }
        /// <summary>
        /// 从一组向量集合中 不重复的随机选择4个向量为一组 最多100组 组合成二维数组 如:[[1,2,3,4][5,6,7,8]...]
        /// </summary>
        /// <param name="vecs">坐标集和</param>
        /// <returns>re空数组则代表不够4个坐标 </returns>
        private static List<Vector3[]> RandomSel4Vec(Vector3[] vecs)
        {
            List<Vector3[]> result = new List<Vector3[]>();
            int len = vecs.Length;
            // 如果坐标数组少于4个，或者为空，则返回空列表
            if (len < 4)
            {
                return result;
            }
            // 确定最大可选择的组数，最多为100组
            int numGroups = Math.Min(len / 4, 100);
            List<Vector3> flattenedList = vecs.ToList();
            Random random = new Random();
            // 遍历每一组
            for (int i = 0; i < numGroups; i++)
            {
                List<Vector3> selectedGroup = new List<Vector3>();

                // 随机选择4个不重复的坐标
                for (int j = 0; j < 4; j++)
                {
                    int index = random.Next(flattenedList.Count);
                    selectedGroup.Add(flattenedList[index]);
                    flattenedList.RemoveAt(index);
                }

                // 将选择的坐标组成的 List 转换为数组，并添加到结果中
                result.Add(selectedGroup.ToArray());
            }
            return result;
        }
        /// <summary>
        /// 设置中间航点
        /// </summary>
        /// <param name="aVec">起点</param>
        /// <param name="bVec">目标点</param>
        /// <param name="middlePos">比例</param>
        /// <returns></returns>
        private static Vector3 SetMiddleVec(Vector3 aVec, Vector3 bVec, double middlePos = 0.5)
        {
            return (bVec - aVec) * middlePos + aVec;
        }
        /// <summary>
        /// 两点距离
        /// </summary>
        /// <param name="v1">坐标1</param>
        /// <param name="v2">坐标2</param>
        /// <returns>两点之间距离</returns>
        private static double GageLength(Vector3 v1, Vector3 v2)
        {
            return Math.Sqrt(GageLengthSquare(v1, v2));
        }
        /// <summary>
        /// 两点距离的平方
        /// </summary>
        /// <param name="v1">坐标1</param>
        /// <param name="v2">坐标2</param>
        /// <returns>两点距离的平方</returns>
        private static double GageLengthSquare(Vector3 v1, Vector3 v2)
        {
            return Math.Pow(v1.X - v2.X, 2) + Math.Pow(v1.Y - v2.Y, 2) + Math.Pow(v1.Z - v2.Z, 2);
        }
        /// <summary>
        /// 点积
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>点积</returns>
        private static double DotPro(Vector3 v1, Vector3 v2)
        {
            return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
        }
        /// <summary>
        /// 叉积
        /// </summary>
        /// <param name="v1">向量1</param>
        /// <param name="v2">向量2</param>
        /// <returns>两个向量叉积</returns>
        private static Vector3 CrossPro(Vector3 v1, Vector3 v2)
        {
            double x = v1.Y * v2.Z - v1.Z * v2.Y;
            double y = v1.Z * v2.X - v1.X * v2.Z;
            double z = v1.X * v2.Y - v1.Y * v2.X;
            return new Vector3(x, y, z);
        }
        /// <summary>
        /// 检查4个点是否在一个平面上
        /// </summary>
        /// <param name="vector1">点1</param>
        /// <param name="vector2">点2</param>
        /// <param name="vector3">点3</param>
        /// <param name="vector4">点4</param>
        /// <returns>true在一个平面 false不在一个平面</returns>
        public static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
        {
            // 计算两条边
            Vector3 side1 = vec2 - vec1;
            Vector3 side2 = vec3 - vec2;
            // 计算法线向量
            Vector3 normal = CrossPro(side1, side2);
            // 计算第四个点到第一个点的向量
            Vector3 toPoint4 = vec4 - vec1;
            // 设置容差值，根据具体情况调整
            float tolerance = 0.01f;
            // 判断四个点是否在同一个平面上
            return Math.Abs(DotPro(toPoint4, normal)) < tolerance;
        }
        /// <summary>
        /// 判断4个点是否在同一条直线上
        /// </summary>
        /// <param name="vector1">点1</param>
        /// <param name="vector2">点2</param>
        /// <param name="vector3">点3</param>
        /// <param name="vector4">点4</param>
        /// <returns>true在一条直线上 false不在一条直线上</returns>
        public static bool IsVecsOnLine(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
        {
            // 计算相邻点之间的方向向量
            Vector3 dir1 = new Vector3(vec2.X - vec1.X, vec2.Y - vec1.Y, vec2.Z - vec1.Z);
            Vector3 dir2 = new Vector3(vec3.X - vec2.X, vec3.Y - vec2.Y, vec3.Z - vec2.Z);
            Vector3 dir3 = new Vector3(vec4.X - vec3.X, vec4.Y - vec3.Y, vec4.Z - vec3.Z);

            // 检查任意两个相邻方向向量的叉积是否为零
            // 这是在三维空间中判断共线的条件
            return CrossPro(dir1, dir2).IsZero() && CrossPro(dir2, dir3).IsZero();
        }
        /// <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>
        /// 坐标置换 向量乘以矩阵(i帽j帽k帽)得到置换后的向量坐标 ps:x指向i帽 y轴指向j帽 z轴指向k帽
        /// </summary>
        /// <param name="vec">要变换的向量</param>
        /// <param name="mat">矩阵标量 既i帽 j帽 k帽 三点的坐标组成的矩阵 “可以想象成臂长为1的操作轴图标”</param>
        /// <returns></returns>
        private static Vector3 MatrixMul(Vector3 vec, Vector3[] mat)
        {
            Vector3 re = new Vector3();//声明返回值
            re.X = vec.X * mat[0].X + vec.Y * mat[1].X + vec.Z * mat[2].X;
            re.Y = vec.X * mat[0].Y + vec.Y * mat[1].Y + vec.Z * mat[2].Y;
            re.Z = vec.X * mat[0].Z + vec.Y * mat[1].Z + vec.Z * mat[2].Z;
            return re;
        }
        /// <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>
        public static double RecentlySquareOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            bool IsEqual(double x, double y)
            {
                if (Math.Abs(x - y) < 1e-7)
                {
                    return true;
                }
                return false;
            }
            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;//两线最近距离的平方
        }
        /// <summary>
        /// 按比例在两条线段上截取对应点间的最小距离 平方
        /// </summary>
        /// <param name="a1">线段1起始点</param>
        /// <param name="a2">线段1起终点</param>
        /// <param name="b1">线段2起始点</param>
        /// <param name="b2">线段2起终点</param>
        /// <returns>最小距离的平方值</returns>
        public 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>
        public 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="startPos">始点坐标集合</param>
        /// <param name="endPos">终点坐标集合</param>
        /// <param name="lineDistanceSquare">航线最小间距平方值</param>
        /// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
        /// <returns></returns>
        public static List<int[]> AirImitation(Vector3[] startPos, Vector3[] endPos, double lineDistanceSquare = 36100, double spaceBetweenSquare = 48400)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = startPos.Length; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
                    if (distanceSquare < lineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
                        if (planeLenSquare < spaceBetweenSquare)
                        {
                            planesCollision.Add(new int[] { a, a + 1 + b });
                        }
                    }
                }
            }
            return planesCollision;
        }
        /// <summary>
        /// 碰撞检测
        /// </summary>
        /// <param name="startPos">始点坐标集合</param>
        /// <param name="endPos">终点坐标集合</param>
        /// <param name="lineDistanceSquare">航线最小间距平方值</param>
        /// <param name="spaceBetweenSquare">飞行过程中最小间距平方值</param>
        /// <returns></returns>
        public static List<int[]> AirImitation(List<Vector3> startPos, List<Vector3> endPos, double lineDistanceSquare = 36100, double spaceBetweenSquare = 48400)
        {
            List<int[]> planesCollision = new List<int[]>(); //所有碰撞的组
            int planeCou = startPos.Count; //获取飞机总数
            for (int a = 0; a < planeCou; a++)
            {
                for (int b = 0; a + 1 + b < planeCou; b++)
                {
                    //判断两条轨迹 之间的最小距离
                    double distanceSquare = RecentlySquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[a + 1 + b]);
                    if (distanceSquare < lineDistanceSquare)
                    {
                        //判断飞机距离是否过近
                        double planeLenSquare = MinDistanceSquareOfLine(startPos[a], endPos[a], startPos[a + 1 + b], endPos[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="startPos">飞机起始坐标集合</param>
        /// <param name="endPos">飞机终点坐标集合</param>
        /// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
        /// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        public static bool OnlyImitation(int onlyPlaneId, Vector3[] startPos, Vector3[] endPos, double lineDistanceSquare = 32400, double spaceBetweenSquare = 90000)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < startPos.Length; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//航线最小距离
                if (distanceSquare < lineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < spaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 单机碰撞检测
        /// </summary>
        /// <param name="onlyPlaneId">飞机的id PS:id从0开始</param>
        /// <param name="startPos">飞机起始坐标集合</param>
        /// <param name="endPos">飞机终点坐标集合</param>
        /// <param name="lineDistanceSquare">航线小距离的平方对比值</param>
        /// <param name="spaceBetweenSquare">飞行过程中最小间距的平方对比值</param>
        /// <returns>true:有碰撞 false:无碰撞</returns>
        public static bool OnlyImitation(int onlyPlaneId, List<Vector3> startPos, List<Vector3> endPos, double lineDistanceSquare = 32400, double spaceBetweenSquare = 90000)
        {
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < startPos.Count; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                // 判断两条轨迹 之间的最小距离
                double distanceSquare = RecentlySquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//航线最小距离
                if (distanceSquare < lineDistanceSquare)
                {
                    double minDistanceSquare = MinDistanceSquareOfLine(startPos[onlyPlaneId], endPos[onlyPlaneId], startPos[contrastId], endPos[contrastId]);//按比例飞行最小间距
                    if (minDistanceSquare < spaceBetweenSquare)
                    {
                        return true;//返回有碰撞
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        /// <summary>
        /// 智能路径 规则：找ab组共同最外圈的点 然后对应找a或b里面最近点 为一组匹配 最后进行碰撞交叉互换
        /// </summary>
        /// <param name="startPos">起始点坐标组</param>
        /// <param name="endPos">目标点坐标组</param>
        /// <param name="swapCount">交换次数</param>
        /// <returns></returns>
        public static Vector3[] ContactABOut(Vector3[] aVecs, Vector3[] bVecs, int swapCount = 5)
        {
            int planeCou = aVecs.Length; // 飞机总数
            List<int[]> match = new List<int[]>(); // ab对应关系
            //记录a b集合索引
            List<int> aIndex = new List<int>();
            List<int> bIndex = new List<int>();
            for (int k = 0; k < planeCou; k++)
            {
                aIndex.Add(k);
                bIndex.Add(k);
            }
            // 遍历找出 AB集合对应关系
            for (int k = 0; k < planeCou; k++)
            {
                // 遍历每次刷新 剩下为未匹配总数
                int remainCou = aIndex.Count;
                // 遍历每次刷新 重心
                List<Vector3> allVecs = new List<Vector3>();
                for (int i = 0; i < remainCou; i++)
                {
                    allVecs.Add(aVecs[aIndex[i]]);
                    allVecs.Add(bVecs[bIndex[i]]);
                }
                Vector3 centerVec = GetPosCenter(allVecs);//重心点
                // 遍历所有ab点距离重心点的距离
                double[] aLens = new double[remainCou];
                double[] bLens = new double[remainCou];
                for (int i = 0; i < remainCou; i++)
                {
                    aLens[i] = GageLengthSquare(aVecs[aIndex[i]], centerVec);
                    bLens[i] = GageLengthSquare(bVecs[bIndex[i]], centerVec);
                }
                // 找出ab集合最外层坐标的下标 即离重心点最远的点
                int aMaxIndex = GetIndexOfMaxOrMin(aLens); // a集合到重心点最长的距离 数组的下标
                int bMaxIndex = GetIndexOfMaxOrMin(bLens); // b集合到重心点最长的距离 数组的下标
                if (aLens[aMaxIndex] > bLens[bMaxIndex])//找出 最外层如果为A集合点 对应B集合最近点 的下标
                {
                    //MessageBox.Show(aIndex[aMaxIndex].ToString());
                    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(aVecs, bVecs, match);// 按照映射 获取a 对应的 新的b集合
            return new_bVecs;

            //交叉 交换
            for (int i = 0; i < swapCount; i++)
            {
                List<int[]> planesCollision = AirImitation(aVecs, new_bVecs);// 获取碰撞组
                List<List<int>> formatCollision = FindConnected(planesCollision);// 格式化碰撞检测组 例如：[[0,2][0,3][3,4][5,6]] 结果[[0,2,3,4][5,6]]
                // 遍历所有交叉组 分组做交换
                foreach (List<int> swap_indices in formatCollision)
                {
                    if (swap_indices.Count <= 6) // 只尝试5组交叉碰撞以下的航线互换
                    {
                        //交叉 生成所有排列
                        List<List<int>> all_permutations = Permutations(swap_indices);
                        //删掉第一个 既原始排列
                        all_permutations.RemoveAt(0);
                        // 按所有的排列 互换航线 并检测出最佳的对应目标点
                        List<int> tempLen = new List<int>(); //记录所有排列的 的碰撞次数
                        List<Vector3[]> tempNew_bVecsS = new List<Vector3[]>();//记录所有排列交换之后的目标坐标集
                        foreach (List<int> indices in all_permutations)
                        {
                            Vector3[] current_array = new_bVecs.ToArray();//复制一个new_bVecs 副本
                            for (int k = 0; k < indices.Count - 1; k++)
                            {
                                int index1 = indices[k];
                                int index2 = indices[k + 1];
                                // 交换元素
                                Vector3 temp = current_array[index1];
                                current_array[index1] = current_array[index2];
                                current_array[index2] = temp;
                            }
                            tempLen.Add(AirImitation(aVecs, current_array).Count);// 迭代 记录所有排列的碰撞次数
                            tempNew_bVecsS.Add(current_array); //迭代 记录所有排列交换之后的目标坐标集
                        }
                        new_bVecs = tempNew_bVecsS[GetRandomMinIndex(tempLen)];
                    }
                }
            }
            return new_bVecs;
        }
        /// <summary>
        /// 智能挫层
        /// </summary>
        /// <param name="startPos">起始坐标集合</param>
        /// <param name="endPos">终点做标集合</param>
        /// <param name="layHight">挫层层高</param>
        /// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点</returns>
        public static List<Vector3[]> CollisionLayer(Vector3[] startPos, Vector3[] endPos, double layHight = 220)
        {
            List<Vector3[]> re = new List<Vector3[]>();
            //获取飞机总数
            int planeCou = startPos.Length;
            //检测两个图形是否在一个平面上
            Vector3[] allPos = startPos.Concat(endPos).ToArray();//合并两个集合
            List<Vector3[]> ppppVecArr = RandomSel4Vec(allPos);//随机选择
            if (ppppVecArr.Count == 0)//两个图形不够四个点 返回 空数组
            {
                return re;
            }
            // 使用 foreach 遍历 List<Vector3[]> 中的元素
            foreach (Vector3[] pppVec in ppppVecArr)
            {
                if (!(IsVecsOnPlane(pppVec[0], pppVec[1], pppVec[2], pppVec[3])))
                {
                    return re;//两个图形不在一个平面上 返回 空数组
                }

            }
            //遍历选出4个不共线的点 ps:方便后续叉积算法线标量
            bool isVecsOnLine = false; Vector3 vec0 = new Vector3(0, 0, 0); Vector3 vec1 = new Vector3(0, 0, 0); Vector3 vec2 = new Vector3(0, 0, 0); Vector3 vec3 = new Vector3(0, 0, 0);
            for (int i = 0; i < planeCou; i++) //遍历所有点 找出不在一条直线上的四个点
            {
                vec0 = startPos[i]; vec1 = endPos[planeCou - i - 1]; vec2 = startPos[planeCou - i - 1]; vec3 = endPos[i];
                isVecsOnLine = IsVecsOnLine(vec0, vec1, vec2, vec3);
                if (isVecsOnLine)
                {
                    break;
                }
            }
            if (isVecsOnLine)
            {
                return re;//两个图案的所有点都在一条直线上 返回 空数组
            }
            //检查完毕后 
            //计算法线向量
            Vector3 side1 = vec0 - vec1;
            Vector3 side2 = vec2 - vec3;
            Vector3 normal = CrossPro(side1, side2);
            Vector3 normalScalar = normal.NormalizEd();//法线标量
            //开始挫层
            for (int i = 0; i < planeCou; i++)
            {
                int shiftCou = 1; //记录循环次数 即层数
                Vector3 aOrigin = startPos[i]; //原点位置
                Vector3 bOrigin = endPos[i]; //原点位置
                while (OnlyImitation(i, startPos, endPos))
                {
                    Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
                    if (shiftCou % 2 == 1)
                    {
                        startPos[i] = aOrigin + shiftVec;
                        endPos[i] = bOrigin + shiftVec;
                    }
                    else
                    {
                        startPos[i] = aOrigin - shiftVec;
                        endPos[i] = bOrigin - shiftVec;
                    }
                    shiftCou += 1;
                }
            }
            re.Add(startPos);
            re.Add(endPos);
            return re;
        }
        /// <summary>
        /// 路径绕行
        /// </summary>
        /// <param name="startPos">起始坐标集合</param>
        /// <param name="endPos">终点做标集合</param>
        /// <returns>返回一个二维数组 长度为1即1个中间航点 长度为3即前中后三个中间航点</returns>
        public static List<List<Vector3>> ABypassB(Vector3[] aVecs, Vector3[] bVecs)
        {
            List<List<Vector3>> re = new List<List<Vector3>>();
            int planeCou = aVecs.Length;
            //第一次绕行
            //起始坐标组重心点 与 目标坐标组重心点
            Vector3 aCenterPoint = GetPosCenter(aVecs);
            Vector3 bCenterPoint = GetPosCenter(bVecs);
            //记录所有航线中心距 ps:起点离起点组中心点距离 + 目标点离目标点组中心点距离
            double[] centerDistanceS = new double[planeCou];
            for (int i = 0; i < planeCou; i++)
            {
                centerDistanceS[i] = GageLengthSquare(aVecs[i], aCenterPoint) + GageLengthSquare(bVecs[i], bCenterPoint);
            }
            //记录从中心开始排序
            int[] sortedIndices = Enumerable.Range(0, centerDistanceS.Length).ToArray();
            Array.Sort(sortedIndices, (x, y) => centerDistanceS[x].CompareTo(centerDistanceS[y]));// 对索引数组按原始数组的值进行排序
            //从中心航线 逐条绕行 ps:映射从中心内部到此到外部顺序
            List<Vector3> tempAVecs = new List<Vector3>();  // 逐条起点坐标组
            List<Vector3> tempBVecs = new List<Vector3>();  // 逐条目标点坐标组
            List<Vector3> middleVecs = new List<Vector3>(); //中心航点坐标组
            List<int> firstCollisionGroup = new List<int>(); //有碰撞飞机列表(第一次绕行)
            for (int i = 0; i < planeCou; i++)
            {
                bool isPassMark = false;//初始标记
                int index = sortedIndices[i];//从内到外排序的索引
                tempAVecs.Add(aVecs[index]);
                tempBVecs.Add(bVecs[index]);
                if (i == 0) //如果是第一条航线 直接添加中间航点
                {
                    middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); //添加中间航点
                }
                else //从第二条开始绕
                {
                    middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index])); // 添加默认中间航点
                    // 首次默认中间航点检测
                    if (!(OnlyImitation(i, tempAVecs, middleVecs))) //前半段碰 碰撞检测
                    {
                        if (!(OnlyImitation(i, middleVecs, tempBVecs))) //后半段碰 碰撞检测
                        {
                            isPassMark = true; //中间航点
                            continue; //不撞则跳出 进行下一航线的绕行
                        }
                        else
                        {
                            middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
                        }
                    }
                    else
                    {
                        middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
                    }
                    // 绕行列表遍历 检测
                    List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
                    foreach (Vector3 v in ringVecs)
                    {
                        middleVecs.Add(v);
                        //检测
                        if (!(OnlyImitation(i, tempAVecs, middleVecs))) //前半段碰 碰撞检测
                        {
                            if (!(OnlyImitation(i, middleVecs, tempBVecs))) //后半段碰 碰撞检测
                            {
                                isPassMark = true; //中间航点 碰撞检测通过标记
                                break; //不撞则跳出 进行下一航线的绕行
                            }
                            else
                            {
                                middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
                            }
                        }
                        else
                        {
                            middleVecs.RemoveAt(i); //碰撞检测未通过删除临时中间航点
                        }
                    }
                }
                if (!(isPassMark)) // 如果绕行失败 添加一个默认中间航点
                {
                    middleVecs.Add(SetMiddleVec(aVecs[index], bVecs[index]));
                    firstCollisionGroup.Add(index);
                }
            }
            //中间航点坐标集 按飞机id顺序重新映射 ps:恢复原id映射
            middleVecs = sortedIndices.Select(index => middleVecs[index]).ToList();
            //没有碰撞 返回一个中间航点 并返回
            if (firstCollisionGroup.Count == 0)
            {
                re.Add(middleVecs);
                return re;
            }

            //第二次绕行 ps:有碰撞进行第二次 前后再各加以航点  嵌套迭代
            List<Vector3> firstMiddleVecs = new List<Vector3>(); //前中间航点
            List<Vector3> secondMiddleVecs = new List<Vector3>(); //后中间航点
            List<int> secondCollisionGroup = new List<int>(); //最终有碰撞飞机列表
            for (int i = 0; i < planeCou; i++)
            {
                firstMiddleVecs.Add(SetMiddleVec(aVecs[i], middleVecs[i], 0));
                secondMiddleVecs.Add(SetMiddleVec(middleVecs[i], bVecs[i], 1));
            }
            foreach (int index in firstCollisionGroup)
            {
                bool isPassMark = false;
                // 正中 绕行列表
                List<Vector3> ringVecs = GetRingVec(aVecs[index], bVecs[index], 0.5); //所有可绕行的中间航点集合
                foreach (Vector3 r in ringVecs)
                {
                    middleVecs[index] = r; //中点逐点
                    List<Vector3> firstRingVecs = GetRingVec(aVecs[index], middleVecs[index], 0, 220, 6); //所有可绕行的 前中间航点集合
                    foreach (Vector3 fr in firstRingVecs)
                    {
                        firstMiddleVecs[index] = fr; //前中点逐点
                        List<Vector3> secondRingVecs = GetRingVec(middleVecs[index], bVecs[index], 1, 220, 6); //所有可绕行的 后中间航点集合
                        foreach (Vector3 sr in secondRingVecs)
                        {
                            secondMiddleVecs[index] = sr; //后中点逐点
                            // 检测
                            if (!(OnlyImitation(index, aVecs, firstMiddleVecs))){
                            }
                            //if not(self.onlyImitation(v, aVecs, firstMiddleVecs)): #前半段碰 碰撞检测
                            //        if not(self.onlyImitation(v, firstMiddleVecs, middleVecs)):#后半段碰 碰撞检测
                            //            if not(self.onlyImitation(v, middleVecs, secondMiddleVecs)): #前半段碰 碰撞检测
                            //                if not(self.onlyImitation(v, secondMiddleVecs, bVecs)):#后半段碰 碰撞检测
                            //                    isPassMark = True #碰撞检测通过标记
                            //                    break #不撞则跳出 进行下一航线的绕行
                        }
                    }
                }
            }

            return re;
        }
        /// <summary>
        /// 在圆圈上 用固定弦长手拉手 分割点
        /// </summary>
        /// <param name="radius">大圆半径</param>
        /// <param name="chordLength">固定弦长 ps:这个值决定绕行点一圈的密集成都值越小越密集</param>
        /// <returns></returns>
        public static List<Vector3> GetVecsOnCircle(double radius, double chordLength = 220)
        {
            List<Vector3> vecs = new List<Vector3>();
            // 计算圆的周长
            double circumference = (double)(2 * Math.PI * radius);
            // 计算弧上的点数
            int numberOfPoints = (int)(circumference / chordLength);
            // 计算每个点的弧度
            double angleIncrement = (double)(2 * Math.PI / numberOfPoints);
            // 生成点的坐标
            for (int i = 0; i < numberOfPoints; i++)
            {
                double angle = i * angleIncrement;
                double x = radius * (double)Math.Cos(angle);
                double y = radius * (double)Math.Sin(angle);
                double z = 0.0f; // 如果是在平面上，Z 可能是 0
                vecs.Add(new Vector3(x, y, z));
            }
            return vecs;
        }
        /// <summary>
        /// 获取a指向b的向量矩阵 ps:偏移次数 加1 会在终点前后往返偏移
        /// </summary>
        /// <param name="aVec">a向量</param>
        /// <param name="bVec">b向量</param>
        /// <param name="middleProportion">默认中点位置比例</param>
        /// <param name="offCount">偏移次数 从0开始,每+1在中点前后往返偏移</param>
        /// <param name="layHight">偏移层高</param>
        /// <returns>矩阵</returns>
        public static Matrix GetBasisMatrix(Vector3 aVec, Vector3 bVec, double middleProportion = 0.5, int offCount = 0, double layHight = 220)
        {
            // 计算前向向量 k帽
            Vector3 k_hat = (bVec - aVec).NormalizEd();
            // 计算右向量，使用 Vector3.UnitY 作为上向量 i帽
            Vector3 i_hat = CrossPro(Vector3.UnitY, k_hat).NormalizEd();
            // 计算上向量 j帽
            Vector3 j_hat = CrossPro(k_hat, i_hat);
            //计算 对应的偏移比例
            double length = (bVec - aVec).GetMag();
            double offShift;//偏移比例
            if (offCount % 2 == 1) //奇数
                offShift = middleProportion + (offCount + 2) / 2 * layHight / length;
            else //偶数
                offShift = middleProportion - (offCount + 1) / 2 * layHight / length;
            // 计算偏向量
            Vector3 off = aVec + (bVec - aVec) * offShift;
            // 构建矩阵
            Matrix matrix = new Matrix
            {
                M11 = k_hat.X,
                M12 = k_hat.Y,
                M13 = k_hat.Z,
                M21 = i_hat.X,
                M22 = i_hat.Y,
                M23 = i_hat.Z,
                M31 = j_hat.X,
                M32 = j_hat.Y,
                M33 = j_hat.Z,
                M41 = off.X,
                M42 = off.Y,
                M43 = off.Z
            };
            return matrix;
        }
        /// <summary>
        /// a b点中间的绕行航点列表
        /// </summary>
        /// <param name="aVec">起点</param>
        /// <param name="bVec">目标点</param>
        /// <param name="middleProportion">中间航点位置比例</param>
        /// <param name="transfer">传递圈函数的弦长 向量矩阵函数的层高</param>
        /// <param name="paunch">绕行航点范围 ps:绕行中间航点肚子(值越大肚子越小)</param>
        /// <returns></returns>
        public static List<Vector3> GetRingVec(Vector3 aVec, Vector3 bVec, double middleProportion, double transfer = 220, double paunch = 3)
        {
            List<Vector3> ringVec = new List<Vector3>(); //记录所有绕行中间航点坐标
            // 根据a到b的长度 算出中间绕行几圈
            int ringCou = (int)Math.Ceiling(GageLength(aVec, bVec) / paunch / transfer); //绝对中心绕行的圈数 即中间航点的'肚子'大小
            if (ringCou < 2) ringCou = 2;  //最少两圈 两层
            if (ringCou > 15) ringCou = 15; //最多15圈 15层
            for (int z = 0; z < ringCou; z++) //迭代层
            {
                Matrix mat = GetBasisMatrix(aVec, bVec, middleProportion, z, transfer); //根据圈数越多 偏移层数也越多 即每层矩阵off位置
                for (int i = 0; i < ringCou; i++) //迭代圈
                {
                    if (i != 0)
                    {
                        List<Vector3> tempCi = GetVecsOnCircle(i * transfer, transfer);
                        foreach (Vector3 vec in tempCi)
                        {
                            ringVec.Add(vec.Multiply(mat));//按照矩阵旋转之后 添加到中间航点列表
                        }
                    }
                    else
                    {
                        ringVec.Add(mat[3]); //第一次循环 并非圈 只在航线上 按层比例取点 即可
                    }
                }
            }
            return ringVec;
        }
    }
}