【Revit 二次开发】各类空间几何问题的判断及操作方法
【Revit 二次开发】各类空间几何问题的判断及操作方法
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.IO;
using Autodesk.Revit.UI;
using Autodesk.Revit.DB;
using Autodesk.Revit.Attributes;
using Autodesk.Revit.UI.Selection;
using Autodesk.Revit.UI.Events;
using Autodesk.Revit.ApplicationServices;
using Autodesk.Revit.DB.Structure;
using Autodesk.Revit.DB.Architecture;
using System.Windows.Forms;
using Autodesk.Revit.DB.Mechanical;
namespace ToolsforAIDesign
{
[Autodesk.Revit.Attributes.Transaction(Autodesk.Revit.Attributes.TransactionMode.Manual)]
[Autodesk.Revit.Attributes.Regeneration(Autodesk.Revit.Attributes.RegenerationOption.Manual)]
[Autodesk.Revit.Attributes.Journaling(Autodesk.Revit.Attributes.JournalingMode.UsingCommandData)]
public class distance : IExternalCommand, IExternalEventHandler
{
public string GetName()
{
return "distance";
}
public void Execute(UIApplication uiApp)
{
try
{
XYZ[] seg1 = new XYZ[2] { new XYZ ( 1.0,0.0,0.0),new XYZ(3.0,0.0,0.0) };
XYZ[] seg2 = new XYZ[2] { new XYZ(1.0, 0.0, 0.0), new XYZ(-3.0, 0.0, 0.0) };
bool b;
double dis = 0.0;
XYZ s1p = new XYZ();
XYZ s2p = new XYZ();
segmentintersectorsegment(seg1, seg2, out b, out dis, out s1p, out s2p);
MessageBox.Show("v1碰撞v2:\n" + b.ToString() + "\n" +
dis.ToString() + "\n" +
s1p.X.ToString() +" "+ s1p.Y.ToString() + " " + s1p.Z.ToString() + "\n" +
s2p.X.ToString() + " " + s2p.Y.ToString() + " " + s2p.Z.ToString());
XYZ[] v1 = new XYZ[8] { new XYZ(0.0, 0.0, 0.0), new XYZ(0.0, 10.0, 0.0), new XYZ(20.0, 10.0, 0.0), new XYZ(20.0, 0.0, 0.0),
new XYZ(20.0, 0.0, 15.0), new XYZ(20.0, 10.0, 15.0), new XYZ(0.0, 10.0, 15.0), new XYZ(0.0, 0.0, 15.0)};
XYZ[] v2 = new XYZ[8] { new XYZ(30.0, 0.0, 0.0), new XYZ(30.0, 10.0, 0.0), new XYZ(50.0, 10.0, 0.0), new XYZ(50.0, 0.0, 0.0),
new XYZ(50.0, 0.0, 15.0), new XYZ(50.0, 10.0, 15.0), new XYZ(30.0, 10.0, 15.0), new XYZ(30.0, 0.0, 15.0)};
XYZ[] v3 = new XYZ[8] {new XYZ(20.0, 10.0, 0.0),new XYZ(20.0, 0.0, 15.0), new XYZ(0.0, 0.0, 0.0), new XYZ(0.0, 0.0, 15.0), new XYZ(0.0, 10.0, 0.0), new XYZ(20.0, 10.0, 15.0), new XYZ(20.0, 0.0, 0.0),
new XYZ(0.0, 10.0, 15.0)};
double[] distancev0 = new double[6] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] distancev1 = new double[6] { 5.0, 5.0, 5.0, 5.0, 5.0, 10.0};
double[] distancev2 = new double[6] { 5.0, 5.0, 5.0, 5.0, 5.0, 15.0 };
double[] distancev3 = new double[6] { 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 };
double[] distancev4 = new double[6] { 0.0,-2.0, 0.0, -5.0, -2.0, 5.0 };
bool checkbool1 = ImpactCheck(v1, v2, distancev1, distancev0);
bool checkbool2 = ImpactCheck(v1, v2, distancev2, distancev0);
bool checkbool3 = ImpactCheck(v1, v2, distancev3, distancev0);
bool checkbool4 = ImpactCheck(v1, v2, distancev4, distancev0);
bool checkbool5 = Impacttwolist(new List { v1}, new List { v2 },new List { distancev4 }, new List { distancev0 });
XYZ[] v3_r = new XYZ[8];
v3_r = trtotr(v1);
MessageBox.Show("v1碰撞v2:\n" + checkbool1.ToString() + "\n"+
checkbool2.ToString() + "\n" +
checkbool3.ToString() + "\n" +
checkbool4.ToString() + "\n" +
checkbool5.ToString());
MessageBox.Show("v3->v3_r" + v3_r[0].ToString() + " " + v3_r[1].ToString() + " "+ v3_r[2].ToString() + " "+ v3_r[3].ToString() + "\n"+
v3_r[4].ToString() + " " + v3_r[5].ToString() + " " + v3_r[6].ToString() + " " + v3_r[7].ToString());
XYZ[] s1st = new XYZ[2];
XYZ[] s2se = new XYZ[2];
s1st[0] = new XYZ(0.0, 0.0, 0.0);
s1st[1] = new XYZ(1.0, 0.0, 0.0);
s2se[0] = new XYZ(0.0, 0.0, -1.0);
s2se[1] = new XYZ(0.0, 2.0, 1.0);
bool s1s2 = b_straightintersectorsegment(s1st, s2se);
bool s1s22;
double d = 0.0;
XYZ aa1 = new XYZ();
XYZ aa2 = new XYZ();
straightintersectorsegment(s1st, s2se, out s1s22, out d, out aa1, out aa2);
MessageBox.Show(s1s2.ToString() + "\n" +
s1s22.ToString() + "\n" +
d.ToString() + "\n" +
aa1.X.ToString() + " " + aa1.Y.ToString() + " " + aa1.Z.ToString() + "\n" +
aa2.X.ToString() + " " + aa2.Y.ToString() + " " + aa2.Z.ToString());
MessageBox.Show("检查增加三棱柱");
XYZ[] trpr = new XYZ[6] { new XYZ(0.0, 0.0, 10.0), new XYZ(4.0, 6.0, 0.0), new XYZ(10.0, 0.0, 0.0), new XYZ(4.0, 6.0, 10.0), new XYZ(0.0, 0.0, 0.0), new XYZ(10.0, 0.0, 10.0)};
XYZ[] trpr2 = new XYZ[6] { new XYZ(3.47096465, -1.38723338, -1.81929649), new XYZ(11.2834155, 4.56609445, 0.0578023684), new XYZ(-0.274381858, 1.27902813, 5.31256366), new XYZ(-2.77127953, 6.06362556, 0.529977599), new XYZ(5.04117128, 12.0169534, 2.40707645), new XYZ(7.53806895, 7.23235596, 7.18966251) };
XYZ[] trr = new XYZ[6];
trr = trprtotrpr(trpr);
XYZ[] trer = new XYZ[3] { new XYZ(0.0,0.0,0.0),new XYZ(10.0,0.0,0.0),new XYZ(4.0,6.0,0.0)};
XYZ[] ltr1 = new XYZ[2] { new XYZ(0.0,0.0,0.0),new XYZ(0.0,0.0,1.0)};
XYZ[] ltr2 = new XYZ[2] { new XYZ(-1.0, 0.0, 0.0), new XYZ(0.0, 0.0, 1.0) };
bool btr;
XYZ ptr = new XYZ();
lineintersectorface(ltr1, trer, out btr, out ptr);
lineintersectorface(ltr2, trer, out btr, out ptr);
XYZ[] trrr = zoom(trr,new double[5] {-2.0,-2.0,-2.0,-2.0,-2.0 });
XYZ[] trpr2new = trprtotrpr(trpr2);
bool pen = volumeintersactorvolume(trr,trpr2new);
}
catch (Exception e)
{
string message = e.Message;
MessageBox.Show(message);
return;
}
}
///
/// volumeintersactorvolume 判断平行四边形体(或者三棱柱)与平行四边形体（或者三棱柱）之间是否相交(包括包含关系)
///
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// true表示相交，false表示不相交
public static bool volumeintersactorvolume(XYZ[] v1, XYZ[] v2)
{
bool b = false;
List v1faces = new List();
List v2faces = new List();
#region 针对长方体或者三棱柱
if (v1.Length == 8)
{
v1faces.AddRange(new List { new XYZ[4]{ v1[0], v1[1] , v1[2] , v1[3] }, new XYZ[4]{ v1[0], v1[3] , v1[4] , v1[7] }, new XYZ[4]{ v1[0], v1[7], v1[6] , v1[1] },
new XYZ[4]{ v1[5], v1[6] , v1[7] , v1[4] }, new XYZ[4]{ v1[5], v1[2] , v1[1] , v1[6] }, new XYZ[4]{ v1[5], v1[4], v1[3] , v1[2] }});
}
if (v2.Length == 8)
{
v2faces.AddRange(new List { new XYZ[4]{ v2[0], v2[1] , v2[2] , v2[3] }, new XYZ[4]{ v2[0], v2[3] , v2[4] , v2[7] }, new XYZ[4]{ v2[0], v2[7], v2[6] , v2[1] },
new XYZ[4]{ v2[5], v2[6] , v2[7] , v2[4] }, new XYZ[4]{ v2[5], v2[2] , v2[1] , v2[6] }, new XYZ[4]{ v2[5], v2[4], v2[3] , v2[2] }});
}
if (v1.Length == 6)
{
v1faces.AddRange(new List { new XYZ[3]{ v1[0], v1[1] , v1[2]}, new XYZ[4]{ v1[0], v1[2] , v1[3] , v1[5] }, new XYZ[4]{ v1[0], v1[5], v1[4] , v1[1] },
new XYZ[3]{ v1[3], v1[4] , v1[5]}, new XYZ[4]{ v1[3], v1[2] , v1[1] , v1[4] }});
}
if (v2.Length == 6)
{
v2faces.AddRange(new List { new XYZ[3]{ v2[0], v2[1] , v2[2]}, new XYZ[4]{ v2[0], v2[2] , v2[3] , v2[5] }, new XYZ[4]{ v2[0], v2[5], v2[4] , v2[1] },
new XYZ[3]{ v2[3], v2[4] , v2[5]}, new XYZ[4]{ v2[3], v2[2] , v2[1] , v2[4] }});
}
for (int i = 0; i < v1.Length; i++) //检查v1是否有顶点在v2内部以及表面
{
if (pointincuboid(v1[i], v2))
{
b = true;
break;
}
}
if (!b)
{
for (int i = 0; i < v2.Length; i++) //检查v2是否有顶点在v1内部以及表面
{
if (pointincuboid(v2[i], v1))
{
b = true;
break;
}
}
if (!b)
{
for (int i = 0; i < v1faces.Count; i++) //检查是否有有限面相交
{
for (int j = 0; j < v2faces.Count; j++)
{
b = faceintersactorface(v1faces[i], v2faces[j]);
if (b)
{
break;
}
}
if (b)
{
break;
}
}
}
}
#endregion
return b;
}
///
/// faceintersactorface 判断有限面（平行四边形或者三角形）与有限面（平行四边形或者三角形）之间是否相交(包含包含关系)
///
/// 空间中一个有限平行四边形面或者三角形面(按照右手螺旋规则布置顶点)
/// 空间中一个有限平行四边形面或者三角形面(按照右手螺旋规则布置顶点)
/// true表示相交，false表示不相交
public static bool faceintersactorface(XYZ[] f1, XYZ[] f2)
{
bool b = false;
//先检查f1的边
b = b_lineintersectorface(new XYZ[2] { f1[f1.Length - 1], f1[0] }, f2);
if (!b)
{
for (int i = 0; i < f1.Length - 1; i++)
{
if (b_lineintersectorface(new XYZ[2] { f1[i + 1], f1[i] }, f2))
{
b = true;
break;
}
}
if (!b)
{
//再检查f2的边
b = b_lineintersectorface(new XYZ[2] { f2[f2.Length - 1], f2[0] }, f1);
if (!b)
{
for (int i = 0; i < f2.Length - 1; i++)
{
if (b_lineintersectorface(new XYZ[2] { f2[i + 1], f2[i] }, f1))
{
b = true;
break;
}
}
}
}
}
return b;
}
///
/// lineintersectorface 判定一根线段是否和有限区域面(平行四边形面或者三角形面)相交，如果相交输出交点p(包含关系)
///
/// 空间中一条线段（两个顶点）
/// 空间中一个有限面（平行四边形面或者三角形面）(按照右手螺旋规则布置顶点)
/// 返回布尔值，true则为相交，false则为不相交
/// 返回相交点
public static void lineintersectorface(XYZ[] l, XYZ[] f, out bool b, out XYZ p)
{
b = false;
p = null;
//首先求解线和面的交点p
XYZ n = (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]) / (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]).GetLength(); //面的单位法向量
if (Math.Abs(n.DotProduct(l[1] - l[0])) > 1.0E-12) //保证不平行
{
XYZ ptem = l[0] + ((n.DotProduct(f[0]) - n.DotProduct(l[0])) / (n.DotProduct(l[1] - l[0]))) * (l[1] - l[0]); //得到交点ptem
//判定p是否在某一个有限面内以及p点是否在线段内
bool b1 = pointincuboid(ptem, f);
bool b2 = pointincuboid(ptem, l);
if (b1 && b2)
{
b = true;
p = ptem;
}
}
else //如果平行还要判定这个线段是否部分或者全部在有限面内
{
if (d_pointtoface(l[0], new XYZ[2] { f[0], (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]) }, 0) < 1.0E-12) //线段在面上，现在需要判定线段是否与有限面相交
{
if (pointincuboid(l[0], f) || pointincuboid(l[1], f)) //针对线段至少有一个端点在面内（包括边上）
{
b = true;
if (pointincuboid(l[0], f))
{
p = l[0];
}
else
{
p = l[1];
}
}
else //线段的端点完全在外边，判断线段是否和某一条边相交（注意平行的情况处理）
{
List edge = new List();
if (f.Length == 3) //三角形面
{
edge.AddRange(new List { new XYZ[2] { f[0], f[1] }, new XYZ[2] { f[1], f[2] }, new XYZ[2] { f[2], f[0] } });
}
if (f.Length == 4) //平行四边形面
{
edge.AddRange(new List { new XYZ[2] { f[0], f[1] }, new XYZ[2] { f[1], f[2] }, new XYZ[2] { f[2], f[3] }, new XYZ[2] { f[3], f[0] } });
}
for (int i = 0; i < edge.Count; i++)
{
double dis = 0.0;
XYZ s1p = null;
XYZ s2p = null;
bool btem = false;
segmentintersectorsegment(l, edge[i], out btem, out dis, out s1p, out s2p);
if (btem)
{
b = true;
p = (s1p + s2p) / 2.0;
break;
}
}
if (!b)
{
b = false;
p = null;
}
}
}
else
{
b = false;
p = null;
}
}
}
///
/// lineintersectorface 判定一根直线是否和无限区域面相交，如果相交输出交点p(包含关系)
///
/// 空间中一条直线（一个顶点，一个x线方向）
/// 空间中一个无限面（一个位置点，一个面法向）
/// 返回布尔值，true则为相交，false则为不相交
/// 返回相交点
public static void straightlineintersectorface(XYZ[] l, XYZ[] f, out bool b, out XYZ p)
{
b = false;
p = null;
//首先求解线和面的交点p
XYZ n = f[1].Normalize(); //面的单位法向量
if (Math.Abs(n.DotProduct(l[1].Normalize())) > 1.0E-12) //保证不平行
{
XYZ ptem = l[0] + ((n.DotProduct(f[0]) - n.DotProduct(l[0])) / (n.DotProduct(l[1]))) * l[1]; //得到交点ptem
b = true;
p = ptem;
}
else //如果平行还要判定这个线段是否部分或者全部在有限面内
{
if (d_pointtoface(l[0], new XYZ[2] { f[0], f[1].Normalize() }, 0) < 1.0E-12) //直线在面上
{
b = true;
p = l[0];
}
else
{
b = false;
p = null;
}
}
}
///
/// b_lineintersectorface 判定一根线段是否和有限区域面（平行四边形面或者三角形面）相交（包含关系）
///
/// 空间中一条线段（两个顶点）
/// 空间中一个有限面（平行四边形面或者三角形面）(按照右手螺旋规则布置顶点)
public static bool b_lineintersectorface(XYZ[] l, XYZ[] f)
{
bool b = false;
//首先求解线和面的交点p
XYZ n = (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]) / (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]).GetLength(); //面的单位法向量
if (Math.Abs(n.DotProduct(l[1] - l[0])) > 1.0E-12) //保证线段与面不平行
{
XYZ p = l[0] + ((n.DotProduct(f[0]) - n.DotProduct(l[0])) / (n.DotProduct(l[1] - l[0]))) * (l[1] - l[0]); //求得交点p
//判定p是否在某一个有限面内以及p点是否在线段内
bool b1 = pointincuboid(p, f);
bool b2 = pointincuboid(p, l);
if (b1 && b2)
{
b = true;
}
}
else //如果线段和面平行还要判定这个线段是否部分或者全部在有限面内
{
if (d_pointtoface(l[0], new XYZ[2] { f[0], (f[1] - f[0]).CrossProduct(f[f.Length - 1] - f[0]) }, 0) < 1.0E-12) //线段在面上，现在需要判定线段是否与有限面相交
{
if (pointincuboid(l[0], f) || pointincuboid(l[1], f)) //针对线段至少有一个端点在面内（包括边上）
{
b = true;
}
else //线段的端点完全在外边，判断线段是否和某一条边相交（注意平行的情况处理）
{
List edge = new List();
if (f.Length == 3) //三角形面
{
edge.AddRange(new List { new XYZ[2] { f[0], f[1] }, new XYZ[2] { f[1], f[2] }, new XYZ[2] { f[2], f[0] } });
}
if (f.Length == 4) //平行四边形面
{
edge.AddRange(new List { new XYZ[2] { f[0], f[1] }, new XYZ[2] { f[1], f[2] }, new XYZ[2] { f[2], f[3] }, new XYZ[2] { f[3], f[0] } });
}
for (int i = 0; i < edge.Count; i++)
{
if (b_segmentintersectorsegment(l, edge[i]))
{
b = true;
break;
}
}
if (!b)
{
b = false;
}
}
}
else
{
b = false;
}
}
return b;
}
///
/// segmentintersectorsegment 求解空间中线段之间的最短距离，并且输出最近点
///
/// 线段（提供两个端点）
/// 线段（提供两个端点）
/// true则线段相交(或者重合)，false则线段不相交
/// 线段之间的最近距离
/// 线段1上的最近点
/// 线段2上的最近点
public static void segmentintersectorsegment(XYZ[] s1, XYZ[] s2, out bool b, out double dis, out XYZ s1p, out XYZ s2p)
{
dis = double.MaxValue;
b = false;
s1p = null;
s2p = null;
//首先求解两线段所在的两条直线的最近距离以及最近点
double d = (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) - Math.Pow((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]), 2.0);
if (d > 1.0E-12) //两根直线不平行
{
double s = ((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s1[0] - s2[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0])) / d;
double t = ((s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s1[1] - s1[0]).DotProduct(s1[0] - s2[0])) / d;
XYZ[] nearpoint = new XYZ[2];
nearpoint[0] = s1[0] + s * (s1[1] - s1[0]);
nearpoint[1] = s2[0] + t * (s2[1] - s2[0]);
double distancenearest = Math.Sqrt((s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])).DotProduct(s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])));
if (pointincuboid(nearpoint[0], s1) && pointincuboid(nearpoint[1], s2)) //最近点在两个线段上(两根线段相交)
{
if (distancenearest < 1.0E-12) //两根线段相交
{
b = true;
}
else //两根线段不相交
{
b = false;
}
dis = distancenearest;
s1p = nearpoint[0];
s2p = nearpoint[1];
}
else //最近点至少有一个不在对应线段上
{
b = false;
#region 先检验四个端点之间
XYZ[] segps1 = new XYZ[4] { s1[0], s1[0], s1[1], s1[1] };
XYZ[] segps2 = new XYZ[4] { s2[0], s2[1], s2[0], s2[1] };
double[] ep = new double[4] { s1[0].DistanceTo(s2[0]), s1[0].DistanceTo(s2[1]), s1[1].DistanceTo(s2[0]), s1[1].DistanceTo(s2[1]) }; //s1[0]-s2[0]、s1[0]-s2[1]、s1[1]-s2[0]、s1[1]-s2[1]
int mineplocal = 0;
for (int i = 1; i < ep.Length; i++)
{
if (ep[i] < ep[mineplocal])
{
mineplocal = i;
}
}
dis = ep[mineplocal];
s1p = segps1[mineplocal];
s2p = segps2[mineplocal];
#endregion
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[4] { s1[0], s1[1], s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]), s2[0], s2[1] };
double[] mapep = new double[4] { d_pointtoface(s1[0],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s1[1],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s1[0]-s2,s1[1]-s2,s2[0]-s1,s2[1]-s1
XYZ[] s1s2map = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]),
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
List s1s2mapin = new List();
for (int i = 0; i < s1s2map.Length; i++)
{
if (((i == 2 || i == 3) && pointincuboid(s1s2map[i], s1)) || ((i == 0 || i == 1) && pointincuboid(s1s2map[i], s2)))
{
s1s2mapin.Add(i);
}
}
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
if (mapep[minmapep] < ep[mineplocal])
{
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
}
#endregion
}
}
else //两直线平行或者重合
{
double distancenearest = d_pointtoface(s1[0], new XYZ[2] { s2[0], s2[1] - s2[0] }, 1);
if (distancenearest < 1.0E-12) //两根直线重合
{
b = false;
#region 先检验四个端点之间
XYZ[] segps1 = new XYZ[4] { s1[0], s1[0], s1[1], s1[1] };
XYZ[] segps2 = new XYZ[4] { s2[0], s2[1], s2[0], s2[1] };
double[] ep = new double[4] { s1[0].DistanceTo(s2[0]), s1[0].DistanceTo(s2[1]), s1[1].DistanceTo(s2[0]), s1[1].DistanceTo(s2[1]) }; //s1[0]-s2[0]、s1[0]-s2[1]、s1[1]-s2[0]、s1[1]-s2[1]
int mineplocal = 0;
for (int i = 1; i < ep.Length; i++)
{
if (ep[i] < ep[mineplocal])
{
mineplocal = i;
}
}
dis = ep[mineplocal];
s1p = segps1[mineplocal];
s2p = segps2[mineplocal];
#endregion
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[4] { s1[0], s1[1], s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]), s2[0], s2[1] };
double[] mapep = new double[4] { d_pointtoface(s1[0],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s1[1],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s1[0]-s2,s1[1]-s2,s2[0]-s1,s2[1]-s1
XYZ[] s1s2map = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]),
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
List s1s2mapin = new List();
for (int i = 0; i < s1s2map.Length; i++)
{
if (((i == 2 || i == 3) && pointincuboid(s1s2map[i], s1)) || ((i == 0 || i == 1) && pointincuboid(s1s2map[i], s2)))
{
s1s2mapin.Add(i);
}
}
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
if (mapep[minmapep] < ep[mineplocal])
{
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
}
if (dis < 1.0E-12)
{
b = true;
}
#endregion
}
else //两根直线不重合
{
b = false;
#region 先检验四个端点之间
XYZ[] segps1 = new XYZ[4] { s1[0], s1[0], s1[1], s1[1] };
XYZ[] segps2 = new XYZ[4] { s2[0], s2[1], s2[0], s2[1] };
double[] ep = new double[4] { s1[0].DistanceTo(s2[0]), s1[0].DistanceTo(s2[1]), s1[1].DistanceTo(s2[0]), s1[1].DistanceTo(s2[1]) }; //s1[0]-s2[0]、s1[0]-s2[1]、s1[1]-s2[0]、s1[1]-s2[1]
int mineplocal = 0;
for (int i = 1; i < ep.Length; i++)
{
if (ep[i] < ep[mineplocal])
{
mineplocal = i;
}
}
dis = ep[mineplocal];
s1p = segps1[mineplocal];
s2p = segps2[mineplocal];
#endregion
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[4] { s1[0], s1[1], s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]), s2[0], s2[1] };
double[] mapep = new double[4] { d_pointtoface(s1[0],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s1[1],new XYZ[2] { s2[0],s2[1]-s2[0]},1),
d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s1[0]-s2,s1[1]-s2,s2[0]-s1,s2[1]-s1
XYZ[] s1s2map = new XYZ[4] { s2[0] + (s1[0] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s2[0] + (s1[1] - s2[0]).DotProduct(s2[1] - s2[0]) / (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]),
s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]),
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
List s1s2mapin = new List();
for (int i = 0; i < s1s2map.Length; i++)
{
if (((i == 2 || i == 3) && pointincuboid(s1s2map[i], s1)) || ((i == 0 || i == 1) && pointincuboid(s1s2map[i], s2)))
{
s1s2mapin.Add(i);
}
}
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
if (mapep[minmapep] < ep[mineplocal])
{
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
}
#endregion
}
}
}
///
/// segmentintersectorsegment 判断线段与线段之间是否相交
///
/// 线段（提供两个端点）
/// 线段（提供两个端点）
///
/// pointijcuboid 判断一个点是否在一个长方体(或者平行四边形体)内部，判断一个空间点是否在三棱柱（包括斜三棱柱）中，判断一个空间点是否在一个空间平行四边形或者三角形内部，判断一个空间点是否在一个空间线段内部
///
/// 空间中的一点
/// 长方体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成（按照固定的规则））或者平行四边形(右手螺旋)或者三角形（右手螺旋）或者线段（两个顶点）
/// ture表示在里面，false表示在外边
public static bool pointincuboid(XYZ q, XYZ[] C)
{
bool determin = true;
double[] volumei = new double[8];
double volume = 0.0;
if (C.Length == 8) //平行四边形六面体
{
volumei[0] = 1.0 / 3.0 * (C[3] - C[0]).CrossProduct(C[1] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[3] - C[0]).CrossProduct(C[1] - C[0]) }, 0);
volumei[1] = 1.0 / 3.0 * (C[7] - C[0]).CrossProduct(C[1] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[7] - C[0]).CrossProduct(C[1] - C[0]) }, 0);
volumei[2] = 1.0 / 3.0 * (C[7] - C[0]).CrossProduct(C[3] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[7] - C[0]).CrossProduct(C[3] - C[0]) }, 0);
volumei[3] = 1.0 / 3.0 * (C[4] - C[5]).CrossProduct(C[6] - C[5]).GetLength() * d_pointtoface(q, new XYZ[2] { C[5], (C[4] - C[5]).CrossProduct(C[6] - C[5]) }, 0);
volumei[4] = 1.0 / 3.0 * (C[4] - C[5]).CrossProduct(C[2] - C[5]).GetLength() * d_pointtoface(q, new XYZ[2] { C[5], (C[4] - C[5]).CrossProduct(C[2] - C[5]) }, 0);
volumei[5] = 1.0 / 3.0 * (C[6] - C[5]).CrossProduct(C[2] - C[5]).GetLength() * d_pointtoface(q, new XYZ[2] { C[5], (C[6] - C[5]).CrossProduct(C[2] - C[5]) }, 0);
volume = (C[3] - C[0]).CrossProduct(C[1] - C[0]).GetLength() * d_pointtoface(C[7], new XYZ[2] { C[0], (C[3] - C[0]).CrossProduct(C[1] - C[0]) }, 0);
if (Math.Abs(volumei[0] + volumei[1] + volumei[2] + volumei[3] + volumei[4] + volumei[5] - volume) > 1.0E-8)
{
determin = false;
}
}
if (C.Length == 6) //三棱柱
{
volumei[0] = 1.0 / 6.0 * (C[1] - C[0]).CrossProduct(C[2] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[1] - C[0]).CrossProduct(C[2] - C[0]) }, 0);
volumei[1] = 1.0 / 3.0 * (C[5] - C[0]).CrossProduct(C[1] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[5] - C[0]).CrossProduct(C[1] - C[0]) }, 0);
volumei[2] = 1.0 / 3.0 * (C[2] - C[0]).CrossProduct(C[5] - C[0]).GetLength() * d_pointtoface(q, new XYZ[2] { C[0], (C[2] - C[0]).CrossProduct(C[5] - C[0]) }, 0);
volumei[3] = 1.0 / 6.0 * (C[4] - C[3]).CrossProduct(C[5] - C[3]).GetLength() * d_pointtoface(q, new XYZ[2] { C[3], (C[4] - C[3]).CrossProduct(C[5] - C[3]) }, 0);
volumei[4] = 1.0 / 3.0 * (C[2] - C[3]).CrossProduct(C[4] - C[3]).GetLength() * d_pointtoface(q, new XYZ[2] { C[3], (C[2] - C[3]).CrossProduct(C[4] - C[3]) }, 0);
volume = 1.0 / 2.0 * (C[1] - C[0]).CrossProduct(C[2] - C[0]).GetLength() * d_pointtoface(C[3], new XYZ[2] { C[0], (C[1] - C[0]).CrossProduct(C[2] - C[0]) }, 0);
if (Math.Abs(volumei[0] + volumei[1] + volumei[2] + volumei[3] + volumei[4] - volume) > 1.0E-8)
{
determin = false;
}
}
if (C.Length == 4) //四边形
{
volumei[0] = 1.0 / 2.0 * (C[1] - C[0]).CrossProduct(q - C[0]).GetLength();
volumei[1] = 1.0 / 2.0 * (C[2] - C[1]).CrossProduct(q - C[1]).GetLength();
volumei[2] = 1.0 / 2.0 * (C[3] - C[2]).CrossProduct(q - C[2]).GetLength();
volumei[3] = 1.0 / 2.0 * (C[0] - C[3]).CrossProduct(q - C[3]).GetLength();
volume = ((C[1] - C[0]).CrossProduct(C[3] - C[0]).GetLength()) / 2.0 + ((C[1] - C[2]).CrossProduct(C[3] - C[2]).GetLength()) / 2.0;
if (Math.Abs(volumei[0] + volumei[1] + volumei[2] + volumei[3] - volume) > 1.0E-8)
{
determin = false;
}
}
if (C.Length == 3) //三角形
{
volumei[0] = 1.0 / 2.0 * (C[1] - C[0]).CrossProduct(q - C[0]).GetLength();
volumei[1] = 1.0 / 2.0 * (C[2] - C[1]).CrossProduct(q - C[1]).GetLength();
volumei[2] = 1.0 / 2.0 * (C[0] - C[2]).CrossProduct(q - C[2]).GetLength();
volume = 1.0 / 2.0 * (C[1] - C[0]).CrossProduct(C[2] - C[0]).GetLength();
if (Math.Abs(volumei[0] + volumei[1] + volumei[2] - volume) > 1.0E-8)
{
determin = false;
}
}
if (C.Length == 2) //线段
{
volumei[0] = (q - C[0]).GetLength();
volumei[1] = (q - C[1]).GetLength();
volume = (C[1] - C[0]).GetLength();
if (Math.Abs(volumei[0] + volumei[1] - volume) > 1.0E-8)
{
determin = false;
}
}
if (C.Length != 8 && C.Length != 6 && C.Length != 4 && C.Length != 3 && C.Length != 2)
{
determin = false;
throw new NotImplementedException("注意提供的体或面或线段的顶点个数不正确");
}
return determin;
}
///
/// pointtoface 获取点到面(线)的最短距离以及面上的最近点
///
/// 面（线）外一点（也可以是面（线）内）
/// 面（线）（面（线）上一点，面（线）的法向(长度大于0.0)）
/// 点到面（线）的最短距离(如果为负值，点位于面的背面)
/// 面（线）上最近点
/// 0为点到面，1为点到线
public static void pointtoface(XYZ q, XYZ[] f, out double d, out XYZ fp, int index)
{
d = 0;
fp = null;
if (index == 0) //点到面的距离
{
if (Math.Sqrt(f[1].DotProduct(f[1])) > 0.0)
{
d = f[1].DotProduct(q - f[0]) / Math.Sqrt(f[1].DotProduct(f[1]));
fp = q - d * f[1] / Math.Sqrt(f[1].DotProduct(f[1]));
}
else
{
d = double.MaxValue;
fp = null;
throw new NotImplementedException("法向量f[1]程度小于等于0");
}
}
if (index == 1) //点到线的距离
{
if (f[1].GetLength() > 0.0)
{
d = ((q - f[0]).DotProduct(f[1] / (f[1].GetLength())) / f[1].GetLength() * f[1] + f[0] - q).GetLength();
fp = (q - f[0]).DotProduct(f[1] / (f[1].GetLength())) / f[1].GetLength() * f[1] + f[0];
}
else
{
d = double.MaxValue;
fp = null;
throw new NotImplementedException("方向向量f[1]程度小于等于0");
}
}
if (index != 1 && index != 0)
{
d = double.MaxValue;
fp = null;
throw new NotImplementedException("index值仅有0（平行四边形体）或者1（平行四边形）");
}
}
///
/// d_pointtoface 获取点到面（线）的最短距离
///
/// 面（线）外一点（也可以是面（线）内）
/// 面（线）（面（线）上一点，面（线）的法向(长度大于0.0)）
/// 0为点到面，1为点到线
public static double d_pointtoface(XYZ q, XYZ[] f, int index)
{
double d = -1.0;
if (index == 0) //点到面
{
if (Math.Sqrt(f[1].DotProduct(f[1])) > 0.0)
{
d = Math.Abs(f[1].DotProduct(q - f[0]) / Math.Sqrt(f[1].DotProduct(f[1])));
}
else
{
d = double.MaxValue;
throw new NotImplementedException("法向量f[1]程度小于等于0");
}
}
if (index == 1) //点到线
{
if (f[1].GetLength() > 0.0)
{
d = ((q - f[0]).DotProduct(f[1] / (f[1].GetLength())) / f[1].GetLength() * f[1] + f[0] - q).GetLength();
}
else
{
d = double.MaxValue;
throw new NotImplementedException("方向向量f[1]程度小于等于0");
}
}
if (index != 1 && index != 0)
{
d = double.MaxValue;
throw new NotImplementedException("index值仅有0（面）或者1（线）");
}
return d;
}
///
/// zoom 将平行四边形体或者三棱柱各面沿着法向放大或者缩小固定距离
///
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// 各面法向距离(平行四边形体按照固定规则（0123、0347、0761、5674、5216、5432）)(三棱柱按照固定规则（0235、2143、0541、012、345）)，单位：m
/// b = new List();
double[] disbackups = new double[dis.Length];
for (int i = 0; i < dis.Length; i++) //将m转化为英尺
{
double temp = dis[i];
disbackups[i] = temp;
dis[i] = temp * 3.2808399;
}
if (v.Length == 8)
{
#region 长方体
//获取边30 / 03、21 / 12、47 / 74、56 / 65延伸向量
XYZ[] edge03 = new XYZ[2] {(((v[7]-v[0]).CrossProduct(v[1]-v[0])/(v[7]-v[0]).CrossProduct(v[1]-v[0]).GetLength()*dis[2]).DotProduct(v[0]-v[3])/ (v[0] - v[3]).GetLength())* (v[0] - v[3]) / (v[0] - v[3]).GetLength() ,
(((v[2]-v[3]).CrossProduct(v[4]-v[3])/(v[2]-v[3]).CrossProduct(v[4]-v[3]).GetLength()*dis[5]).DotProduct(v[3]-v[0])/ (v[3] - v[0]).GetLength())* (v[3] - v[0]) / (v[3] - v[0]).GetLength()};
//获取边10 / 01、23 / 32、67 / 76、54 / 45延伸向量
XYZ[] edge01 = new XYZ[2] {(((v[3]-v[0]).CrossProduct(v[7]-v[0])/(v[3]-v[0]).CrossProduct(v[7]-v[0]).GetLength()*dis[1]).DotProduct(v[0]-v[1])/ (v[0] - v[1]).GetLength())* (v[0] - v[1]) / (v[0] - v[1]).GetLength() ,
(((v[6]-v[1]).CrossProduct(v[2]-v[1])/(v[6]-v[1]).CrossProduct(v[2]-v[1]).GetLength()*dis[4]).DotProduct(v[1]-v[0])/ (v[1] - v[0]).GetLength())* (v[1] - v[0]) / (v[1] - v[0]).GetLength()};
//获取边70 / 07、43 / 34、61 / 16、52 / 25延伸向量
XYZ[] edge07 = new XYZ[2] {(((v[1]-v[0]).CrossProduct(v[3]-v[0])/(v[1]-v[0]).CrossProduct(v[3]-v[0]).GetLength()*dis[0]).DotProduct(v[0]-v[7])/ (v[0] - v[7]).GetLength())* (v[0] - v[7]) / (v[0] - v[7]).GetLength() ,
(((v[4]-v[7]).CrossProduct(v[6]-v[7])/(v[4]-v[7]).CrossProduct(v[6]-v[7]).GetLength()*dis[3]).DotProduct(v[7]-v[0])/ (v[7] - v[0]).GetLength())* (v[7] - v[0]) / (v[7] - v[0]).GetLength()};
b.Add(v[0] + edge03[0] + edge01[0] + edge07[0]);
b.Add(v[1] + edge03[0] + edge01[1] + edge07[0]);
b.Add(v[2] + edge03[1] + edge01[1] + edge07[0]);
b.Add(v[3] + edge03[1] + edge01[0] + edge07[0]);
b.Add(v[4] + edge03[1] + edge01[0] + edge07[1]);
b.Add(v[5] + edge03[1] + edge01[1] + edge07[1]);
b.Add(v[6] + edge03[0] + edge01[1] + edge07[1]);
b.Add(v[7] + edge03[0] + edge01[0] + edge07[1]);
#endregion
}
if (v.Length == 6)
{
#region 三棱柱
//获取边10 / 01、45 / 54延伸向量 dis[0]指0235面外延dis[0]，dis[1]指2143外延dis[1]
XYZ[] edge10014554 = new XYZ[2] {dis[0]/(((v[2]-v[0]).CrossProduct(v[5]-v[0])).Normalize()).DotProduct((v[0]-v[1]).Normalize())*((v[0]-v[1]).Normalize()),
dis[1]/(((v[4]-v[1]).CrossProduct(v[2]-v[1])).Normalize()).DotProduct((v[1]-v[0]).Normalize())*((v[1]-v[0]).Normalize())};
//获取边20 / 02、35 / 53延伸向量 dis[2]指0541面外延dis[2]，dis[1]指2143外延dis[1]
XYZ[] edge20023553 = new XYZ[2] {dis[2]/(((v[5]-v[0]).CrossProduct(v[1]-v[0])).Normalize()).DotProduct((v[0]-v[2]).Normalize())*((v[0]-v[2]).Normalize()),
dis[1]/(((v[1]-v[2]).CrossProduct(v[3]-v[2])).Normalize()).DotProduct((v[2]-v[0]).Normalize())*((v[2]-v[0]).Normalize())};
//获取边21 / 12、34 / 43延伸向量 dis[2]指1054面外延dis[2]，dis[0]指0235外延dis[0]
XYZ[] edge21123443 = new XYZ[2] {dis[2]/(((v[0]-v[1]).CrossProduct(v[4]-v[1])).Normalize()).DotProduct((v[1]-v[2]).Normalize())*((v[1]-v[2]).Normalize()),
dis[0]/(((v[3]-v[2]).CrossProduct(v[0]-v[2])).Normalize()).DotProduct((v[2]-v[1]).Normalize())*((v[2]-v[1]).Normalize())};
//获取边50 / 05、41 / 14、32 / 23延伸向量 dis[3]指012面外延dis[0]，dis[4]指345外延dis[4]
XYZ[] edge052314 = new XYZ[2] {dis[3]/(((v[1]-v[0]).CrossProduct(v[2]-v[0])).Normalize()).DotProduct((v[0]-v[5]).Normalize())*((v[0]-v[5]).Normalize()),
dis[4]/(((v[3]-v[5]).CrossProduct(v[4]-v[5])).Normalize()).DotProduct((v[5]-v[0]).Normalize())*((v[5]-v[0]).Normalize())};
//b.Add(v[0] + edge10014554[0] + edge20023553[0] + edge052314[0]);
//b.Add(v[1] + edge10014554[1] + edge21123443[0] + edge052314[0]);
//b.Add(v[2] + edge20023553[1] + edge21123443[1] + edge052314[0]);
//b.Add(v[3] + edge20023553[1] + edge21123443[1] + edge052314[1]);
//b.Add(v[4] + edge10014554[1] + edge21123443[0] + edge052314[1]);
//b.Add(v[5] + edge10014554[0] + edge20023553[0] + edge052314[1]);
b.Add(v[0] + edge052314[0]);
b.Add(v[1] + edge052314[0]);
b.Add(v[2] + edge052314[0]);
b.Add(v[3] + edge052314[1]);
b.Add(v[4] + edge052314[1]);
b.Add(v[5] + edge052314[1]);
#endregion
}
XYZ[] b_r = b.ToArray();
for (int i = 0; i < disbackups.Length; i++) //还原dis
{
double temp = disbackups[i];
dis[i] = temp;
}
return b_r;
}
///
/// vertextoCentroid 将平行四边形体的8顶点表示法与中心半径表示法(v[1]->v[2]->v[3]符合右手螺旋准则)相互转换
///
/// 平行四边形体（8顶点表示法或中心半径表示法(v[1]->v[2]->v[3]符合右手螺旋准则)）
/// 平行四边形体（8顶点表示法或中心半径表示法(v[1]->v[2]->v[3]符合右手螺旋准则)）
public static XYZ[] vertextoCentroid(XYZ[] v)
{
XYZ[] b8 = new XYZ[8];
XYZ[] b4 = new XYZ[4];
if (v.Length == 8)
{
XYZ b40 = v[0].Add(v[5]).Multiply(0.5);
XYZ b41 = (v[3] - v[0]) / 2.0;
XYZ b42 = (v[1] - v[0]) / 2.0;
XYZ b43 = (v[7] - v[0]) / 2.0;
b4 = new XYZ[4] { b40, b41, b42, b43 };
return b4;
}
else if (v.Length == 4)
{
XYZ b80 = v[0] + (-v[1] - v[2] - v[3]);
XYZ b81 = v[0] + (-v[1] + v[2] - v[3]);
XYZ b82 = v[0] + (v[1] + v[2] - v[3]);
XYZ b83 = v[0] + (v[1] - v[2] - v[3]);
XYZ b84 = v[0] + (v[1] - v[2] + v[3]);
XYZ b85 = v[0] + (v[1] + v[2] + v[3]);
XYZ b86 = v[0] + (-v[1] + v[2] + v[3]);
XYZ b87 = v[0] + (-v[1] - v[2] + v[3]);
b8 = new XYZ[8] { b80, b81, b82, b83, b84, b85, b86, b87 };
return b8;
}
else
{
return v;
throw new NotImplementedException("v的长度不为8或者4");
}
}
///
/// ImpactCheck 对两个平行四边形体进行碰撞检测，碰撞限定距离给定（这种距离限定于面和面之间的距离）
///
/// 平行四边形体(作为被放大对象)（由8个顶点坐标构成(按照固定的规则)）
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）
/// v1各面法向距离(按照固定规则（0123、0347、0761、5674、5216、5432）)，单位：m//
/// v2各面法向距离(按照固定规则（0123、0347、0761、5674、5216、5432）)，单位：m
/// true表示碰撞，false表示不碰撞
public static bool ImpactCheck(XYZ[] v1, XYZ[] v2, double[] dis1, double[] dis2)
{
bool b = false;
//根据给定距离首先放大或者缩小平行四边形体
XYZ[] v1tem1 = zoom(v1, dis1);
XYZ[] v1tem2 = zoom(v2, dis2);
//碰撞检测
b = volumeintersactorvolume(v1tem1, v1tem2);
return b;
}
///
/// Nestbool 判定v1是否被嵌套在v2中
///
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// true表示判定v1被嵌套在v2中,false表示没有
public static bool Nestbool(XYZ[] v1, XYZ[] v2)
{
bool b = true;
for (int i = 0; i < v1.Length; i++)
{
if (!pointincuboid(v1[i], v2)) //只要有一个顶点没有被嵌套其中就算没有被嵌套
{
b = false;
break;
}
}
return b;
}
///
/// sumXYZAarry double[]二范数
///
/// double[]
/// double[]二范数
public static double sumAarry(double[] a)
{
double sumr = 0.0;
for (int i = 0; i < a.Length; i++)
{
sumr += Math.Pow(a[i], 2.0);
}
sumr += Math.Sqrt(sumr);
return sumr;
}
///
/// Impacttwo 两个对象之间的碰撞检测，每一个对象都是由多个平行四边形体或者三棱柱组成
///
/// 多个平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// 多个平行四边形体（由8个顶点坐标构成(按照固定的规则)）或者三棱柱（由6个顶点坐标构成(按照固定的规则)）
/// true表示碰撞，false表示不碰撞
public static bool Impacttwolist(List obejct1, List obejct2, List dis1, List dis2)
{
bool b = false; ;
for (int i = 0; i < obejct1.Count; i++)
{
for (int j = 0; j < obejct2.Count; j++)
{
if(j==37)
{
//MessageBox.Show(j.ToString());
}
if (ImpactCheck(obejct1[i], obejct2[j], dis1[i], dis2[j]))
{
b = true;
break; //碰撞退出
}
}
if (b)
{
b = true;
break;
}
}
return b;
}
///
/// trtotr 将一个平行四边形体由任意8个点的表达方式转换成规则的8个点的表达方式
///
/// 平行四边形体（由8个顶点坐标构成(无规则)）
/// 平行四边形体（由8个顶点坐标构成(按照固定的规则)）
public static XYZ[] trtotr(XYZ[] tro)
{
XYZ[] tr_rule = new XYZ[8];
List pointtr = new List();
bool p = false;
int p0 = 0;
int p1 = 0;
int p2 = 0;
int p3 = 0;
List edgeij = new List();
for (int i = 0; i < tro.Length - 1; i++) //存边
{
for (int j = i + 1; j < tro.Length; j++)
{
edgeij.Add(new int[2] { i, j });
}
}
#region 找第一个边的两个顶点
for (int i = 0; i < edgeij.Count; i++)
{
int index = 0;
for (int j = 0; j < edgeij.Count; j++)
{
if (j != i && (tro[edgeij[i][1]] - tro[edgeij[i][0]]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tro[edgeij[i][1]] - tro[edgeij[i][0]]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength()) < 1.0E-10) //根据平行关系来寻找
{
index += 1;
if (index == 3)
{
break;
}
}
}
if (index == 3)
{
pointtr.Add(tro[edgeij[i][0]]); //找到第一条边tro[edgeij[i][1]]- tro[edgeij[i][0]]的两个顶点
pointtr.Add(tro[edgeij[i][1]]);
p0 = edgeij[i][0];
p1 = edgeij[i][1];
edgeij.RemoveAt(i);
p = true;
break;
}
else
{
index = 0;
}
}
#endregion
if (p)
{
#region 找与第一个边垂直的另一个边，构成一个面
p = false;
for (int i = 0; i < tro.Length; i++)
{
if (i != p0 && i != p1)
{
int index = 0;
for (int j = 0; j < edgeij.Count; j++)
{
if (!((edgeij[j][1] == p1 && edgeij[j][0] == i) || (edgeij[j][1] == i && edgeij[j][0] == p1)))
{
if ((tro[p1] - tro[i]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tro[p1] - tro[i]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength()) < 1.0E-10)
{
index += 1;
if (index == 3)
{
break;
}
}
}
}
if (index == 3)
{
pointtr.Add(tro[i]); //找到第二条边(tro[p1] - tro[i])]的第二个顶点
p2 = i;
p = true;
break;
}
else
{
index = 0;
}
}
}
#endregion
if (p)
{
#region 找到与前三个点共面的第四个点
p = false;
for (int i = 0; i < tro.Length; i++)
{
if (i != p0 && i != p1 && i != p2)
{
if (d_pointtoface(tro[i], new XYZ[2] { pointtr[0], (pointtr[1] - pointtr[0]).CrossProduct(pointtr[2] - pointtr[0]) }, 0) < 1.0E-10)
{
pointtr.Add(tro[i]); //找到第四个顶点
p3 = i;
p = true;
break;
}
}
}
#endregion
if (p)
{
#region 调整面的法向
for (int i = 0; i < tro.Length; i++) //判断找到的一个面是否是按照外法向右手螺旋保存顶点的
{
if (i != p0 && i != p1 && i != p2 && i != p3)
{
if ((tro[i] - pointtr[0]).DotProduct((pointtr[1] - pointtr[0]).CrossProduct(pointtr[2] - pointtr[1])) < 0.0) //外法向
{
break;
}
else //内法向 需要调整
{
pointtr.Reverse();
break;
}
}
}
#endregion
#region 寻找对面
XYZ[] tros = new XYZ[4];
int indextros = 0;
for (int j = 0; j < tro.Length; j++) //将剩余的4个顶点放进tros中去
{
if (j != p0 && j != p1 && j != p2 && j != p3)
{
tros[indextros] = tro[j];
indextros += 1;
}
}
for (int i = pointtr.Count - 1; i >= 0; i--) //按规定规则存放另外一面几个点，这里采用的是按照距离来寻找的
{
bool ppb = false;
for (int trosi = 0; trosi < tros.Length; trosi++) //遍历剩余几个点
{
#region 找到四个平行的边
int index3 = 0;
for (int j = 0; j < edgeij.Count; j++)
{
if (tro[edgeij[j][0]].DistanceTo(pointtr[i]) > 1.0E-12 && tro[edgeij[j][1]].DistanceTo(pointtr[i]) > 1.0E-12 && (tros[trosi] - pointtr[i]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tros[trosi] - pointtr[i]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength()) < 1.0E-10) //根据平行关系来寻找
{
index3 += 1;
if (index3 == 3)
{
ppb = true;
break;
}
}
}
#endregion
if (ppb)
{
XYZ tem = tros[trosi];
pointtr.Add(tem);
break;
}
}
}
#endregion
}
else
{
throw new NotImplementedException("这不是平行四边形体");
}
}
else
{
throw new NotImplementedException("这不是平行四边形体");
}
}
else
{
throw new NotImplementedException("这不是平行四边形体");
}
tr_rule = pointtr.ToArray();
return tr_rule;
}
///
/// findtr 根据给定的OBB局部坐标系（笛卡尔坐标系）寻找OBB盒子
///
/// 空间点云
/// 相互垂直的两个VECTOR（第三个VECTOR会根据已知的两个右手螺旋确定）
/// OBB盒子的8个顶点（按照规定规则）
public static XYZ[] findtr(List points, XYZ[] vector)
{
XYZ[] tr = new XYZ[8];
XYZ unitvectorx = vector[0].Normalize();
XYZ unitvectory = vector[1].Normalize();
XYZ unitvectorz = vector[0].CrossProduct(vector[1]).Normalize();
//确定unitvectorz轴的边界上的点
XYZ zmax = points[0];
XYZ zmin = points[0];
for (int i = 1; i < points.Count; i++)
{
if (points[i].DotProduct(unitvectorz) > zmax.DotProduct(unitvectorz))
{
XYZ tem = points[i];
zmax = tem;
}
if (points[i].DotProduct(unitvectorz) < zmin.DotProduct(unitvectorz))
{
XYZ tem = points[i];
zmin = tem;
}
}
//确定unitvectorx轴的边界上的点
XYZ xmax = points[0];
XYZ xmin = points[0];
for (int i = 1; i < points.Count; i++)
{
if (points[i].DotProduct(unitvectorx) > xmax.DotProduct(unitvectorx))
{
XYZ tem = points[i];
xmax = tem;
}
if (points[i].DotProduct(unitvectorx) < xmin.DotProduct(unitvectorx))
{
XYZ tem = points[i];
xmin = tem;
}
}
//确定unitvectory轴的边界上的点
XYZ ymax = points[0];
XYZ ymin = points[0];
for (int i = 1; i < points.Count; i++)
{
if (points[i].DotProduct(unitvectory) > ymax.DotProduct(unitvectory))
{
XYZ tem = points[i];
ymax = tem;
}
if (points[i].DotProduct(unitvectory) < ymin.DotProduct(unitvectory))
{
XYZ tem = points[i];
ymin = tem;
}
}
//降低一下精度 舍入8位小数
zmax = new XYZ(Math.Round(zmax.X, 8), Math.Round(zmax.Y, 8), Math.Round(zmax.Z, 8));
zmin = new XYZ(Math.Round(zmin.X, 8), Math.Round(zmin.Y, 8), Math.Round(zmin.Z, 8));
xmax = new XYZ(Math.Round(xmax.X, 8), Math.Round(xmax.Y, 8), Math.Round(xmax.Z, 8));
xmin = new XYZ(Math.Round(xmin.X, 8), Math.Round(xmin.Y, 8), Math.Round(xmin.Z, 8));
ymax = new XYZ(Math.Round(ymax.X, 8), Math.Round(ymax.Y, 8), Math.Round(ymax.Z, 8));
ymin = new XYZ(Math.Round(ymin.X, 8), Math.Round(ymin.Y, 8), Math.Round(ymin.Z, 8));
//
XYZ[] trtem = new XYZ[8];
trtem[0] = threeplanestopoint(new XYZ[2] { xmin, unitvectorx }, new XYZ[2] { ymin, unitvectory }, new XYZ[2] { zmin, unitvectorz });
trtem[1] = threeplanestopoint(new XYZ[2] { xmin, unitvectorx }, new XYZ[2] { ymax, unitvectory }, new XYZ[2] { zmin, unitvectorz });
trtem[2] = threeplanestopoint(new XYZ[2] { xmax, unitvectorx }, new XYZ[2] { ymax, unitvectory }, new XYZ[2] { zmin, unitvectorz });
trtem[3] = threeplanestopoint(new XYZ[2] { xmax, unitvectorx }, new XYZ[2] { ymin, unitvectory }, new XYZ[2] { zmin, unitvectorz });
trtem[4] = threeplanestopoint(new XYZ[2] { xmax, unitvectorx }, new XYZ[2] { ymin, unitvectory }, new XYZ[2] { zmax, unitvectorz });
trtem[5] = threeplanestopoint(new XYZ[2] { xmax, unitvectorx }, new XYZ[2] { ymax, unitvectory }, new XYZ[2] { zmax, unitvectorz });
trtem[6] = threeplanestopoint(new XYZ[2] { xmin, unitvectorx }, new XYZ[2] { ymax, unitvectory }, new XYZ[2] { zmax, unitvectorz });
trtem[7] = threeplanestopoint(new XYZ[2] { xmin, unitvectorx }, new XYZ[2] { ymin, unitvectory }, new XYZ[2] { zmax, unitvectorz });
tr = trtotr(trtem);
return tr;
}
///
/// threeplanestopoint 求相交的三个面的交点
///
/// 面（面上一点和面的法向量构成）
/// 面（面上一点和面的法向量构成）
/// 面（面上一点和面的法向量构成）
/// 面的相交点，若没有相交点则输出为null
public static XYZ threeplanestopoint(XYZ[] p1, XYZ[] p2, XYZ[] p3)
{
XYZ point = null;
try
{
XYZ d = new XYZ(p1[1].DotProduct(p1[0]), p2[1].DotProduct(p2[0]), p3[1].DotProduct(p3[0]));
XYZ m1 = new XYZ(p1[1].X, p2[1].X, p3[1].X);
XYZ m2 = new XYZ(p1[1].Y, p2[1].Y, p3[1].Y);
XYZ m3 = new XYZ(p1[1].Z, p2[1].Z, p3[1].Z);
XYZ u = m2.CrossProduct(m3);
XYZ v = m1.CrossProduct(d);
double x1 = d.DotProduct(u) / m1.DotProduct(u);
double x2 = m3.DotProduct(v) / m1.DotProduct(u);
double x3 = -1.0 * m2.DotProduct(v) / m1.DotProduct(u);
point = new XYZ(x1, x2, x3);
}
catch (Exception e)
{
point = null;
}
return point;
}
///
/// trtotr 将一个三棱柱由任意6个点的表达方式转换成规则的6个点的表达方式
///
/// 三棱柱（由6个顶点坐标构成(无规则)）
/// 三棱柱（由6个顶点坐标构成(按照固定的规则)）
public static XYZ[] trprtotrpr(XYZ[] tro, double precision = 1.0E-7)
{
XYZ[] tr_rule = new XYZ[6];
bool p = false;
List edgeij = new List();
for (int i = 0; i < tro.Length - 1; i++) //存边
{
for (int j = i + 1; j < tro.Length; j++)
{
edgeij.Add(new int[2] { i, j });
}
}
#region 找第一个边的两个顶点（如果找到一条边其拥有两个平行边，那么这个边视为三棱柱的侧面棱边）
List paralleledgesXYZ = new List();
List paralleledgesNUM = new List();
for (int i = 0; i < edgeij.Count; i++)
{
int index = 0;
for (int j = 0; j < edgeij.Count; j++)
{
// MessageBox.Show(i.ToString()+" "+j.ToString()+"\n"+((tro[edgeij[i][1]] - tro[edgeij[i][0]]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tro[edgeij[i][1]] - tro[edgeij[i][0]]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength())).ToString());
// double tete = (tro[edgeij[i][1]] - tro[edgeij[i][0]]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tro[edgeij[i][1]] - tro[edgeij[i][0]]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength());
if (j != i && (tro[edgeij[i][1]] - tro[edgeij[i][0]]).CrossProduct(tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength() / ((tro[edgeij[i][1]] - tro[edgeij[i][0]]).GetLength() * (tro[edgeij[j][1]] - tro[edgeij[j][0]]).GetLength()) < precision) //根据平行关系来寻找
{
index += 1;
paralleledgesXYZ.Add(new XYZ[2] { tro[edgeij[j][0]], tro[edgeij[j][1]] });
paralleledgesNUM.Add(new int[2] { edgeij[j][0], edgeij[j][1] });
if (index == 2) //找到第i条边的两条平行棱边就退出
{
break;
}
}
}
if (index == 2)
{
paralleledgesXYZ.Insert(0, new XYZ[2] { tro[edgeij[i][0]], tro[edgeij[i][1]] });
paralleledgesNUM.Insert(0, new int[2] { edgeij[i][0], edgeij[i][1] });
p = true;
break;
}
else
{
index = 0;
paralleledgesXYZ.Clear();
paralleledgesNUM.Clear();
}
}
#endregion
if (p)
{
//检查1边端点顺序是否和0边一致
double tete = (((paralleledgesXYZ[1][0] - paralleledgesXYZ[0][0]).Normalize()).CrossProduct((paralleledgesXYZ[1][1] - paralleledgesXYZ[0][1]).Normalize())).GetLength();
if ((((paralleledgesXYZ[1][0] - paralleledgesXYZ[0][0]).Normalize()).CrossProduct((paralleledgesXYZ[1][1] - paralleledgesXYZ[0][1]).Normalize())).GetLength() >= precision)
{
XYZ pr0 = paralleledgesXYZ[1][1];
XYZ pr1 = paralleledgesXYZ[1][0];
paralleledgesXYZ.RemoveAt(1);
paralleledgesXYZ.Insert(1, new XYZ[2] { pr0, pr1 });
int num0 = paralleledgesNUM[1][1];
int num1 = paralleledgesNUM[1][0];
paralleledgesNUM.RemoveAt(1);
paralleledgesNUM.Insert(1, new int[2] { num0, num1 });
}
//检查2边端点顺序是否和0边一致
if ((((paralleledgesXYZ[2][0] - paralleledgesXYZ[0][0]).Normalize()).CrossProduct((paralleledgesXYZ[2][1] - paralleledgesXYZ[0][1]).Normalize())).GetLength() >= precision)
{
XYZ pr0 = paralleledgesXYZ[2][1];
XYZ pr1 = paralleledgesXYZ[2][0];
paralleledgesXYZ.RemoveAt(2);
paralleledgesXYZ.Add(new XYZ[2] { pr0, pr1 });
int num0 = paralleledgesNUM[2][1];
int num1 = paralleledgesNUM[2][0];
paralleledgesNUM.RemoveAt(2);
paralleledgesNUM.Add(new int[2] { num0, num1 });
}
//检查三条边的0端点构成的三角面的法向是否为外法向
if (((paralleledgesXYZ[0][1] - paralleledgesXYZ[0][0]).Normalize()).DotProduct(((paralleledgesXYZ[1][0] - paralleledgesXYZ[0][0]).CrossProduct(paralleledgesXYZ[2][0] - paralleledgesXYZ[0][0])).Normalize()) > 0.0)
{
paralleledgesXYZ.Reverse();
paralleledgesNUM.Reverse();
}
//将3和2要对齐
XYZ p0 = paralleledgesXYZ[0][0];
XYZ p1 = paralleledgesXYZ[1][0];
XYZ p2 = paralleledgesXYZ[2][0];
XYZ p3 = paralleledgesXYZ[2][1];
XYZ p4 = paralleledgesXYZ[1][1];
XYZ p5 = paralleledgesXYZ[0][1];
//检测上下底面是否平行
if (((((p1 - p0).CrossProduct(p2 - p0)).Normalize()).CrossProduct(((p4 - p3).CrossProduct(p5 - p3)).Normalize())).GetLength() < precision) //检测到上下底面平行
{
tr_rule = new XYZ[6] { p0, p1, p2, p3, p4, p5 };
}
else
{
throw new NotImplementedException("这不是三棱柱");
}
}
else
{
throw new NotImplementedException("这不是三棱柱");
}
return tr_rule;
}
///
/// b_straightintersectorsegment 判断一根直线s1和一个线段s2是否相交
///
/// 直线（线上一点和方向向量）
/// 线段（线段两个端点）
///
/// straightintersectorsegment 求解空间中线段和直线之间的最短距离，并且输出最近点
///
/// 直线（线上一点和方向向量）
/// 线段（提供两个端点）
/// true则直线和线段相交(或者重合)，false则直线和线段不相交
/// 直线s1st和线段s2se之间的最近距离
/// 直线s1st上的最近点
/// 线段s2se上的最近点
public static void straightintersectorsegment(XYZ[] s1st, XYZ[] s2se, out bool b, out double dis, out XYZ s1p, out XYZ s2p)
{
XYZ[] s1 = new XYZ[2]; //直线
XYZ[] s2 = new XYZ[2]; //线段
s2 = s2se;
s1[0] = s1st[0];
s1[1] = s1st[0] + 1.0 * 3.2808399 * s1st[1];
dis = double.MaxValue;
b = false;
s1p = null;
s2p = null;
//首先求解两线段所在的两条直线的最近距离以及最近点
double d = (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) - Math.Pow((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]), 2.0);
if (d > 1.0E-12) //两根直线不平行
{
double s = ((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s1[0] - s2[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0])) / d;
double t = ((s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s1[1] - s1[0]).DotProduct(s1[0] - s2[0])) / d;
XYZ[] nearpoint = new XYZ[2];
nearpoint[0] = s1[0] + s * (s1[1] - s1[0]);
nearpoint[1] = s2[0] + t * (s2[1] - s2[0]);
double distancenearest = Math.Sqrt((s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])).DotProduct(s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])));
if (pointincuboid(nearpoint[1], s2)) //最近点在线段s2se上(直线和线段相交)
{
if (distancenearest < 1.0E-12) //直线和线段相交
{
b = true;
}
else //直线和线段不相交
{
b = false;
}
dis = distancenearest;
s1p = nearpoint[0];
s2p = nearpoint[1];
}
else //最近点不在线段s2se上
{
b = false;
#region 检验端点与投影点之间
XYZ[] s1s2 = new XYZ[2] {s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[2] { s2[0], s2[1] };
double[] mapep = new double[2] { d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s2[0]-s1,s2[1]-s1
List s1s2mapin = new List();
s1s2mapin.Add(0);
s1s2mapin.Add(1);
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
#endregion
}
}
else //两直线平行或者重合
{
double distancenearest = d_pointtoface(s1[0], new XYZ[2] { s2[0], s2[1] - s2[0] }, 1);
if (distancenearest < 1.0E-12) //两根直线重合
{
b = false;
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[2] { s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[2] { s2[0], s2[1] };
double[] mapep = new double[2] { d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s2[0]-s1,s2[1]-s1
List s1s2mapin = new List();
s1s2mapin.Add(0);
s1s2mapin.Add(1);
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
if (dis < 1.0E-12)
{
b = true;
}
#endregion
}
else //两根直线不重合
{
b = false;
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[2] { s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[2] { s2[0], s2[1] };
double[] mapep = new double[2] { d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s2[0]-s1,s2[1]-s1
List s1s2mapin = new List();
s1s2mapin.Add(0);
s1s2mapin.Add(1);
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
#endregion
}
}
}
///
/// straightintersectorstraight 求解空间中直线和直线之间的最短距离，并且输出最近点
///
/// 直线（线上一点和方向向量）
/// 直线（线上一点和方向向量））
/// true则直线和直线相交(或者重合)，false则直线和直线不相交
/// 直线s1st和直线s2se之间的最近距离
/// 直线s1st上的最近点
/// 直线s2se上的最近点
public static void straightintersectorstraight(XYZ[] s1st, XYZ[] s2st, out bool b, out double dis, out XYZ s1p, out XYZ s2p)
{
XYZ[] s1 = new XYZ[2]; //直线1
XYZ[] s2 = new XYZ[2]; //直线2
s2[0] = s2st[0];
s2[1] = s2st[0] + 1.0 * 3.2808399 * s2st[1];
s1[0] = s1st[0];
s1[1] = s1st[0] + 1.0 * 3.2808399 * s1st[1];
dis = double.MaxValue;
b = false;
s1p = null;
s2p = null;
//首先求解两条直线的最近距离以及最近点
double d = (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) - Math.Pow((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]), 2.0);
if (d > 1.0E-12) //两根直线不平行
{
double s = ((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s1[0] - s2[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0])) / d;
double t = ((s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s1[1] - s1[0]).DotProduct(s1[0] - s2[0])) / d;
XYZ[] nearpoint = new XYZ[2];
nearpoint[0] = s1[0] + s * (s1[1] - s1[0]);
nearpoint[1] = s2[0] + t * (s2[1] - s2[0]);
double distancenearest = Math.Sqrt((s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])).DotProduct(s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])));
dis = distancenearest;
s1p = nearpoint[0];
s2p = nearpoint[1];
if (dis < 1.0E-8)
{
b = true;
}
}
else //两直线平行或者重合
{
double distancenearest = d_pointtoface(s1[0], new XYZ[2] { s2[0], s2[1] - s2[0] }, 1);
if (distancenearest < 1.0E-12) //两根直线重合
{
b = false;
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[2] { s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[2] { s2[0], s2[1] };
double[] mapep = new double[2] { d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s2[0]-s1,s2[1]-s1
List s1s2mapin = new List();
s1s2mapin.Add(0);
s1s2mapin.Add(1);
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
if (dis < 1.0E-12)
{
b = true;
}
#endregion
}
else //两根直线不重合
{
b = false;
#region 再检验端点与投影点之间
XYZ[] s1s2 = new XYZ[2] { s1[0] + (s2[0] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0]) ,
s1[0] + (s2[1] - s1[0]).DotProduct(s1[1] - s1[0]) / (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s1[1] - s1[0])};
XYZ[] s2s1 = new XYZ[2] { s2[0], s2[1] };
double[] mapep = new double[2] { d_pointtoface(s2[0],new XYZ[2] { s1[0],s1[1]-s1[0]},1),
d_pointtoface(s2[1],new XYZ[2] { s1[0],s1[1]-s1[0]},1)}; //s2[0]-s1,s2[1]-s1
List s1s2mapin = new List();
s1s2mapin.Add(0);
s1s2mapin.Add(1);
if (s1s2mapin.Count > 0)
{
int minma = 0;
int minmapep = s1s2mapin[minma];
for (int i = 1; i < s1s2mapin.Count; i++)
{
if (mapep[s1s2mapin[i]] < mapep[s1s2mapin[minma]])
{
minma = i;
}
}
minmapep = s1s2mapin[minma];
dis = mapep[minmapep];
s1p = s1s2[minmapep];
s2p = s2s1[minmapep];
}
#endregion
}
}
}
///
/// 求解一群点的几何中心
///
/// 群点坐标
/// 中心坐标
public static XYZ centerofbox(XYZ[] box)
{
XYZ centerPoint = new XYZ();
if (box.Length >= 1)
{
centerPoint = box[0];
for (int i = 1; i < box.Length - 1; i++)
{
centerPoint += box[i];
}
centerPoint /= box.Length;
}
return centerPoint;
}
///
/// 将平面多边形放大
///
/// 平面多边形（顶点按照顺序不要求右手准则或者左手准则，但是要连续）
/// 放大（正值）或缩小（负值）的距离(不作要求)
/// ZoomPolygon(List Polygonorigin, double distance)
{
List zoompolygon = new List();
#region Find Normal Vector
double normallength = ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(Polygonorigin[1] - Polygonorigin[0])).GetLength();
XYZ normal = null;
if (normallength <= 1.0E-8)
{
int index = 1;
while (normallength <= 1.0E-8 && index < Polygonorigin.Count - 1)
{
if ((((Polygonorigin[index] - Polygonorigin[index - 1]).Normalize()).CrossProduct((Polygonorigin[index + 1] - Polygonorigin[index])).Normalize()).GetLength() > 1.0E-8)
{
normal = ((Polygonorigin[index] - Polygonorigin[index - 1]).CrossProduct(Polygonorigin[index + 1] - Polygonorigin[index])).Normalize();
break;
}
}
if (normal == null && (((Polygonorigin[Polygonorigin.Count - 1] - Polygonorigin[Polygonorigin.Count - 2]).Normalize()).CrossProduct((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1])).Normalize()).GetLength() > 1.0E-8)
{
normal = ((Polygonorigin[Polygonorigin.Count - 1] - Polygonorigin[Polygonorigin.Count - 2]).CrossProduct(Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1])).Normalize();
}
}
else
{
normal = ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(Polygonorigin[1] - Polygonorigin[0])).Normalize();
}
if (normal == null)
{
MessageBox.Show("Can't find Normal Vector");
return zoompolygon;
}
#endregion
#region Pan edges in vertices order
for (int i = 0; i < Polygonorigin.Count; i++)
{
if (i == 0)
{
XYZ[] edge1 = new XYZ[2];
edge1[0] = Polygonorigin[Polygonorigin.Count - 1] + ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(normal)).Normalize() * distance;
XYZ tem = Polygonorigin[0] + ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(normal)).Normalize() * distance;
edge1[1] = (tem - edge1[0]).Normalize();
XYZ[] edge2 = new XYZ[2];
edge2[0] = Polygonorigin[0] + ((Polygonorigin[1] - Polygonorigin[0]).CrossProduct(normal)).Normalize() * distance;
tem = Polygonorigin[1] + ((Polygonorigin[1] - Polygonorigin[0]).CrossProduct(normal)).Normalize() * distance;
edge2[1] = (tem - edge2[0]).Normalize();
bool b = false;
double d = 0.0;
XYZ p1, p2;
straightintersectorstraight(edge1, edge2, out b, out d, out p1, out p2); //Get Intersection Vertex of Adjacent two Edges
XYZ intersectionpoint = new XYZ();
intersectionpoint = (p1 + p2) / 2.0; //Intersection Vertex
zoompolygon.Add(intersectionpoint);
continue;
}
if (i > 0 && i < Polygonorigin.Count - 1)
{
XYZ[] edge1 = new XYZ[2];
edge1[0] = Polygonorigin[i - 1] + ((Polygonorigin[i] - Polygonorigin[i - 1]).CrossProduct(normal)).Normalize() * distance;
XYZ tem = Polygonorigin[i] + ((Polygonorigin[i] - Polygonorigin[i - 1]).CrossProduct(normal)).Normalize() * distance;
edge1[1] = (tem - edge1[0]).Normalize();
XYZ[] edge2 = new XYZ[2];
edge2[0] = Polygonorigin[i] + ((Polygonorigin[i + 1] - Polygonorigin[i]).CrossProduct(normal)).Normalize() * distance;
tem = Polygonorigin[i + 1] + ((Polygonorigin[i + 1] - Polygonorigin[i]).CrossProduct(normal)).Normalize() * distance;
edge2[1] = (tem - edge2[0]).Normalize();
bool b = false;
double d = 0.0;
XYZ p1, p2;
straightintersectorstraight(edge1, edge2, out b, out d, out p1, out p2); //Get Intersection Vertex of Adjacent two Edges
XYZ intersectionpoint = new XYZ();
intersectionpoint = (p1 + p2) / 2.0; //Intersection Vertex
zoompolygon.Add(intersectionpoint);
continue;
}
if (i == Polygonorigin.Count - 1)
{
XYZ[] edge1 = new XYZ[2];
edge1[0] = Polygonorigin[Polygonorigin.Count - 2] + ((Polygonorigin[Polygonorigin.Count - 1] - Polygonorigin[Polygonorigin.Count - 2]).CrossProduct(normal)).Normalize() * distance;
XYZ tem = Polygonorigin[Polygonorigin.Count - 1] + ((Polygonorigin[Polygonorigin.Count - 1] - Polygonorigin[Polygonorigin.Count - 2]).CrossProduct(normal)).Normalize() * distance;
edge1[1] = (tem - edge1[0]).Normalize();
XYZ[] edge2 = new XYZ[2];
edge2[0] = Polygonorigin[Polygonorigin.Count - 1] + ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(normal)).Normalize() * distance;
tem = Polygonorigin[0] + ((Polygonorigin[0] - Polygonorigin[Polygonorigin.Count - 1]).CrossProduct(normal)).Normalize() * distance;
edge2[1] = (tem - edge2[0]).Normalize();
bool b = false;
double d = 0.0;
XYZ p1, p2;
straightintersectorstraight(edge1, edge2, out b, out d, out p1, out p2); //Get Intersection Vertex of Adjacent two Edges
XYZ intersectionpoint = new XYZ();
intersectionpoint = (p1 + p2) / 2.0; //Intersection Vertex
zoompolygon.Add(intersectionpoint);
continue;
}
}
#endregion
return zoompolygon;
}
///
/// 判断点群是否在一个平面上（如果只有一个点，则输出false和null，并且报错）
///
///
///
/// true:在一个平面上和所在的平面，false:不在一个平面上和null
public static bool IsCoplanar(List points, out XYZ[] pointsPlane)
{
bool Res_IsCoplanar = true;
pointsPlane = new XYZ[2];
if (points.Count > 3)
{
XYZ edge1 = new XYZ();
XYZ edge2 = new XYZ();
XYZ normal = new XYZ();
XYZ normalPrevious = new XYZ();
edge1 = (points[1] - points[0]).Normalize();
edge2 = (points[2] - points[1]).Normalize();
normal = (edge1).CrossProduct(edge2);
normalPrevious = (edge1).CrossProduct(edge2);
bool label = normal.IsZeroLength() ? false : true;
for (int i = 3; i < points.Count; i++)
{
if (label)
{
edge1 = (points[i - 1] - points[i - 2]).Normalize();
edge2 = (points[i] - points[i - 1]).Normalize();
normalPrevious = (edge1).CrossProduct(edge2);
if (!normalPrevious.IsZeroLength())
{
if (!((normalPrevious.Normalize()).CrossProduct(normal.Normalize())).IsZeroLength())
{
Res_IsCoplanar = false;
break;
}
}
}
else
{
edge1 = (points[i - 1] - points[i - 2]).Normalize();
edge2 = (points[i] - points[i - 1]).Normalize();
normal = ((edge1).CrossProduct(edge2));
normal = normal.IsZeroLength() ? normal : normal.Normalize();
normalPrevious = (edge1).CrossProduct(edge2);
label = normal.IsZeroLength() ? false : true;
}
}
if (Res_IsCoplanar)
{
if (label)
{
pointsPlane[0] = points[0];
pointsPlane[1] = normal.Normalize();
}
else
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).IsZeroLength())
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).IsZeroLength())
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisZ)).Normalize();
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).Normalize();
}
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).Normalize();
}
}
}
else
{
pointsPlane = null;
}
}
else if (points.Count == 3)
{
XYZ edge1 = new XYZ();
XYZ edge2 = new XYZ();
XYZ normal = new XYZ();
XYZ normalPrevious = new XYZ();
edge1 = (points[1] - points[0]).Normalize();
edge2 = (points[2] - points[1]).Normalize();
normal = (edge1).CrossProduct(edge2);
normalPrevious = (edge1).CrossProduct(edge2);
bool label = normal.IsZeroLength() ? false : true;
if (label)
{
pointsPlane[0] = points[0];
pointsPlane[1] = normal.Normalize();
}
else
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).IsZeroLength())
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).IsZeroLength())
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisZ)).Normalize();
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).Normalize();
}
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).Normalize();
}
}
}
else if (points.Count == 2)
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).IsZeroLength())
{
if ((((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).IsZeroLength())
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisZ)).Normalize();
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisY)).Normalize();
}
}
else
{
pointsPlane[0] = points[0];
pointsPlane[1] = (((points[1] - points[0]).Normalize()).CrossProduct(XYZ.BasisX)).Normalize();
}
}
else
{
Res_IsCoplanar = false;
pointsPlane = null;
MessageBox.Show("路径至少一条线！");
}
return Res_IsCoplanar;
}
///
/// circlepoints
///
/// 拐弯处两侧四个点(0，1;2，3)
/// 拐弯半径(单位m)
/// circlepoints(List curvepoints, double radius)
{
List circlepoint = new List();
radius = radius / 0.3048; //将单位m转换成英尺 1英尺等于0.3048m
if (curvepoints.Count != 4)
{
MessageBox.Show("第一个参数curvepoints应该为4个点");
return circlepoint;
}
//确保拐弯半径要至少达到5cm
if (radius < 0.05 / 0.3048)
{
radius = 0.05 / 0.3048;
}
//判断curvepoints是否在一个平面上，如果不在将第四个点移到前三个点组成的面上（投影）
if (((curvepoints[2] - curvepoints[1]).CrossProduct(curvepoints[0] - curvepoints[1])).GetLength() > 1.0E-12)
{
if (d_pointtoface(curvepoints[3], new XYZ[2] { curvepoints[1], (curvepoints[0] - curvepoints[1]).CrossProduct(curvepoints[2] - curvepoints[1]) }, 0) > 1.0E-12)
{
XYZ forthpoint = new XYZ();
double d1 = 0.0;
pointtoface(curvepoints[3], new XYZ[2] { curvepoints[1], (curvepoints[0] - curvepoints[1]).CrossProduct(curvepoints[2] - curvepoints[1]) }, out d1, out forthpoint, 0);
curvepoints.RemoveAt(3);
curvepoints.Add(forthpoint);
}
}
//求圆弧上的控制点
#region 求圆弧上的控制点
XYZ intersectpoint = new XYZ();
XYZ[] s1 = new XYZ[2] { curvepoints[1], curvepoints[0] };
XYZ[] s2 = new XYZ[2] { curvepoints[2], curvepoints[3] };
double d = (s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0]) - Math.Pow((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]), 2.0);
if (d > 1.0E-12) //两根直线不平行
{
double s = ((s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s1[0] - s2[0]) * (s2[1] - s2[0]).DotProduct(s2[1] - s2[0])) / d;
double t = ((s1[1] - s1[0]).DotProduct(s1[1] - s1[0]) * (s2[1] - s2[0]).DotProduct(s1[0] - s2[0]) - (s1[1] - s1[0]).DotProduct(s2[1] - s2[0]) * (s1[1] - s1[0]).DotProduct(s1[0] - s2[0])) / d;
XYZ[] nearpoint = new XYZ[2];
nearpoint[0] = s1[0] + s * (s1[1] - s1[0]);
nearpoint[1] = s2[0] + t * (s2[1] - s2[0]);
double distancenearest = Math.Sqrt((s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])).DotProduct(s1[0] + s * (s1[1] - s1[0]) - s2[0] - t * (s2[1] - s2[0])));
if (distancenearest < 1.0E-12) //两根直线相交
{
intersectpoint = (nearpoint[0] + nearpoint[1]) / 2.0;
//求夹角（夹角为向量（curvepoints[1], curvepoints[0]）和向量（curvepoints[2], curvepoints[3]）的夹角）
double AngleSin = ((curvepoints[0] - curvepoints[1]).CrossProduct(curvepoints[3] - curvepoints[2])).GetLength() / ((curvepoints[0] - curvepoints[1]).GetLength() * (curvepoints[3] - curvepoints[2]).GetLength());
double AngleCos = (curvepoints[0] - curvepoints[1]).DotProduct(curvepoints[3] - curvepoints[2]) / ((curvepoints[0] - curvepoints[1]).GetLength() * (curvepoints[3] - curvepoints[2]).GetLength());
double Anglehalf = Math.Acos(AngleCos) * 180.0 / Math.PI / 2.0;
//求两根直线上的四个点
XYZ cutoffpointbehind01 = intersectpoint + (curvepoints[0] - curvepoints[1]).Normalize() * radius / Math.Tan(Anglehalf * Math.PI / 180.0);
XYZ cutoffpointfront01 = intersectpoint + (curvepoints[1] - curvepoints[0]).Normalize() * radius / Math.Tan(Anglehalf * Math.PI / 180.0);
XYZ cutoffpointbehind23 = intersectpoint + (curvepoints[2] - curvepoints[3]).Normalize() * radius / Math.Tan(Anglehalf * Math.PI / 180.0);
XYZ cutoffpointfront23 = intersectpoint + (curvepoints[3] - curvepoints[2]).Normalize() * radius / Math.Tan(Anglehalf * Math.PI / 180.0);
//找到圆心和对应的切点（确保走线满足右手螺旋要求）
XYZ centercirle = new XYZ();
XYZ cutoffpoint01 = new XYZ();
XYZ cutoffpoint23 = new XYZ();
XYZ centercirle1 = intersectpoint + ((curvepoints[0] - curvepoints[1]).Normalize() + (curvepoints[3] - curvepoints[2]).Normalize()).Normalize() * radius / Math.Sin(Anglehalf * Math.PI / 180.0);
XYZ centercirle2 = intersectpoint + ((curvepoints[1] - curvepoints[0]).Normalize() + (curvepoints[2] - curvepoints[3]).Normalize()).Normalize() * radius / Math.Sin(Anglehalf * Math.PI / 180.0);
if (((curvepoints[1] - curvepoints[0]).CrossProduct(centercirle1 - cutoffpointbehind01)).DotProduct((curvepoints[1] - curvepoints[0]).CrossProduct(curvepoints[3] - curvepoints[2])) > 0.0 &&
((cutoffpointfront23 - centercirle1).CrossProduct(curvepoints[3] - curvepoints[2])).DotProduct((curvepoints[1] - curvepoints[0]).CrossProduct(curvepoints[3] - curvepoints[2])) > 0.0)
{
centercirle = centercirle1;
cutoffpoint01 = cutoffpointbehind01;
cutoffpoint23 = cutoffpointfront23;
}
else
{
centercirle = centercirle2;
cutoffpoint01 = cutoffpointfront01;
cutoffpoint23 = cutoffpointbehind23;
}
//确定圆弧上的三个点（角平分线上一点、两侧角平分线各一点）
XYZ pointanglehalf = new XYZ();
XYZ pointangle01half = new XYZ();
XYZ pointanglehalf23 = new XYZ();
XYZ pointanglehalf1 = centercirle + (centercirle - intersectpoint).Normalize() * radius;
XYZ pointanglehalf2 = centercirle - (centercirle - intersectpoint).Normalize() * radius;
if (((pointanglehalf1 - cutoffpoint01).CrossProduct(cutoffpoint23 - pointanglehalf1)).DotProduct((curvepoints[1] - curvepoints[0]).CrossProduct(curvepoints[3] - curvepoints[2])) > 0.0)
{
pointanglehalf = pointanglehalf1;
pointangle01half = centercirle + (cutoffpoint01 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
pointanglehalf23 = centercirle + (cutoffpoint23 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
}
else
{
pointanglehalf = pointanglehalf2;
pointangle01half = centercirle + (cutoffpoint01 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
pointanglehalf23 = centercirle + (cutoffpoint23 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
}
//确定最终的一系列的点（包括直线段的几个点和圆弧段的几个点）
if ((cutoffpoint01 - curvepoints[0]).DotProduct(curvepoints[1] - curvepoints[0]) <= 0.0)
{
#region 确定布置点
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
else if ((cutoffpoint01 - curvepoints[0]).DotProduct(curvepoints[1] - curvepoints[0]) > 0.0 && (cutoffpoint01 - curvepoints[1]).DotProduct(curvepoints[1] - curvepoints[0]) <= 0.0 && (cutoffpoint01 - curvepoints[1]).GetLength() >= 0.01 * 3.2808399)
{
#region 确定布置点
circlepoint.Add(curvepoints[0]);
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
else if ((cutoffpoint01 - curvepoints[0]).DotProduct(curvepoints[1] - curvepoints[0]) > 0.0 && (cutoffpoint01 - curvepoints[1]).DotProduct(curvepoints[1] - curvepoints[0]) <= 0.0 && (cutoffpoint01 - curvepoints[1]).GetLength() < 0.01 * 3.2808399)
{
#region 确定布置点
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
else if ((cutoffpoint01 - curvepoints[1]).DotProduct(curvepoints[1] - curvepoints[0]) < 0.0 && (cutoffpoint01 - curvepoints[1]).GetLength() >= 0.01 * 3.2808399)
{
#region 确定布置点
circlepoint.Add(curvepoints[0]);
circlepoint.Add(curvepoints[1]);
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
else
{
#region 确定布置点
circlepoint.Add(curvepoints[0]);
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
}
else
{
MessageBox.Show("4个点不在一个平面上");
return circlepoint;
}
}
else //两根直线平行
{
//找到圆心和对应的切点（确保走线满足右手螺旋要求）
XYZ prjection1_23 = new XYZ();
double doubleradius = 0.0;
pointtoface(curvepoints[1], new XYZ[2] { curvepoints[2], curvepoints[3] - curvepoints[2] }, out doubleradius, out prjection1_23, 1);
XYZ centercirle = curvepoints[1] + (prjection1_23 - curvepoints[1]) / 2.0;
XYZ cutoffpoint01 = curvepoints[1];
XYZ cutoffpoint23 = prjection1_23;
//确定圆弧上的三个点（角平分线上一点、两侧角平分线各一点）
radius = doubleradius;
XYZ pointanglehalf = new XYZ();
XYZ pointangle01half = new XYZ();
XYZ pointanglehalf23 = new XYZ();
XYZ pointanglehalf1 = centercirle + (curvepoints[1] - curvepoints[0]).Normalize() * radius;
XYZ pointanglehalf2 = centercirle - (curvepoints[1] - curvepoints[0]).Normalize() * radius;
if ((pointanglehalf1 - cutoffpoint01).DotProduct(curvepoints[1] - curvepoints[0]) <= 0.0)
{
pointanglehalf = pointanglehalf1;
pointangle01half = centercirle + (cutoffpoint01 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
pointanglehalf23 = centercirle + (cutoffpoint23 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
}
else
{
pointanglehalf = pointanglehalf2;
pointangle01half = centercirle + (cutoffpoint01 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
pointanglehalf23 = centercirle + (cutoffpoint23 - centercirle + pointanglehalf - centercirle).Normalize() * radius;
}
//确定最终的一系列的点（包括直线段的几个点和圆弧段的几个点）
#region 确定布置点
circlepoint.Add(curvepoints[0]);
circlepoint.Add(cutoffpoint01);
circlepoint.Add(pointangle01half);
circlepoint.Add(pointanglehalf);
circlepoint.Add(pointanglehalf23);
if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
else if ((cutoffpoint23 - curvepoints[3]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) >= 0.0 && (cutoffpoint23 - curvepoints[3]).GetLength() < 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
}
else if ((cutoffpoint23 - curvepoints[2]).DotProduct(curvepoints[3] - curvepoints[2]) < 0.0 && (cutoffpoint23 - curvepoints[2]).GetLength() >= 0.01 * 3.2808399)
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[2]);
circlepoint.Add(curvepoints[3]);
}
else
{
circlepoint.Add(cutoffpoint23);
circlepoint.Add(curvepoints[3]);
}
#endregion
}
#endregion
return circlepoint;
}
///
/// circleLine 将路径灌完处转换为圆弧
///
/// 路径点
/// 对应每个连接点的拐弯半径(无明确单位，单位最后与points保持一致)
/// 拐弯角度下限值
/// 拐弯半径上限值
/// circleLine(List points, List radians, double lowerlimit, double upperlimit)
{
List curvepoints = new List();
for (int i = 0; i < points.Count; i++)
{
XYZ tempoints = new XYZ();
tempoints = points[i];
curvepoints.Add(tempoints);
}
List curves = new List();
if (curvepoints.Count > 2)
{
curvepoints.Add(points[points.Count - 1] - points[points.Count - 2] + points[points.Count - 1]);
radians.Add(0.0);
int i = 1;
while (i < curvepoints.Count - 1)
{
if (radians[i - 1] > 1.0E-8 &&
Math.Acos((curvepoints[i - 1] - curvepoints[i]).Normalize().DotProduct((curvepoints[i + 1] - curvepoints[i]).Normalize())) * 180.0 / Math.PI < upperlimit &&
Math.Acos((curvepoints[i - 1] - curvepoints[i]).Normalize().DotProduct((curvepoints[i + 1] - curvepoints[i]).Normalize())) * 180.0 / Math.PI > lowerlimit)
{
List newpoints = new List();
newpoints = circlepoints(new List() { curvepoints[i - 1], curvepoints[i], curvepoints[i], curvepoints[i + 1] }, radians[i - 1] * 0.3048);
if (newpoints.Count == 7)
{
Curve curve1 = Line.CreateBound(curvepoints[i - 1], newpoints[1]) as Curve;
Curve arc = Arc.Create(newpoints[1], newpoints[5], newpoints[3]) as Curve;
curves.Add(curve1);
curves.Add(arc);
curvepoints.RemoveAt(i);
curvepoints.Insert(i, newpoints[5]);
}
else if (newpoints.Count == 6)
{
if (((curvepoints[i] - curvepoints[i - 1]).Normalize().CrossProduct((newpoints[1] - newpoints[0]).Normalize())).GetLength() > 1.0E-8 &&
curvepoints[i - 1].DistanceTo(newpoints[0]) < 1.0 / 304.8)
{
List modifiednewpoints = new List();
if (curves.Count > 0)
{
Curve previouscurve = curves[curves.Count - 1];
modifiednewpoints.Add(previouscurve.GetEndPoint(1));
}
Curve arc = Arc.Create(curves.Count == 0 ? newpoints[0] : modifiednewpoints[0], newpoints[4], newpoints[2]) as Curve;
curves.Add(arc);
curvepoints.RemoveAt(i);
curvepoints.Insert(i, newpoints[4]);
}
else if (((curvepoints[i] - curvepoints[i + 1]).Normalize().CrossProduct((newpoints[4] - newpoints[5]).Normalize())).GetLength() > 1.0E-8 &&
curvepoints[i + 1].DistanceTo(newpoints[5]) < 1.0 / 304.8)
{
Curve curve1 = Line.CreateBound(curvepoints[i - 1], newpoints[1]) as Curve;
Curve arc = Arc.Create(newpoints[1], newpoints[5], newpoints[3]) as Curve;
curves.Add(curve1);
curves.Add(arc);
curvepoints.RemoveAt(i);
curvepoints.RemoveAt(i);
radians.RemoveAt(i - 1);
curvepoints.Insert(i, newpoints[5]);
}
else
{
Curve curve = Line.CreateBound(curvepoints[i - 1], curvepoints[i]) as Curve;
curves.Add(curve);
}
}
else if (newpoints.Count == 5)
{
if (curvepoints[i - 1].DistanceTo(newpoints[0]) < 1.0 / 304.8 &&
curvepoints[i + 1].DistanceTo(newpoints[4]) < 1.0 / 304.8)
{
List modifiednewpoints = new List();
if (curves.Count > 0)
{
Curve previouscurve = curves[curves.Count - 1];
modifiednewpoints.Add(previouscurve.GetEndPoint(1));
}
Curve arc = Arc.Create(curves.Count == 0 ? newpoints[0] : modifiednewpoints[0], newpoints[4], newpoints[2]) as Curve;
curves.Add(arc);
curvepoints.RemoveAt(i);
curvepoints.RemoveAt(i);
radians.RemoveAt(i - 1);
curvepoints.Insert(i, newpoints[4]);
}
else
{
Curve curve = Line.CreateBound(curvepoints[i - 1], curvepoints[i]) as Curve;
curves.Add(curve);
}
}
}
else
{
Curve curve = Line.CreateBound(curvepoints[i - 1], curvepoints[i]) as Curve;
curves.Add(curve);
}
i++;
}
}
else if (curvepoints.Count == 2)
{
Curve curve = Line.CreateBound(curvepoints[0], curvepoints[1]) as Curve;
curves.Add(curve);
}
return curves;
}
public Autodesk.Revit.UI.Result Execute(ExternalCommandData revit, ref string message, ElementSet elements)
{
throw new NotImplementedException();
}
}
}