public static double dist3D_Line_to_Line(Line L1, Line L2)
{
XYZ u = L1.GetEndPoint(1) - L1.GetEndPoint(0);
XYZ v = L2.GetEndPoint(1) - L2.GetEndPoint(0);
XYZ w = L1.GetEndPoint(0) - L2.GetEndPoint(0);
double a = u.DotProduct( u); // always >= 0
double b = u.DotProduct(v);
double c = v.DotProduct( v); // always >= 0
double d = u.DotProduct( w);
double e = v.DotProduct( w);
double D = a * c - b * b; // always >= 0
double sc=0;
double tc=0;
double SMALL_NUM = 0.000001;
// compute the line parameters of the two closest points
if (D < SMALL_NUM)
{ // the lines are almost parallel
sc = 0.0;
tc = (b > c ? d / b : e / c); // use the largest denominator
}
else
{
sc = (b * e - c * d) / D;
tc = (a * e - b * d) / D;
}
// get the difference of the two closest points
XYZ dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc)
return dP.GetLength(); // return the closest distance
}
ไม่มีความคิดเห็น:
แสดงความคิดเห็น