// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
private static bool onSegment(LogicVector3 p, LogicVector3 q, LogicVector3 r)
{
if (q.x <= MathUtils.Max(p.x, r.x) && q.x >= MathUtils.Min(p.x, r.x) &&
q.z <= MathUtils.Max(p.z, r.z) && q.z >= MathUtils.Min(p.z, r.z))
{
return(true);
}
return(false);
}
// To find orientation of ordered triplet (p, q, r).
// The function returns following values
// 0 --> p, q and r are colinear
// 1 --> Clockwise
// 2 --> Counterclockwise
private static int orientation(LogicVector3 p, LogicVector3 q, LogicVector3 r)
{
// See https://www.geeksforgeeks.org/orientation-3-ordered-points/
// for details of below formula.
//long val = (long)(q.z - p.z) * (long)(r.x - q.x) - (long)(q.x - p.x) * (long)(r.z - q.z);
long val = (long)(q.z - p.z) * (long)(r.x - q.x) - (long)(q.x - p.x) * (long)(r.z - q.z);
if (val == 0)
{
return(0); // colinear
}
return((val > 0) ? 1 : 2);
}
static void Main(string[] args)
{
LogicVector3 p1 = new LogicVector3(23630, 2000, -21215);
LogicVector3 q1 = new LogicVector3(37702, 2000, -21115);
LogicVector3 p2 = new LogicVector3(-251414, 2000, -27466);
LogicVector3 q2 = new LogicVector3(-181414, 2000, 93774);
//LogicVector3 p1 = new LogicVector3(-124705, 2000, -122537);
//LogicVector3 q1 = new LogicVector3(-117868, 2000, -130577);
//LogicVector3 p2 = new LogicVector3(-340000, 2000, -20000);
//LogicVector3 q2 = new LogicVector3(-270000, 2000, 101240);
bool b = doIntersect(p1, q1, p2, q2);
//Console.ReadLine();
}
private void OnDrawGizmos()
{
LogicVector3 p = start + new LogicVector3(moveX, moveY, moveZ);
LogicVector3 q = end + new LogicVector3(moveX, moveY, moveZ);
Gizmos.color = Color.green;
Gizmos.DrawLine(new Vector3(p.x, p.y, p.z), new Vector3(q.x, q.y, q.z));
Gizmos.color = Color.red;
Gizmos.DrawSphere(new Vector3(center.x, center.y, center.z), radius);
bool b = IntersectSegCircle(center, radius, p, q);
Debug.LogError("================== " + b);
}
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
private static bool doIntersect(LogicVector3 p1, LogicVector3 q1, LogicVector3 p2, LogicVector3 q2)
{
// Find the four orientations needed for general and
// special cases
int o1 = orientation(p1, q1, p2);
int o2 = orientation(p1, q1, q2);
int o3 = orientation(p2, q2, p1);
int o4 = orientation(p2, q2, q1);
// General case
if (o1 != o2 && o3 != o4)
{
return(true);
}
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(p1, p2, q1))
{
return(true);
}
// p1, q1 and q2 are colinear and q2 lies on segment p1q1
if (o2 == 0 && onSegment(p1, q2, q1))
{
return(true);
}
// p2, q2 and p1 are colinear and p1 lies on segment p2q2
if (o3 == 0 && onSegment(p2, p1, q2))
{
return(true);
}
// p2, q2 and q1 are colinear and q1 lies on segment p2q2
if (o4 == 0 && onSegment(p2, q1, q2))
{
return(true);
}
return(false); // Doesn't fall in any of the above cases
}
private bool IntersectSegCircle(LogicVector3 center, int radius, LogicVector3 p, LogicVector3 q)
{
//一元二次方程 a(x^2) + bx + c = 0
center.y = 0;
p.y = 0;
q.y = 0;
LogicVector3 d = q - p;
LogicVector3 f = p - center;
int a = LogicVector3.DotD4(d, d);//必然 > 0,有最小值
int b = 2 * LogicVector3.DotD4(f, d);
int c = LogicVector3.DotD4(f, f) - (int)(((long)radius * (long)radius) / MathUtils.iPointUnit);
long discriminant = (long)b * (long)b - 4 * (long)a * (long)c;
//方程有解
if (discriminant >= 0)
{
int value_0 = c;
int value_1 = a + b + c;
//在[0,1]上存在值
if (value_0 > 0 && value_1 < 0)
{
return(true);
}
if (value_0 < 0 && value_1 > 0)
{
return(true);
}
if (value_0 == 0 || value_1 == 0)
{
return(true);
}
//最小值时,0 <= -b/2a <= 1, 且 a > 0 (必然)
bool bMinIn01 = false;
if (-b <= 2 * a && -b >= 0)
{
bMinIn01 = true;
}
if (bMinIn01)
{
//最小值为value = (4ac-b*b)/(4a), 因 discriminant = b*b - 4ac 且 discriminant >=0
//如discriminant > 0 ,4ac-b*b <= 0, 符号由-4a决定
if (discriminant == 0)
{
return(true);
}
else
{
//value_sign_min = -4 * a; 必然 < 0
if (value_0 > 0 && value_1 > 0) //value_0和value_1 必然同时 > 或 < 0
{
return(true);
}
}
}
}
return(false);
}