//Interset segment sa, sb against cylinder specified by p, d and r
static bool IntersectSegmentCylinder(VInt3 sa, VInt3 sb, VInt3 p, VInt3 q, VFixedPoint r, ref VInt3 normal, ref VFixedPoint t)
{
if (r < Globals.EPS2)
{
return(false);
}
VInt3 d = q - p, m = sa - p, n = sb - sa;
VFixedPoint md = VInt3.Dot(m, d);
VFixedPoint nd = VInt3.Dot(n, d);
VFixedPoint dd = d.sqrMagnitude;
if (md < VFixedPoint.Zero && md + nd < VFixedPoint.Zero)
{
return(false);
}
if (md > dd && md + nd > dd)
{
return(false);
}
VFixedPoint nn = n.sqrMagnitude;
VFixedPoint mn = VInt3.Dot(m, n);
VFixedPoint a = dd * nn - nd * nd;
VFixedPoint k = m.sqrMagnitude - r * r;
VFixedPoint c = dd * k - md * md;
//if start point is within cylinder, not intersect
if (c < VFixedPoint.Zero)
{
return(false);
}
//if segment is parallel to cylinder, they do not intersect with each other
if (a.Abs() < Globals.EPS)
{
return(false);
}
VFixedPoint b = dd * mn - nd * md;
VFixedPoint discr = b * b - a * c;
if (discr < VFixedPoint.Zero)
{
return(false);
}
VFixedPoint discrSqrt = FMath.Sqrt(discr);
t = (-b - discrSqrt) / a;
if (t < VFixedPoint.Zero || t > VFixedPoint.One)
{
return(false);
}
//we don't need result of segment and endcap
/*if(md + t * nd < VFixedPoint.Zero)
* {
* if (nd <= VFixedPoint.Zero) return false;
* t = -md / nd;
* return k + t * (mn + t * nn) * 2 <= VFixedPoint.Zero;
* }
* else if(md + t * nd > dd)
* {
* if (nd >= VFixedPoint.Zero) return false;
* t = (dd - md) / nd;
* return k + dd - md * 2 + t * ((md - nd) * 2 + t * nn) <= VFixedPoint.Zero;
* }*/
VInt3 hitPoint = sa + n * t;
VFixedPoint param = VFixedPoint.Zero;
Distance.distancePointSegmentSquared(p, q, hitPoint, ref param);
if (param <= VFixedPoint.Zero || param >= VFixedPoint.One)
{
return(false);
}
VInt3 closestPoint = p * (VFixedPoint.One - param) + q * param;
normal = (hitPoint - closestPoint) / r;
return(true);
}
static bool sweepSphereVsTri(VInt3[] triVerts, VInt3 normal, VInt3 center, VFixedPoint radius, VInt3 dir, VFixedPoint length, ref VFixedPoint impactDistance, ref bool directHit, bool testInitialialOverlap)
{
directHit = false;
VInt3 edge10 = triVerts[1] - triVerts[0];
VInt3 edge20 = triVerts[2] - triVerts[0];
if (testInitialialOverlap)
{
VInt3 cp = Distance.closestPtPointTriangle2(center, triVerts[0], triVerts[1], triVerts[2]);
if ((cp - center).sqrMagnitude <= radius * radius)
{
impactDistance = VFixedPoint.Zero;
return(true);
}
}
VFixedPoint u = VFixedPoint.Zero, v = VFixedPoint.Zero;
{
VInt3 R = normal * radius;
if (VInt3.Dot(dir, R) >= VFixedPoint.Zero)
{
R *= -1;
}
// The first point of the sphere to hit the triangle plane is the point of the sphere nearest to
// the triangle plane. Hence, we use center - (normal*radius) below.
// PT: casting against the extruded triangle in direction R is the same as casting from a ray moved by -R
VFixedPoint t = VFixedPoint.Zero;
int r = rayTriSpecial(center - R, dir, triVerts[0], edge10, edge20, ref t, ref u, ref v);
if (r == 0)
{
return(false);
}
if (r == 2)
{
if (t < VFixedPoint.Zero)
{
return(false);
}
impactDistance = t;
directHit = true;
return(true);
}
}
//
// Let's do some art!
//
// The triangle gets divided into the following areas (based on the barycentric coordinates (u,v)):
//
// \ A0 /
// \ /
// \ /
// \/ 0
// A02 * A01
// u / / \ \ v
// * / \ *
// / \ .
// 2 / \ 1
// ------*--------------*-------
// / \ .
// A2 / A12 \ A1
//
//
// Based on the area where the computed triangle plane intersection point lies in, a different sweep test will be applied.
//
// A) A01, A02, A12 : Test sphere against the corresponding edge
// B) A0, A1, A2 : Test sphere against the corresponding vertex
//
// Unfortunately, B) does not work for long, thin triangles. Hence there is some extra code which does a conservative check and
// switches to edge tests if necessary.
//
bool TestSphere = false;
int e0 = 0, e1 = 0;
if (u < VFixedPoint.Zero)
{
if (v < VFixedPoint.Zero)
{
e0 = 0;
VInt3 intersectPoint = triVerts[1] * u + triVerts[2] * v + triVerts[0] * (VFixedPoint.One - u - v);
TestSphere = edgeOrVertexTest(intersectPoint, triVerts, 0, 1, 2, ref e1);
}
else if (u + v > VFixedPoint.One)
{
e0 = 2;
VInt3 intersectPoint = triVerts[1] * u + triVerts[2] * v + triVerts[0] * (VFixedPoint.One - u - v);
TestSphere = edgeOrVertexTest(intersectPoint, triVerts, 2, 0, 1, ref e1);
}
else
{
TestSphere = false;
e0 = 0; e1 = 2;
}
}
else
{
if (v < VFixedPoint.Zero)
{
if (u + v > VFixedPoint.One)
{
e0 = 1;
VInt3 intersectPoint = triVerts[1] * u + triVerts[2] * v + triVerts[0] * (VFixedPoint.One - u - v);
TestSphere = edgeOrVertexTest(intersectPoint, triVerts, 1, 0, 2, ref e1);
}
else
{
TestSphere = false;
e0 = 0;
e1 = 1;
}
}
else
{
TestSphere = false;
e0 = 1;
e1 = 2;
}
}
return(testRayVsSphereOrCapsule(ref impactDistance, TestSphere, center, radius, dir, length, triVerts, e0, e1));
}
