private float SegmentToSegmentClosestPoint(geoLine Line1, geoLine Line2, ref geoPoint ClosestPt1, ref geoPoint ClosestPt2)
{
geoPoint pt1 = null;
geoPoint pt2 = null;
geoPoint pt3 = null;
geoPoint pt4 = null;
pt1 = Line1.P1;
pt2 = Line1.P2;
pt3 = Line2.P1;
pt4 = Line2.P2;
// using arrays for saving differencess
geoPoint u = new geoPoint(pt2.X - pt1.X, pt2.Y - pt1.Y, pt2.Z - pt1.Z);
geoPoint v = new geoPoint(pt4.X - pt3.X, pt4.Y - pt3.Y, pt4.Z - pt3.Z);
geoPoint w = new geoPoint(pt1.X - pt3.X, pt1.Y - pt3.Y, pt1.Z - pt3.Z);
float a = u.X * u.X + u.Y * u.Y + u.Z * u.Z;
//U.U>=0
float b = u.X * v.X + u.Y * v.Y + u.Z * v.Z;
//U.V
float c = v.X * v.X + v.Y * v.Y + v.Z * v.Z;
//V.V>=0
float d = u.X * w.X + u.Y * w.Y + u.Z * w.Z;
//U.W
float e = v.X * w.X + v.Y * w.Y + v.Z * w.Z;
//V.W
float D_ = a * c - b * b;
// >=0
float sN = D_;
float sD = D_;
float tN = D_;
float tD = D_;
float sc = D_;
float tc = D_;
if (D_ < 1E-13)
{
//PARALLEL
sN = 0;
sD = 1;
tN = e;
tD = c;
}
else
{
//Inifinite
sN = (b * e) - (c * d);
tN = (a * e) - (b * d);
if (sN < 0)
{
sN = 0;
tN = e;
tD = c;
}
else if (sN > sD)
{
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0)
{
tN = 0;
if (-d < 0)
{
sN = 0;
}
else if (-d > a)
{
sN = sD;
}
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{
tN = tD;
if ((-d + b) < 0)
{
sN = 0;
}
else if ((-d + b) > a)
{
sN = sD;
}
else
{
sN = (-d + b);
sD = a;
}
}
sc = sN / sD;
tc = tN / tD;
geoPoint dp = new geoPoint(w.X + sc * u.X - v.X * tc, w.Y + sc * u.Y - v.Y * tc, w.Z + sc * u.Z - v.Z * tc);
//Dim dis As float = Math.Sqrt(dp.X * dp.X + dp.Y * dp.Y + dp.Z * dp.Z)
ClosestPt1 = new geoPoint(pt1.X + sc * u.X, pt1.Y + sc * u.Y, pt1.Z + sc * u.Z);
ClosestPt2 = new geoPoint(pt3.X + tc * v.X, pt3.Y + tc * v.Y, pt3.Z + tc * v.Z);
return(ClosestPt1.distance(ref ClosestPt2));
}
public float distance(GameObject obj1, GameObject obj2)
{
Mesh mesh1 = obj1.GetComponent <MeshFilter>().mesh;
Mesh mesh2 = obj2.GetComponent <MeshFilter>().mesh;
Vector3[] vertices1 = mesh1.vertices;
Vector3[] vertices2 = mesh2.vertices;
Vector3 pos1 = obj1.transform.position;
Vector3 scl1 = obj1.transform.localScale;
Vector3 pos2 = obj2.transform.position;
Vector3 scl2 = obj2.transform.localScale;
for (int i = 0; i < vertices1.Length; i++)
{
Quaternion rotation = Quaternion.Euler(obj1.transform.rotation.eulerAngles);
vertices1[i] = rotation * vertices1[i];
vertices1 [i].x = vertices1 [i].x * scl1.x;
vertices1 [i].y = vertices1 [i].y * scl1.y;
vertices1 [i].z = vertices1 [i].z * scl1.z;
vertices1[i] += pos1;
}
for (int j = 0; j < vertices2.Length; j++)
{
Quaternion rotation = Quaternion.Euler(obj2.transform.rotation.eulerAngles);
vertices2[j] = rotation * vertices2[j];
vertices2 [j].x = vertices2 [j].x * scl2.x;
vertices2 [j].y = vertices2 [j].y * scl2.y;
vertices2 [j].z = vertices2 [j].z * scl2.z;
vertices2[j] += pos2;
}
float MinClearance;
float distance;
geoPoint Obj1ClosestPoint = new geoPoint();
geoPoint Obj2ClosestPoint = new geoPoint();
geoPoint[] Obj1Points = new geoPoint[4];
geoPoint[] Obj2Points = new geoPoint[4];
geoLine[] Obj1Lines = new geoLine[4];
geoLine[] Obj2Lines = new geoLine[4];
MinClearance = 1000;
for (int i = 0; i < vertices1.Length; i += 4)
{
for (int j = 0; j < vertices2.Length; j += 4)
{
Obj1Points[0] = new geoPoint(vertices1[i].x, vertices1[i].y, vertices1[i].z);
Obj1Points[1] = new geoPoint(vertices1[i + 1].x, vertices1[i + 1].y, vertices1[i + 1].z);
Obj1Points[2] = new geoPoint(vertices1[i + 2].x, vertices1[i + 2].y, vertices1[i + 2].z);
Obj1Points[3] = new geoPoint(vertices1[i + 3].x, vertices1[i + 3].y, vertices1[i + 3].z);
Obj2Points[0] = new geoPoint(vertices2[j].x, vertices2[j].y, vertices2[j].z);
Obj2Points[1] = new geoPoint(vertices2[j + 1].x, vertices2[j + 1].y, vertices2[j + 1].z);
Obj2Points[2] = new geoPoint(vertices2[j + 2].x, vertices2[j + 2].y, vertices2[j + 2].z);
Obj2Points[3] = new geoPoint(vertices2[j + 3].x, vertices2[j + 3].y, vertices2[j + 3].z);
Obj1Lines[0] = new geoLine(Obj1Points[0], Obj1Points[1]);
Obj1Lines[1] = new geoLine(Obj1Points[1], Obj1Points[2]);
Obj1Lines[2] = new geoLine(Obj1Points[2], Obj1Points[3]);
Obj1Lines[3] = new geoLine(Obj1Points[3], Obj1Points[0]);
Obj2Lines[0] = new geoLine(Obj2Points[0], Obj2Points[1]);
Obj2Lines[1] = new geoLine(Obj2Points[1], Obj2Points[2]);
Obj2Lines[2] = new geoLine(Obj2Points[2], Obj2Points[3]);
Obj2Lines[3] = new geoLine(Obj2Points[3], Obj2Points[0]);
distance = List_PointPolygonClosestDistances(Obj1Points, Obj2Points, ref Obj2ClosestPoint, ref Obj1ClosestPoint);
if (distance < MinClearance)
{
MinClearance = distance;
}
distance = List_SegmentSegmentClosestDistance(Obj1Lines, Obj2Lines, ref Obj1ClosestPoint, ref Obj2ClosestPoint);
if (distance < MinClearance)
{
MinClearance = distance;
}
distance = List_PointPolygonClosestDistances(Obj2Points, Obj1Points, ref Obj2ClosestPoint, ref Obj1ClosestPoint);
if (distance < MinClearance)
{
MinClearance = distance;
}
}
}
return(MinClearance);
}
