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/b2Distance.h>
21#include <Box2D/Collision/b2TimeOfImpact.h>
22#include <Box2D/Collision/Shapes/b2CircleShape.h>
23#include <Box2D/Collision/Shapes/b2PolygonShape.h>
24#include <Box2D/Common/b2Timer.h>
25
26#include <stdio.h>
27
28float32 b2_toiTime, b2_toiMaxTime;
29int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
30int32 b2_toiRootIters, b2_toiMaxRootIters;
31
32//
33struct b2SeparationFunction
34{
35 enum Type
36 {
37 e_points,
38 e_faceA,
39 e_faceB
40 };
41
42 // TODO_ERIN might not need to return the separation
43
44 float32 Initialize(const b2SimplexCache* cache,
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;
69 float32 s = m_axis.Normalize();
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
244 const b2DistanceProxy* m_proxyA;
245 const b2DistanceProxy* m_proxyB;
246 b2Sweep m_sweepA, m_sweepB;
247 Type m_type;
248 b2Vec2 m_localPoint;
249 b2Vec2 m_axis;
250};
251
252// CCD via the local separating axis method. This seeks progression
253// by computing the largest time at which separation is maintained.
254void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
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!
312 output->state = b2TOIOutput::e_overlapped;
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.
326 b2SeparationFunction fcn;
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!
369 output->state = b2TOIOutput::e_separated;
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
481 b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
482
483 float32 time = timer.GetMilliseconds();
484 b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
485 b2_toiTime += time;
486}
487