b2TimeOfImpact.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 #include <Box2D/Common/b2Timer.h>
25 
26 #include <stdio.h>
27 
31 
32 //
34 {
35  enum Type
36  {
40  };
41 
42  // TODO_ERIN might not need to return the separation
43 
45  const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
46  const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
47  float32 t1)
48  {
49  m_proxyA = proxyA;
50  m_proxyB = proxyB;
51  int32 count = cache->count;
52  b2Assert(0 < count && count < 3);
53 
54  m_sweepA = sweepA;
55  m_sweepB = sweepB;
56 
57  b2Transform xfA, xfB;
58  m_sweepA.GetTransform(&xfA, t1);
59  m_sweepB.GetTransform(&xfB, t1);
60 
61  if (count == 1)
62  {
63  m_type = e_points;
64  b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
65  b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
66  b2Vec2 pointA = b2Mul(xfA, localPointA);
67  b2Vec2 pointB = b2Mul(xfB, localPointB);
68  m_axis = pointB - pointA;
70  return s;
71  }
72  else if (cache->indexA[0] == cache->indexA[1])
73  {
74  // Two points on B and one on A.
75  m_type = e_faceB;
76  b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
77  b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
78 
79  m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
80  m_axis.Normalize();
81  b2Vec2 normal = b2Mul(xfB.q, m_axis);
82 
83  m_localPoint = 0.5f * (localPointB1 + localPointB2);
84  b2Vec2 pointB = b2Mul(xfB, m_localPoint);
85 
86  b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
87  b2Vec2 pointA = b2Mul(xfA, localPointA);
88 
89  float32 s = b2Dot(pointA - pointB, normal);
90  if (s < 0.0f)
91  {
92  m_axis = -m_axis;
93  s = -s;
94  }
95  return s;
96  }
97  else
98  {
99  // Two points on A and one or two points on B.
100  m_type = e_faceA;
101  b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
102  b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
103 
104  m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
105  m_axis.Normalize();
106  b2Vec2 normal = b2Mul(xfA.q, m_axis);
107 
108  m_localPoint = 0.5f * (localPointA1 + localPointA2);
109  b2Vec2 pointA = b2Mul(xfA, m_localPoint);
110 
111  b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
112  b2Vec2 pointB = b2Mul(xfB, localPointB);
113 
114  float32 s = b2Dot(pointB - pointA, normal);
115  if (s < 0.0f)
116  {
117  m_axis = -m_axis;
118  s = -s;
119  }
120  return s;
121  }
122  }
123 
124  //
125  float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
126  {
127  b2Transform xfA, xfB;
128  m_sweepA.GetTransform(&xfA, t);
129  m_sweepB.GetTransform(&xfB, t);
130 
131  switch (m_type)
132  {
133  case e_points:
134  {
135  b2Vec2 axisA = b2MulT(xfA.q, m_axis);
136  b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
137 
138  *indexA = m_proxyA->GetSupport(axisA);
139  *indexB = m_proxyB->GetSupport(axisB);
140 
141  b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
142  b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
143 
144  b2Vec2 pointA = b2Mul(xfA, localPointA);
145  b2Vec2 pointB = b2Mul(xfB, localPointB);
146 
147  float32 separation = b2Dot(pointB - pointA, m_axis);
148  return separation;
149  }
150 
151  case e_faceA:
152  {
153  b2Vec2 normal = b2Mul(xfA.q, m_axis);
154  b2Vec2 pointA = b2Mul(xfA, m_localPoint);
155 
156  b2Vec2 axisB = b2MulT(xfB.q, -normal);
157 
158  *indexA = -1;
159  *indexB = m_proxyB->GetSupport(axisB);
160 
161  b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
162  b2Vec2 pointB = b2Mul(xfB, localPointB);
163 
164  float32 separation = b2Dot(pointB - pointA, normal);
165  return separation;
166  }
167 
168  case e_faceB:
169  {
170  b2Vec2 normal = b2Mul(xfB.q, m_axis);
171  b2Vec2 pointB = b2Mul(xfB, m_localPoint);
172 
173  b2Vec2 axisA = b2MulT(xfA.q, -normal);
174 
175  *indexB = -1;
176  *indexA = m_proxyA->GetSupport(axisA);
177 
178  b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
179  b2Vec2 pointA = b2Mul(xfA, localPointA);
180 
181  float32 separation = b2Dot(pointA - pointB, normal);
182  return separation;
183  }
184 
185  default:
186  b2Assert(false);
187  *indexA = -1;
188  *indexB = -1;
189  return 0.0f;
190  }
191  }
192 
193  //
194  float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
195  {
196  b2Transform xfA, xfB;
197  m_sweepA.GetTransform(&xfA, t);
198  m_sweepB.GetTransform(&xfB, t);
199 
200  switch (m_type)
201  {
202  case e_points:
203  {
204  b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
205  b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
206 
207  b2Vec2 pointA = b2Mul(xfA, localPointA);
208  b2Vec2 pointB = b2Mul(xfB, localPointB);
209  float32 separation = b2Dot(pointB - pointA, m_axis);
210 
211  return separation;
212  }
213 
214  case e_faceA:
215  {
216  b2Vec2 normal = b2Mul(xfA.q, m_axis);
217  b2Vec2 pointA = b2Mul(xfA, m_localPoint);
218 
219  b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
220  b2Vec2 pointB = b2Mul(xfB, localPointB);
221 
222  float32 separation = b2Dot(pointB - pointA, normal);
223  return separation;
224  }
225 
226  case e_faceB:
227  {
228  b2Vec2 normal = b2Mul(xfB.q, m_axis);
229  b2Vec2 pointB = b2Mul(xfB, m_localPoint);
230 
231  b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
232  b2Vec2 pointA = b2Mul(xfA, localPointA);
233 
234  float32 separation = b2Dot(pointA - pointB, normal);
235  return separation;
236  }
237 
238  default:
239  b2Assert(false);
240  return 0.0f;
241  }
242  }
243 
250 };
251 
252 // CCD via the local separating axis method. This seeks progression
253 // by computing the largest time at which separation is maintained.
255 {
256  b2Timer timer;
257 
258  ++b2_toiCalls;
259 
260  output->state = b2TOIOutput::e_unknown;
261  output->t = input->tMax;
262 
263  const b2DistanceProxy* proxyA = &input->proxyA;
264  const b2DistanceProxy* proxyB = &input->proxyB;
265 
266  b2Sweep sweepA = input->sweepA;
267  b2Sweep sweepB = input->sweepB;
268 
269  // Large rotations can make the root finder fail, so we normalize the
270  // sweep angles.
271  sweepA.Normalize();
272  sweepB.Normalize();
273 
274  float32 tMax = input->tMax;
275 
276  float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
277  float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
278  float32 tolerance = 0.25f * b2_linearSlop;
279  b2Assert(target > tolerance);
280 
281  float32 t1 = 0.0f;
282  const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
283  int32 iter = 0;
284 
285  // Prepare input for distance query.
286  b2SimplexCache cache;
287  cache.count = 0;
288  b2DistanceInput distanceInput;
289  distanceInput.proxyA = input->proxyA;
290  distanceInput.proxyB = input->proxyB;
291  distanceInput.useRadii = false;
292 
293  // The outer loop progressively attempts to compute new separating axes.
294  // This loop terminates when an axis is repeated (no progress is made).
295  for(;;)
296  {
297  b2Transform xfA, xfB;
298  sweepA.GetTransform(&xfA, t1);
299  sweepB.GetTransform(&xfB, t1);
300 
301  // Get the distance between shapes. We can also use the results
302  // to get a separating axis.
303  distanceInput.transformA = xfA;
304  distanceInput.transformB = xfB;
305  b2DistanceOutput distanceOutput;
306  b2Distance(&distanceOutput, &cache, &distanceInput);
307 
308  // If the shapes are overlapped, we give up on continuous collision.
309  if (distanceOutput.distance <= 0.0f)
310  {
311  // Failure!
313  output->t = 0.0f;
314  break;
315  }
316 
317  if (distanceOutput.distance < target + tolerance)
318  {
319  // Victory!
320  output->state = b2TOIOutput::e_touching;
321  output->t = t1;
322  break;
323  }
324 
325  // Initialize the separating axis.
327  fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
328 #if 0
329  // Dump the curve seen by the root finder
330  {
331  const int32 N = 100;
332  float32 dx = 1.0f / N;
333  float32 xs[N+1];
334  float32 fs[N+1];
335 
336  float32 x = 0.0f;
337 
338  for (int32 i = 0; i <= N; ++i)
339  {
340  sweepA.GetTransform(&xfA, x);
341  sweepB.GetTransform(&xfB, x);
342  float32 f = fcn.Evaluate(xfA, xfB) - target;
343 
344  printf("%g %g\n", x, f);
345 
346  xs[i] = x;
347  fs[i] = f;
348 
349  x += dx;
350  }
351  }
352 #endif
353 
354  // Compute the TOI on the separating axis. We do this by successively
355  // resolving the deepest point. This loop is bounded by the number of vertices.
356  bool done = false;
357  float32 t2 = tMax;
358  int32 pushBackIter = 0;
359  for (;;)
360  {
361  // Find the deepest point at t2. Store the witness point indices.
362  int32 indexA, indexB;
363  float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
364 
365  // Is the final configuration separated?
366  if (s2 > target + tolerance)
367  {
368  // Victory!
370  output->t = tMax;
371  done = true;
372  break;
373  }
374 
375  // Has the separation reached tolerance?
376  if (s2 > target - tolerance)
377  {
378  // Advance the sweeps
379  t1 = t2;
380  break;
381  }
382 
383  // Compute the initial separation of the witness points.
384  float32 s1 = fcn.Evaluate(indexA, indexB, t1);
385 
386  // Check for initial overlap. This might happen if the root finder
387  // runs out of iterations.
388  if (s1 < target - tolerance)
389  {
390  output->state = b2TOIOutput::e_failed;
391  output->t = t1;
392  done = true;
393  break;
394  }
395 
396  // Check for touching
397  if (s1 <= target + tolerance)
398  {
399  // Victory! t1 should hold the TOI (could be 0.0).
400  output->state = b2TOIOutput::e_touching;
401  output->t = t1;
402  done = true;
403  break;
404  }
405 
406  // Compute 1D root of: f(x) - target = 0
407  int32 rootIterCount = 0;
408  float32 a1 = t1, a2 = t2;
409  for (;;)
410  {
411  // Use a mix of the secant rule and bisection.
412  float32 t;
413  if (rootIterCount & 1)
414  {
415  // Secant rule to improve convergence.
416  t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
417  }
418  else
419  {
420  // Bisection to guarantee progress.
421  t = 0.5f * (a1 + a2);
422  }
423 
424  ++rootIterCount;
425  ++b2_toiRootIters;
426 
427  float32 s = fcn.Evaluate(indexA, indexB, t);
428 
429  if (b2Abs(s - target) < tolerance)
430  {
431  // t2 holds a tentative value for t1
432  t2 = t;
433  break;
434  }
435 
436  // Ensure we continue to bracket the root.
437  if (s > target)
438  {
439  a1 = t;
440  s1 = s;
441  }
442  else
443  {
444  a2 = t;
445  s2 = s;
446  }
447 
448  if (rootIterCount == 50)
449  {
450  break;
451  }
452  }
453 
454  b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
455 
456  ++pushBackIter;
457 
458  if (pushBackIter == b2_maxPolygonVertices)
459  {
460  break;
461  }
462  }
463 
464  ++iter;
465  ++b2_toiIters;
466 
467  if (done)
468  {
469  break;
470  }
471 
472  if (iter == k_maxIterations)
473  {
474  // Root finder got stuck. Semi-victory.
475  output->state = b2TOIOutput::e_failed;
476  output->t = t1;
477  break;
478  }
479  }
480 
482 
483  float32 time = timer.GetMilliseconds();
485  b2_toiTime += time;
486 }
int timer
b2Sweep sweepB
float32 b2_toiTime
#define printf
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:405
int32 b2_toiCalls
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:432
int32 b2_toiRootIters
s1
#define b2_linearSlop
Definition: b2Settings.h:68
const b2DistanceProxy * m_proxyA
b2Rot q
Definition: b2Math.h:372
void b2TimeOfImpact(b2TOIOutput *output, const b2TOIInput *input)
uint8 indexA[3]
vertices on shape A
Definition: b2Distance.h:61
GLenum GLenum GLenum input
GLdouble GLdouble t
float32 GetMilliseconds() const
Get the time since construction or the last reset.
Definition: b2Timer.cpp:96
void GetTransform(b2Transform *xfb, float32 beta) const
Definition: b2Math.h:691
float32 tMax
b2DistanceProxy proxyA
Definition: b2Distance.h:70
Input parameters for b2TimeOfImpact.
T b2Max(T a, T b)
Definition: b2Math.h:642
double a2
A 2D column vector.
Definition: b2Math.h:52
b2DistanceProxy proxyA
#define N(a, b, c)
const b2Vec2 & GetVertex(int32 index) const
Get a vertex by index. Used by b2Distance.
Definition: b2Distance.h:101
s2
GLdouble s
signed int int32
Definition: b2Settings.h:31
uint16 count
Definition: b2Distance.h:60
b2DistanceProxy proxyB
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 b2_toiMaxRootIters
GLint GLint GLint GLint GLint x
b2Transform transformA
Definition: b2Distance.h:72
b2Sweep sweepA
b2Transform transformB
Definition: b2Distance.h:73
int32 GetSupport(const b2Vec2 &d) const
Get the supporting vertex index in the given direction.
Definition: b2Distance.h:107
float32 Initialize(const b2SimplexCache *cache, const b2DistanceProxy *proxyA, const b2Sweep &sweepA, const b2DistanceProxy *proxyB, const b2Sweep &sweepB, float32 t1)
#define b2_maxPolygonVertices
Definition: b2Settings.h:54
GLenum target
b2Vec2 b2MulT(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:439
float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
uint8 indexB[3]
vertices on shape B
Definition: b2Distance.h:62
void b2Distance(b2DistanceOutput *output, b2SimplexCache *cache, const b2DistanceInput *input)
Definition: b2Distance.cpp:444
const b2DistanceProxy * m_proxyB
Output for b2Distance.
Definition: b2Distance.h:78
void Normalize()
Normalize the angles.
Definition: b2Math.h:711
#define b2Assert(A)
Definition: b2Settings.h:27
float32 distance
Definition: b2Distance.h:82
int32 b2_toiMaxIters
T b2Abs(T a)
Definition: b2Math.h:615
float32 m_radius
Definition: b2Distance.h:52
GLuint GLuint GLsizei count
float32 FindMinSeparation(int32 *indexA, int32 *indexB, float32 t) const
double a1
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:113
int32 b2_toiIters
b2DistanceProxy proxyB
Definition: b2Distance.h:71
float32 b2_toiMaxTime
float float32
Definition: b2Settings.h:35
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f


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