// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2Transform& xfA,
const b2PolygonShape* polyB, const b2Transform& xfB)
{
manifold->m_pointCount = 0;
float32 totalRadius = polyA->m_radius + polyB->m_radius;
int32 edgeA = 0;
float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int32 edgeB = 0;
float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
const b2PolygonShape* poly1; // reference polygon
const b2PolygonShape* poly2; // incident polygon
b2Transform xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold->m_type = b2Manifold::e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold->m_type = b2Manifold::e_faceA;
flip = 0;
}
b2ClipVertex incidentEdge[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
b2Vec2 v11 = vertices1[edge1];
b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Mul(xf1.R, localTangent);
b2Vec2 normal = b2Cross(tangent, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->m_localPlaneNormal = localNormal;
manifold->m_localPoint = planePoint;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->m_points + pointCount;
cp->m_localPoint = b2MulT(xf2, clipPoints2[i].v);
cp->m_id = clipPoints2[i].id;
cp->m_id.features.flip = flip;
++pointCount;
}
}
manifold->m_pointCount = pointCount;
}
// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
// is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB_in, const b2Transform& xfB)
{
manifold->pointCount = 0;
b2Transform xf = b2MulT(xfA, xfB);
// Create a polygon for edge shape A
b2PolygonShape polygonA;
polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2);
// Build polygonB in frame A
b2PolygonShape polygonB;
polygonB.m_radius = polygonB_in->m_radius;
polygonB.m_vertexCount = polygonB_in->m_vertexCount;
polygonB.m_centroid = b2Mul(xf, polygonB_in->m_centroid);
for (int32 i = 0; i < polygonB.m_vertexCount; ++i)
{
polygonB.m_vertices[i] = b2Mul(xf, polygonB_in->m_vertices[i]);
polygonB.m_normals[i] = b2Mul(xf.R, polygonB_in->m_normals[i]);
}
float32 totalRadius = polygonA.m_radius + polygonB.m_radius;
// Edge geometry
b2Vec2 v1 = edgeA->m_vertex1;
b2Vec2 v2 = edgeA->m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 edgeNormal(e.y, -e.x);
edgeNormal.Normalize();
// Determine side
bool isFrontSide = b2Dot(edgeNormal, polygonB.m_centroid - v1) >= 0.0f;
if (isFrontSide == false)
{
edgeNormal = -edgeNormal;
}
// Compute primary separating axis
b2EPAxis edgeAxis = b2EPEdgeSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (edgeAxis.separation > totalRadius)
{
// Shapes are separated
return;
}
// Classify adjacent edges
b2EdgeType types[2] = {b2_isolated, b2_isolated};
if (edgeA->m_hasVertex0)
{
b2Vec2 v0 = edgeA->m_vertex0;
float32 s = b2Dot(edgeNormal, v0 - v1);
if (s > 0.1f * b2_linearSlop)
{
types[0] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[0] = b2_flat;
}
else
{
types[0] = b2_convex;
}
}
if (edgeA->m_hasVertex3)
{
b2Vec2 v3 = edgeA->m_vertex3;
float32 s = b2Dot(edgeNormal, v3 - v2);
if (s > 0.1f * b2_linearSlop)
{
types[1] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[1] = b2_flat;
}
else
{
types[1] = b2_convex;
}
}
if (types[0] == b2_convex)
{
// Check separation on previous edge.
b2Vec2 v0 = edgeA->m_vertex0;
b2Vec2 e0 = v1 - v0;
b2Vec2 n0(e0.y, -e0.x);
n0.Normalize();
if (isFrontSide == false)
{
n0 = -n0;
}
b2EPAxis axis1 = b2EPEdgeSeparation(v0, v1, n0, &polygonB, totalRadius);
if (axis1.separation > edgeAxis.separation)
{
// The polygon should collide with previous edge
return;
}
}
if (types[1] == b2_convex)
{
// Check separation on next edge.
b2Vec2 v3 = edgeA->m_vertex3;
b2Vec2 e2 = v3 - v2;
b2Vec2 n2(e2.y, -e2.x);
n2.Normalize();
if (isFrontSide == false)
{
n2 = -n2;
}
b2EPAxis axis2 = b2EPEdgeSeparation(v2, v3, n2, &polygonB, totalRadius);
if (axis2.separation > edgeAxis.separation)
{
// The polygon should collide with the next edge
return;
}
}
b2EPAxis polygonAxis = b2EPPolygonSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (polygonAxis.separation > totalRadius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2PolygonShape* poly1;
b2PolygonShape* poly2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
poly1 = &polygonA;
poly2 = &polygonB;
if (isFrontSide == false)
{
primaryAxis.index = 1;
}
manifold->type = b2Manifold::e_faceA;
}
else
{
poly1 = &polygonB;
poly2 = &polygonA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
b2FindIncidentEdge(incidentEdge, poly1, primaryAxis.index, poly2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 tangent = v12 - v11;
tangent.Normalize();
b2Vec2 normal = b2Cross(tangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = normal;
manifold->localPoint = planePoint;
}
else
{
manifold->localNormal = b2MulT(xf.R, normal);
manifold->localPoint = b2MulT(xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
if (cp->id.cf.typeA == b2ContactFeature::e_vertex && types[cp->id.cf.indexA] == b2_flat)
{
continue;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
