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.
b2CollideEdgeAndCircle(b2Manifold * manifold,const b2EdgeShape * edgeA,const b2Transform & xfA,const b2CircleShape * circleB,const b2Transform & xfB)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
Collide(b2Manifold * manifold,const b2EdgeShape * edgeA,const b2Transform & xfA,const b2PolygonShape * polygonB,const b2Transform & xfB)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
ComputeEdgeSeparation()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
ComputePolygonSeparation()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
b2CollideEdgeAndPolygon(b2Manifold * manifold,const b2EdgeShape * edgeA,const b2Transform & xfA,const b2PolygonShape * polygonB,const b2Transform & xfB)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