/* * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. * Permission is granted to anyone to use this software for any purpose, * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, an acknowledgment in the product documentation would be * appreciated but is not required. * 2. Altered source versions must be plainly marked as such, and must not be * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ #include <Box2D/Collision/b2Collision.h> #include <Box2D/Collision/Shapes/b2CircleShape.h> #include <Box2D/Collision/Shapes/b2EdgeShape.h> #include <Box2D/Collision/Shapes/b2PolygonShape.h> // Compute contact points for edge versus circle. // This accounts for edge connectivity. void b2CollideEdgeAndCircle(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2CircleShape* circleB, const b2Transform& xfB) { manifold->pointCount = 0; // Compute circle in frame of edge b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; b2Vec2 e = B - A; // Barycentric coordinates float32 u = b2Dot(e, B - Q); float32 v = b2Dot(e, Q - A); float32 radius = edgeA->m_radius + circleB->m_radius; b2ContactFeature cf; cf.indexB = 0; cf.typeB = b2ContactFeature::e_vertex; // Region A if (v <= 0.0f) { b2Vec2 P = A; b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA->m_hasVertex0) { b2Vec2 A1 = edgeA->m_vertex0; b2Vec2 B1 = A; b2Vec2 e1 = B1 - A1; float32 u1 = b2Dot(e1, B1 - Q); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.indexA = 0; cf.typeA = b2ContactFeature::e_vertex; manifold->pointCount = 1; manifold->type = b2Manifold::e_circles; manifold->localNormal.SetZero(); manifold->localPoint = P; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; return; } // Region B if (u <= 0.0f) { b2Vec2 P = B; b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA->m_hasVertex3) { b2Vec2 B2 = edgeA->m_vertex3; b2Vec2 A2 = B; b2Vec2 e2 = B2 - A2; float32 v2 = b2Dot(e2, Q - A2); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.indexA = 1; cf.typeA = b2ContactFeature::e_vertex; manifold->pointCount = 1; manifold->type = b2Manifold::e_circles; manifold->localNormal.SetZero(); manifold->localPoint = P; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; return; } // Region AB float32 den = b2Dot(e, e); b2Assert(den > 0.0f); b2Vec2 P = (1.0f / den) * (u * A + v * B); b2Vec2 d = Q - P; float32 dd = b2Dot(d, d); if (dd > radius * radius) { return; } b2Vec2 n(-e.y, e.x); if (b2Dot(n, Q - A) < 0.0f) { n.Set(-n.x, -n.y); } n.Normalize(); cf.indexA = 0; cf.typeA = b2ContactFeature::e_face; manifold->pointCount = 1; manifold->type = b2Manifold::e_faceA; manifold->localNormal = n; manifold->localPoint = A; manifold->points[0].id.key = 0; manifold->points[0].id.cf = cf; manifold->points[0].localPoint = circleB->m_p; } // This structure is used to keep track of the best separating axis. struct b2EPAxis { enum Type { e_unknown, e_edgeA, e_edgeB }; Type type; int32 index; float32 separation; }; // This holds polygon B expressed in frame A. struct b2TempPolygon { b2Vec2 vertices[b2_maxPolygonVertices]; b2Vec2 normals[b2_maxPolygonVertices]; int32 count; }; // Reference face used for clipping struct b2ReferenceFace { int32 i1, i2; b2Vec2 v1, v2; b2Vec2 normal; b2Vec2 sideNormal1; float32 sideOffset1; b2Vec2 sideNormal2; float32 sideOffset2; }; // This class collides and edge and a polygon, taking into account edge adjacency. struct b2EPCollider { void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2PolygonShape* polygonB, const b2Transform& xfB); b2EPAxis ComputeEdgeSeparation(); b2EPAxis ComputePolygonSeparation(); enum VertexType { e_isolated, e_concave, e_convex }; b2TempPolygon m_polygonB; b2Transform m_xf; b2Vec2 m_centroidB; b2Vec2 m_v0, m_v1, m_v2, m_v3; b2Vec2 m_normal0, m_normal1, m_normal2; b2Vec2 m_normal; VertexType m_type1, m_type2; b2Vec2 m_lowerLimit, m_upperLimit; float32 m_radius; bool m_front; }; // 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; } b2EPAxis b2EPCollider::ComputeEdgeSeparation() { b2EPAxis axis; axis.type = b2EPAxis::e_edgeA; axis.index = m_front ? 0 : 1; axis.separation = FLT_MAX; for (int32 i = 0; i < m_polygonB.count; ++i) { float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); if (s < axis.separation) { axis.separation = s; } } return axis; } b2EPAxis b2EPCollider::ComputePolygonSeparation() { b2EPAxis axis; axis.type = b2EPAxis::e_unknown; axis.index = -1; axis.separation = -FLT_MAX; b2Vec2 perp(-m_normal.y, m_normal.x); for (int32 i = 0; i < m_polygonB.count; ++i) { b2Vec2 n = -m_polygonB.normals[i]; float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); float32 s = b2Min(s1, s2); if (s > m_radius) { // No collision axis.type = b2EPAxis::e_edgeB; axis.index = i; axis.separation = s; return axis; } // Adjacency if (b2Dot(n, perp) >= 0.0f) { if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) { continue; } } else { if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) { continue; } } if (s > axis.separation) { axis.type = b2EPAxis::e_edgeB; axis.index = i; axis.separation = s; } } return axis; } void b2CollideEdgeAndPolygon( b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2PolygonShape* polygonB, const b2Transform& xfB) { b2EPCollider collider; collider.Collide(manifold, edgeA, xfA, polygonB, xfB); }