1 /*
2 * Copyright (c) 2007 Erin Catto http://www.gphysics.com
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 "Shapes/b2Shape.h"
21
22 // This algorithm uses conservative advancement to compute the time of
23 // impact (TOI) of two shapes.
24 // Refs: Bullet, Young Kim
b2TimeOfImpact(const b2Shape * shape1,const b2Sweep & sweep1,const b2Shape * shape2,const b2Sweep & sweep2)25 float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
26 const b2Shape* shape2, const b2Sweep& sweep2)
27 {
28 float32 r1 = shape1->GetSweepRadius();
29 float32 r2 = shape2->GetSweepRadius();
30
31 b2Assert(sweep1.t0 == sweep2.t0);
32 b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
33
34 float32 t0 = sweep1.t0;
35 b2Vec2 v1 = sweep1.c - sweep1.c0;
36 b2Vec2 v2 = sweep2.c - sweep2.c0;
37 float32 omega1 = sweep1.a - sweep1.a0;
38 float32 omega2 = sweep2.a - sweep2.a0;
39
40 float32 alpha = 0.0f;
41
42 b2Vec2 p1, p2;
43 const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
44 int32 iter = 0;
45 b2Vec2 normal = b2Vec2_zero;
46 float32 distance = 0.0f;
47 float32 targetDistance = 0.0f;
48 for(;;)
49 {
50 float32 t = (1.0f - alpha) * t0 + alpha;
51 b2XForm xf1, xf2;
52 sweep1.GetXForm(&xf1, t);
53 sweep2.GetXForm(&xf2, t);
54
55 // Get the distance between shapes.
56 distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
57
58 if (iter == 0)
59 {
60 // Compute a reasonable target distance to give some breathing room
61 // for conservative advancement.
62 if (distance > 2.0f * b2_toiSlop)
63 {
64 targetDistance = 1.5f * b2_toiSlop;
65 }
66 else
67 {
68 targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
69 }
70 }
71
72 if (distance - targetDistance < 0.05f * b2_toiSlop || iter == k_maxIterations)
73 {
74 break;
75 }
76
77 normal = p2 - p1;
78 normal.Normalize();
79
80 // Compute upper bound on remaining movement.
81 float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
82 if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
83 {
84 alpha = 1.0f;
85 break;
86 }
87
88 // Get the conservative time increment. Don't advance all the way.
89 float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
90 //float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
91 float32 newAlpha = alpha + dAlpha;
92
93 // The shapes may be moving apart or a safe distance apart.
94 if (newAlpha < 0.0f || 1.0f < newAlpha)
95 {
96 alpha = 1.0f;
97 break;
98 }
99
100 // Ensure significant advancement.
101 if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
102 {
103 break;
104 }
105
106 alpha = newAlpha;
107
108 ++iter;
109 }
110
111 return alpha;
112 }
113