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); Vector2Helper.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); Vector2Helper.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); Vector2Helper.Normalize(ref tangent, out edgeMagnitude, out tangent); Vector2Helper.RightPerp(ref tangent, out normal); float dir; Vector2.Dot(ref normal, ref ray.Direction, out dir); if (MathF.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; }