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

namespace FlyBase
{
    public struct Vector3
    {
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
        /// <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>
        /// <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>
        /// <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>
        /// 打印坐标
        /// </summary>
        /// <returns>坐标字符串</returns>
        public string PosToString()
        {
            string x = Convert.ToString(this.X);
            string y = Convert.ToString(this.Y);
            string z = Convert.ToString(this.Z);
            return string.Format($"X轴:{x} Y轴:{y} Z轴:{z}");
        }
    }
    //坐标操作与验证
    public static class FlyVecFun
    {
        //逐行读取 文档
        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 = FlyVecFun.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 = FlyVecFun.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 = FlyVecFun.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 = FlyVecFun.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;//返回坐标集合
        }
        /// <summary>
        /// Arraylist 转 Vector3[] 坐标集
        /// </summary>
        /// <param name="arr">Arraylist 坐标集</param>
        /// <returns>Vector3[] 坐标集</returns>
        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;
        }
        //重写 泛值List int[]
        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>
        /// 两点距离
        /// </summary>
        /// <param name="v1">坐标1</param>
        /// <param name="v2">坐标2</param>
        /// <returns>两点之间距离</returns>
        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);
        }
        /// <summary>
        /// 坐标置换 向量乘以矩阵(i帽j帽k帽)得到置换后的向量坐标 ps:x指向i帽 y轴指向j帽 z轴指向k帽
        /// </summary>
        /// <param name="vec">要变换的向量</param>
        /// <param name="mat">矩阵标量 既i帽 j帽 k帽 三点的坐标组成的矩阵</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;
        }
        //两条线段之间的最近距离
        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;
        }
        /// <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>
        /// <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;
        }
        /*
        //从最外层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 = 180, double spaceBetween = 1200, 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 Vector3[] ABypassB(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos, List<int> check, double[] mRate, int loopIndex = 800, double offsetDistance = 60)//法线螺旋线绕行
        {
            Console.WriteLine("check:{0}", check.Count);
            bool pa;
            //设置绕行
            foreach (int id in check)
            {
                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 = FlyVecFun.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;
        }
        private static List<Vector3[]> ThreeStageABypassB(Vector3[] startPos, Vector3[] fMiddlePos, Vector3[] middlePos, Vector3[] bMiddlePos, Vector3[] endPos, List<int> check, double[] mRate)//三段绕
        {
            //初始化
            double[] fmRate = { 0 };
            double[] bmRate = { 1 };
            //二段绕行
            foreach (int id in check)
            {
                bool 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 < 800; i++)
                    {
                        if (i != 0)//第一次保证原位置 ps:先不移动位置 检测一次 移动其他飞机之后 可能让本架飞机不碰撞
                        {
                            //获取法线向量
                            Vector3 a = startPos[id];
                            Vector3 b = endPos[id];
                            Vector3 c = middlePos[id];
                            Vector3 mp = FlyVecFun.CrossPro(a - c, b - c);//乘差算 坐标原点上面的法线 方向
                            mp.Normalize(60);
                            middlePos[id] = mp + c;
                        }
                        //绕一段 和 三段
                        Vector3[] tempFMiddlePos;//临时存放 一段
                        Vector3[] tempBMiddlePos;//临时存放 三段
                        List<int> xId = new List<int>();//临时存放当前id 用于绕行方法
                        xId.Add(id);
                        if (OnlyImitation(id, startPos, middlePos) || OnlyImitation(id, middlePos, endPos))//因为二段改变位置 检测直接二段绕行 是否通过
                        {
                            tempFMiddlePos = FlyVecFun.ABypassB(startPos, fMiddlePos, middlePos, xId, fmRate, 45, 90);//一段绕行
                            if (OnlyImitation(id, startPos, tempFMiddlePos)) continue;//检测一段 前 重新循环二段
                            else if (OnlyImitation(id, tempFMiddlePos, middlePos)) continue;//检测一段 后 重新循环二段

                            tempBMiddlePos = FlyVecFun.ABypassB(middlePos, bMiddlePos, endPos, xId, bmRate, 45, 90);//三段绕行
                            if (OnlyImitation(id, startPos, tempBMiddlePos)) continue;//检测三段 前 重新循环二段
                            else if (OnlyImitation(id, fMiddlePos, middlePos)) continue;//检测三段 后 重新循环二段

                            fMiddlePos = tempFMiddlePos;//如果通过 更新一段坐标
                            bMiddlePos = tempBMiddlePos;//如果通过 更新三段坐标
                        }
                        pa = true;
                        break;//跳出
                    }
                    if (pa) break;
                }
                if (!pa) middlePos[id] = initialPos;//绕行不成功 位置还原
            }
            List<Vector3[]> re = new List<Vector3[]>();
            re.Add(fMiddlePos);
            re.Add(middlePos);
            re.Add(bMiddlePos);
            return re;
        }
        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;
        }
        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);//加中间航点
            bool pa = false;
            strPrint("中(二)段绕行开始");
            while (true)//中段绕行循环
            {
                //前中后所有碰撞的飞机id
                check = ABNeedCheck(startPos, middlePos, endPos);
                //绕行
                if (check.Count != 0)//有碰撞
                {
                    middlePos = ABypassB(startPos, middlePos, endPos, check, mRate);
                }
                //重新检查碰撞
                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, 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, 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;
        }
    }
}