// 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;}
}
// 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;}
