/// <summary>
/// Determines whether two 2D segments AB and CD are intersecting.
/// </summary>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="c">The endpoint C of segment CD.</param>
/// <param name="d">The endpoint D of segment CD.</param>
/// <param name="s">The normalized dot product between CD and AC on the XZ plane.</param>
/// <param name="t">The normalized dot product between AB and AC on the XZ plane.</param>
/// <returns>A value indicating whether the two segments are intersecting.</returns>
internal static bool SegmentSegment2D(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, out float s, out float t)
{
Vector3 u = b - a;
Vector3 v = d - c;
Vector3 w = a - c;
float magnitude;
Vector3Extensions.PerpDotXZ(ref u, ref v, out magnitude);
if (Math.Abs(magnitude) < 1e-6f)
{
//TODO is NaN the best value to set here?
s = float.NaN;
t = float.NaN;
return(false);
}
Vector3Extensions.PerpDotXZ(ref v, ref w, out s);
Vector3Extensions.PerpDotXZ(ref u, ref w, out t);
s /= magnitude;
t /= magnitude;
return(true);
}
/// <summary>
/// Finds the distance between a point and triangle ABC.
/// </summary>
/// <param name="p">A point.</param>
/// <param name="a">The first vertex of the triangle.</param>
/// <param name="b">The second vertex of the triangle.</param>
/// <param name="c">The third vertex of the triangle.</param>
/// <param name="height">The height between the point and the triangle.</param>
/// <returns>A value indicating whether the point is contained within the triangle.</returns>
internal static bool PointToTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, out float height)
{
Vector3 v0 = c - a;
Vector3 v1 = b - a;
Vector3 v2 = p - a;
float dot00, dot01, dot02, dot11, dot12;
Vector3Extensions.Dot2D(ref v0, ref v0, out dot00);
Vector3Extensions.Dot2D(ref v0, ref v1, out dot01);
Vector3Extensions.Dot2D(ref v0, ref v2, out dot02);
Vector3Extensions.Dot2D(ref v1, ref v1, out dot11);
Vector3Extensions.Dot2D(ref v1, ref v2, out dot12);
//compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
const float EPS = 1E-4f;
//if point lies inside triangle, return interpolated y-coordinate
if (u >= -EPS && v >= -EPS && (u + v) <= 1 + EPS)
{
height = a.Y + v0.Y * u + v1.Y * v;
return(true);
}
height = float.MaxValue;
return(false);
}
internal static void ProjectPoly(Vector3 axis, Vector3[] poly, int npoly, out float rmin, out float rmax)
{
Vector3Extensions.Dot2D(ref axis, ref poly[0], out rmin);
Vector3Extensions.Dot2D(ref axis, ref poly[0], out rmax);
for (int i = 1; i < npoly; i++)
{
float d;
Vector3Extensions.Dot2D(ref axis, ref poly[i], out d);
rmin = Math.Min(rmin, d);
rmax = Math.Max(rmax, d);
}
}
/// <summary>
/// Determines whether two 2D segments AB and CD are intersecting.
/// </summary>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="c">The endpoint C of segment CD.</param>
/// <param name="d">The endpoint D of segment CD.</param>
/// <returns>A value indicating whether the two segments are intersecting.</returns>
internal static bool SegmentSegment2D(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d)
{
float a1, a2, a3;
Vector3Extensions.Cross2D(ref a, ref b, ref d, out a1);
Vector3Extensions.Cross2D(ref a, ref b, ref c, out a2);
if (a1 * a2 < 0.0f)
{
Vector3Extensions.Cross2D(ref c, ref d, ref a, out a3);
float a4 = a3 + a2 - a1;
if (a3 * a4 < 0.0f)
{
return(true);
}
}
return(false);
}
/// <summary>
/// Determine whether a ray (origin, dir) is intersecting a segment AB.
/// </summary>
/// <param name="origin">The origin of the ray.</param>
/// <param name="dir">The direction of the ray.</param>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="t">The parameter t</param>
/// <returns>A value indicating whether the ray is intersecting with the segment.</returns>
public static bool RaySegment(Vector3 origin, Vector3 dir, Vector3 a, Vector3 b, out float t)
{
//default if not intersectng
t = 0;
Vector3 v = b - a;
Vector3 w = origin - a;
float d;
Vector3Extensions.PerpDotXZ(ref dir, ref v, out d);
d *= -1;
if (Math.Abs(d) < 1e-6f)
{
return(false);
}
d = 1.0f / d;
Vector3Extensions.PerpDotXZ(ref v, ref w, out t);
t *= -d;
if (t < 0 || t > 1)
{
return(false);
}
float s;
Vector3Extensions.PerpDotXZ(ref dir, ref w, out s);
s *= -d;
if (s < 0 || s > 1)
{
return(false);
}
return(true);
}
