// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2XForm& xfA,
const b2PolygonShape* polyB, const b2XForm& xfB)
{
manifold->pointCount = 0;
int32 edgeA = 0;
float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
return;
int32 edgeB = 0;
float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
return;
const b2PolygonShape* poly1; // reference poly
const b2PolygonShape* poly2; // incident poly
b2XForm xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_relativeTol = 0.98f;
const float32 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[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->GetVertexCount();
const b2Vec2* vertices1 = poly1->GetVertices();
b2Vec2 v11 = vertices1[edge1];
b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
b2Vec2 dv = v12 - v11;
b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
float32 frontOffset = b2Dot(frontNormal, v11);
float32 sideOffset1 = -b2Dot(sideNormal, v11);
float32 sideOffset2 = b2Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex clipPoints1[2];
ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->normal = flip ? -frontNormal : frontNormal;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
cp->separation = separation;
cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
cp->id.features.flip = flip;
++pointCount;
}
}
manifold->pointCount = pointCount;}
// The normal points from 1 to 2
void b2CollidePolygons(btManifoldResult* manifold,
const btBox2dShape* polyA, const btTransform& xfA,
const btBox2dShape* polyB, const btTransform& xfB)
{
int edgeA = 0;
btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
return;
int edgeB = 0;
btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
return;
const btBox2dShape* poly1; // reference poly
const btBox2dShape* poly2; // incident poly
btTransform xf1, xf2;
int edge1; // reference edge
unsigned char flip;
const btScalar k_relativeTol = 0.98f;
const btScalar 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[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1->getVertexCount();
const btVector3* vertices1 = poly1->getVertices();
btVector3 v11 = vertices1[edge1];
btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
//btVector3 dv = v12 - v11;
btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11);
sideNormal.normalize();
btVector3 frontNormal = btCrossS(sideNormal, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
btScalar frontOffset = b2Dot(frontNormal, v11);
btScalar sideOffset1 = -b2Dot(sideNormal, v11);
btScalar sideOffset2 = b2Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex clipPoints1[2];
clipPoints1[0].v.setValue(0,0,0);
clipPoints1[1].v.setValue(0,0,0);
ClipVertex clipPoints2[2];
clipPoints2[0].v.setValue(0,0,0);
clipPoints2[1].v.setValue(0,0,0);
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
btVector3 manifoldNormal = flip ? -frontNormal : frontNormal;
int pointCount = 0;
for (int i = 0; i < b2_maxManifoldPoints; ++i)
{
btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
//b2ManifoldPoint* cp = manifold->points + pointCount;
//btScalar separation = separation;
//cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
//cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation);
// cp->id = clipPoints2[i].id;
// cp->id.features.flip = flip;
++pointCount;
}
}
// manifold->pointCount = pointCount;}
}
// Collide an 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 b2EPCollider::Collide(b2Manifold* manifold)
{
manifold->pointCount = 0;
ComputeAdjacency();
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
// This can happen on the middle edge of a 3-edge zig-zag chain.
if (edgeAxis.type == b2EPAxis::e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxis::e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2EPProxy* proxy1;
b2EPProxy* proxy2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
proxy1 = &m_proxyA;
proxy2 = &m_proxyB;
manifold->type = b2Manifold::e_faceA;
}
else
{
proxy1 = &m_proxyB;
proxy2 = &m_proxyA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);
int32 count1 = proxy1->count;
const b2Vec2* vertices1 = proxy1->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) + m_radius;
float32 sideOffset2 = b2Dot(tangent, v12) + m_radius;
// 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(m_xf.R, normal);
manifold->localPoint = b2MulT(m_xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_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;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
