121 lines
2.8 KiB
C#
121 lines
2.8 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
|
|
[ExecuteInEditMode]
|
|
public class LineCross : MonoBehaviour {
|
|
public LineSegment L1;
|
|
public LineSegment L2;
|
|
// Use this for initialization
|
|
void Start () {
|
|
|
|
}
|
|
|
|
// Update is called once per frame
|
|
void Update () {
|
|
|
|
}
|
|
|
|
float SegmentToSegment( LineSegment S1, LineSegment S2)
|
|
{
|
|
float SMALL_NUM = 0.0001f;
|
|
Vector3 u = S1.P2 - S1.P1;
|
|
Vector3 v = S2.P2 - S2.P1;
|
|
Vector3 w = S1.P1 - S2.P1;
|
|
float a = Vector3.Dot(u,u); // always >= 0
|
|
float b = Vector3.Dot(u,v);
|
|
float c = Vector3.Dot(v,v); // always >= 0
|
|
float d = Vector3.Dot(u,w);
|
|
float e = Vector3.Dot(v,w);
|
|
float D = a*c - b*b; // always >= 0
|
|
float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
|
|
float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
|
|
|
|
|
|
|
|
// compute the line parameters of the two closest points
|
|
if (D < SMALL_NUM) { // the lines are almost parallel
|
|
sN = 0.0f; // force using point P0 on segment S1
|
|
sD = 1.0f; // to prevent possible division by 0.0 later
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
else { // get the closest points on the infinite lines
|
|
sN = (b*e - c*d);
|
|
tN = (a*e - b*d);
|
|
|
|
if (sN < 0.0f) { // sc < 0 => the s=0 edge is visible
|
|
sN = 0.0f;
|
|
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
|
|
sN = sD;
|
|
tN = e + b;
|
|
tD = c;
|
|
}
|
|
}
|
|
|
|
if (tN < 0.0f) { // tc < 0 => the t=0 edge is visible
|
|
tN = 0.0f;
|
|
// recompute sc for this edge
|
|
if (-d < 0.0f)
|
|
sN = 0.0f;
|
|
else if (-d > a)
|
|
sN = sD;
|
|
else {
|
|
sN = -d;
|
|
sD = a;
|
|
}
|
|
}
|
|
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
|
|
tN = tD;
|
|
// recompute sc for this edge
|
|
if ((-d + b) < 0.0f)
|
|
sN = 0;
|
|
else if ((-d + b) > a)
|
|
sN = sD;
|
|
else {
|
|
sN = (-d + b);
|
|
sD = a;
|
|
}
|
|
}
|
|
|
|
Debug.Log ("SN = " + sN);
|
|
Debug.Log ("SD = " + sD);
|
|
Debug.Log ("TN = " + tN);
|
|
Debug.Log ("TD = " + tD);
|
|
|
|
// finally do the division to get sc and tc
|
|
sc = (Mathf.Abs(sN) < SMALL_NUM ? 0.0f : sN / sD);
|
|
tc = (Mathf.Abs(tN) < SMALL_NUM ? 0.0f : tN / tD);
|
|
|
|
Vector3 t1 = S1.P1 + u * sc;
|
|
Vector3 t2 = S2.P1 + v * tc;
|
|
|
|
Gizmos.DrawWireSphere (t1, 0.02f);
|
|
Gizmos.DrawWireSphere (t2, 0.02f);
|
|
|
|
// get the difference of the two closest points
|
|
Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
|
|
|
|
return dP.magnitude; // return the closest distance
|
|
}
|
|
|
|
void OnDrawGizmos(){
|
|
float sphereSize = 0.1f;
|
|
|
|
Gizmos.DrawWireSphere (L1.P1, sphereSize);
|
|
Gizmos.DrawWireSphere (L1.P2, sphereSize);
|
|
|
|
Gizmos.DrawWireSphere (L2.P1, sphereSize);
|
|
Gizmos.DrawWireSphere (L2.P2, sphereSize);
|
|
|
|
|
|
|
|
Debug.DrawLine (L1.P1, L1.P2, Color.red);
|
|
Debug.DrawLine (L2.P1, L2.P2, Color.green);
|
|
Debug.Log (SegmentToSegment (L1, L2));
|
|
}
|
|
}
|