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