private static Vector2 Project(Polygon polygon, Vector2 axis)
{
var vertices = polygon.Vertices;
double min = Vector2.Dot(axis, vertices[0]);
double max = min;
for (int i = 1; i < vertices.Length; i++)
{
// NOTE: the axis must be normalized to get accurate projections
double p = Vector2.Dot(axis, vertices[i]);
if (p < min)
{
min = p;
}
else if (p > max)
{
max = p;
}
}
var proj = new Vector2((float)min, (float)max);
return proj;
}
/// <summary>
/// http://www.codezealot.org/archives/55
/// </summary>
/// <param name="p1">The polygon that should be moved if there is a collision</param>
/// <param name="p2"></param>
/// <returns></returns>
public static Vector2 PolygonCollision(Polygon p1, Polygon p2)
{
float overlap = 999999999; // really large value;
Vector2 smallest = new Vector2(0, 0);
var axes1 = GetAxes(p1);
var axes2 = GetAxes(p2);
var distinctAxes = axes1.Concat(axes2).Distinct();
var axes = distinctAxes.ToArray();
// loop over the axes of the first polygon
for (int i = 0; i < axes.ToArray().Length; i++)
{
Vector2 axis = axes[i];
// project both shapes onto the axis
Vector2 projection1 = Project(p1, axis);
Vector2 projection2 = Project(p2, axis);
// do the projections overlap?
if (!DoOverlap(projection1, projection2))
{
// then we can guarantee that the shapes do not overlap
return Vector2.Zero;
}
else
{
// get the overlap
float o = GetOverlap(projection1, projection2);
// check for minimum
if (o < overlap)
{
// then set this one as the smallest
overlap = o;
smallest = axis;
}
}
}
//// loop over the axes of the second polygon
//for (int i = 0; i < axes2.Length; i++)
//{
// var axis = axes2[i];
// // project both shapes onto the axis
// Vector2 projection1 = Project(p1, axis);
// Vector2 projection2 = Project(p2, axis);
// // do the projections overlap?
// if (!DoOverlap(projection1, projection2))
// {
// // then we can guarantee that the shapes do not overlap
// return Vector2.Zero;
// }
// else
// {
// // get the overlap
// float o = GetOverlap(projection1, projection2);
// // check for minimum
// if (o < overlap)
// {
// // then set this one as the smallest
// overlap = o;
// smallest = axis;
// }
// }
//}
Vector2 mtv = smallest * overlap;
// if we get here then we know that every axis had overlap on it
// so we can guarantee an intersection
// If the dot between p1 and p2 is negative we have to switch
if (Vector2.Dot(mtv, p1.Center - p2.Center) < 0)
mtv *= -1;
return mtv;
}
private static Vector2[] GetAxes(Polygon polygon)
{
var vertices = polygon.Vertices;
var axes = new List<Vector2>();
HashSet<Vector2> normals = new HashSet<Vector2>();
// loop over the vertices
for (int i = 0; i < vertices.Length; i++)
{
// get the current vertex
var p1 = vertices[i];
// get the next vertex
var p2 = vertices[i + 1 == vertices.Length ? 0 : i + 1];
// subtract the two to get the edge vector
var edge = p1 - p2;
// get the normal
var normal = new Vector2(-edge.Y, edge.X);
// the perp method is just (x, y) => (-y, x) for clockwise-wound polygons or (y, -x) for ccw
normal.Normalize();
// If we already have this normal, or one pointing in the opposite direction then
// we don't need to add this axis
if (normals.Contains(normal) || normals.Contains(normal * -1))
{
continue;
}
else
{
normals.Add(normal);
}
axes.Add(normal);
}
return axes.ToArray();
}