1 /* 2 * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org 3 * 4 * This software is provided 'as-is', without any express or implied 5 * warranty. In no event will the authors be held liable for any damages 6 * arising from the use of this software. 7 * Permission is granted to anyone to use this software for any purpose, 8 * including commercial applications, and to alter it and redistribute it 9 * freely, subject to the following restrictions: 10 * 1. The origin of this software must not be misrepresented; you must not 11 * claim that you wrote the original software. If you use this software 12 * in a product, an acknowledgment in the product documentation would be 13 * appreciated but is not required. 14 * 2. Altered source versions must be plainly marked as such, and must not be 15 * misrepresented as being the original software. 16 * 3. This notice may not be removed or altered from any source distribution. 17 */ 18 19 #include <Box2D/Collision/b2Collision.h> 20 #include <Box2D/Collision/Shapes/b2CircleShape.h> 21 #include <Box2D/Collision/Shapes/b2EdgeShape.h> 22 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 23 24 25 // Compute contact points for edge versus circle. 26 // This accounts for edge connectivity. 27 void b2CollideEdgeAndCircle(b2Manifold* manifold, 28 const b2EdgeShape* edgeA, const b2Transform& xfA, 29 const b2CircleShape* circleB, const b2Transform& xfB) 30 { 31 manifold->pointCount = 0; 32 33 // Compute circle in frame of edge 34 b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); 35 36 b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; 37 b2Vec2 e = B - A; 38 39 // Barycentric coordinates 40 float32 u = b2Dot(e, B - Q); 41 float32 v = b2Dot(e, Q - A); 42 43 float32 radius = edgeA->m_radius + circleB->m_radius; 44 45 b2ContactFeature cf; 46 cf.indexB = 0; 47 cf.typeB = b2ContactFeature::e_vertex; 48 49 // Region A 50 if (v <= 0.0f) 51 { 52 b2Vec2 P = A; 53 b2Vec2 d = Q - P; 54 float32 dd = b2Dot(d, d); 55 if (dd > radius * radius) 56 { 57 return; 58 } 59 60 // Is there an edge connected to A? 61 if (edgeA->m_hasVertex0) 62 { 63 b2Vec2 A1 = edgeA->m_vertex0; 64 b2Vec2 B1 = A; 65 b2Vec2 e1 = B1 - A1; 66 float32 u1 = b2Dot(e1, B1 - Q); 67 68 // Is the circle in Region AB of the previous edge? 69 if (u1 > 0.0f) 70 { 71 return; 72 } 73 } 74 75 cf.indexA = 0; 76 cf.typeA = b2ContactFeature::e_vertex; 77 manifold->pointCount = 1; 78 manifold->type = b2Manifold::e_circles; 79 manifold->localNormal.SetZero(); 80 manifold->localPoint = P; 81 manifold->points[0].id.key = 0; 82 manifold->points[0].id.cf = cf; 83 manifold->points[0].localPoint = circleB->m_p; 84 return; 85 } 86 87 // Region B 88 if (u <= 0.0f) 89 { 90 b2Vec2 P = B; 91 b2Vec2 d = Q - P; 92 float32 dd = b2Dot(d, d); 93 if (dd > radius * radius) 94 { 95 return; 96 } 97 98 // Is there an edge connected to B? 99 if (edgeA->m_hasVertex3) 100 { 101 b2Vec2 B2 = edgeA->m_vertex3; 102 b2Vec2 A2 = B; 103 b2Vec2 e2 = B2 - A2; 104 float32 v2 = b2Dot(e2, Q - A2); 105 106 // Is the circle in Region AB of the next edge? 107 if (v2 > 0.0f) 108 { 109 return; 110 } 111 } 112 113 cf.indexA = 1; 114 cf.typeA = b2ContactFeature::e_vertex; 115 manifold->pointCount = 1; 116 manifold->type = b2Manifold::e_circles; 117 manifold->localNormal.SetZero(); 118 manifold->localPoint = P; 119 manifold->points[0].id.key = 0; 120 manifold->points[0].id.cf = cf; 121 manifold->points[0].localPoint = circleB->m_p; 122 return; 123 } 124 125 // Region AB 126 float32 den = b2Dot(e, e); 127 b2Assert(den > 0.0f); 128 b2Vec2 P = (1.0f / den) * (u * A + v * B); 129 b2Vec2 d = Q - P; 130 float32 dd = b2Dot(d, d); 131 if (dd > radius * radius) 132 { 133 return; 134 } 135 136 b2Vec2 n(-e.y, e.x); 137 if (b2Dot(n, Q - A) < 0.0f) 138 { 139 n.Set(-n.x, -n.y); 140 } 141 n.Normalize(); 142 143 cf.indexA = 0; 144 cf.typeA = b2ContactFeature::e_face; 145 manifold->pointCount = 1; 146 manifold->type = b2Manifold::e_faceA; 147 manifold->localNormal = n; 148 manifold->localPoint = A; 149 manifold->points[0].id.key = 0; 150 manifold->points[0].id.cf = cf; 151 manifold->points[0].localPoint = circleB->m_p; 152 } 153 154 // This structure is used to keep track of the best separating axis. 155 struct b2EPAxis 156 { 157 enum Type 158 { 159 e_unknown, 160 e_edgeA, 161 e_edgeB 162 }; 163 164 Type type; 165 int32 index; 166 float32 separation; 167 }; 168 169 // This holds polygon B expressed in frame A. 170 struct b2TempPolygon 171 { 172 b2Vec2 vertices[b2_maxPolygonVertices]; 173 b2Vec2 normals[b2_maxPolygonVertices]; 174 int32 count; 175 }; 176 177 // Reference face used for clipping 178 struct b2ReferenceFace 179 { 180 int32 i1, i2; 181 182 b2Vec2 v1, v2; 183 184 b2Vec2 normal; 185 186 b2Vec2 sideNormal1; 187 float32 sideOffset1; 188 189 b2Vec2 sideNormal2; 190 float32 sideOffset2; 191 }; 192 193 // This class collides and edge and a polygon, taking into account edge adjacency. 194 struct b2EPCollider 195 { 196 void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, 197 const b2PolygonShape* polygonB, const b2Transform& xfB); 198 b2EPAxis ComputeEdgeSeparation(); 199 b2EPAxis ComputePolygonSeparation(); 200 201 enum VertexType 202 { 203 e_isolated, 204 e_concave, 205 e_convex 206 }; 207 208 b2TempPolygon m_polygonB; 209 210 b2Transform m_xf; 211 b2Vec2 m_centroidB; 212 b2Vec2 m_v0, m_v1, m_v2, m_v3; 213 b2Vec2 m_normal0, m_normal1, m_normal2; 214 b2Vec2 m_normal; 215 VertexType m_type1, m_type2; 216 b2Vec2 m_lowerLimit, m_upperLimit; 217 float32 m_radius; 218 bool m_front; 219 }; 220 221 // Algorithm: 222 // 1. Classify v1 and v2 223 // 2. Classify polygon centroid as front or back 224 // 3. Flip normal if necessary 225 // 4. Initialize normal range to [-pi, pi] about face normal 226 // 5. Adjust normal range according to adjacent edges 227 // 6. Visit each separating axes, only accept axes within the range 228 // 7. Return if _any_ axis indicates separation 229 // 8. Clip 230 void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, 231 const b2PolygonShape* polygonB, const b2Transform& xfB) 232 { 233 m_xf = b2MulT(xfA, xfB); 234 235 m_centroidB = b2Mul(m_xf, polygonB->m_centroid); 236 237 m_v0 = edgeA->m_vertex0; 238 m_v1 = edgeA->m_vertex1; 239 m_v2 = edgeA->m_vertex2; 240 m_v3 = edgeA->m_vertex3; 241 242 bool hasVertex0 = edgeA->m_hasVertex0; 243 bool hasVertex3 = edgeA->m_hasVertex3; 244 245 b2Vec2 edge1 = m_v2 - m_v1; 246 edge1.Normalize(); 247 m_normal1.Set(edge1.y, -edge1.x); 248 float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); 249 float32 offset0 = 0.0f, offset2 = 0.0f; 250 bool convex1 = false, convex2 = false; 251 252 // Is there a preceding edge? 253 if (hasVertex0) 254 { 255 b2Vec2 edge0 = m_v1 - m_v0; 256 edge0.Normalize(); 257 m_normal0.Set(edge0.y, -edge0.x); 258 convex1 = b2Cross(edge0, edge1) >= 0.0f; 259 offset0 = b2Dot(m_normal0, m_centroidB - m_v0); 260 } 261 262 // Is there a following edge? 263 if (hasVertex3) 264 { 265 b2Vec2 edge2 = m_v3 - m_v2; 266 edge2.Normalize(); 267 m_normal2.Set(edge2.y, -edge2.x); 268 convex2 = b2Cross(edge1, edge2) > 0.0f; 269 offset2 = b2Dot(m_normal2, m_centroidB - m_v2); 270 } 271 272 // Determine front or back collision. Determine collision normal limits. 273 if (hasVertex0 && hasVertex3) 274 { 275 if (convex1 && convex2) 276 { 277 m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; 278 if (m_front) 279 { 280 m_normal = m_normal1; 281 m_lowerLimit = m_normal0; 282 m_upperLimit = m_normal2; 283 } 284 else 285 { 286 m_normal = -m_normal1; 287 m_lowerLimit = -m_normal1; 288 m_upperLimit = -m_normal1; 289 } 290 } 291 else if (convex1) 292 { 293 m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); 294 if (m_front) 295 { 296 m_normal = m_normal1; 297 m_lowerLimit = m_normal0; 298 m_upperLimit = m_normal1; 299 } 300 else 301 { 302 m_normal = -m_normal1; 303 m_lowerLimit = -m_normal2; 304 m_upperLimit = -m_normal1; 305 } 306 } 307 else if (convex2) 308 { 309 m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); 310 if (m_front) 311 { 312 m_normal = m_normal1; 313 m_lowerLimit = m_normal1; 314 m_upperLimit = m_normal2; 315 } 316 else 317 { 318 m_normal = -m_normal1; 319 m_lowerLimit = -m_normal1; 320 m_upperLimit = -m_normal0; 321 } 322 } 323 else 324 { 325 m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; 326 if (m_front) 327 { 328 m_normal = m_normal1; 329 m_lowerLimit = m_normal1; 330 m_upperLimit = m_normal1; 331 } 332 else 333 { 334 m_normal = -m_normal1; 335 m_lowerLimit = -m_normal2; 336 m_upperLimit = -m_normal0; 337 } 338 } 339 } 340 else if (hasVertex0) 341 { 342 if (convex1) 343 { 344 m_front = offset0 >= 0.0f || offset1 >= 0.0f; 345 if (m_front) 346 { 347 m_normal = m_normal1; 348 m_lowerLimit = m_normal0; 349 m_upperLimit = -m_normal1; 350 } 351 else 352 { 353 m_normal = -m_normal1; 354 m_lowerLimit = m_normal1; 355 m_upperLimit = -m_normal1; 356 } 357 } 358 else 359 { 360 m_front = offset0 >= 0.0f && offset1 >= 0.0f; 361 if (m_front) 362 { 363 m_normal = m_normal1; 364 m_lowerLimit = m_normal1; 365 m_upperLimit = -m_normal1; 366 } 367 else 368 { 369 m_normal = -m_normal1; 370 m_lowerLimit = m_normal1; 371 m_upperLimit = -m_normal0; 372 } 373 } 374 } 375 else if (hasVertex3) 376 { 377 if (convex2) 378 { 379 m_front = offset1 >= 0.0f || offset2 >= 0.0f; 380 if (m_front) 381 { 382 m_normal = m_normal1; 383 m_lowerLimit = -m_normal1; 384 m_upperLimit = m_normal2; 385 } 386 else 387 { 388 m_normal = -m_normal1; 389 m_lowerLimit = -m_normal1; 390 m_upperLimit = m_normal1; 391 } 392 } 393 else 394 { 395 m_front = offset1 >= 0.0f && offset2 >= 0.0f; 396 if (m_front) 397 { 398 m_normal = m_normal1; 399 m_lowerLimit = -m_normal1; 400 m_upperLimit = m_normal1; 401 } 402 else 403 { 404 m_normal = -m_normal1; 405 m_lowerLimit = -m_normal2; 406 m_upperLimit = m_normal1; 407 } 408 } 409 } 410 else 411 { 412 m_front = offset1 >= 0.0f; 413 if (m_front) 414 { 415 m_normal = m_normal1; 416 m_lowerLimit = -m_normal1; 417 m_upperLimit = -m_normal1; 418 } 419 else 420 { 421 m_normal = -m_normal1; 422 m_lowerLimit = m_normal1; 423 m_upperLimit = m_normal1; 424 } 425 } 426 427 // Get polygonB in frameA 428 m_polygonB.count = polygonB->m_count; 429 for (int32 i = 0; i < polygonB->m_count; ++i) 430 { 431 m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); 432 m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); 433 } 434 435 m_radius = 2.0f * b2_polygonRadius; 436 437 manifold->pointCount = 0; 438 439 b2EPAxis edgeAxis = ComputeEdgeSeparation(); 440 441 // If no valid normal can be found than this edge should not collide. 442 if (edgeAxis.type == b2EPAxis::e_unknown) 443 { 444 return; 445 } 446 447 if (edgeAxis.separation > m_radius) 448 { 449 return; 450 } 451 452 b2EPAxis polygonAxis = ComputePolygonSeparation(); 453 if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) 454 { 455 return; 456 } 457 458 // Use hysteresis for jitter reduction. 459 const float32 k_relativeTol = 0.98f; 460 const float32 k_absoluteTol = 0.001f; 461 462 b2EPAxis primaryAxis; 463 if (polygonAxis.type == b2EPAxis::e_unknown) 464 { 465 primaryAxis = edgeAxis; 466 } 467 else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) 468 { 469 primaryAxis = polygonAxis; 470 } 471 else 472 { 473 primaryAxis = edgeAxis; 474 } 475 476 b2ClipVertex ie[2]; 477 b2ReferenceFace rf; 478 if (primaryAxis.type == b2EPAxis::e_edgeA) 479 { 480 manifold->type = b2Manifold::e_faceA; 481 482 // Search for the polygon normal that is most anti-parallel to the edge normal. 483 int32 bestIndex = 0; 484 float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); 485 for (int32 i = 1; i < m_polygonB.count; ++i) 486 { 487 float32 value = b2Dot(m_normal, m_polygonB.normals[i]); 488 if (value < bestValue) 489 { 490 bestValue = value; 491 bestIndex = i; 492 } 493 } 494 495 int32 i1 = bestIndex; 496 int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; 497 498 ie[0].v = m_polygonB.vertices[i1]; 499 ie[0].id.cf.indexA = 0; 500 ie[0].id.cf.indexB = static_cast<uint8>(i1); 501 ie[0].id.cf.typeA = b2ContactFeature::e_face; 502 ie[0].id.cf.typeB = b2ContactFeature::e_vertex; 503 504 ie[1].v = m_polygonB.vertices[i2]; 505 ie[1].id.cf.indexA = 0; 506 ie[1].id.cf.indexB = static_cast<uint8>(i2); 507 ie[1].id.cf.typeA = b2ContactFeature::e_face; 508 ie[1].id.cf.typeB = b2ContactFeature::e_vertex; 509 510 if (m_front) 511 { 512 rf.i1 = 0; 513 rf.i2 = 1; 514 rf.v1 = m_v1; 515 rf.v2 = m_v2; 516 rf.normal = m_normal1; 517 } 518 else 519 { 520 rf.i1 = 1; 521 rf.i2 = 0; 522 rf.v1 = m_v2; 523 rf.v2 = m_v1; 524 rf.normal = -m_normal1; 525 } 526 } 527 else 528 { 529 manifold->type = b2Manifold::e_faceB; 530 531 ie[0].v = m_v1; 532 ie[0].id.cf.indexA = 0; 533 ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index); 534 ie[0].id.cf.typeA = b2ContactFeature::e_vertex; 535 ie[0].id.cf.typeB = b2ContactFeature::e_face; 536 537 ie[1].v = m_v2; 538 ie[1].id.cf.indexA = 0; 539 ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index); 540 ie[1].id.cf.typeA = b2ContactFeature::e_vertex; 541 ie[1].id.cf.typeB = b2ContactFeature::e_face; 542 543 rf.i1 = primaryAxis.index; 544 rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; 545 rf.v1 = m_polygonB.vertices[rf.i1]; 546 rf.v2 = m_polygonB.vertices[rf.i2]; 547 rf.normal = m_polygonB.normals[rf.i1]; 548 } 549 550 rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); 551 rf.sideNormal2 = -rf.sideNormal1; 552 rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); 553 rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); 554 555 // Clip incident edge against extruded edge1 side edges. 556 b2ClipVertex clipPoints1[2]; 557 b2ClipVertex clipPoints2[2]; 558 int32 np; 559 560 // Clip to box side 1 561 np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); 562 563 if (np < b2_maxManifoldPoints) 564 { 565 return; 566 } 567 568 // Clip to negative box side 1 569 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); 570 571 if (np < b2_maxManifoldPoints) 572 { 573 return; 574 } 575 576 // Now clipPoints2 contains the clipped points. 577 if (primaryAxis.type == b2EPAxis::e_edgeA) 578 { 579 manifold->localNormal = rf.normal; 580 manifold->localPoint = rf.v1; 581 } 582 else 583 { 584 manifold->localNormal = polygonB->m_normals[rf.i1]; 585 manifold->localPoint = polygonB->m_vertices[rf.i1]; 586 } 587 588 int32 pointCount = 0; 589 for (int32 i = 0; i < b2_maxManifoldPoints; ++i) 590 { 591 float32 separation; 592 593 separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); 594 595 if (separation <= m_radius) 596 { 597 b2ManifoldPoint* cp = manifold->points + pointCount; 598 599 if (primaryAxis.type == b2EPAxis::e_edgeA) 600 { 601 cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); 602 cp->id = clipPoints2[i].id; 603 } 604 else 605 { 606 cp->localPoint = clipPoints2[i].v; 607 cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; 608 cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; 609 cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; 610 cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; 611 } 612 613 ++pointCount; 614 } 615 } 616 617 manifold->pointCount = pointCount; 618 } 619 620 b2EPAxis b2EPCollider::ComputeEdgeSeparation() 621 { 622 b2EPAxis axis; 623 axis.type = b2EPAxis::e_edgeA; 624 axis.index = m_front ? 0 : 1; 625 axis.separation = FLT_MAX; 626 627 for (int32 i = 0; i < m_polygonB.count; ++i) 628 { 629 float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); 630 if (s < axis.separation) 631 { 632 axis.separation = s; 633 } 634 } 635 636 return axis; 637 } 638 639 b2EPAxis b2EPCollider::ComputePolygonSeparation() 640 { 641 b2EPAxis axis; 642 axis.type = b2EPAxis::e_unknown; 643 axis.index = -1; 644 axis.separation = -FLT_MAX; 645 646 b2Vec2 perp(-m_normal.y, m_normal.x); 647 648 for (int32 i = 0; i < m_polygonB.count; ++i) 649 { 650 b2Vec2 n = -m_polygonB.normals[i]; 651 652 float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); 653 float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); 654 float32 s = b2Min(s1, s2); 655 656 if (s > m_radius) 657 { 658 // No collision 659 axis.type = b2EPAxis::e_edgeB; 660 axis.index = i; 661 axis.separation = s; 662 return axis; 663 } 664 665 // Adjacency 666 if (b2Dot(n, perp) >= 0.0f) 667 { 668 if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) 669 { 670 continue; 671 } 672 } 673 else 674 { 675 if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) 676 { 677 continue; 678 } 679 } 680 681 if (s > axis.separation) 682 { 683 axis.type = b2EPAxis::e_edgeB; 684 axis.index = i; 685 axis.separation = s; 686 } 687 } 688 689 return axis; 690 } 691 692 void b2CollideEdgeAndPolygon( b2Manifold* manifold, 693 const b2EdgeShape* edgeA, const b2Transform& xfA, 694 const b2PolygonShape* polygonB, const b2Transform& xfB) 695 { 696 b2EPCollider collider; 697 collider.Collide(manifold, edgeA, xfA, polygonB, xfB); 698 } 699
