commit ba51f93b170196344af3facf5e44b74f356f3392 Author: 小电脑 Date: Mon Apr 11 15:17:13 2022 +0800 逐帧 备份 diff --git a/FlieOperate/FileBase.cs b/FlieOperate/FileBase.cs new file mode 100644 index 0000000..3edce87 --- /dev/null +++ b/FlieOperate/FileBase.cs @@ -0,0 +1,112 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Text; +using System.Threading.Tasks; +using System.Windows.Forms; + +namespace FlieOperate +{ + /// + /// 关于文件的 操作类 + /// + public class FileBase + { + /// + /// 资源管理器 文件类型 中文说明 和扩展名是 成对关系 数组长度要一致 + /// + public static string[] FileClass { get; set; } = new string[] { "航点文件" }; + /// + /// 扩展名 ps: "*.jpg;*.png" 这个格式算一组 + /// + public static string[] Pascal { get; set; } = new string[] { "*.fcgm;" }; + /// + /// 整合文件类型及扩展名 + /// "航点文件|*.fcgm;*.txt|航点文件|*.fcgm;*.*";//资源管理器限制扩展名 例子 + /// + /// 文件类型及扩展名 + private static string SetFileClass() + { + string str = ""; + for (int i = 0; i < FileClass.Length; i++) + { + str += FileClass[i] + "|" + Pascal[i]; + if (i + 1 != FileClass.Length)//最后一次 不加| + { + str += "|"; + } + } + return str; + } + /// + /// 读取文件 存到字符串 + /// + /// 文件路径 + /// 文件内容 字符串 + public static string ReadFlieToStr(string FliePath) + { + FileStream fs = new FileStream(FliePath, FileMode.Open); + StreamReader fileStream = new StreamReader(fs); + string str = ""; + string line; + while ((line = fileStream.ReadLine()) != null) + { + str += line; + } + return str; + } + /// + /// 字符串 保存到指定文件 + /// + /// 指定文件和路径 + /// 字符串 + private static void SaveStrToFile(string filePath, string str) + { + str = str.TrimEnd((char[])"\n\r".ToCharArray());//去除最后的回车符 + string stream = null; + if (File.Exists(filePath)) + { + StreamReader reader = new StreamReader(filePath); + stream = reader.ReadToEnd(); + reader.Close(); + } + FileStream fs = new FileStream(filePath, FileMode.OpenOrCreate); + StreamWriter sw = new StreamWriter(fs); + sw.Write(str); + sw.Close(); + fs.Close(); + } + public static void SaveExplorer(string str) + { + SaveFileDialog sfd = new SaveFileDialog(); + sfd.Filter = SetFileClass();//设置文件类型 + sfd.AddExtension = true;//设置自动在文件名中添加扩展名 + if (sfd.ShowDialog() == DialogResult.OK) + { + SaveStrToFile(sfd.FileName, str); + } + } + /// + /// 资源管理器 获取文件路径 + /// + /// 资源管理器标题栏 + /// out参数 判断是否获取到文件路径 + /// 文件路径 + public static string OpenExplorer(string titName, out bool isSelect) + { + Microsoft.Win32.OpenFileDialog ofdl = new Microsoft.Win32.OpenFileDialog();//打开资源管理器 + ofdl.Filter = SetFileClass();//设置文件类型 + ofdl.Title = titName; + if (ofdl.ShowDialog() == true)//执行打开资源管理器 并判断有没有获取到文件名 + { + isSelect = true; + } + else + { + isSelect = false; + } + return ofdl.FileName; + } + } +} diff --git a/FlieOperate/FlieOperate.csproj b/FlieOperate/FlieOperate.csproj new file mode 100644 index 0000000..e509561 --- /dev/null +++ b/FlieOperate/FlieOperate.csproj @@ -0,0 +1,60 @@ + + + + + Debug + AnyCPU + {C354FD6B-5863-4246-BB48-6DF14A85C161} + Library + Properties + FlieOperate + FlieOperate + v4.7.2 + 512 + true + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + ..\FlyCube\packages\Newtonsoft.Json.13.0.1\lib\net45\Newtonsoft.Json.dll + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FlieOperate/JsonBase.cs b/FlieOperate/JsonBase.cs new file mode 100644 index 0000000..da924fd --- /dev/null +++ b/FlieOperate/JsonBase.cs @@ -0,0 +1,80 @@ +using System.Collections.Generic; +using Newtonsoft.Json; + +namespace FlieOperate +{ + /// + /// 飞行魔方地面站 fcgm Json文件层级 结构模型 + /// + public class FcgmJsonModel + { + public string ID { get; set; } + public string ver { get; set; } + public int coptercount { get; set; } + public int taskcount { get; set; } + public List groups { get; set; } + public class Groups + { + public string groupName { get; set; } + public List copterIndexList { get; set; } + } + public List> locate { get; set; } + public List tasks { get; set; } + public class Tasks + { + public int type { get; set; }//100：起飞 0：普通航点 6：降落 + public int takeoffnumber { get; set; }//起飞任务独有 + public int takeoffTime { get; set; }//起飞任务独有 + public bool staggerRoutes { get; set; } + public int flytoTime { get; set; } + public int loiterTime { get; set; } + public string taskname { get; set; } + public List singleCopterInfos { get; set; } + public class SingleCopterInfos + { + public int waitTime { get; set; }//起飞降落 独有 + public double x { get; set; } + public double y { get; set; } + public double targetAlt { get; set; } + public List ledInfos { get; set; } + public class LedInfos + { + public double Delay { get; set; } + public int LEDMode { get; set; } + public double LEDInterval { get; set; } + public int LEDRate { get; set; } + public int LEDTimes { get; set; } + public string LEDRGB { get; set; } + } + public bool isLandWaypoint { get; set; }//返航点 + public bool isChangeSpeed { get; set; }//改变速度 + public double levelSpeed { get; set; } + public double upSpeed { get; set; } + public double downSpeed { get; set; } + } + } + } + public class JsonBase + { + /// + /// 航点文件转存到FcgmJsonModel对象里面 反序列化 + /// + /// 航点文件路径 + /// 航点FcgmJsonModel对象 + public static FcgmJsonModel JsonToModel(string FliePath) + { + string str = FileBase.ReadFlieToStr(FliePath); + FcgmJsonModel fcgmModel = JsonConvert.DeserializeObject(str);//反系列化 + return fcgmModel; + } + /// + /// FcgmJsonModel对象 序列化 成 json + /// + /// + /// + public static string ModelTojson(FcgmJsonModel fcgmJsonModel) + { + return JsonConvert.SerializeObject(fcgmJsonModel);　　　　//转为字符串 + } + } +} diff --git a/FlyBase/FlyBase.cs b/FlyBase/FlyBase.cs new file mode 100644 index 0000000..1daffd4 --- /dev/null +++ b/FlyBase/FlyBase.cs @@ -0,0 +1,1116 @@ +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; } + /// + /// 构造 初始化 + /// + /// x坐标 + /// y坐标 + /// 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, int i) + { + return new Vector3(v1.X + (double)i, v1.Y + (double)i, v1.Z + (double)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, int i) + { + return new Vector3(v1.X - (double)i, v1.Y - (double)i, v1.Z - (double)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, int i) + { + return new Vector3(v1.X * (double)i, v1.Y * (double)i, v1.Z * (double)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, int i) + { + return new Vector3(v1.X / (double)i, v1.Y / (double)i, v1.Z / (double)i); + } + /// + /// 重载== 向量==向量 + /// + /// 向量1 + /// 向量2 + /// 布尔值 判断向量是否相等 + 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; + } + /// + /// 重载！= 向量！=向量 + /// + /// 向量1 + /// 向量2 + /// 布尔值 判断向量是否不相等 + 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 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=""(?\-{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 = 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 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 = FlyVecFun.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 = FlyVecFun.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[] 坐标集 + /// + /// 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; + } + //重写 泛值List int[] + private static List TwoArrToArr(List twoArr) + { + 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; + } + /// + /// 两点距离 + /// + /// 坐标1 + /// 坐标2 + /// 两点之间距离 + 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帽 + /// + /// 要变换的向量 + /// 矩阵标量 既i帽 j帽 k帽 三点的坐标组成的矩阵 + /// + 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; + } + /// + /// 获取坐标集合的重心或中心 + /// + /// 坐标集合 + /// 默认返回为true重心 false则为中心 + /// 重心或中心坐标 + 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); + } + } + /// + /// 获取坐标集合 的总宽度 高度 长度 + /// + /// 坐标集合 + /// 返回数组[0]宽度[1]高度[2]长度 + public static double[] GetVecsWithHighLength(Vector3[] pos) + { + List w = new List(); + List h = new List(); + List l = new List(); + 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 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 = 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 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 Vector3[] ABypassB(Vector3[] startPos, Vector3[] middlePos, Vector3[] endPos, List 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 ThreeStageABypassB(Vector3[] startPos, Vector3[] fMiddlePos, Vector3[] middlePos, Vector3[] bMiddlePos, Vector3[] endPos, List 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 xId = new List();//临时存放当前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 re = new List(); + re.Add(fMiddlePos); + re.Add(middlePos); + re.Add(bMiddlePos); + return re; + } + 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; + } + 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);//加中间航点 + 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; + } + } +} \ No newline at end of file diff --git a/FlyCube/MainWindow.xaml b/FlyCube/MainWindow.xaml new file mode 100644 index 0000000..9b60084 --- /dev/null +++ b/FlyCube/MainWindow.xaml @@ -0,0 +1,26 @@ + + + + + + + + + +