public static void CollideCircles(ref Manifold manifold, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2)
{
manifold.PointCount = 0;
Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition());
Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition());
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float r1 = circle1.GetRadius();
float r2 = circle2.GetRadius();
float radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float separation;
if (distSqr < Settings.FLT_EPSILON)
{
separation = -radiusSum;
manifold.Normal = new Vector2(0.0f, 1.0f);
}
else
{
float dist = CommonMath.Sqrt(distSqr);
separation = dist - radiusSum;
float a = 1.0f / dist;
manifold.Normal.X = a * d.X;
manifold.Normal.Y = a * d.Y;
}
manifold.PointCount = 1;
manifold.Points[0].ID.Key = 0;
manifold.Points[0].Separation = separation;
p1 += r1 * manifold.Normal;
p2 -= r2 * manifold.Normal;
Vector2 p = 0.5f * (p1 + p2);
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, p);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, p);
}
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vector2[] vertices2 = poly2.GetVertices();
Vector2[] normals2 = poly2.Normals;
//Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = CommonMath.MulT(xf2.R, CommonMath.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new ClipVertex[2];
c[0].V = CommonMath.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = CommonMath.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="edge1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] vertices1 = poly1.GetVertices();
Vector2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vector2[] vertices2 = poly2.GetVertices();
//Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = CommonMath.Mul(xf1.R, normals1[edge1]);
Vector2 normal1 = CommonMath.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = CommonMath.Mul(xf1, vertices1[edge1]);
Vector2 v2 = CommonMath.Mul(xf2, vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return(separation);
}
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = CommonMath.Mul(xf2, circle.GetLocalPosition());
Vector2 cLocal = CommonMath.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = circle.GetRadius();
int vertexCount = polygon.VertexCount;
Vector2[] vertices = polygon.GetVertices();
Vector2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, normals[normalIndex]);
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
Vector2 position = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position);
manifold.Points[0].Separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vector2 e = vertices[vertIndex2] - vertices[vertIndex1];
float length = CommonMath.Normalize(ref e);
//Box2DXDebug.Assert(length > Settings.FLT_EPSILON);
// Project the center onto the edge.
float u = Vector2.Dot(cLocal - vertices[vertIndex1], e);
Vector2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold.Points[0].ID.Features.IncidentEdge = NullFeature;
manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex;
manifold.Points[0].ID.Features.IncidentVertex = NullFeature;
}
Vector2 d = cLocal - p;
float dist = CommonMath.Normalize(ref d);
if (dist > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Normal = CommonMath.Mul(xf1.R, d);
Vector2 position_ = c - radius * manifold.Normal;
manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position_);
manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position_);
manifold.Points[0].Separation = dist - radius;
manifold.Points[0].ID.Features.ReferenceEdge = 0;
manifold.Points[0].ID.Features.Flip = 0;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
{
manifold.PointCount = 0;
int edgeA = 0;
float separationA = FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
{
return;
}
int edgeB = 0;
float separationB = FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
XForm xf1, xf2;
int edge1; // reference edge
byte flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vector2[] vertices1 = poly1.GetVertices();
Vector2 v11 = vertices1[edge1];
Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vector2 dv = v12 - v11;
Vector2 sideNormal = CommonMath.Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = CommonMath.Cross(sideNormal, 1.0f);
v11 = CommonMath.Mul(xf1, v11);
v12 = CommonMath.Mul(xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.Normal = flip != 0 ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= 0.0f)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.Separation = separation;
cp.LocalPoint1 = CommonMath.MulT(xfA, clipPoints2[i].V);
cp.LocalPoint2 = CommonMath.MulT(xfB, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
/// <param name="edgeIndex"></param>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = CommonMath.Mul(xf2, poly2.GetCentroid()) - CommonMath.Mul(xf1, poly1.GetCentroid());
Vector2 dLocal1 = CommonMath.MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return(s);
}
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return(sPrev);
}
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return(sNext);
}
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return(s);
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
