b2Distance.cpp
Go to the documentation of this file.
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 
24 
25 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
27 
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 
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 
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 
96 struct 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 
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 
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 
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;
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
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
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 
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 
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 }
d
const GLdouble * v
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:405
const b2Vec2 b2Vec2_zero(0.0f, 0.0f)
Useful constant.
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:432
b2Vec2 * m_vertices
The vertices. Owned by this class.
Definition: b2ChainShape.h:86
void Set(const b2Shape *shape, int32 index)
Definition: b2Distance.cpp:28
void ReadCache(const b2SimplexCache *cache, const b2DistanceProxy *proxyA, const b2Transform &transformA, const b2DistanceProxy *proxyB, const b2Transform &transformB)
Definition: b2Distance.cpp:98
b2Vec2 GetSearchDirection() const
Definition: b2Distance.cpp:161
unsigned short uint16
Definition: b2Settings.h:33
b2Rot q
Definition: b2Math.h:372
b2SimplexVertex m_v1
Definition: b2Distance.cpp:268
uint8 indexA[3]
vertices on shape A
Definition: b2Distance.h:61
GLenum GLenum GLenum input
void GetWitnessPoints(b2Vec2 *pA, b2Vec2 *pB) const
Definition: b2Distance.cpp:213
#define b2_epsilon
Definition: b2Settings.h:39
float32 LengthSquared() const
Definition: b2Math.h:107
b2DistanceProxy proxyA
Definition: b2Distance.h:70
int32 iterations
number of GJK iterations used
Definition: b2Distance.h:83
T b2Max(T a, T b)
Definition: b2Math.h:642
float32 m_radius
Definition: b2Shape.h:93
int32 b2_gjkMaxIters
Definition: b2Distance.cpp:26
int32 b2_gjkCalls
Definition: b2Distance.cpp:26
A circle shape.
Definition: b2CircleShape.h:25
A 2D column vector.
Definition: b2Math.h:52
b2Vec2 pointB
closest point on shapeB
Definition: b2Distance.h:81
const b2Vec2 & GetVertex(int32 index) const
Get a vertex by index. Used by b2Distance.
Definition: b2Distance.h:101
b2Vec2 m_p
Position.
Definition: b2CircleShape.h:62
b2Vec2 pointA
closest point on shapeA
Definition: b2Distance.h:80
signed int int32
Definition: b2Settings.h:31
uint16 count
Definition: b2Distance.h:60
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:411
int32 m_count
Definition: b2Distance.cpp:269
b2Vec2 GetClosestPoint() const
Definition: b2Distance.cpp:190
b2Vec2 m_buffer[2]
Definition: b2Distance.h:49
Type GetType() const
Definition: b2Shape.h:96
b2SimplexVertex m_v3
Definition: b2Distance.cpp:268
b2Transform transformA
Definition: b2Distance.h:72
b2Transform transformB
Definition: b2Distance.h:73
GLfloat GLfloat p
b2Vec2 m_vertices[b2_maxPolygonVertices]
int32 GetSupport(const b2Vec2 &d) const
Get the supporting vertex index in the given direction.
Definition: b2Distance.h:107
float32 GetMetric() const
Definition: b2Distance.cpp:242
const b2Vec2 * m_vertices
Definition: b2Distance.h:50
void WriteCache(b2SimplexCache *cache) const
Definition: b2Distance.cpp:149
b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:439
unsigned char uint8
Definition: b2Settings.h:32
float32 metric
length or area
Definition: b2Distance.h:59
uint8 indexB[3]
vertices on shape B
Definition: b2Distance.h:62
void b2Distance(b2DistanceOutput *output, b2SimplexCache *cache, const b2DistanceInput *input)
Definition: b2Distance.cpp:444
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
GLuint index
void Solve3()
Definition: b2Distance.cpp:335
Output for b2Distance.
Definition: b2Distance.h:78
#define b2Assert(A)
Definition: b2Settings.h:27
b2Vec2 m_vertex1
These are the edge vertices.
Definition: b2EdgeShape.h:55
float32 distance
Definition: b2Distance.h:82
float32 m_radius
Definition: b2Distance.h:52
#define b2_maxFloat
Definition: b2Settings.h:38
int32 m_count
The vertex count.
Definition: b2ChainShape.h:89
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:113
int32 b2_gjkIters
Definition: b2Distance.cpp:26
void Solve2()
Definition: b2Distance.cpp:296
b2DistanceProxy proxyB
Definition: b2Distance.h:71
float float32
Definition: b2Settings.h:35
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f


mvsim
Author(s):
autogenerated on Fri May 7 2021 03:05:51