FlyTest/FlightRoute/FlightRoute.cs

using System;
using System.Collections;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;


namespace FlightRoute
{
    //定义坐标系统 结构体
    public struct Vector3
    {
        public double x;
        public double y;
        public double z;
        //初始化
        public Vector3(double x, double y, double z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }
        //重载二元坐标加法+
        public static Vector3 operator +(Vector3 v1, Vector3 v2)
        {
            return new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
        }
        //重载一元坐标加法+
        public static Vector3 operator +(Vector3 v1, double i)
        {
            return new Vector3(v1.x + i, v1.y + i, v1.z + i);
        }
        //重载二元坐标减法-
        public static Vector3 operator -(Vector3 v1, Vector3 v2)
        {
            return new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
        }
        //重载一元坐标加法-
        public static Vector3 operator -(Vector3 v1, double i)
        {
            return new Vector3(v1.x - i, v1.y - i, v1.z - i);
        }
        //重载一元坐标乘法*
        public static Vector3 operator *(Vector3 v1, double i)
        {
            return new Vector3(v1.x * i, v1.y * i, v1.z * i);
        }
        //重载一元坐标除法/
        public static Vector3 operator /(Vector3 v1, double i)
        {
            return new Vector3(v1.x / i, v1.y / i, v1.z / i);
        }
        //重载==
        public static bool operator ==(Vector3 v1, Vector3 v2)
        {
            if (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z) return true;
            return false;
        }
        //重载!=
        public static bool operator !=(Vector3 v1, Vector3 v2)
        {
            if (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z) return true;
            return false;
        }
        //模长
        public double getMag()
        {
            return Math.Sqrt(Math.Pow(this.x, 2) + Math.Pow(this.y, 2) + Math.Pow(this.z, 2));
        }
        //标准化
        public void normalize(double multiple = 1.0)
        {
            double magSq = Math.Pow(this.x, 2) + Math.Pow(this.y, 2) + Math.Pow(this.z, 2);
            if (magSq > 0)
            {
                double oneOverMag = multiple / Math.Sqrt(magSq);
                this.x *= oneOverMag;
                this.y *= oneOverMag;
                this.z *= oneOverMag;
            }
        }
        //标准化 返回一个标准化之后的值 不改变自身
        public Vector3 normalizEd(double multiple = 1.0)
        {
            Vector3 re = new Vector3();
            double magSq = Math.Pow(this.x, 2) + Math.Pow(this.y, 2) + Math.Pow(this.z, 2);
            if (magSq > 0)
            {
                double oneOverMag = multiple / Math.Sqrt(magSq);
                re.x = this.x * oneOverMag;
                re.y = this.y * oneOverMag;
                re.z = this.z * oneOverMag;
            }
            return re;
        }
        //归零 改变自身数值 一般配合归位使用
        public void setZero(Vector3 v2)
        {
            this.x -= v2.x;
            this.y -= v2.y;
            this.z -= v2.z;
        }
        //归零 返回一个归零值 不改变自身
        public Vector3 setZeroEd(Vector3 v2)
        {
            Vector3 re = new Vector3(this.x - v2.x, this.y - v2.y, this.z - v2.z);
            return re;
        }
        //归位
        public void setFormerly(Vector3 v2)
        {
            this.x += v2.x;
            this.y += v2.y;
            this.z += v2.z;
        }
        //打印
        public string toString()
        {
            string x = Convert.ToString(this.x);
            string y = Convert.ToString(this.y);
            string z = Convert.ToString(this.z);
            return string.Format($"X轴:{x} Y轴:{y} Z轴:{z}");
        }
    }
    //坐标操作与验证
    public static class FlyBase
    {
        //逐行读取 文档
        private static string[] ReadFile(string path)
        {
            StreamReader sr = new StreamReader(path, Encoding.Default);
            String line;
            ArrayList arrlist = new ArrayList();
            while ((line = sr.ReadLine()) != null)
            {
                arrlist.Add(line);
            }
            if (arrlist.Count == 1)//判断 svg的文件内容 是否写在一行里面
            {
                string temp = (string)arrlist[0];
                string[] strArray = Regex.Split(temp, "/>");
                return strArray;
            }
            string[] arr = new string[arrlist.Count];
            int key = 0;
            foreach (var item in arrlist)
            {
                arr[key] = (string)item;
                key++;
            }
            return arr;
        }
        //svg航点坐标文档 转坐标集合 PS:灯光映射 支持格式
        public static Vector3[] SvgToPosForLight(string path)
        {
            //读取文档内容
            string[] arr;
            arr = FlyBase.ReadFile(path);
            //获取飞机x轴 z轴 坐标
            ArrayList tempVec = new ArrayList();//记录飞机坐标
            string cx = @"cx=""(?<mark>\-{0,1}\d*\.{0,1}\d*)""";
            string cy = @"cy=""(?<mark>\-{0,1}\d*\.{0,1}\d*)""";
            string r = @"r=""(?<mark>\d*\.{0,1}\d*)""";
            foreach (string item in arr)
            {
                if (item.IndexOf("<circle") >= 0)//找到svg里面<circle>标签
                {
                    Regex reg = new Regex(cx);
                    string strX = reg.Match(item).Groups["mark"].ToString();//正则匹配获取x轴
                    reg = new Regex(cy);
                    string strZ = reg.Match(item).Groups["mark"].ToString();//正则匹配获取y轴 在三维里面用作z轴
                    double x = Convert.ToDouble(strX);
                    double z = Convert.ToDouble(strZ);
                    tempVec.Add(new Vector3(x, 0, z));
                }
            }
            Vector3[] vec = ArrToVec(tempVec);//把arrlist转换成坐标集合
            return vec;//返回坐标集合
        }
        //txt航点坐标文档 转坐标集合 PS:目前是C4D坐标文档
        public static List<Vector3[]> TxtToPos(string path, out string[] flightPointNames)
        {
            //读取文档内容
            string[] arr;
            arr = FlyBase.ReadFile(path);
            //处理文档内容
            int group = 0;//获取有几组坐标
            foreach (string item in arr)
            {
                if (item.IndexOf(" ") <= 0)//找到航点名称
                {
                    group++;
                }
            }
            int planesCou = (arr.Length - group) / group;//获取飞机总数
            flightPointNames = new string[group];//记录航点名称
            int gKey = 0;//航点名称数组下标
            int pKey = 0;//所有坐标序号
            Vector3[] pos = new Vector3[planesCou];//临时记录飞机坐标
            List<Vector3[]> re = new List<Vector3[]>();//返回值
            for (int i = 0; i < arr.Length; i++)
            {
                if (i % (planesCou + 1) == 0)//分析是否是航点名称 行
                {
                    flightPointNames[gKey] = arr[i];//记录航点名称
                    gKey++;
                }
                else//否则 当坐标行处理
                {
                    string[] tempArr = arr[i].Split(' ');//每行坐标分割
                    pos[pKey % planesCou] = new Vector3(Convert.ToDouble(tempArr[1]), Convert.ToDouble(tempArr[2]), Convert.ToDouble(tempArr[3]));
                    if (pKey % planesCou == planesCou - 1)//当一组组标循环完成 之后记录到返回组
                    {
                        re.Add(pos);
                        pos = new Vector3[planesCou];//重新new一下 更新内存地址
                    }
                    pKey++;
                }
            }
            return re;//out flightPointNames 输出航点名称,re返回值 返回航点集合
        }
        //svg航点坐标文档 转坐标集合
        public static Vector3[] SvgToPos(string path)
        {
            //读取文档内容
            string[] arr;
            arr = FlyBase.ReadFile(path);
            //获取飞机x轴 z轴 坐标
            ArrayList tempVec = new ArrayList();//记录飞机坐标
            string cx = @"cx=""(?<mark>\-{0,1}\d*\.{0,1}\d*)""";
            string cy = @"cy=""(?<mark>\-{0,1}\d*\.{0,1}\d*)""";
            string r = @"r=""(?<mark>\d*\.{0,1}\d*)""";
            foreach (string item in arr)
            {
                if (item.IndexOf("<circle") >= 0)//找到svg里面<circle>标签
                {
                    Regex reg = new Regex(cx);
                    string strX = reg.Match(item).Groups["mark"].ToString();//正则匹配获取x轴
                    reg = new Regex(cy);
                    string strZ = reg.Match(item).Groups["mark"].ToString();//正则匹配获取y轴 在三维里面用作z轴
                    reg = new Regex(r);
                    string radius = reg.Match(item).Groups["mark"].ToString();//正则匹配获取球半径
                    //按比例缩放(半径为100公分) 获取世界坐标
                    double Rate = 100 / Convert.ToDouble(radius);
                    double x = Rate * Convert.ToDouble(strX);
                    double z = Rate * Convert.ToDouble(strZ) * -1;
                    tempVec.Add(new Vector3(x, 0, z));
                }
            }
            //坐标集原点相对位置 移到坐标集质心
            Vector3[] vec = ArrToVec(tempVec);//把arrlist转换成坐标集合
            Vector3 centerPos = GetPosCenter(vec);//获取中心点
            int key = 0;
            foreach (Vector3 item in vec)
            {
                item.setZero(centerPos);
                vec[key] = item;
                key++;
            }
            return vec;//返回坐标集合
        }
        //obj航点坐标文档 转坐标集合
        public static Vector3[] ObjToPos(string path)
        {
            //读取文档内容
            string[] arr;
            arr = FlyBase.ReadFile(path);
            //获取飞机x轴 z轴 坐标
            string pCou = @"#\s+(?<mark>\d*)\s+vertices";//匹配obj里面标记点的数量
            string pPos = @"v\s+(?<markX>\-{0,1}\d*\.{0,1}\d*)\s+(?<markY>\-{0,1}\d*\.{0,1}\d*)\s+(?<markZ>\-{0,1}\d*\.{0,1}\d*)";
            int linage = 0;//行数
            string tempPos;//临时记录一下 xyz坐标
            ArrayList tempVec = new ArrayList();//记录飞机坐标
            foreach (string item in arr)
            {
                Regex reg = new Regex(pCou);
                string cou = reg.Match(item).Groups["mark"].ToString();//正则匹配获取x轴
                if (cou != "")
                {
                    for (int i = Convert.ToInt32(cou); i > 0; i--)
                    {
                        tempPos = arr[linage - i];
                        reg = new Regex(pPos);
                        string strX = reg.Match(tempPos).Groups["markX"].ToString();
                        string strY = reg.Match(tempPos).Groups["markY"].ToString();
                        string strZ = reg.Match(tempPos).Groups["markZ"].ToString();
                        tempVec.Add(new Vector3(Convert.ToDouble(strX), Convert.ToDouble(strY), Convert.ToDouble(strZ)));
                    }
                }
                linage++;
            }
            //坐标集原点相对位置 移到坐标集质心
            Vector3[] vec = ArrToVec(tempVec);//把arrlist转换成坐标集合
            //重新定义中心点为原点
            //Vector3 centerPos = getPosCenter(vec);//获取中心点
            //int key = 0;
            //foreach (Vector3 item in vec)
            //{
            //    item.setZero(centerPos);
            //    vec[key] = item;
            //    key++;
            //}
            return vec;//返回坐标集合
        }
        //Arraylist 转 Vector3[] 坐标集
        private 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;
        }
        //数组最大值 最小值
        private static double GetMaxOrMin(double[] arr, bool isMax = true)
        {
            ArrayList list = new ArrayList(arr);
            list.Sort();
            if (isMax) return Convert.ToDouble(list[list.Count - 1]);
            else return Convert.ToDouble(list[0]);

        }
        //二维数组打成一维 去掉重复
        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;
        }
        private static List<int> TwoArrToArr(List<int[]> twoArr)//重写 泛值List int[]
        {
            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;
        }
        //两点距离
        private static double GageLength(Vector3 v1, Vector3 v2)
        {
            return Math.Sqrt(Math.Pow(v1.x - v2.x, 2) + Math.Pow(v1.y - v2.y, 2) + Math.Pow(v1.z - v2.z, 2));
        }
        //点乘 用来求朝向 返回0到1之间
        private static double DotPro(Vector3 v1, Vector3 v2)
        {
            return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
        }
        //叉乘 
        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);
        }
        //坐标置换 向量乘以矩阵(i帽j帽k帽)得到置换后的向量坐标 ps:x指向i帽 y轴指向j帽 z轴指向k帽
        private static Vector3 MatrixMul(Vector3 vec, Vector3[] mat)//vec:要变换的向量 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;
        }
        //两条线段之间的最近距离
        private static bool IsEqual(double a, double b)
        {
            if (Math.Abs(a - b) < 1e-7)
            {
                return true;
            }
            return false;
        }
        private static double[] RecentlyOfLine(Vector3 a1, Vector3 a2, Vector3 b1, Vector3 b2)
        {
            if (a1 == a2) a2 += 1;
            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);
            double[] re = new double[3];
            re[0] = sc;//最近点在第一条线占比
            re[1] = tc;//最近点在第二条线占比
            re[2] = Math.Sqrt(dx * dx + dy * dy + dz * dz);//两线最近距离
            return re;
        }
        //获取坐标组重心或中心
        private 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);
            }
        }
        /*
        //从最外层AB规划路径
        private static void contactABOut(Vector3[] aPos, Vector3[] bPos,bool stressRefresh = true)
        {
            if (!stressRefresh)//不刷新重心 初始化重心
            {
                Vector3[] abPos = new Vector3[aPos.Length + bPos.Length];
                aPos.CopyTo(abPos, 0);
                bPos.CopyTo(abPos, aPos.Length);
                Vector3 centerPos=getPosCenter(abPos);//重心
            }
            while (aPos.Length != 0)
            {
                if (stressRefresh)//刷新重心
                {
                    Vector3[] abPos = new Vector3[aPos.Length + bPos.Length];
                    aPos.CopyTo(abPos, 0);
                    bPos.CopyTo(abPos, aPos.Length);
                    Vector3 centerPos = getPosCenter(abPos);//重心
                }

            }
        }
        */
        //输出日志回调函数
        public delegate void SomeCalculateWay(string str);
        //碰撞检测
        public static List<int[]> AirImitation(Vector3[] startPos, Vector3[] endPos, double lineDistance = 180, double spaceBetween = 900, int checkFps = 10)
        {
            int cou = startPos.Length;//飞机总数
            //获取所有飞行时间 和航线距离
            double[] planesLen = new double[cou];//记录所有航线距离
            double[] planesFlyTime = new double[cou];//记录所有飞行时间
            for (int i = 0; i < cou; i++)
            {
                double len = GageLength(startPos[i], endPos[i]);//距离
                planesLen[i] = len;
                double flyTime = len / 300;//时间
                planesFlyTime[i] = flyTime;
            }
            //选出航线有交叉的飞机 用于模拟飞行碰撞检测
            List<int[]> needCheck = new List<int[]>();//需要检查的飞机 二维数组 记录交叉组
            List<double> distances = new List<double>();//记录所有需要检查飞机 航线的最短距离
            for (int i = 0; i < cou; i++)
            {
                for (int x = 0; x < cou; x++)
                {
                    if (i + 1 + x < cou)
                    {
                        double distance = RecentlyOfLine(startPos[i], endPos[i], startPos[i + 1 + x], endPos[i + 1 + x])[2];//航线距离
                        if (distance < lineDistance)
                        {
                            int[] z = new int[] { i, i + 1 + x };
                            needCheck.Add(z);
                            distances.Add(distance);
                        }
                    }
                    else break;
                }
            }
            //碰撞检测
            List<int[]> planesCollision = new List<int[]>();//记录碰撞组
            for (int i = 0; i < needCheck.Count; i++)
            {
                double zongTime;
                int frist = (needCheck[i])[0];//第一架
                int second = (needCheck[i])[1];//第二架
                Vector3 fristFormerly = endPos[frist];//记第一架 终点原位
                Vector3 secondFormerly = endPos[second];//记第二架 终点原位
                //获取对比飞机 飞行时间较长的时间
                if (planesFlyTime[frist] >= planesFlyTime[second]) zongTime = planesFlyTime[frist] + 1;//最长飞行时间 多加1秒放bug
                else zongTime = planesFlyTime[second] + 1;//最长飞行时间 多加1秒放bug
                Convert.ToInt32(zongTime);
                //以帧为单位计算 检测对比飞机之间距离
                for (int currentTime = 0; currentTime < zongTime * checkFps; currentTime++)
                {
                    endPos[frist].setZero(startPos[frist]);//归零
                    endPos[frist].normalize(300 / checkFps * currentTime);//当前时间飞行 标准化长度 坐标
                    if (endPos[frist].getMag() >= planesLen[frist]) endPos[frist] = fristFormerly;//溢出距离 当前坐标定义为终点位置
                    else endPos[frist].setFormerly(startPos[frist]);//归位 当前时间飞机坐标

                    endPos[second].setZero(startPos[second]);//归零
                    endPos[second].normalize(300 / checkFps * currentTime);//当前时间飞行 标准化长度 坐标
                    if (endPos[second].getMag() >= planesLen[second]) endPos[second] = secondFormerly;//溢出距离 当前坐标定义为终点位置
                    else endPos[second].setFormerly(startPos[second]);//归位 当前时间飞机坐标

                    //判断距离 碰撞记录id
                    if (GageLength(endPos[frist], endPos[second]) < spaceBetween + 300 * ((double)distances[i] / lineDistance))//间距小于 基础间距+300*(航线实际距离/航线检测最小距离) 既:例基础间距+航线距离的反比
                    {
                        endPos[frist] = fristFormerly;//第一架 终点回归原位
                        endPos[second] = secondFormerly;//第二架 终点回归原位
                        int[] z = new int[] { frist, second };
                        planesCollision.Add(z);
                        break;
                    }
                    endPos[frist] = fristFormerly;//第一架 终点回归原位
                    endPos[second] = secondFormerly;//第二架 终点回归原位
                }
            }
            return planesCollision;//返回二维数组 飞机ID从0开始
        }
        //单机碰撞检测
        public static bool OnlyImitation(int onlyPlaneId, Vector3[] startPos, Vector3[] endPos, double lineDistance = 190, double spaceBetween = 1600, int checkFps = 10)//飞机id从0开始
        {
            //指定飞机 起始坐标 终点坐标 飞行时间
            Vector3 onlyStartVec = startPos[onlyPlaneId];
            Vector3 onlyEndVec = endPos[onlyPlaneId];
            double onlyFlyTime = GageLength(onlyStartVec, onlyEndVec) / 300;
            //选出与指定飞机 航线有交叉的飞机 用于模拟飞行碰撞检测
            for (int contrastId = 0; contrastId < startPos.Length; contrastId++)
            {
                if (onlyPlaneId == contrastId) continue;//不和自己比较
                Vector3 StartVec = startPos[contrastId];//对比飞机起始坐标
                Vector3 EndVec = endPos[contrastId];//对比飞机结束坐标
                // 判断两条轨迹 之间的最小距离
                double distance = RecentlyOfLine(onlyStartVec, onlyEndVec, StartVec, EndVec)[2];//航线最小距离
                if (distance < lineDistance)
                {
                    double flyTime = GageLength(EndVec, StartVec) / 300;//获取飞行总时间
                    //指定飞机 和 当前比较飞机 以飞行时间长得为总时间
                    int zongFlyTime;//声明 以航线较长的飞行时间 为总时长
                    if (onlyFlyTime >= flyTime) zongFlyTime = (int)onlyFlyTime + 1;//总时间延长一到两秒
                    else zongFlyTime = (int)flyTime + 1;//总时间延长一到两秒
                    //碰撞检测
                    for (int currentTime = 0; currentTime < zongFlyTime * checkFps; currentTime++)
                    {
                        //飞机当前坐标
                        Vector3 onlyCurrentVec = onlyEndVec.setZeroEd(onlyStartVec);//归零的　当前坐标
                        onlyCurrentVec.normalize(300 / checkFps * currentTime);//当前时间飞行 标准化长度 坐标
                        if (onlyCurrentVec.getMag() >= GageLength(onlyEndVec, onlyStartVec)) onlyCurrentVec = onlyEndVec;//溢出距离 当前坐标定义为终点位置
                        else onlyCurrentVec.setFormerly(onlyStartVec);//归位 当前时间飞机坐标

                        //要进行比较飞机 当前坐标
                        Vector3 currentVec = EndVec.setZeroEd(StartVec);//归零的　当前坐标
                        currentVec.normalize(300 / checkFps * currentTime);//当前时间飞行 标准化长度 坐标
                        if (currentVec.getMag() >= GageLength(EndVec, StartVec)) currentVec = EndVec;//溢出距离 当前坐标定义为终点位置
                        else currentVec.setFormerly(StartVec);//归位 当前时间飞机坐标

                        double planeLen = GageLength(onlyCurrentVec, currentVec);//获取检测飞机s 当前间距
                        //间距小于 基础间距+800*(航线实际距离/航线检测最小距离) 既:例基础间距+航线距离的反比
                        if (planeLen < spaceBetween + 300 * (distance / lineDistance))
                        {
                            return true;//返回有碰撞
                        }
                    }
                }
            }
            return false;//返回没有碰撞;
        }
        //智能挫层
        public static ArrayList CollisionLayer(Vector3[] startPos, Vector3[] endPos, string axes = "y")
        {
            //获取所有碰撞飞机id
            List<int> planesCollisionInt = new List<int>();//声明碰撞id组
            int key = 0;
            foreach (var item in startPos)//遍历所有飞机 单体检测碰撞
            {
                if (OnlyImitation(key, startPos, endPos)) planesCollisionInt.Add(key);//如果碰撞 添加到碰撞id组
                key++;
            }
            //记录第一图案原位
            double defaultPos = startPos[0].y;
            //有碰撞飞机先拉开
            int k = 0;
            foreach (int i in planesCollisionInt)
            {
                startPos[i].y = startPos[i].y + 9400 + k * 300;
                k++;
            }
            //中间航点挫层
            foreach (int i in planesCollisionInt)
            {
                int shiftCou = 0;//记录循环次数
                while (true)
                {
                    if (shiftCou % 2 == 1)
                    {
                        startPos[i].y = defaultPos + (shiftCou + 1) / 2 * 350;
                        endPos[i].y = defaultPos + (shiftCou + 1) / 2 * 350;
                    }
                    else
                    {
                        startPos[i].y = defaultPos + shiftCou / 2 * -350;
                        endPos[i].y = defaultPos + shiftCou / 2 * -350;
                    }
                    if (!(OnlyImitation(i, startPos, endPos))) break;//如果 不碰撞跳出
                    shiftCou++;
                }
            }
            ArrayList middle = new ArrayList();
            middle.Add(startPos);
            middle.Add(endPos);
            return middle;//返回一个二维向量数组 middle[0]是第一个中间航点 middle[1]是第二个中间航点
        }
        //添加中间航点 群组
        private static Vector3[] SetMiddleCurvedValue(Vector3[] startPos, Vector3[] endPos, double middlePos = 0.5, double scale = 1)
        {
            int cou = startPos.Length;//飞机总数
            Vector3[] mps = new Vector3[cou];
            //算中间航点
            for (int i = 0; i < cou; i++)
            {
                Vector3 ap = startPos[i];
                Vector3 bp = endPos[i];
                mps[i] = (bp - ap) * middlePos + ap;
            }
            Vector3 centerPos = GetPosCenter(mps);//计算所有中间航点的重心点
            //中间航点缩放
            for (int i = 0; i < cou; i++)
            {
                mps[i] = (mps[i] - centerPos) * scale + centerPos + .1;
            }
            return mps;
        }
        //添加中间航点 单体
        private static Vector3 SetOddMiddleCurvedValue(Vector3 startPos, Vector3 endPos, double middlePos = 0.5)
        {
            //算中间航点 
            Vector3 mp = (endPos - startPos) * middlePos + startPos + .1;
            return mp;
        }
        //3D绕行与让行 ps:起点s 中点s 终点s 需要检查的ID组 绕行比例  每个比例的绕行次数  绕行的法线偏移距离
        private static List<int> ABNeedCheck(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos)//前航点到中间航点 中间航点到终点 所有碰撞飞机id
        {
            List<int> re = new List<int>();
            //获取从起点到中点再到结束点 所有碰撞飞机
            int key = 0;
            foreach (var item in startPos)
            {
                if (OnlyImitation(key, startPos, middlePos) || OnlyImitation(key, middlePos, endPos)) re.Add(key);//记录 起点到中点 或者 中点到终点有碰撞的飞机
                key++;
            }
            return re;
        }
        private static Vector3[] ABypassB(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos, List<int> check, double[] mRate, SomeCalculateWay StrPrint, int loopIndex = 800, double offsetDistance = 60)//法线螺旋线绕行
        {
            StrPrint("需要绕行总共" + check.Count + "架。");
            bool pa;
            //设置绕行
            foreach (int id in check)
            {
                StrPrint(Convert.ToString(id));
                pa = false;
                Vector3 initialPos = middlePos[id];//记录初始位置 如果绕行不成功 还原位置
                for (int k = 0; k < mRate.Length; k++)
                {
                    //初始化位置
                    middlePos[id] = SetOddMiddleCurvedValue(startPos[id], endPos[id], mRate[k]);
                    //碰撞绕行
                    for (int i = 0; i < loopIndex; i++)
                    {
                        if (i != 0)//第一次保证原位置 ps:先不移动位置 检测一次 移动其他飞机之后 可能让本架飞机不碰撞
                        {
                            //获取法线向量
                            Vector3 a = startPos[id];
                            Vector3 b = endPos[id];
                            Vector3 c = middlePos[id];
                            Vector3 mp = FlyBase.CrossPro(a - c, b - c);//乘差算 坐标原点上面的法线 方向向量
                            if (k % 2 == 0) mp.normalize(offsetDistance);//正螺旋
                            else mp.normalize(offsetDistance * -1);//逆螺旋
                            middlePos[id] = mp + c;
                        }
                        //碰撞检测
                        if (OnlyImitation(id, startPos, middlePos)) continue;//前半段检测
                        if (OnlyImitation(id, middlePos, endPos)) continue;//后半段检测
                        pa = true;
                        break;
                    }
                    if (pa) break;
                }
                if (!pa) middlePos[id] = initialPos;//让行不成功 位置还原
            }
            return middlePos;
        }
        public static List<Vector3[]> ExeABypassB(Vector3[] startPos, Vector3[] endPos, out bool isSuccess, SomeCalculateWay StrPrint)//绕行获取中间航点
        {
            isSuccess = false;
            List<Vector3[]> middlePosS = new List<Vector3[]>();//声明返回坐标集合 可能是1 到 3组坐标 既 1到3个中间航点
            List<int> check = new List<int>();//记录需要绕行的id组
            List<int> checkEd = new List<int>();//复查绕行未通过的id组
            //中段绕行 初始化
            double[] mRate = { .5, .6, .4, .7, .3 };//中点绕行的比例
            Vector3[] middlePos = SetMiddleCurvedValue(startPos, endPos, 0.5, 1.2);//加中间航点
            bool pa = false;
            StrPrint("中(二)段绕行开始");
            while (true)//中段绕行循环
            {
                //前中后所有碰撞的飞机id
                check = ABNeedCheck(startPos, middlePos, endPos);
                //绕行
                if (check.Count != 0)//有碰撞
                {
                    middlePos = ABypassB(startPos, middlePos, endPos, check, mRate, StrPrint);
                }
                //重新检查碰撞
                checkEd = ABNeedCheck(startPos, middlePos, endPos);
                if (checkEd.Count == 0)//如果绕行没有碰撞
                {
                    //StrPrint("成功");
                    middlePosS.Add(middlePos);//添加中间坐标组
                    isSuccess = true;
                    return middlePosS;//第一次绕行 没有碰撞  直接返回
                }
                if (check.SequenceEqual(checkEd))
                {
                    if (pa)//判定第二次　check 和 checkEd一致
                    {
                        //StrPrint("一致跳出");//如果一个中间航点 算不过去 跳出然后 进行三段绕行
                        break;//连续两次check 和 checkEd一致 跳出
                    }
                    pa = true;
                }
                else pa = false;
            }
            //一段绕行初始化
            Vector3[] fMiddlePos = SetMiddleCurvedValue(startPos, middlePos, 0, 1);
            double[] fmRate = { 0, .05, .1, .15, .2 };//中点绕行的比例
            pa = false;
            check = ABNeedCheck(startPos, fMiddlePos, middlePos);
            if (check.Count != 0)//检测是否进行一段循环
            {
                StrPrint("一段绕行开始");
                while (true)//一段绕行 循环体
                {
                    //前中后所有碰撞的飞机id
                    check = ABNeedCheck(startPos, fMiddlePos, middlePos);
                    if (check.Count != 0) fMiddlePos = ABypassB(startPos, fMiddlePos, middlePos, check, fmRate, StrPrint, 1500, 15);//有碰撞 绕行
                    else//没有碰撞 添加返回值并跳出 一段绕行 循环
                    {
                        //StrPrint("成功");
                        isSuccess = true;
                        middlePosS.Add(fMiddlePos);//添加一段 绕行结果
                        break;
                    }
                    //重新检查碰撞
                    checkEd = ABNeedCheck(startPos, fMiddlePos, middlePos);
                    //多次绕行 结果相同 则判定未能成功通过 但是也会添加 绕行结果
                    if (check.SequenceEqual(checkEd))//如果两次 碰撞检测结果完全相同
                    {
                        if (pa)//判定第二次　check 和 checkEd一致
                        {
                            //StrPrint("一致跳出");//如果一个中间航点 算不过去 跳出然后 进行三段绕行
                            isSuccess = false;
                            middlePosS.Add(fMiddlePos);//添加一段 绕行结果
                            break;//连续两次check 和 checkEd一致 跳出
                        }
                        pa = true;
                    }
                    else pa = false;
                }
            }
            //添加中段绕行结果
            middlePosS.Add(middlePos);
            //三段绕行初始化
            Vector3[] bMiddlePos = SetMiddleCurvedValue(middlePos, endPos, 1, 1);
            double[] bmRate = { 1, .95, 0.9, .85, .8 };//中点绕行的比例
            pa = false;
            check = ABNeedCheck(middlePos, bMiddlePos, endPos);
            if (check.Count != 0)//检测是否进行三段循环
            {
                StrPrint("三段绕行开始");
                while (true)//一段绕行 循环体
                {
                    //前中后所有碰撞的飞机id
                    check = ABNeedCheck(middlePos, bMiddlePos, endPos);
                    if (check.Count != 0) bMiddlePos = ABypassB(middlePos, bMiddlePos, endPos, check, bmRate, StrPrint, 1500, 15);//有碰撞　绕行
                    else//没有碰撞 添加返回值并跳出 三段绕行 循环
                    {
                        //StrPrint("成功");
                        isSuccess = true;
                        middlePosS.Add(bMiddlePos);//添加三段 绕行结果
                        break;
                    }
                    //重新检查碰撞
                    checkEd = ABNeedCheck(middlePos, bMiddlePos, endPos);
                    //多次绕行 结果相同 则判定未能成功通过 但是也会添加 绕行结果
                    if (check.SequenceEqual(checkEd))//如果两次 碰撞检测结果完全相同
                    {
                        if (pa)//判定第二次　check 和 checkEd一致
                        {
                            //StrPrint("一致跳出");//如果一个中间航点 算不过去 跳出然后 进行三段绕行
                            isSuccess = false;
                            middlePosS.Add(bMiddlePos);//添加三段 绕行结果
                            break;//连续两次check 和 checkEd一致 跳出
                        }
                        pa = true;
                    }
                    else pa = false;
                }
            }
            //三段绕行未果 进行三点同时绕行

            return middlePosS;
        }
        //碰撞次数排序
        public static List<int> CollisionTaxis(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos)
        {
            List<int> re = new List<int>();
            AirImitation(startPos, middlePos);
            AirImitation(middlePos, endPos);
            return re;
        }
    }
}