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/b2Distance.h>
20#include <Box2D/Collision/Shapes/b2CircleShape.h>
21#include <Box2D/Collision/Shapes/b2EdgeShape.h>
22#include <Box2D/Collision/Shapes/b2ChainShape.h>
23#include <Box2D/Collision/Shapes/b2PolygonShape.h>
24
25// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
26int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
27
28void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
29{
30 switch (shape->GetType())
31 {
32 case b2Shape::e_circle:
33 {
34 const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
35 m_vertices = &circle->m_p;
36 m_count = 1;
37 m_radius = circle->m_radius;
38 }
39 break;
40
41 case b2Shape::e_polygon:
42 {
43 const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
44 m_vertices = polygon->m_vertices;
45 m_count = polygon->m_count;
46 m_radius = polygon->m_radius;
47 }
48 break;
49
50 case b2Shape::e_chain:
51 {
52 const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
53 b2Assert(0 <= index && index < chain->m_count);
54
55 m_buffer[0] = chain->m_vertices[index];
56 if (index + 1 < chain->m_count)
57 {
58 m_buffer[1] = chain->m_vertices[index + 1];
59 }
60 else
61 {
62 m_buffer[1] = chain->m_vertices[0];
63 }
64
65 m_vertices = m_buffer;
66 m_count = 2;
67 m_radius = chain->m_radius;
68 }
69 break;
70
71 case b2Shape::e_edge:
72 {
73 const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
74 m_vertices = &edge->m_vertex1;
75 m_count = 2;
76 m_radius = edge->m_radius;
77 }
78 break;
79
80 default:
81 b2Assert(false);
82 }
83}
84
85
86struct b2SimplexVertex
87{
88 b2Vec2 wA; // support point in proxyA
89 b2Vec2 wB; // support point in proxyB
90 b2Vec2 w; // wB - wA
91 float32 a; // barycentric coordinate for closest point
92 int32 indexA; // wA index
93 int32 indexB; // wB index
94};
95
96struct b2Simplex
97{
98 void ReadCache( const b2SimplexCache* cache,
99 const b2DistanceProxy* proxyA, const b2Transform& transformA,
100 const b2DistanceProxy* proxyB, const b2Transform& transformB)
101 {
102 b2Assert(cache->count <= 3);
103
104 // Copy data from cache.
105 m_count = cache->count;
106 b2SimplexVertex* vertices = &m_v1;
107 for (int32 i = 0; i < m_count; ++i)
108 {
109 b2SimplexVertex* v = vertices + i;
110 v->indexA = cache->indexA[i];
111 v->indexB = cache->indexB[i];
112 b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
113 b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
114 v->wA = b2Mul(transformA, wALocal);
115 v->wB = b2Mul(transformB, wBLocal);
116 v->w = v->wB - v->wA;
117 v->a = 0.0f;
118 }
119
120 // Compute the new simplex metric, if it is substantially different than
121 // old metric then flush the simplex.
122 if (m_count > 1)
123 {
124 float32 metric1 = cache->metric;
125 float32 metric2 = GetMetric();
126 if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
127 {
128 // Reset the simplex.
129 m_count = 0;
130 }
131 }
132
133 // If the cache is empty or invalid ...
134 if (m_count == 0)
135 {
136 b2SimplexVertex* v = vertices + 0;
137 v->indexA = 0;
138 v->indexB = 0;
139 b2Vec2 wALocal = proxyA->GetVertex(0);
140 b2Vec2 wBLocal = proxyB->GetVertex(0);
141 v->wA = b2Mul(transformA, wALocal);
142 v->wB = b2Mul(transformB, wBLocal);
143 v->w = v->wB - v->wA;
144 v->a = 1.0f;
145 m_count = 1;
146 }
147 }
148
149 void WriteCache(b2SimplexCache* cache) const
150 {
151 cache->metric = GetMetric();
152 cache->count = uint16(m_count);
153 const b2SimplexVertex* vertices = &m_v1;
154 for (int32 i = 0; i < m_count; ++i)
155 {
156 cache->indexA[i] = uint8(vertices[i].indexA);
157 cache->indexB[i] = uint8(vertices[i].indexB);
158 }
159 }
160
161 b2Vec2 GetSearchDirection() const
162 {
163 switch (m_count)
164 {
165 case 1:
166 return -m_v1.w;
167
168 case 2:
169 {
170 b2Vec2 e12 = m_v2.w - m_v1.w;
171 float32 sgn = b2Cross(e12, -m_v1.w);
172 if (sgn > 0.0f)
173 {
174 // Origin is left of e12.
175 return b2Cross(1.0f, e12);
176 }
177 else
178 {
179 // Origin is right of e12.
180 return b2Cross(e12, 1.0f);
181 }
182 }
183
184 default:
185 b2Assert(false);
186 return b2Vec2_zero;
187 }
188 }
189
190 b2Vec2 GetClosestPoint() const
191 {
192 switch (m_count)
193 {
194 case 0:
195 b2Assert(false);
196 return b2Vec2_zero;
197
198 case 1:
199 return m_v1.w;
200
201 case 2:
202 return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
203
204 case 3:
205 return b2Vec2_zero;
206
207 default:
208 b2Assert(false);
209 return b2Vec2_zero;
210 }
211 }
212
213 void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
214 {
215 switch (m_count)
216 {
217 case 0:
218 b2Assert(false);
219 break;
220
221 case 1:
222 *pA = m_v1.wA;
223 *pB = m_v1.wB;
224 break;
225
226 case 2:
227 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
228 *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
229 break;
230
231 case 3:
232 *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
233 *pB = *pA;
234 break;
235
236 default:
237 b2Assert(false);
238 break;
239 }
240 }
241
242 float32 GetMetric() const
243 {
244 switch (m_count)
245 {
246 case 0:
247 b2Assert(false);
248 return 0.0f;
249
250 case 1:
251 return 0.0f;
252
253 case 2:
254 return b2Distance(m_v1.w, m_v2.w);
255
256 case 3:
257 return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
258
259 default:
260 b2Assert(false);
261 return 0.0f;
262 }
263 }
264
265 void Solve2();
266 void Solve3();
267
268 b2SimplexVertex m_v1, m_v2, m_v3;
269 int32 m_count;
270};
271
272
273// Solve a line segment using barycentric coordinates.
274//
275// p = a1 * w1 + a2 * w2
276// a1 + a2 = 1
277//
278// The vector from the origin to the closest point on the line is
279// perpendicular to the line.
280// e12 = w2 - w1
281// dot(p, e) = 0
282// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
283//
284// 2-by-2 linear system
285// [1 1 ][a1] = [1]
286// [w1.e12 w2.e12][a2] = [0]
287//
288// Define
289// d12_1 = dot(w2, e12)
290// d12_2 = -dot(w1, e12)
291// d12 = d12_1 + d12_2
292//
293// Solution
294// a1 = d12_1 / d12
295// a2 = d12_2 / d12
296void b2Simplex::Solve2()
297{
298 b2Vec2 w1 = m_v1.w;
299 b2Vec2 w2 = m_v2.w;
300 b2Vec2 e12 = w2 - w1;
301
302 // w1 region
303 float32 d12_2 = -b2Dot(w1, e12);
304 if (d12_2 <= 0.0f)
305 {
306 // a2 <= 0, so we clamp it to 0
307 m_v1.a = 1.0f;
308 m_count = 1;
309 return;
310 }
311
312 // w2 region
313 float32 d12_1 = b2Dot(w2, e12);
314 if (d12_1 <= 0.0f)
315 {
316 // a1 <= 0, so we clamp it to 0
317 m_v2.a = 1.0f;
318 m_count = 1;
319 m_v1 = m_v2;
320 return;
321 }
322
323 // Must be in e12 region.
324 float32 inv_d12 = 1.0f / (d12_1 + d12_2);
325 m_v1.a = d12_1 * inv_d12;
326 m_v2.a = d12_2 * inv_d12;
327 m_count = 2;
328}
329
330// Possible regions:
331// - points[2]
332// - edge points[0]-points[2]
333// - edge points[1]-points[2]
334// - inside the triangle
335void b2Simplex::Solve3()
336{
337 b2Vec2 w1 = m_v1.w;
338 b2Vec2 w2 = m_v2.w;
339 b2Vec2 w3 = m_v3.w;
340
341 // Edge12
342 // [1 1 ][a1] = [1]
343 // [w1.e12 w2.e12][a2] = [0]
344 // a3 = 0
345 b2Vec2 e12 = w2 - w1;
346 float32 w1e12 = b2Dot(w1, e12);
347 float32 w2e12 = b2Dot(w2, e12);
348 float32 d12_1 = w2e12;
349 float32 d12_2 = -w1e12;
350
351 // Edge13
352 // [1 1 ][a1] = [1]
353 // [w1.e13 w3.e13][a3] = [0]
354 // a2 = 0
355 b2Vec2 e13 = w3 - w1;
356 float32 w1e13 = b2Dot(w1, e13);
357 float32 w3e13 = b2Dot(w3, e13);
358 float32 d13_1 = w3e13;
359 float32 d13_2 = -w1e13;
360
361 // Edge23
362 // [1 1 ][a2] = [1]
363 // [w2.e23 w3.e23][a3] = [0]
364 // a1 = 0
365 b2Vec2 e23 = w3 - w2;
366 float32 w2e23 = b2Dot(w2, e23);
367 float32 w3e23 = b2Dot(w3, e23);
368 float32 d23_1 = w3e23;
369 float32 d23_2 = -w2e23;
370
371 // Triangle123
372 float32 n123 = b2Cross(e12, e13);
373
374 float32 d123_1 = n123 * b2Cross(w2, w3);
375 float32 d123_2 = n123 * b2Cross(w3, w1);
376 float32 d123_3 = n123 * b2Cross(w1, w2);
377
378 // w1 region
379 if (d12_2 <= 0.0f && d13_2 <= 0.0f)
380 {
381 m_v1.a = 1.0f;
382 m_count = 1;
383 return;
384 }
385
386 // e12
387 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
388 {
389 float32 inv_d12 = 1.0f / (d12_1 + d12_2);
390 m_v1.a = d12_1 * inv_d12;
391 m_v2.a = d12_2 * inv_d12;
392 m_count = 2;
393 return;
394 }
395
396 // e13
397 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
398 {
399 float32 inv_d13 = 1.0f / (d13_1 + d13_2);
400 m_v1.a = d13_1 * inv_d13;
401 m_v3.a = d13_2 * inv_d13;
402 m_count = 2;
403 m_v2 = m_v3;
404 return;
405 }
406
407 // w2 region
408 if (d12_1 <= 0.0f && d23_2 <= 0.0f)
409 {
410 m_v2.a = 1.0f;
411 m_count = 1;
412 m_v1 = m_v2;
413 return;
414 }
415
416 // w3 region
417 if (d13_1 <= 0.0f && d23_1 <= 0.0f)
418 {
419 m_v3.a = 1.0f;
420 m_count = 1;
421 m_v1 = m_v3;
422 return;
423 }
424
425 // e23
426 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
427 {
428 float32 inv_d23 = 1.0f / (d23_1 + d23_2);
429 m_v2.a = d23_1 * inv_d23;
430 m_v3.a = d23_2 * inv_d23;
431 m_count = 2;
432 m_v1 = m_v3;
433 return;
434 }
435
436 // Must be in triangle123
437 float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
438 m_v1.a = d123_1 * inv_d123;
439 m_v2.a = d123_2 * inv_d123;
440 m_v3.a = d123_3 * inv_d123;
441 m_count = 3;
442}
443
444void b2Distance(b2DistanceOutput* output,
445 b2SimplexCache* cache,
446 const b2DistanceInput* input)
447{
448 ++b2_gjkCalls;
449
450 const b2DistanceProxy* proxyA = &input->proxyA;
451 const b2DistanceProxy* proxyB = &input->proxyB;
452
453 b2Transform transformA = input->transformA;
454 b2Transform transformB = input->transformB;
455
456 // Initialize the simplex.
457 b2Simplex simplex;
458 simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
459
460 // Get simplex vertices as an array.
461 b2SimplexVertex* vertices = &simplex.m_v1;
462 const int32 k_maxIters = 20;
463
464 // These store the vertices of the last simplex so that we
465 // can check for duplicates and prevent cycling.
466 int32 saveA[3], saveB[3];
467 int32 saveCount = 0;
468
469 float32 distanceSqr1 = b2_maxFloat;
470 float32 distanceSqr2 = distanceSqr1;
471
472 // Main iteration loop.
473 int32 iter = 0;
474 while (iter < k_maxIters)
475 {
476 // Copy simplex so we can identify duplicates.
477 saveCount = simplex.m_count;
478 for (int32 i = 0; i < saveCount; ++i)
479 {
480 saveA[i] = vertices[i].indexA;
481 saveB[i] = vertices[i].indexB;
482 }
483
484 switch (simplex.m_count)
485 {
486 case 1:
487 break;
488
489 case 2:
490 simplex.Solve2();
491 break;
492
493 case 3:
494 simplex.Solve3();
495 break;
496
497 default:
498 b2Assert(false);
499 }
500
501 // If we have 3 points, then the origin is in the corresponding triangle.
502 if (simplex.m_count == 3)
503 {
504 break;
505 }
506
507 // Compute closest point.
508 b2Vec2 p = simplex.GetClosestPoint();
509 distanceSqr2 = p.LengthSquared();
510
511 // Ensure progress
512 if (distanceSqr2 >= distanceSqr1)
513 {
514 //break;
515 }
516 distanceSqr1 = distanceSqr2;
517
518 // Get search direction.
519 b2Vec2 d = simplex.GetSearchDirection();
520
521 // Ensure the search direction is numerically fit.
522 if (d.LengthSquared() < b2_epsilon * b2_epsilon)
523 {
524 // The origin is probably contained by a line segment
525 // or triangle. Thus the shapes are overlapped.
526
527 // We can't return zero here even though there may be overlap.
528 // In case the simplex is a point, segment, or triangle it is difficult
529 // to determine if the origin is contained in the CSO or very close to it.
530 break;
531 }
532
533 // Compute a tentative new simplex vertex using support points.
534 b2SimplexVertex* vertex = vertices + simplex.m_count;
535 vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
536 vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
537 b2Vec2 wBLocal;
538 vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
539 vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
540 vertex->w = vertex->wB - vertex->wA;
541
542 // Iteration count is equated to the number of support point calls.
543 ++iter;
544 ++b2_gjkIters;
545
546 // Check for duplicate support points. This is the main termination criteria.
547 bool duplicate = false;
548 for (int32 i = 0; i < saveCount; ++i)
549 {
550 if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
551 {
552 duplicate = true;
553 break;
554 }
555 }
556
557 // If we found a duplicate support point we must exit to avoid cycling.
558 if (duplicate)
559 {
560 break;
561 }
562
563 // New vertex is ok and needed.
564 ++simplex.m_count;
565 }
566
567 b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
568
569 // Prepare output.
570 simplex.GetWitnessPoints(&output->pointA, &output->pointB);
571 output->distance = b2Distance(output->pointA, output->pointB);
572 output->iterations = iter;
573
574 // Cache the simplex.
575 simplex.WriteCache(cache);
576
577 // Apply radii if requested.
578 if (input->useRadii)
579 {
580 float32 rA = proxyA->m_radius;
581 float32 rB = proxyB->m_radius;
582
583 if (output->distance > rA + rB && output->distance > b2_epsilon)
584 {
585 // Shapes are still no overlapped.
586 // Move the witness points to the outer surface.
587 output->distance -= rA + rB;
588 b2Vec2 normal = output->pointB - output->pointA;
589 normal.Normalize();
590 output->pointA += rA * normal;
591 output->pointB -= rB * normal;
592 }
593 else
594 {
595 // Shapes are overlapped when radii are considered.
596 // Move the witness points to the middle.
597 b2Vec2 p = 0.5f * (output->pointA + output->pointB);
598 output->pointA = p;
599 output->pointB = p;
600 output->distance = 0.0f;
601 }
602 }
603}
604