00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <Box2D/Collision/b2Collision.h>
00020 #include <Box2D/Collision/Shapes/b2CircleShape.h>
00021 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
00022 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00023
00024
00025
00026
00027 void b2CollideEdgeAndCircle(b2Manifold* manifold,
00028 const b2EdgeShape* edgeA, const b2Transform& xfA,
00029 const b2CircleShape* circleB, const b2Transform& xfB)
00030 {
00031 manifold->pointCount = 0;
00032
00033
00034 b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
00035
00036 b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
00037 b2Vec2 e = B - A;
00038
00039
00040 float32 u = b2Dot(e, B - Q);
00041 float32 v = b2Dot(e, Q - A);
00042
00043 float32 radius = edgeA->m_radius + circleB->m_radius;
00044
00045 b2ContactFeature cf;
00046 cf.indexB = 0;
00047 cf.typeB = b2ContactFeature::e_vertex;
00048
00049
00050 if (v <= 0.0f)
00051 {
00052 b2Vec2 P = A;
00053 b2Vec2 d = Q - P;
00054 float32 dd = b2Dot(d, d);
00055 if (dd > radius * radius)
00056 {
00057 return;
00058 }
00059
00060
00061 if (edgeA->m_hasVertex0)
00062 {
00063 b2Vec2 A1 = edgeA->m_vertex0;
00064 b2Vec2 B1 = A;
00065 b2Vec2 e1 = B1 - A1;
00066 float32 u1 = b2Dot(e1, B1 - Q);
00067
00068
00069 if (u1 > 0.0f)
00070 {
00071 return;
00072 }
00073 }
00074
00075 cf.indexA = 0;
00076 cf.typeA = b2ContactFeature::e_vertex;
00077 manifold->pointCount = 1;
00078 manifold->type = b2Manifold::e_circles;
00079 manifold->localNormal.SetZero();
00080 manifold->localPoint = P;
00081 manifold->points[0].id.key = 0;
00082 manifold->points[0].id.cf = cf;
00083 manifold->points[0].localPoint = circleB->m_p;
00084 return;
00085 }
00086
00087
00088 if (u <= 0.0f)
00089 {
00090 b2Vec2 P = B;
00091 b2Vec2 d = Q - P;
00092 float32 dd = b2Dot(d, d);
00093 if (dd > radius * radius)
00094 {
00095 return;
00096 }
00097
00098
00099 if (edgeA->m_hasVertex3)
00100 {
00101 b2Vec2 B2 = edgeA->m_vertex3;
00102 b2Vec2 A2 = B;
00103 b2Vec2 e2 = B2 - A2;
00104 float32 v2 = b2Dot(e2, Q - A2);
00105
00106
00107 if (v2 > 0.0f)
00108 {
00109 return;
00110 }
00111 }
00112
00113 cf.indexA = 1;
00114 cf.typeA = b2ContactFeature::e_vertex;
00115 manifold->pointCount = 1;
00116 manifold->type = b2Manifold::e_circles;
00117 manifold->localNormal.SetZero();
00118 manifold->localPoint = P;
00119 manifold->points[0].id.key = 0;
00120 manifold->points[0].id.cf = cf;
00121 manifold->points[0].localPoint = circleB->m_p;
00122 return;
00123 }
00124
00125
00126 float32 den = b2Dot(e, e);
00127 b2Assert(den > 0.0f);
00128 b2Vec2 P = (1.0f / den) * (u * A + v * B);
00129 b2Vec2 d = Q - P;
00130 float32 dd = b2Dot(d, d);
00131 if (dd > radius * radius)
00132 {
00133 return;
00134 }
00135
00136 b2Vec2 n(-e.y, e.x);
00137 if (b2Dot(n, Q - A) < 0.0f)
00138 {
00139 n.Set(-n.x, -n.y);
00140 }
00141 n.Normalize();
00142
00143 cf.indexA = 0;
00144 cf.typeA = b2ContactFeature::e_face;
00145 manifold->pointCount = 1;
00146 manifold->type = b2Manifold::e_faceA;
00147 manifold->localNormal = n;
00148 manifold->localPoint = A;
00149 manifold->points[0].id.key = 0;
00150 manifold->points[0].id.cf = cf;
00151 manifold->points[0].localPoint = circleB->m_p;
00152 }
00153
00154
00155 struct b2EPAxis
00156 {
00157 enum Type
00158 {
00159 e_unknown,
00160 e_edgeA,
00161 e_edgeB
00162 };
00163
00164 Type type;
00165 int32 index;
00166 float32 separation;
00167 };
00168
00169
00170 struct b2TempPolygon
00171 {
00172 b2Vec2 vertices[b2_maxPolygonVertices];
00173 b2Vec2 normals[b2_maxPolygonVertices];
00174 int32 count;
00175 };
00176
00177
00178 struct b2ReferenceFace
00179 {
00180 int32 i1, i2;
00181
00182 b2Vec2 v1, v2;
00183
00184 b2Vec2 normal;
00185
00186 b2Vec2 sideNormal1;
00187 float32 sideOffset1;
00188
00189 b2Vec2 sideNormal2;
00190 float32 sideOffset2;
00191 };
00192
00193
00194 struct b2EPCollider
00195 {
00196 void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
00197 const b2PolygonShape* polygonB, const b2Transform& xfB);
00198 b2EPAxis ComputeEdgeSeparation();
00199 b2EPAxis ComputePolygonSeparation();
00200
00201 enum VertexType
00202 {
00203 e_isolated,
00204 e_concave,
00205 e_convex
00206 };
00207
00208 b2TempPolygon m_polygonB;
00209
00210 b2Transform m_xf;
00211 b2Vec2 m_centroidB;
00212 b2Vec2 m_v0, m_v1, m_v2, m_v3;
00213 b2Vec2 m_normal0, m_normal1, m_normal2;
00214 b2Vec2 m_normal;
00215 VertexType m_type1, m_type2;
00216 b2Vec2 m_lowerLimit, m_upperLimit;
00217 float32 m_radius;
00218 bool m_front;
00219 };
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230 void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
00231 const b2PolygonShape* polygonB, const b2Transform& xfB)
00232 {
00233 m_xf = b2MulT(xfA, xfB);
00234
00235 m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
00236
00237 m_v0 = edgeA->m_vertex0;
00238 m_v1 = edgeA->m_vertex1;
00239 m_v2 = edgeA->m_vertex2;
00240 m_v3 = edgeA->m_vertex3;
00241
00242 bool hasVertex0 = edgeA->m_hasVertex0;
00243 bool hasVertex3 = edgeA->m_hasVertex3;
00244
00245 b2Vec2 edge1 = m_v2 - m_v1;
00246 edge1.Normalize();
00247 m_normal1.Set(edge1.y, -edge1.x);
00248 float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
00249 float32 offset0 = 0.0f, offset2 = 0.0f;
00250 bool convex1 = false, convex2 = false;
00251
00252
00253 if (hasVertex0)
00254 {
00255 b2Vec2 edge0 = m_v1 - m_v0;
00256 edge0.Normalize();
00257 m_normal0.Set(edge0.y, -edge0.x);
00258 convex1 = b2Cross(edge0, edge1) >= 0.0f;
00259 offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
00260 }
00261
00262
00263 if (hasVertex3)
00264 {
00265 b2Vec2 edge2 = m_v3 - m_v2;
00266 edge2.Normalize();
00267 m_normal2.Set(edge2.y, -edge2.x);
00268 convex2 = b2Cross(edge1, edge2) > 0.0f;
00269 offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
00270 }
00271
00272
00273 if (hasVertex0 && hasVertex3)
00274 {
00275 if (convex1 && convex2)
00276 {
00277 m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
00278 if (m_front)
00279 {
00280 m_normal = m_normal1;
00281 m_lowerLimit = m_normal0;
00282 m_upperLimit = m_normal2;
00283 }
00284 else
00285 {
00286 m_normal = -m_normal1;
00287 m_lowerLimit = -m_normal1;
00288 m_upperLimit = -m_normal1;
00289 }
00290 }
00291 else if (convex1)
00292 {
00293 m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
00294 if (m_front)
00295 {
00296 m_normal = m_normal1;
00297 m_lowerLimit = m_normal0;
00298 m_upperLimit = m_normal1;
00299 }
00300 else
00301 {
00302 m_normal = -m_normal1;
00303 m_lowerLimit = -m_normal2;
00304 m_upperLimit = -m_normal1;
00305 }
00306 }
00307 else if (convex2)
00308 {
00309 m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
00310 if (m_front)
00311 {
00312 m_normal = m_normal1;
00313 m_lowerLimit = m_normal1;
00314 m_upperLimit = m_normal2;
00315 }
00316 else
00317 {
00318 m_normal = -m_normal1;
00319 m_lowerLimit = -m_normal1;
00320 m_upperLimit = -m_normal0;
00321 }
00322 }
00323 else
00324 {
00325 m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
00326 if (m_front)
00327 {
00328 m_normal = m_normal1;
00329 m_lowerLimit = m_normal1;
00330 m_upperLimit = m_normal1;
00331 }
00332 else
00333 {
00334 m_normal = -m_normal1;
00335 m_lowerLimit = -m_normal2;
00336 m_upperLimit = -m_normal0;
00337 }
00338 }
00339 }
00340 else if (hasVertex0)
00341 {
00342 if (convex1)
00343 {
00344 m_front = offset0 >= 0.0f || offset1 >= 0.0f;
00345 if (m_front)
00346 {
00347 m_normal = m_normal1;
00348 m_lowerLimit = m_normal0;
00349 m_upperLimit = -m_normal1;
00350 }
00351 else
00352 {
00353 m_normal = -m_normal1;
00354 m_lowerLimit = m_normal1;
00355 m_upperLimit = -m_normal1;
00356 }
00357 }
00358 else
00359 {
00360 m_front = offset0 >= 0.0f && offset1 >= 0.0f;
00361 if (m_front)
00362 {
00363 m_normal = m_normal1;
00364 m_lowerLimit = m_normal1;
00365 m_upperLimit = -m_normal1;
00366 }
00367 else
00368 {
00369 m_normal = -m_normal1;
00370 m_lowerLimit = m_normal1;
00371 m_upperLimit = -m_normal0;
00372 }
00373 }
00374 }
00375 else if (hasVertex3)
00376 {
00377 if (convex2)
00378 {
00379 m_front = offset1 >= 0.0f || offset2 >= 0.0f;
00380 if (m_front)
00381 {
00382 m_normal = m_normal1;
00383 m_lowerLimit = -m_normal1;
00384 m_upperLimit = m_normal2;
00385 }
00386 else
00387 {
00388 m_normal = -m_normal1;
00389 m_lowerLimit = -m_normal1;
00390 m_upperLimit = m_normal1;
00391 }
00392 }
00393 else
00394 {
00395 m_front = offset1 >= 0.0f && offset2 >= 0.0f;
00396 if (m_front)
00397 {
00398 m_normal = m_normal1;
00399 m_lowerLimit = -m_normal1;
00400 m_upperLimit = m_normal1;
00401 }
00402 else
00403 {
00404 m_normal = -m_normal1;
00405 m_lowerLimit = -m_normal2;
00406 m_upperLimit = m_normal1;
00407 }
00408 }
00409 }
00410 else
00411 {
00412 m_front = offset1 >= 0.0f;
00413 if (m_front)
00414 {
00415 m_normal = m_normal1;
00416 m_lowerLimit = -m_normal1;
00417 m_upperLimit = -m_normal1;
00418 }
00419 else
00420 {
00421 m_normal = -m_normal1;
00422 m_lowerLimit = m_normal1;
00423 m_upperLimit = m_normal1;
00424 }
00425 }
00426
00427
00428 m_polygonB.count = polygonB->m_count;
00429 for (int32 i = 0; i < polygonB->m_count; ++i)
00430 {
00431 m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
00432 m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
00433 }
00434
00435 m_radius = 2.0f * b2_polygonRadius;
00436
00437 manifold->pointCount = 0;
00438
00439 b2EPAxis edgeAxis = ComputeEdgeSeparation();
00440
00441
00442 if (edgeAxis.type == b2EPAxis::e_unknown)
00443 {
00444 return;
00445 }
00446
00447 if (edgeAxis.separation > m_radius)
00448 {
00449 return;
00450 }
00451
00452 b2EPAxis polygonAxis = ComputePolygonSeparation();
00453 if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
00454 {
00455 return;
00456 }
00457
00458
00459 const float32 k_relativeTol = 0.98f;
00460 const float32 k_absoluteTol = 0.001f;
00461
00462 b2EPAxis primaryAxis;
00463 if (polygonAxis.type == b2EPAxis::e_unknown)
00464 {
00465 primaryAxis = edgeAxis;
00466 }
00467 else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
00468 {
00469 primaryAxis = polygonAxis;
00470 }
00471 else
00472 {
00473 primaryAxis = edgeAxis;
00474 }
00475
00476 b2ClipVertex ie[2];
00477 b2ReferenceFace rf;
00478 if (primaryAxis.type == b2EPAxis::e_edgeA)
00479 {
00480 manifold->type = b2Manifold::e_faceA;
00481
00482
00483 int32 bestIndex = 0;
00484 float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
00485 for (int32 i = 1; i < m_polygonB.count; ++i)
00486 {
00487 float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
00488 if (value < bestValue)
00489 {
00490 bestValue = value;
00491 bestIndex = i;
00492 }
00493 }
00494
00495 int32 i1 = bestIndex;
00496 int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
00497
00498 ie[0].v = m_polygonB.vertices[i1];
00499 ie[0].id.cf.indexA = 0;
00500 ie[0].id.cf.indexB = static_cast<uint8>(i1);
00501 ie[0].id.cf.typeA = b2ContactFeature::e_face;
00502 ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
00503
00504 ie[1].v = m_polygonB.vertices[i2];
00505 ie[1].id.cf.indexA = 0;
00506 ie[1].id.cf.indexB = static_cast<uint8>(i2);
00507 ie[1].id.cf.typeA = b2ContactFeature::e_face;
00508 ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
00509
00510 if (m_front)
00511 {
00512 rf.i1 = 0;
00513 rf.i2 = 1;
00514 rf.v1 = m_v1;
00515 rf.v2 = m_v2;
00516 rf.normal = m_normal1;
00517 }
00518 else
00519 {
00520 rf.i1 = 1;
00521 rf.i2 = 0;
00522 rf.v1 = m_v2;
00523 rf.v2 = m_v1;
00524 rf.normal = -m_normal1;
00525 }
00526 }
00527 else
00528 {
00529 manifold->type = b2Manifold::e_faceB;
00530
00531 ie[0].v = m_v1;
00532 ie[0].id.cf.indexA = 0;
00533 ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
00534 ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
00535 ie[0].id.cf.typeB = b2ContactFeature::e_face;
00536
00537 ie[1].v = m_v2;
00538 ie[1].id.cf.indexA = 0;
00539 ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
00540 ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
00541 ie[1].id.cf.typeB = b2ContactFeature::e_face;
00542
00543 rf.i1 = primaryAxis.index;
00544 rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
00545 rf.v1 = m_polygonB.vertices[rf.i1];
00546 rf.v2 = m_polygonB.vertices[rf.i2];
00547 rf.normal = m_polygonB.normals[rf.i1];
00548 }
00549
00550 rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
00551 rf.sideNormal2 = -rf.sideNormal1;
00552 rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
00553 rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
00554
00555
00556 b2ClipVertex clipPoints1[2];
00557 b2ClipVertex clipPoints2[2];
00558 int32 np;
00559
00560
00561 np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
00562
00563 if (np < b2_maxManifoldPoints)
00564 {
00565 return;
00566 }
00567
00568
00569 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
00570
00571 if (np < b2_maxManifoldPoints)
00572 {
00573 return;
00574 }
00575
00576
00577 if (primaryAxis.type == b2EPAxis::e_edgeA)
00578 {
00579 manifold->localNormal = rf.normal;
00580 manifold->localPoint = rf.v1;
00581 }
00582 else
00583 {
00584 manifold->localNormal = polygonB->m_normals[rf.i1];
00585 manifold->localPoint = polygonB->m_vertices[rf.i1];
00586 }
00587
00588 int32 pointCount = 0;
00589 for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
00590 {
00591 float32 separation;
00592
00593 separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
00594
00595 if (separation <= m_radius)
00596 {
00597 b2ManifoldPoint* cp = manifold->points + pointCount;
00598
00599 if (primaryAxis.type == b2EPAxis::e_edgeA)
00600 {
00601 cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
00602 cp->id = clipPoints2[i].id;
00603 }
00604 else
00605 {
00606 cp->localPoint = clipPoints2[i].v;
00607 cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
00608 cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
00609 cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
00610 cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
00611 }
00612
00613 ++pointCount;
00614 }
00615 }
00616
00617 manifold->pointCount = pointCount;
00618 }
00619
00620 b2EPAxis b2EPCollider::ComputeEdgeSeparation()
00621 {
00622 b2EPAxis axis;
00623 axis.type = b2EPAxis::e_edgeA;
00624 axis.index = m_front ? 0 : 1;
00625 axis.separation = FLT_MAX;
00626
00627 for (int32 i = 0; i < m_polygonB.count; ++i)
00628 {
00629 float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
00630 if (s < axis.separation)
00631 {
00632 axis.separation = s;
00633 }
00634 }
00635
00636 return axis;
00637 }
00638
00639 b2EPAxis b2EPCollider::ComputePolygonSeparation()
00640 {
00641 b2EPAxis axis;
00642 axis.type = b2EPAxis::e_unknown;
00643 axis.index = -1;
00644 axis.separation = -FLT_MAX;
00645
00646 b2Vec2 perp(-m_normal.y, m_normal.x);
00647
00648 for (int32 i = 0; i < m_polygonB.count; ++i)
00649 {
00650 b2Vec2 n = -m_polygonB.normals[i];
00651
00652 float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
00653 float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
00654 float32 s = b2Min(s1, s2);
00655
00656 if (s > m_radius)
00657 {
00658
00659 axis.type = b2EPAxis::e_edgeB;
00660 axis.index = i;
00661 axis.separation = s;
00662 return axis;
00663 }
00664
00665
00666 if (b2Dot(n, perp) >= 0.0f)
00667 {
00668 if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
00669 {
00670 continue;
00671 }
00672 }
00673 else
00674 {
00675 if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
00676 {
00677 continue;
00678 }
00679 }
00680
00681 if (s > axis.separation)
00682 {
00683 axis.type = b2EPAxis::e_edgeB;
00684 axis.index = i;
00685 axis.separation = s;
00686 }
00687 }
00688
00689 return axis;
00690 }
00691
00692 void b2CollideEdgeAndPolygon( b2Manifold* manifold,
00693 const b2EdgeShape* edgeA, const b2Transform& xfA,
00694 const b2PolygonShape* polygonB, const b2Transform& xfB)
00695 {
00696 b2EPCollider collider;
00697 collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
00698 }