File indexing completed on 2024-05-19 14:56:21
0001 /* 0002 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/b2Collision.h> 0020 #include <Box2D/Collision/Shapes/b2CircleShape.h> 0021 #include <Box2D/Collision/Shapes/b2EdgeShape.h> 0022 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0023 0024 0025 // Compute contact points for edge versus circle. 0026 // This accounts for edge connectivity. 0027 void b2CollideEdgeAndCircle(b2Manifold* manifold, 0028 const b2EdgeShape* edgeA, const b2Transform& xfA, 0029 const b2CircleShape* circleB, const b2Transform& xfB) 0030 { 0031 manifold->pointCount = 0; 0032 0033 // Compute circle in frame of edge 0034 b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); 0035 0036 b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; 0037 b2Vec2 e = B - A; 0038 0039 // Barycentric coordinates 0040 float32 u = b2Dot(e, B - Q); 0041 float32 v = b2Dot(e, Q - A); 0042 0043 float32 radius = edgeA->m_radius + circleB->m_radius; 0044 0045 b2ContactFeature cf; 0046 cf.indexB = 0; 0047 cf.typeB = b2ContactFeature::e_vertex; 0048 0049 // Region A 0050 if (v <= 0.0f) 0051 { 0052 b2Vec2 P = A; 0053 b2Vec2 d = Q - P; 0054 float32 dd = b2Dot(d, d); 0055 if (dd > radius * radius) 0056 { 0057 return; 0058 } 0059 0060 // Is there an edge connected to A? 0061 if (edgeA->m_hasVertex0) 0062 { 0063 b2Vec2 A1 = edgeA->m_vertex0; 0064 b2Vec2 B1 = A; 0065 b2Vec2 e1 = B1 - A1; 0066 float32 u1 = b2Dot(e1, B1 - Q); 0067 0068 // Is the circle in Region AB of the previous edge? 0069 if (u1 > 0.0f) 0070 { 0071 return; 0072 } 0073 } 0074 0075 cf.indexA = 0; 0076 cf.typeA = b2ContactFeature::e_vertex; 0077 manifold->pointCount = 1; 0078 manifold->type = b2Manifold::e_circles; 0079 manifold->localNormal.SetZero(); 0080 manifold->localPoint = P; 0081 manifold->points[0].id.key = 0; 0082 manifold->points[0].id.cf = cf; 0083 manifold->points[0].localPoint = circleB->m_p; 0084 return; 0085 } 0086 0087 // Region B 0088 if (u <= 0.0f) 0089 { 0090 b2Vec2 P = B; 0091 b2Vec2 d = Q - P; 0092 float32 dd = b2Dot(d, d); 0093 if (dd > radius * radius) 0094 { 0095 return; 0096 } 0097 0098 // Is there an edge connected to B? 0099 if (edgeA->m_hasVertex3) 0100 { 0101 b2Vec2 B2 = edgeA->m_vertex3; 0102 b2Vec2 A2 = B; 0103 b2Vec2 e2 = B2 - A2; 0104 float32 v2 = b2Dot(e2, Q - A2); 0105 0106 // Is the circle in Region AB of the next edge? 0107 if (v2 > 0.0f) 0108 { 0109 return; 0110 } 0111 } 0112 0113 cf.indexA = 1; 0114 cf.typeA = b2ContactFeature::e_vertex; 0115 manifold->pointCount = 1; 0116 manifold->type = b2Manifold::e_circles; 0117 manifold->localNormal.SetZero(); 0118 manifold->localPoint = P; 0119 manifold->points[0].id.key = 0; 0120 manifold->points[0].id.cf = cf; 0121 manifold->points[0].localPoint = circleB->m_p; 0122 return; 0123 } 0124 0125 // Region AB 0126 float32 den = b2Dot(e, e); 0127 b2Assert(den > 0.0f); 0128 b2Vec2 P = (1.0f / den) * (u * A + v * B); 0129 b2Vec2 d = Q - P; 0130 float32 dd = b2Dot(d, d); 0131 if (dd > radius * radius) 0132 { 0133 return; 0134 } 0135 0136 b2Vec2 n(-e.y, e.x); 0137 if (b2Dot(n, Q - A) < 0.0f) 0138 { 0139 n.Set(-n.x, -n.y); 0140 } 0141 n.Normalize(); 0142 0143 cf.indexA = 0; 0144 cf.typeA = b2ContactFeature::e_face; 0145 manifold->pointCount = 1; 0146 manifold->type = b2Manifold::e_faceA; 0147 manifold->localNormal = n; 0148 manifold->localPoint = A; 0149 manifold->points[0].id.key = 0; 0150 manifold->points[0].id.cf = cf; 0151 manifold->points[0].localPoint = circleB->m_p; 0152 } 0153 0154 // This structure is used to keep track of the best separating axis. 0155 struct b2EPAxis 0156 { 0157 enum Type 0158 { 0159 e_unknown, 0160 e_edgeA, 0161 e_edgeB 0162 }; 0163 0164 Type type; 0165 int32 index; 0166 float32 separation; 0167 }; 0168 0169 // This holds polygon B expressed in frame A. 0170 struct b2TempPolygon 0171 { 0172 b2Vec2 vertices[b2_maxPolygonVertices]; 0173 b2Vec2 normals[b2_maxPolygonVertices]; 0174 int32 count; 0175 }; 0176 0177 // Reference face used for clipping 0178 struct b2ReferenceFace 0179 { 0180 int32 i1, i2; 0181 0182 b2Vec2 v1, v2; 0183 0184 b2Vec2 normal; 0185 0186 b2Vec2 sideNormal1; 0187 float32 sideOffset1; 0188 0189 b2Vec2 sideNormal2; 0190 float32 sideOffset2; 0191 }; 0192 0193 // This class collides and edge and a polygon, taking into account edge adjacency. 0194 struct b2EPCollider 0195 { 0196 void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, 0197 const b2PolygonShape* polygonB, const b2Transform& xfB); 0198 b2EPAxis ComputeEdgeSeparation(); 0199 b2EPAxis ComputePolygonSeparation(); 0200 0201 enum VertexType 0202 { 0203 e_isolated, 0204 e_concave, 0205 e_convex 0206 }; 0207 0208 b2TempPolygon m_polygonB; 0209 0210 b2Transform m_xf; 0211 b2Vec2 m_centroidB; 0212 b2Vec2 m_v0, m_v1, m_v2, m_v3; 0213 b2Vec2 m_normal0, m_normal1, m_normal2; 0214 b2Vec2 m_normal; 0215 VertexType m_type1, m_type2; 0216 b2Vec2 m_lowerLimit, m_upperLimit; 0217 float32 m_radius; 0218 bool m_front; 0219 }; 0220 0221 // Algorithm: 0222 // 1. Classify v1 and v2 0223 // 2. Classify polygon centroid as front or back 0224 // 3. Flip normal if necessary 0225 // 4. Initialize normal range to [-pi, pi] about face normal 0226 // 5. Adjust normal range according to adjacent edges 0227 // 6. Visit each separating axes, only accept axes within the range 0228 // 7. Return if _any_ axis indicates separation 0229 // 8. Clip 0230 void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, 0231 const b2PolygonShape* polygonB, const b2Transform& xfB) 0232 { 0233 m_xf = b2MulT(xfA, xfB); 0234 0235 m_centroidB = b2Mul(m_xf, polygonB->m_centroid); 0236 0237 m_v0 = edgeA->m_vertex0; 0238 m_v1 = edgeA->m_vertex1; 0239 m_v2 = edgeA->m_vertex2; 0240 m_v3 = edgeA->m_vertex3; 0241 0242 bool hasVertex0 = edgeA->m_hasVertex0; 0243 bool hasVertex3 = edgeA->m_hasVertex3; 0244 0245 b2Vec2 edge1 = m_v2 - m_v1; 0246 edge1.Normalize(); 0247 m_normal1.Set(edge1.y, -edge1.x); 0248 float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); 0249 float32 offset0 = 0.0f, offset2 = 0.0f; 0250 bool convex1 = false, convex2 = false; 0251 0252 // Is there a preceding edge? 0253 if (hasVertex0) 0254 { 0255 b2Vec2 edge0 = m_v1 - m_v0; 0256 edge0.Normalize(); 0257 m_normal0.Set(edge0.y, -edge0.x); 0258 convex1 = b2Cross(edge0, edge1) >= 0.0f; 0259 offset0 = b2Dot(m_normal0, m_centroidB - m_v0); 0260 } 0261 0262 // Is there a following edge? 0263 if (hasVertex3) 0264 { 0265 b2Vec2 edge2 = m_v3 - m_v2; 0266 edge2.Normalize(); 0267 m_normal2.Set(edge2.y, -edge2.x); 0268 convex2 = b2Cross(edge1, edge2) > 0.0f; 0269 offset2 = b2Dot(m_normal2, m_centroidB - m_v2); 0270 } 0271 0272 // Determine front or back collision. Determine collision normal limits. 0273 if (hasVertex0 && hasVertex3) 0274 { 0275 if (convex1 && convex2) 0276 { 0277 m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; 0278 if (m_front) 0279 { 0280 m_normal = m_normal1; 0281 m_lowerLimit = m_normal0; 0282 m_upperLimit = m_normal2; 0283 } 0284 else 0285 { 0286 m_normal = -m_normal1; 0287 m_lowerLimit = -m_normal1; 0288 m_upperLimit = -m_normal1; 0289 } 0290 } 0291 else if (convex1) 0292 { 0293 m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); 0294 if (m_front) 0295 { 0296 m_normal = m_normal1; 0297 m_lowerLimit = m_normal0; 0298 m_upperLimit = m_normal1; 0299 } 0300 else 0301 { 0302 m_normal = -m_normal1; 0303 m_lowerLimit = -m_normal2; 0304 m_upperLimit = -m_normal1; 0305 } 0306 } 0307 else if (convex2) 0308 { 0309 m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); 0310 if (m_front) 0311 { 0312 m_normal = m_normal1; 0313 m_lowerLimit = m_normal1; 0314 m_upperLimit = m_normal2; 0315 } 0316 else 0317 { 0318 m_normal = -m_normal1; 0319 m_lowerLimit = -m_normal1; 0320 m_upperLimit = -m_normal0; 0321 } 0322 } 0323 else 0324 { 0325 m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; 0326 if (m_front) 0327 { 0328 m_normal = m_normal1; 0329 m_lowerLimit = m_normal1; 0330 m_upperLimit = m_normal1; 0331 } 0332 else 0333 { 0334 m_normal = -m_normal1; 0335 m_lowerLimit = -m_normal2; 0336 m_upperLimit = -m_normal0; 0337 } 0338 } 0339 } 0340 else if (hasVertex0) 0341 { 0342 if (convex1) 0343 { 0344 m_front = offset0 >= 0.0f || offset1 >= 0.0f; 0345 if (m_front) 0346 { 0347 m_normal = m_normal1; 0348 m_lowerLimit = m_normal0; 0349 m_upperLimit = -m_normal1; 0350 } 0351 else 0352 { 0353 m_normal = -m_normal1; 0354 m_lowerLimit = m_normal1; 0355 m_upperLimit = -m_normal1; 0356 } 0357 } 0358 else 0359 { 0360 m_front = offset0 >= 0.0f && offset1 >= 0.0f; 0361 if (m_front) 0362 { 0363 m_normal = m_normal1; 0364 m_lowerLimit = m_normal1; 0365 m_upperLimit = -m_normal1; 0366 } 0367 else 0368 { 0369 m_normal = -m_normal1; 0370 m_lowerLimit = m_normal1; 0371 m_upperLimit = -m_normal0; 0372 } 0373 } 0374 } 0375 else if (hasVertex3) 0376 { 0377 if (convex2) 0378 { 0379 m_front = offset1 >= 0.0f || offset2 >= 0.0f; 0380 if (m_front) 0381 { 0382 m_normal = m_normal1; 0383 m_lowerLimit = -m_normal1; 0384 m_upperLimit = m_normal2; 0385 } 0386 else 0387 { 0388 m_normal = -m_normal1; 0389 m_lowerLimit = -m_normal1; 0390 m_upperLimit = m_normal1; 0391 } 0392 } 0393 else 0394 { 0395 m_front = offset1 >= 0.0f && offset2 >= 0.0f; 0396 if (m_front) 0397 { 0398 m_normal = m_normal1; 0399 m_lowerLimit = -m_normal1; 0400 m_upperLimit = m_normal1; 0401 } 0402 else 0403 { 0404 m_normal = -m_normal1; 0405 m_lowerLimit = -m_normal2; 0406 m_upperLimit = m_normal1; 0407 } 0408 } 0409 } 0410 else 0411 { 0412 m_front = offset1 >= 0.0f; 0413 if (m_front) 0414 { 0415 m_normal = m_normal1; 0416 m_lowerLimit = -m_normal1; 0417 m_upperLimit = -m_normal1; 0418 } 0419 else 0420 { 0421 m_normal = -m_normal1; 0422 m_lowerLimit = m_normal1; 0423 m_upperLimit = m_normal1; 0424 } 0425 } 0426 0427 // Get polygonB in frameA 0428 m_polygonB.count = polygonB->m_count; 0429 for (int32 i = 0; i < polygonB->m_count; ++i) 0430 { 0431 m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); 0432 m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); 0433 } 0434 0435 m_radius = 2.0f * b2_polygonRadius; 0436 0437 manifold->pointCount = 0; 0438 0439 b2EPAxis edgeAxis = ComputeEdgeSeparation(); 0440 0441 // If no valid normal can be found than this edge should not collide. 0442 if (edgeAxis.type == b2EPAxis::e_unknown) 0443 { 0444 return; 0445 } 0446 0447 if (edgeAxis.separation > m_radius) 0448 { 0449 return; 0450 } 0451 0452 b2EPAxis polygonAxis = ComputePolygonSeparation(); 0453 if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) 0454 { 0455 return; 0456 } 0457 0458 // Use hysteresis for jitter reduction. 0459 const float32 k_relativeTol = 0.98f; 0460 const float32 k_absoluteTol = 0.001f; 0461 0462 b2EPAxis primaryAxis; 0463 if (polygonAxis.type == b2EPAxis::e_unknown) 0464 { 0465 primaryAxis = edgeAxis; 0466 } 0467 else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) 0468 { 0469 primaryAxis = polygonAxis; 0470 } 0471 else 0472 { 0473 primaryAxis = edgeAxis; 0474 } 0475 0476 b2ClipVertex ie[2]; 0477 b2ReferenceFace rf; 0478 if (primaryAxis.type == b2EPAxis::e_edgeA) 0479 { 0480 manifold->type = b2Manifold::e_faceA; 0481 0482 // Search for the polygon normal that is most anti-parallel to the edge normal. 0483 int32 bestIndex = 0; 0484 float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); 0485 for (int32 i = 1; i < m_polygonB.count; ++i) 0486 { 0487 float32 value = b2Dot(m_normal, m_polygonB.normals[i]); 0488 if (value < bestValue) 0489 { 0490 bestValue = value; 0491 bestIndex = i; 0492 } 0493 } 0494 0495 int32 i1 = bestIndex; 0496 int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; 0497 0498 ie[0].v = m_polygonB.vertices[i1]; 0499 ie[0].id.cf.indexA = 0; 0500 ie[0].id.cf.indexB = static_cast<uint8>(i1); 0501 ie[0].id.cf.typeA = b2ContactFeature::e_face; 0502 ie[0].id.cf.typeB = b2ContactFeature::e_vertex; 0503 0504 ie[1].v = m_polygonB.vertices[i2]; 0505 ie[1].id.cf.indexA = 0; 0506 ie[1].id.cf.indexB = static_cast<uint8>(i2); 0507 ie[1].id.cf.typeA = b2ContactFeature::e_face; 0508 ie[1].id.cf.typeB = b2ContactFeature::e_vertex; 0509 0510 if (m_front) 0511 { 0512 rf.i1 = 0; 0513 rf.i2 = 1; 0514 rf.v1 = m_v1; 0515 rf.v2 = m_v2; 0516 rf.normal = m_normal1; 0517 } 0518 else 0519 { 0520 rf.i1 = 1; 0521 rf.i2 = 0; 0522 rf.v1 = m_v2; 0523 rf.v2 = m_v1; 0524 rf.normal = -m_normal1; 0525 } 0526 } 0527 else 0528 { 0529 manifold->type = b2Manifold::e_faceB; 0530 0531 ie[0].v = m_v1; 0532 ie[0].id.cf.indexA = 0; 0533 ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index); 0534 ie[0].id.cf.typeA = b2ContactFeature::e_vertex; 0535 ie[0].id.cf.typeB = b2ContactFeature::e_face; 0536 0537 ie[1].v = m_v2; 0538 ie[1].id.cf.indexA = 0; 0539 ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index); 0540 ie[1].id.cf.typeA = b2ContactFeature::e_vertex; 0541 ie[1].id.cf.typeB = b2ContactFeature::e_face; 0542 0543 rf.i1 = primaryAxis.index; 0544 rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; 0545 rf.v1 = m_polygonB.vertices[rf.i1]; 0546 rf.v2 = m_polygonB.vertices[rf.i2]; 0547 rf.normal = m_polygonB.normals[rf.i1]; 0548 } 0549 0550 rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); 0551 rf.sideNormal2 = -rf.sideNormal1; 0552 rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); 0553 rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); 0554 0555 // Clip incident edge against extruded edge1 side edges. 0556 b2ClipVertex clipPoints1[2]; 0557 b2ClipVertex clipPoints2[2]; 0558 int32 np; 0559 0560 // Clip to box side 1 0561 np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); 0562 0563 if (np < b2_maxManifoldPoints) 0564 { 0565 return; 0566 } 0567 0568 // Clip to negative box side 1 0569 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); 0570 0571 if (np < b2_maxManifoldPoints) 0572 { 0573 return; 0574 } 0575 0576 // Now clipPoints2 contains the clipped points. 0577 if (primaryAxis.type == b2EPAxis::e_edgeA) 0578 { 0579 manifold->localNormal = rf.normal; 0580 manifold->localPoint = rf.v1; 0581 } 0582 else 0583 { 0584 manifold->localNormal = polygonB->m_normals[rf.i1]; 0585 manifold->localPoint = polygonB->m_vertices[rf.i1]; 0586 } 0587 0588 int32 pointCount = 0; 0589 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) 0590 { 0591 float32 separation; 0592 0593 separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); 0594 0595 if (separation <= m_radius) 0596 { 0597 b2ManifoldPoint* cp = manifold->points + pointCount; 0598 0599 if (primaryAxis.type == b2EPAxis::e_edgeA) 0600 { 0601 cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); 0602 cp->id = clipPoints2[i].id; 0603 } 0604 else 0605 { 0606 cp->localPoint = clipPoints2[i].v; 0607 cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; 0608 cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; 0609 cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; 0610 cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; 0611 } 0612 0613 ++pointCount; 0614 } 0615 } 0616 0617 manifold->pointCount = pointCount; 0618 } 0619 0620 b2EPAxis b2EPCollider::ComputeEdgeSeparation() 0621 { 0622 b2EPAxis axis; 0623 axis.type = b2EPAxis::e_edgeA; 0624 axis.index = m_front ? 0 : 1; 0625 axis.separation = FLT_MAX; 0626 0627 for (int32 i = 0; i < m_polygonB.count; ++i) 0628 { 0629 float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); 0630 if (s < axis.separation) 0631 { 0632 axis.separation = s; 0633 } 0634 } 0635 0636 return axis; 0637 } 0638 0639 b2EPAxis b2EPCollider::ComputePolygonSeparation() 0640 { 0641 b2EPAxis axis; 0642 axis.type = b2EPAxis::e_unknown; 0643 axis.index = -1; 0644 axis.separation = -FLT_MAX; 0645 0646 b2Vec2 perp(-m_normal.y, m_normal.x); 0647 0648 for (int32 i = 0; i < m_polygonB.count; ++i) 0649 { 0650 b2Vec2 n = -m_polygonB.normals[i]; 0651 0652 float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); 0653 float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); 0654 float32 s = b2Min(s1, s2); 0655 0656 if (s > m_radius) 0657 { 0658 // No collision 0659 axis.type = b2EPAxis::e_edgeB; 0660 axis.index = i; 0661 axis.separation = s; 0662 return axis; 0663 } 0664 0665 // Adjacency 0666 if (b2Dot(n, perp) >= 0.0f) 0667 { 0668 if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) 0669 { 0670 continue; 0671 } 0672 } 0673 else 0674 { 0675 if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) 0676 { 0677 continue; 0678 } 0679 } 0680 0681 if (s > axis.separation) 0682 { 0683 axis.type = b2EPAxis::e_edgeB; 0684 axis.index = i; 0685 axis.separation = s; 0686 } 0687 } 0688 0689 return axis; 0690 } 0691 0692 void b2CollideEdgeAndPolygon( b2Manifold* manifold, 0693 const b2EdgeShape* edgeA, const b2Transform& xfA, 0694 const b2PolygonShape* polygonB, const b2Transform& xfB) 0695 { 0696 b2EPCollider collider; 0697 collider.Collide(manifold, edgeA, xfA, polygonB, xfB); 0698 }