增加 点to面投影函数 检测最多点共面函数 修改错层函数（现在不判断是否是平面 是否共面 支持多层直接错层 不成功返回 re长度为0）
This commit is contained in:
szdot
parent
b53fa88b7d
commit
aa30959d71
@ -648,42 +648,6 @@ namespace FlightRouteV2
|
||||
return new_bVecs;
|
||||
}
|
||||
/// <summary>
|
||||
/// 从一组向量集合中 不重复的随机选择4个向量为一组 最多100组 组合成二维数组 如:[[1,2,3,4][5,6,7,8]...]
|
||||
/// </summary>
|
||||
/// <param name="vecs">坐标集和</param>
|
||||
/// <returns>re空数组则代表不够4个坐标 </returns>
|
||||
private static List<Vector3[]> RandomSel4Vec(Vector3[] vecs)
|
||||
{
|
||||
List<Vector3[]> result = new List<Vector3[]>();
|
||||
int len = vecs.Length;
|
||||
// 如果坐标数组少于4个，或者为空，则返回空列表
|
||||
if (len < 4)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
// 确定最大可选择的组数，最多为100组
|
||||
int numGroups = Math.Min(len / 4, 100);
|
||||
List<Vector3> flattenedList = vecs.ToList();
|
||||
Random random = new Random();
|
||||
// 遍历每一组
|
||||
for (int i = 0; i < numGroups; i++)
|
||||
{
|
||||
List<Vector3> selectedGroup = new List<Vector3>();
|
||||
|
||||
// 随机选择4个不重复的坐标
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
int index = random.Next(flattenedList.Count);
|
||||
selectedGroup.Add(flattenedList[index]);
|
||||
flattenedList.RemoveAt(index);
|
||||
}
|
||||
|
||||
// 将选择的坐标组成的 List 转换为数组，并添加到结果中
|
||||
result.Add(selectedGroup.ToArray());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
/// <summary>
|
||||
/// 设置中间航点
|
||||
/// </summary>
|
||||
/// <param name="aVec">起点</param>
|
||||
@ -725,7 +689,7 @@ namespace FlightRouteV2
|
||||
return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
|
||||
}
|
||||
/// <summary>
|
||||
/// 叉积
|
||||
/// 叉积 ps：法线向量
|
||||
/// </summary>
|
||||
/// <param name="v1">向量1</param>
|
||||
/// <param name="v2">向量2</param>
|
||||
@ -743,7 +707,7 @@ namespace FlightRouteV2
|
||||
/// <param name="v1">第一个向量</param>
|
||||
/// <param name="v2">第二个向量</param>
|
||||
/// <returns>角度</returns>
|
||||
public static double AngleBetween(Vector3 v1, Vector3 v2)
|
||||
private static double AngleBetween(Vector3 v1, Vector3 v2)
|
||||
{
|
||||
// 计算点积
|
||||
double dotProduct = DotPro(v1, v2);
|
||||
@ -761,6 +725,77 @@ namespace FlightRouteV2
|
||||
return thetaDegrees;
|
||||
}
|
||||
/// <summary>
|
||||
/// 计算某个点到平面垂线与平面的交点
|
||||
/// </summary>
|
||||
/// <param name="vec1">平面上的点1</param>
|
||||
/// <param name="vec2">平面上的点2</param>
|
||||
/// <param name="vec3">平面上的点3</param>
|
||||
/// <param name="vec4">垂直于平面的点</param>
|
||||
/// <returns>交点坐标</returns>
|
||||
private static Vector3 CalculateIntersectionPoint(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
{
|
||||
// 计算平面的法线向量
|
||||
Vector3 normal = CrossPro(vec2 - vec1, vec3 - vec1);
|
||||
normal.Normalize();
|
||||
// 计算第4个点到平面的距离
|
||||
double distance = DotPro(normal, vec1);
|
||||
// 计算第4个点到平面的投影点坐标
|
||||
double projection = DotPro(normal, vec4) - distance;
|
||||
// 计算交点坐标
|
||||
Vector3 intersectionPoint = vec4 - normal * projection;
|
||||
return intersectionPoint;
|
||||
}
|
||||
/// <summary>
|
||||
/// 找到能组成平面的点的最大数量，并返回组成最大平面的点的索引。
|
||||
/// </summary>
|
||||
/// <param name="vecs">Vector3 点的列表。</param>
|
||||
/// <returns>组成最大平面的点的索引。</returns>
|
||||
public static List<int> FindMaxPlaneIndices(Vector3[] vecs)
|
||||
{
|
||||
int maxPointsOnPlane = 0;
|
||||
List<int> maxPointsIndices = new List<int>(); //记录返回值
|
||||
int planeCou = vecs.Length; // 飞机总数
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
for (int j = i + 1; j < planeCou; j++)
|
||||
{
|
||||
for (int k = j + 1; k < planeCou; k++)
|
||||
{
|
||||
int currentPointsOnPlane = 3; // 当前遍历的三个点肯定在同一平面上
|
||||
|
||||
for (int l = k + 1; l < planeCou; l++)
|
||||
{
|
||||
if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
|
||||
{
|
||||
// 当前的 l 也在同一平面上
|
||||
currentPointsOnPlane++;
|
||||
}
|
||||
}
|
||||
|
||||
// 检查当前平面是否比之前找到的平面更大
|
||||
if (currentPointsOnPlane > maxPointsOnPlane)
|
||||
{
|
||||
maxPointsOnPlane = currentPointsOnPlane;
|
||||
maxPointsIndices.Clear();
|
||||
maxPointsIndices.Add(i);
|
||||
maxPointsIndices.Add(j);
|
||||
maxPointsIndices.Add(k);
|
||||
|
||||
// 添加当前平面的 l 索引
|
||||
for (int l = k + 1; l < planeCou; l++)
|
||||
{
|
||||
if (IsVecsOnPlane(vecs[i], vecs[j], vecs[k], vecs[l]))
|
||||
{
|
||||
maxPointsIndices.Add(l);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return maxPointsIndices;
|
||||
}
|
||||
/// <summary>
|
||||
/// 检查4个点是否在一个平面上
|
||||
/// </summary>
|
||||
/// <param name="vector1">点1</param>
|
||||
@ -768,7 +803,7 @@ namespace FlightRouteV2
|
||||
/// <param name="vector3">点3</param>
|
||||
/// <param name="vector4">点4</param>
|
||||
/// <returns>true在一个平面 false不在一个平面</returns>
|
||||
public static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
private static bool IsVecsOnPlane(Vector3 vec1, Vector3 vec2, Vector3 vec3, Vector3 vec4)
|
||||
{
|
||||
//计算三个向量
|
||||
Vector3 v1v2 = vec2 - vec1;
|
||||
@ -1447,12 +1482,10 @@ namespace FlightRouteV2
|
||||
/// </summary>
|
||||
/// <param name="aVecs">起始坐标集合</param>
|
||||
/// <param name="bVecs">终点做标集合</param>
|
||||
/// <param name="aName">起始坐标航点名称</param>
|
||||
/// <param name="bName">终点坐标航点名称</param>
|
||||
/// <param name="StrPrint">日志输出 回调函数</param>
|
||||
/// <param name="layHight">错层层高</param>
|
||||
/// <returns>返回一个二维向量坐标集合 middle[0]是第一个中间航点 middle[1]是第二个中间航点 返回空数组则代表两个图形不在一个平面上或者不够4个点</returns>
|
||||
public static List<List<Vector3>> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,string aName,string bName ,SomeCalculateWay StrPrint, double layHight = 185)
|
||||
public static List<List<Vector3>> CollisionLayer(Vector3[] aVecs, Vector3[] bVecs,SomeCalculateWay StrPrint, double layHight = 185)
|
||||
{
|
||||
long t = DateTimeOffset.UtcNow.ToUnixTimeSeconds();
|
||||
StrPrint("-------错层，开始-------");
|
||||
@ -1467,102 +1500,71 @@ namespace FlightRouteV2
|
||||
}
|
||||
//获取飞机总数
|
||||
int planeCou = aVecs.Length;
|
||||
if (planeCou < 2)//至少不少于2架飞机 才开始错层
|
||||
|
||||
Vector3[] new_aVecs = aVecs.ToArray(); //a图副本
|
||||
Vector3[] new_bVecs = bVecs.ToArray(); //b图副本
|
||||
|
||||
///把所有点压在 最“共面”上
|
||||
List<int> maxVecsOfCoplane = FindMaxPlaneIndices(aVecs);// 找出A图共面最多点的索引
|
||||
if (maxVecsOfCoplane.Count < 4) //a图至少要有4个点 共面
|
||||
{
|
||||
StrPrint("图案至少要两个点阵！");
|
||||
StrPrint("a图案至少有4个点以上共面，否则不可执行错层处理");
|
||||
StrPrint($"-------错层结束-------");
|
||||
return re;
|
||||
}
|
||||
//共面上取三个点
|
||||
Vector3 vec0 = new_aVecs[maxVecsOfCoplane[0]];
|
||||
Vector3 vec1 = new_aVecs[maxVecsOfCoplane[1]];
|
||||
Vector3 vec2 = new_aVecs[maxVecsOfCoplane[2]];
|
||||
for (int i = 0; i < planeCou; i++) //遍历 把a图和b图点压到 “共面”上
|
||||
{
|
||||
if (!(maxVecsOfCoplane.Contains(i))) //除去在共面内的面
|
||||
{
|
||||
new_aVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_aVecs[i]); //压平到共面上
|
||||
}
|
||||
new_bVecs[i] = CalculateIntersectionPoint(vec0, vec1, vec2, new_bVecs[i]); //压平到共面上
|
||||
}
|
||||
|
||||
///从ab图案中取最外层3个点 确定一个面 ps:方便后续叉积算法线标量
|
||||
Vector3 vec0 = new Vector3(0, 0, 0); //记录最外圈一个坐标
|
||||
Vector3 vec1 = new Vector3(0, 0, 0); //记录离最外圈坐标最远的一个点坐标
|
||||
Vector3 vec2 = new Vector3(0, 0, 0); //最外圈距离 和最远距离 之和最远的一个点
|
||||
// 求重心
|
||||
List<Vector3> allVecs = new List<Vector3>();
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
allVecs.Add(aVecs[i]);
|
||||
allVecs.Add(bVecs[i]);
|
||||
}
|
||||
Vector3 centerVec = GetPosCenter(allVecs);//重心点
|
||||
// 遍历所有ab点
|
||||
double tempLong = 0;
|
||||
double currentLong;
|
||||
for (int i = 0; i < planeCou * 2; i++) //取最外圈一个坐标
|
||||
{
|
||||
currentLong = GageLength(allVecs[i], centerVec);
|
||||
if (tempLong < currentLong)
|
||||
{
|
||||
vec0 = allVecs[i];
|
||||
tempLong = currentLong;
|
||||
}
|
||||
}
|
||||
tempLong = 0;
|
||||
for (int i = 0; i < planeCou * 2; i++) //取最外圈点
|
||||
{
|
||||
currentLong = GageLength(allVecs[i], vec0);
|
||||
if (tempLong < currentLong)
|
||||
{
|
||||
vec1 = allVecs[i];
|
||||
tempLong = currentLong;
|
||||
}
|
||||
}
|
||||
tempLong = 0;
|
||||
for (int i = 0; i < planeCou * 2; i++) //取最外圈点
|
||||
{
|
||||
currentLong =(GageLength(allVecs[i], vec0) + GageLength(allVecs[i], vec1));
|
||||
if (tempLong < currentLong)
|
||||
{
|
||||
vec2 = allVecs[i];
|
||||
tempLong = currentLong;
|
||||
}
|
||||
}
|
||||
///遍历两个图形所有点 是否在一个平面上
|
||||
string aNoPlaneSN= "";//记录a图案所有不在一个平面上的点
|
||||
string bNoPlaneSN = "";//记录b图案所有不在一个平面上的点
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
if (!IsVecsOnPlane(vec0, vec1, vec2, aVecs[i])) aNoPlaneSN += $"{i+1},";
|
||||
if (!IsVecsOnPlane(vec0, vec1, vec2, bVecs[i])) bNoPlaneSN += $"{i+1},";
|
||||
}
|
||||
if (aNoPlaneSN != "" || bNoPlaneSN != "")
|
||||
{
|
||||
if (aNoPlaneSN != "") StrPrint($"“{aName}”:{aNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!");
|
||||
if (bNoPlaneSN != "") StrPrint($"“{bName}”:{bNoPlaneSN}号飞机 ,不在一个平面，故不能做搓层处理!");
|
||||
StrPrint($"-------错层,结束-------");
|
||||
return re;//两个图形不在一个平面上 返回 空数组
|
||||
}
|
||||
///检查完毕后
|
||||
//计算法线向量
|
||||
///计算法线向量
|
||||
Vector3 side1 = vec1 - vec0;
|
||||
Vector3 side2 = vec2 - vec0;
|
||||
Vector3 normal = CrossPro(side1, side2);
|
||||
Vector3 normalScalar = normal.NormalizEd();//法线标量
|
||||
//开始错层
|
||||
///开始错层
|
||||
for (int i = 0; i < planeCou; i++)
|
||||
{
|
||||
int shiftCou = 1; //记录循环次数 即层数
|
||||
Vector3 aOrigin = aVecs[i]; //原点位置
|
||||
Vector3 bOrigin = bVecs[i]; //原点位置
|
||||
while (OnlyImitation(i, aVecs, bVecs))
|
||||
Vector3 aOrigin = new_aVecs[i]; //原点位置
|
||||
Vector3 bOrigin = new_bVecs[i]; //原点位置
|
||||
while (OnlyImitation(i, new_aVecs, new_bVecs))
|
||||
{
|
||||
Vector3 shiftVec = normalScalar * ((shiftCou + 1) / 2 * layHight);
|
||||
if (shiftCou % 2 == 1)
|
||||
{
|
||||
aVecs[i] = aOrigin + shiftVec;
|
||||
bVecs[i] = bOrigin + shiftVec;
|
||||
new_aVecs[i] = aOrigin + shiftVec;
|
||||
new_bVecs[i] = bOrigin + shiftVec;
|
||||
}
|
||||
else
|
||||
{
|
||||
aVecs[i] = aOrigin - shiftVec;
|
||||
bVecs[i] = bOrigin - shiftVec;
|
||||
new_aVecs[i] = aOrigin - shiftVec;
|
||||
new_bVecs[i] = bOrigin - shiftVec;
|
||||
}
|
||||
shiftCou += 1;
|
||||
}
|
||||
}
|
||||
re.Add(aVecs.ToList());
|
||||
re.Add(bVecs.ToList());
|
||||
///计算碰撞
|
||||
planesCollision = AirImitation(aVecs, new_aVecs).Concat(AirImitation(new_aVecs, new_bVecs)).ToList(); //获取碰撞组
|
||||
planesCollision = planesCollision.Concat(AirImitation(new_bVecs, bVecs)).ToList();
|
||||
if (planesCollision.Count == 0)
|
||||
{
|
||||
re.Add(new_aVecs.ToList());
|
||||
re.Add(new_bVecs.ToList());
|
||||
StrPrint($"错层成功");
|
||||
}
|
||||
else
|
||||
{
|
||||
StrPrint($"检测有碰撞，错层失败");
|
||||
}
|
||||
t = DateTimeOffset.UtcNow.ToUnixTimeSeconds() - t;
|
||||
StrPrint($"用时：{t}秒");
|
||||
StrPrint($"-------错层结束-------");
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -43,13 +43,31 @@ namespace FlyCube
|
||||
List<Vector3[]> abVecs = FileBase.TxtToPos("C:/Users/szdot/Desktop/1.txt", out string[] fightNames);//从txt文件里面读取航点 信息
|
||||
Vector3[] aVecs = abVecs[0].ToArray();
|
||||
Vector3[] bVecs = abVecs[1].ToArray();
|
||||
Vector3[] vecs = FlyVecFun.NormalPull(aVecs, bVecs, StrPrintAsync);
|
||||
Vector3[] new_bVecs = FlyVecFun.ContactABOut(aVecs, bVecs, StrPrint);
|
||||
List<List<Vector3>> listVecs = FlyVecFun.CollisionLayer(aVecs, new_bVecs, StrPrintAsync);
|
||||
|
||||
string txta = "";
|
||||
string txtb = "";
|
||||
string txtd = "";
|
||||
for (int i = 0; i < vecs.Length; i++)
|
||||
for (int i = 0; i < bVecs.Length; i++)
|
||||
{
|
||||
txtd += i + " 0" + " " + vecs[i].X + " " + vecs[i].Y + " " + vecs[i].Z + "\r\n";
|
||||
txta += i + " 0" + " " + listVecs[0][i].X + " " + listVecs[0][i].Y + " " + listVecs[0][i].Z + "\r\n";
|
||||
txtb += i + " 0" + " " + listVecs[1][i].X + " " + listVecs[1][i].Y + " " + listVecs[1][i].Z + "\r\n";
|
||||
txtd += i + " 0" + " " + new_bVecs[i].X + " " + new_bVecs[i].Y + " " + new_bVecs[i].Z + "\r\n";
|
||||
}
|
||||
string patha = "C:/Users/szdot/Desktop/a.txt";
|
||||
string pathb = "C:/Users/szdot/Desktop/b.txt";
|
||||
string pathd = "C:/Users/szdot/Desktop/d.txt";
|
||||
if (File.Exists(patha))
|
||||
{
|
||||
File.Delete(patha);
|
||||
}
|
||||
SaveFile(patha, txta);
|
||||
if (File.Exists(pathb))
|
||||
{
|
||||
File.Delete(pathb);
|
||||
}
|
||||
SaveFile(pathb, txtb);
|
||||
if (File.Exists(pathd))
|
||||
{
|
||||
File.Delete(pathd);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user