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=""(?\-{0,1}\d*\.{0,1}\d*)"""; string cy = @"cy=""(?\-{0,1}\d*\.{0,1}\d*)"""; string r = @"r=""(?\d*\.{0,1}\d*)"""; foreach (string item in arr) { if (item.IndexOf("= 0)//找到svg里面标签 { 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 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 re = new List();//返回值 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=""(?\-{0,1}\d*\.{0,1}\d*)"""; string cy = @"cy=""(?\-{0,1}\d*\.{0,1}\d*)"""; string r = @"r=""(?\d*\.{0,1}\d*)"""; foreach (string item in arr) { if (item.IndexOf("= 0)//找到svg里面标签 { 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+(?\d*)\s+vertices";//匹配obj里面标记点的数量 string pPos = @"v\s+(?\-{0,1}\d*\.{0,1}\d*)\s+(?\-{0,1}\d*\.{0,1}\d*)\s+(?\-{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 TwoArrToArr(List twoArr)//重写 泛值List int[] { List arr = new List(); 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 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 needCheck = new List();//需要检查的飞机 二维数组 记录交叉组 List distances = new List();//记录所有需要检查飞机 航线的最短距离 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 planesCollision = new List();//记录碰撞组 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 planesCollisionInt = new List();//声明碰撞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 ABNeedCheck(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos)//前航点到中间航点 中间航点到终点 所有碰撞飞机id { List re = new List(); //获取从起点到中点再到结束点 所有碰撞飞机 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 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 ExeABypassB(Vector3[] startPos, Vector3[] endPos, out bool isSuccess, SomeCalculateWay StrPrint)//绕行获取中间航点 { isSuccess = false; List middlePosS = new List();//声明返回坐标集合 可能是1 到 3组坐标 既 1到3个中间航点 List check = new List();//记录需要绕行的id组 List checkEd = new List();//复查绕行未通过的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 CollisionTaxis(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos) { List re = new List(); AirImitation(startPos, middlePos); AirImitation(middlePos, endPos); return re; } } }