public static void GetDistanceSq(ref Vector2 vertex1, ref Vector2 vertex2, ref Vector2 point,
out float result)
{
float edgeLength;
Vector2 edge, local;
Vector2.Subtract(ref point, ref vertex2, out local);
Vector2.Subtract(ref vertex1, ref vertex2, out edge);
Vectors2.Normalize(ref edge, out edgeLength, out edge);
var nProj = local.Y * edge.X - local.X * edge.Y;
var tProj = local.X * edge.X + local.Y * edge.Y;
if (tProj < 0)
{
result = tProj * tProj + nProj * nProj;
}
else if (tProj > edgeLength)
{
tProj -= edgeLength;
result = tProj * tProj + nProj * nProj;
}
else
{
result = nProj * nProj;
}
}
public static bool Collide(BoundingCircle circle1, BoundingCircle circle2, out Vector2 contactPoint,
out float distance, out Vector2 normal)
{
Vector2.Subtract(ref circle2.Center, ref circle1.Center, out normal);
Vectors2.Normalize(ref normal, out distance, out normal);
distance = distance - (circle1.Radius + circle2.Radius);
bool collision = (distance <= 0);
if (collision)
{
contactPoint.X = circle2.Center.X - normal.X * circle2.Radius;
contactPoint.Y = circle2.Center.Y - normal.Y * circle2.Radius;
}
else
{
contactPoint = Vector2.Zero;
}
return(collision);
}
public static void Intersects(ref Vector2 vertex1, ref Vector2 vertex2, ref Ray2D ray, out float result,
float tolerance = 0.0001f)
{
Vector2 tangent, normal;
float edgeMagnitude;
Vector2.Subtract(ref vertex1, ref vertex2, out tangent);
Vectors2.Normalize(ref tangent, out edgeMagnitude, out tangent);
Vectors2.RightPerp(ref tangent, out normal);
float dir;
Vector2.Dot(ref normal, ref ray.Direction, out dir);
if (Math.Abs(dir) >= tolerance)
{
Vector2 originDiff;
Vector2.Subtract(ref ray.Origin, ref vertex2, out originDiff);
float actualDistance;
Vector2.Dot(ref normal, ref originDiff, out actualDistance);
var distanceFromOrigin = -(actualDistance / dir);
if (distanceFromOrigin >= 0)
{
Vector2 intersectPos;
Vector2.Multiply(ref ray.Direction, distanceFromOrigin, out intersectPos);
Vector2.Add(ref intersectPos, ref originDiff, out intersectPos);
float distanceFromSecond;
Vector2.Dot(ref intersectPos, ref tangent, out distanceFromSecond);
if (distanceFromSecond >= 0 && distanceFromSecond <= edgeMagnitude) // Hit!
{
result = distanceFromOrigin;
return;
}
}
}
// No hit.
result = -1;
}
