// Calculate the projection of a polygon on an axis and returns it as a [min, max] interval
public static void ProjectPolygon(Vector2 axis, Polygon polygon, ref float min, ref float max)
{
// To project a point on an axis use the dot product
var d = axis.DotProduct(polygon.Points[0]);
min = d;
max = d;
for (var i = 0; i < polygon.Points.Length; i++)
{
d = polygon.Points[i].DotProduct(axis);
if (d < min)
{
min = d;
}
else
{
if (d > max)
{
max = d;
}
}
}
}
public static bool RayLineIntersect(Vector2 ro, Vector2 rd, Vector2 l1, Vector2 l2, out float t)
{
Vector2 seg = l2 - l1;
Vector2 segPerp = new Vector2(seg.Y, -seg.X);
float perpDotd = rd.DotProduct(segPerp);
// If lines are parallel, return false.
if (Math.Abs(perpDotd) <= float.Epsilon)
{
t = float.MaxValue;
return(false);
}
Vector2 d = l1 - ro;
t = segPerp.DotProduct(d) / perpDotd;
float s = new Vector2(rd.Y, -rd.X).DotProduct(d) / perpDotd;
// If intersect is in right direction and in segment bounds, return true.
return(t >= 0.0f && s >= 0.0f && s <= 1.0f);
}
// Structure that stores the results of the PolygonCollision function
public static CollisionResult Collision(Polygon polygonA, Polygon polygonB, Vector2 velocity)
{
if (polygonA._Points.Length == 0 || polygonA._Points.Length == 0)
{
throw new ArgumentException("param polygonA or polygonB point count are zero.");
}
var result = new CollisionResult();
result.Intersect = true;
result.WillIntersect = true;
var edgeCountA = polygonA.Edges.Length;
var edgeCountB = polygonB.Edges.Length;
var minIntervalDistance = float.PositiveInfinity;
var translationAxis = new Vector2();
Vector2 edge;
// Loop through all the edges of both polygons
for (var edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
{
if (edgeIndex < edgeCountA)
{
edge = polygonA.Edges[edgeIndex];
}
else
{
edge = polygonB.Edges[edgeIndex - edgeCountA];
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
var axis = new Vector2(-edge.Y, edge.X);
axis.Normalize();
// Find the projection of the polygon on the current axis
float minA = 0;
float minB = 0;
float maxA = 0;
float maxB = 0;
Polygon.ProjectPolygon(axis, polygonA, ref minA, ref maxA);
Polygon.ProjectPolygon(axis, polygonB, ref minB, ref maxB);
// Check if the polygon projections are currentlty intersecting
if (Polygon.IntervalDistance(minA, maxA, minB, maxB) > 0)
{
result.Intersect = false;
}
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
var velocityProjection = axis.DotProduct(velocity);
// Get the projection of polygon A during the movement
if (velocityProjection < 0)
{
minA += velocityProjection;
}
else
{
maxA += velocityProjection;
}
// Do the same test as above for the new projection
var intervalDistance = Polygon.IntervalDistance(minA, maxA, minB, maxB);
if (intervalDistance > 0)
{
result.WillIntersect = false;
}
// If the polygons are not intersecting and won't intersect, exit the loop
if (!result.Intersect && !result.WillIntersect)
{
break;
}
// Check if the current interval distance is the minimum one. If so store
// the interval distance and the current distance.
// This will be used to calculate the minimum translation Vector2
intervalDistance = Math.Abs(intervalDistance);
if (intervalDistance < minIntervalDistance)
{
minIntervalDistance = intervalDistance;
translationAxis = axis;
var d = polygonA.Center - polygonB.Center;
if (d.DotProduct(translationAxis) < 0)
{
translationAxis = -translationAxis;
}
}
}
// The minimum translation Vector2 can be used to push the polygons appart.
// First moves the polygons by their velocity
// then move polygonA by MinimumTranslationVector2.
if (result.WillIntersect)
{
result.MinimumTranslationVector2 = translationAxis * minIntervalDistance;
}
return(result);
}
public static bool RayLineIntersect(Vector2 ro, Vector2 rd, Vector2 l1, Vector2 l2, out float t)
{
Vector2 seg = l2 - l1;
Vector2 segPerp = new Vector2(seg.Y, -seg.X);
float perpDotd = rd.DotProduct(segPerp);
// If lines are parallel, return false.
if (Math.Abs(perpDotd) <= float.Epsilon)
{
t = float.MaxValue;
return false;
}
Vector2 d = l1 - ro;
t = segPerp.DotProduct(d) / perpDotd;
float s = new Vector2(rd.Y, -rd.X).DotProduct(d) / perpDotd;
// If intersect is in right direction and in segment bounds, return true.
return t >= 0.0f && s >= 0.0f && s <= 1.0f;
}
// Structure that stores the results of the PolygonCollision function
public static CollisionResult Collision(Polygon polygonA, Polygon polygonB, Vector2 velocity)
{
if(polygonA._Points.Length== 0 || polygonA._Points.Length== 0)
{
throw new ArgumentException("param polygonA or polygonB point count are zero.");
}
var result = new CollisionResult();
result.Intersect = true;
result.WillIntersect = true;
var edgeCountA = polygonA.Edges.Length;
var edgeCountB = polygonB.Edges.Length;
var minIntervalDistance = float.PositiveInfinity;
var translationAxis = new Vector2();
Vector2 edge;
// Loop through all the edges of both polygons
for(var edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
{
if(edgeIndex < edgeCountA)
{
edge = polygonA.Edges[edgeIndex];
}
else
{
edge = polygonB.Edges[edgeIndex - edgeCountA];
}
// ===== 1. Find if the polygons are currently intersecting =====
// Find the axis perpendicular to the current edge
var axis = new Vector2(-edge.Y, edge.X);
axis.Normalize();
// Find the projection of the polygon on the current axis
float minA = 0;
float minB = 0;
float maxA = 0;
float maxB = 0;
Polygon.ProjectPolygon(axis, polygonA, ref minA, ref maxA);
Polygon.ProjectPolygon(axis, polygonB, ref minB, ref maxB);
// Check if the polygon projections are currentlty intersecting
if(Polygon.IntervalDistance(minA, maxA, minB, maxB) > 0)
{
result.Intersect = false;
}
// ===== 2. Now find if the polygons *will* intersect =====
// Project the velocity on the current axis
var velocityProjection = axis.DotProduct(velocity);
// Get the projection of polygon A during the movement
if(velocityProjection < 0)
{
minA += velocityProjection;
}
else
{
maxA += velocityProjection;
}
// Do the same test as above for the new projection
var intervalDistance = Polygon.IntervalDistance(minA, maxA, minB, maxB);
if(intervalDistance > 0)
{
result.WillIntersect = false;
}
// If the polygons are not intersecting and won't intersect, exit the loop
if(!result.Intersect && !result.WillIntersect)
{
break;
}
// Check if the current interval distance is the minimum one. If so store
// the interval distance and the current distance.
// This will be used to calculate the minimum translation Vector2
intervalDistance = Math.Abs(intervalDistance);
if(intervalDistance < minIntervalDistance)
{
minIntervalDistance = intervalDistance;
translationAxis = axis;
var d = polygonA.Center - polygonB.Center;
if(d.DotProduct(translationAxis) < 0)
{
translationAxis = -translationAxis;
}
}
}
// The minimum translation Vector2 can be used to push the polygons appart.
// First moves the polygons by their velocity
// then move polygonA by MinimumTranslationVector2.
if(result.WillIntersect)
{
result.MinimumTranslationVector2 = translationAxis * minIntervalDistance;
}
return result;
}
// Calculate the projection of a polygon on an axis and returns it as a [min, max] interval
public static void ProjectPolygon(Vector2 axis, Polygon polygon, ref float min, ref float max)
{
// To project a point on an axis use the dot product
var d = axis.DotProduct(polygon.Points[0]);
min = d;
max = d;
for(var i = 0; i < polygon.Points.Length; i++)
{
d = polygon.Points[i].DotProduct(axis);
if(d < min)
{
min = d;
}
else
{
if(d > max)
{
max = d;
}
}
}
}
