// 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; }
// Algorithm: // 1. Classify v1 and v2 // 2. Classify polygon centroid as front or back // 3. Flip normal if necessary // 4. Initialize normal range to [-pi, pi] about face normal // 5. Adjust normal range according to adjacent edges // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2PolygonShape* polygonB, const b2Transform& xfB) { m_xf = b2MulT(xfA, xfB); m_centroidB = b2Mul(m_xf, polygonB->m_centroid); m_v0 = edgeA->m_vertex0; m_v1 = edgeA->m_vertex1; m_v2 = edgeA->m_vertex2; m_v3 = edgeA->m_vertex3; bool hasVertex0 = edgeA->m_hasVertex0; bool hasVertex3 = edgeA->m_hasVertex3; b2Vec2 edge1 = m_v2 - m_v1; edge1.Normalize(); m_normal1.Set(edge1.y, -edge1.x); float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); float32 offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { b2Vec2 edge0 = m_v1 - m_v0; edge0.Normalize(); m_normal0.Set(edge0.y, -edge0.x); convex1 = b2Cross(edge0, edge1) >= 0.0f; offset0 = b2Dot(m_normal0, m_centroidB - m_v0); } // Is there a following edge? if (hasVertex3) { b2Vec2 edge2 = m_v3 - m_v2; edge2.Normalize(); m_normal2.Set(edge2.y, -edge2.x); convex2 = b2Cross(edge1, edge2) > 0.0f; offset2 = b2Dot(m_normal2, m_centroidB - m_v2); } // Determine front or back collision. Determine collision normal limits. if (hasVertex0 && hasVertex3) { if (convex1 && convex2) { m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } } else if (convex1) { m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal1; } } else if (convex2) { m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal0; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal0; } } } else if (hasVertex0) { if (convex1) { m_front = offset0 >= 0.0f || offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal0; } } } else if (hasVertex3) { if (convex2) { m_front = offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } } else { m_front = offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = m_normal1; } } } else { m_front = offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } } // Get polygonB in frameA m_polygonB.count = polygonB->m_vertexCount; for (int32 i = 0; i < polygonB->m_vertexCount; ++i) { m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); } m_radius = 2.0f * b2_polygonRadius; manifold->pointCount = 0; b2EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. 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; } b2ClipVertex ie[2]; b2ReferenceFace rf; if (primaryAxis.type == b2EPAxis::e_edgeA) { manifold->type = b2Manifold::e_faceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int32 bestIndex = 0; float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); for (int32 i = 1; i < m_polygonB.count; ++i) { float32 value = b2Dot(m_normal, m_polygonB.normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int32 i1 = bestIndex; int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; ie[0].v = m_polygonB.vertices[i1]; ie[0].id.cf.indexA = 0; ie[0].id.cf.indexB = i1; ie[0].id.cf.typeA = b2ContactFeature::e_face; ie[0].id.cf.typeB = b2ContactFeature::e_vertex; ie[1].v = m_polygonB.vertices[i2]; ie[1].id.cf.indexA = 0; ie[1].id.cf.indexB = i2; ie[1].id.cf.typeA = b2ContactFeature::e_face; ie[1].id.cf.typeB = b2ContactFeature::e_vertex; if (m_front) { rf.i1 = 0; rf.i2 = 1; rf.v1 = m_v1; rf.v2 = m_v2; rf.normal = m_normal1; } else { rf.i1 = 1; rf.i2 = 0; rf.v1 = m_v2; rf.v2 = m_v1; rf.normal = -m_normal1; } } else { manifold->type = b2Manifold::e_faceB; ie[0].v = m_v1; ie[0].id.cf.indexA = 0; ie[0].id.cf.indexB = primaryAxis.index; ie[0].id.cf.typeA = b2ContactFeature::e_vertex; ie[0].id.cf.typeB = b2ContactFeature::e_face; ie[1].v = m_v2; ie[1].id.cf.indexA = 0; ie[1].id.cf.indexB = primaryAxis.index; ie[1].id.cf.typeA = b2ContactFeature::e_vertex; ie[1].id.cf.typeB = b2ContactFeature::e_face; rf.i1 = primaryAxis.index; rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; rf.v1 = m_polygonB.vertices[rf.i1]; rf.v2 = m_polygonB.vertices[rf.i2]; rf.normal = m_polygonB.normals[rf.i1]; } rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); rf.sideNormal2 = -rf.sideNormal1; rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); // Clip incident edge against extruded edge1 side edges. b2ClipVertex clipPoints1[2]; b2ClipVertex clipPoints2[2]; int32 np; // Clip to box side 1 np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); if (np < b2_maxManifoldPoints) { return; } // Clip to negative box side 1 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); if (np < b2_maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == b2EPAxis::e_edgeA) { manifold->localNormal = rf.normal; manifold->localPoint = rf.v1; } else { manifold->localNormal = polygonB->m_normals[rf.i1]; manifold->localPoint = polygonB->m_vertices[rf.i1]; } int32 pointCount = 0; for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { float32 separation; separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); 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; }
// 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; }
// 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; }