// 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;} }
// The normal points from A to B int Collide(Contact* contacts, Body* bodyA, Body* bodyB) { // Setup Vec2 hA = 0.5f * bodyA->width; Vec2 hB = 0.5f * bodyB->width; Vec2 posA = bodyA->position; Vec2 posB = bodyB->position; Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation); Mat22 RotAT = RotA.Transpose(); Mat22 RotBT = RotB.Transpose(); Vec2 a1 = RotA.col1, a2 = RotA.col2; Vec2 b1 = RotB.col1, b2 = RotB.col2; Vec2 dp = posB - posA; Vec2 dA = RotAT * dp; Vec2 dB = RotBT * dp; Mat22 C = RotAT * RotB; Mat22 absC = Abs(C); Mat22 absCT = absC.Transpose(); // Box A faces Vec2 faceA = Abs(dA) - hA - absC * hB; if (faceA.x > 0.0f || faceA.y > 0.0f) return 0; // Box B faces Vec2 faceB = Abs(dB) - absCT * hA - hB; if (faceB.x > 0.0f || faceB.y > 0.0f) return 0; // Find best axis Axis axis; float separation; Vec2 normal; // Box A faces axis = FACE_A_X; separation = faceA.x; normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1; const float relativeTol = 0.95f; const float absoluteTol = 0.01f; if (faceA.y > relativeTol * separation + absoluteTol * hA.y) { axis = FACE_A_Y; separation = faceA.y; normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2; } // Box B faces if (faceB.x > relativeTol * separation + absoluteTol * hB.x) { axis = FACE_B_X; separation = faceB.x; normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1; } if (faceB.y > relativeTol * separation + absoluteTol * hB.y) { axis = FACE_B_Y; separation = faceB.y; normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2; } // Setup clipping plane data based on the separating axis Vec2 frontNormal, sideNormal; ClipVertex incidentEdge[2]; float front, negSide, posSide; char negEdge, posEdge; // Compute the clipping lines and the line segment to be clipped. switch (axis) { case FACE_A_X: { frontNormal = normal; front = Dot(posA, frontNormal) + hA.x; sideNormal = RotA.col2; float side = Dot(posA, sideNormal); negSide = -side + hA.y; posSide = side + hA.y; negEdge = EDGE3; posEdge = EDGE1; ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); } break; case FACE_A_Y: { frontNormal = normal; front = Dot(posA, frontNormal) + hA.y; sideNormal = RotA.col1; float side = Dot(posA, sideNormal); negSide = -side + hA.x; posSide = side + hA.x; negEdge = EDGE2; posEdge = EDGE4; ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); } break; case FACE_B_X: { frontNormal = -normal; front = Dot(posB, frontNormal) + hB.x; sideNormal = RotB.col2; float side = Dot(posB, sideNormal); negSide = -side + hB.y; posSide = side + hB.y; negEdge = EDGE3; posEdge = EDGE1; ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); } break; case FACE_B_Y: { frontNormal = -normal; front = Dot(posB, frontNormal) + hB.y; sideNormal = RotB.col1; float side = Dot(posB, sideNormal); negSide = -side + hB.x; posSide = side + hB.x; negEdge = EDGE2; posEdge = EDGE4; ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); } break; } // clip other face with 5 box planes (1 face plane, 4 edge planes) ClipVertex clipPoints1[2]; ClipVertex clipPoints2[2]; int np; // Clip to box side 1 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge); if (np < 2) return 0; // Clip to negative box side 1 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge); if (np < 2) return 0; // Now clipPoints2 contains the clipping points. // Due to roundoff, it is possible that clipping removes all points. int numContacts = 0; for (int i = 0; i < 2; ++i) { float separation = Dot(frontNormal, clipPoints2[i].v) - front; if (separation <= 0) { contacts[numContacts].separation = separation; contacts[numContacts].normal = normal; // slide contact point onto reference face (easy to cull) contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal; contacts[numContacts].feature = clipPoints2[i].fp; if (axis == FACE_B_X || axis == FACE_B_Y) Flip(contacts[numContacts].feature); ++numContacts; } } return numContacts; }