-
-
Notifications
You must be signed in to change notification settings - Fork 540
/
TimeOfImpact.java
249 lines (208 loc) · 8.36 KB
/
TimeOfImpact.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
/*
* FXGL - JavaFX Game Library. The MIT License (MIT).
* Copyright (c) AlmasB (almaslvl@gmail.com).
* See LICENSE for details.
*/
package com.almasb.fxgl.physics.box2d.collision;
import com.almasb.fxgl.core.math.FXGLMath;
import com.almasb.fxgl.physics.box2d.collision.Distance.SimplexCache;
import com.almasb.fxgl.physics.box2d.common.JBoxSettings;
import com.almasb.fxgl.physics.box2d.common.Sweep;
import com.almasb.fxgl.physics.box2d.common.Transform;
import com.almasb.fxgl.physics.box2d.pooling.IWorldPool;
/**
* Class used for computing the time of impact. This class should not be constructed usually, just
* retrieve from the pool.
*
* @author daniel
*/
public class TimeOfImpact {
private static final int MAX_ITERATIONS = 1000;
/**
* Input parameters for TOI
*
* @author Daniel Murphy
*/
public static class TOIInput {
public final DistanceProxy proxyA = new DistanceProxy();
public final DistanceProxy proxyB = new DistanceProxy();
public final Sweep sweepA = new Sweep();
public final Sweep sweepB = new Sweep();
/**
* defines sweep interval [0, tMax]
*/
public float tMax;
}
public enum TOIOutputState {
UNKNOWN, FAILED, OVERLAPPED, TOUCHING, SEPARATED
}
/**
* Output parameters for TimeOfImpact
*
* @author daniel
*/
public static class TOIOutput {
public TOIOutputState state;
public float t;
}
// djm pooling
private final SimplexCache cache = new SimplexCache();
private final DistanceInput distanceInput = new DistanceInput();
private final Transform xfA = new Transform();
private final Transform xfB = new Transform();
private final DistanceOutput distanceOutput = new DistanceOutput();
private final SeparationFunction fcn = new SeparationFunction();
private final int[] indexes = new int[2];
private final Sweep sweepA = new Sweep();
private final Sweep sweepB = new Sweep();
private final IWorldPool pool;
public TimeOfImpact(IWorldPool pool) {
this.pool = pool;
}
/**
* Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction
* between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
* non-tunneling collision. If you change the time interval, you should call this function again.
* Note: use Distance to compute the contact point and normal at the time of impact.
*/
public final void timeOfImpact(TOIOutput output, TOIInput input) {
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
output.state = TOIOutputState.UNKNOWN;
output.t = input.tMax;
final DistanceProxy proxyA = input.proxyA;
final DistanceProxy proxyB = input.proxyB;
sweepA.set(input.sweepA);
sweepB.set(input.sweepB);
// Large rotations can make the root finder fail, so we normalize the sweep angles.
sweepA.normalize();
sweepB.normalize();
float tMax = input.tMax;
float totalRadius = proxyA.m_radius + proxyB.m_radius;
// djm: whats with all these constants?
float target = Math.max(JBoxSettings.linearSlop, totalRadius - 3.0f * JBoxSettings.linearSlop);
float tolerance = 0.25f * JBoxSettings.linearSlop;
assert target > tolerance;
float t1 = 0f;
int iter = 0;
cache.count = 0;
distanceInput.proxyA = input.proxyA;
distanceInput.proxyB = input.proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (; ; ) {
sweepA.getTransform(xfA, t1);
sweepB.getTransform(xfB, t1);
// Get the distance between shapes.
// We can also use the results to get a separating axis
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
pool.getDistance().distance(distanceOutput, cache, distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0f) {
// Failure!
output.state = TOIOutputState.OVERLAPPED;
output.t = 0f;
break;
}
if (distanceOutput.distance < target + tolerance) {
// Victory!
output.state = TOIOutputState.TOUCHING;
output.t = t1;
break;
}
// Initialize the separating axis.
fcn.initialize(cache, proxyA, sweepA, proxyB, sweepB, t1);
// Compute the TOI on the separating axis.
// We do this by successively resolving the deepest point.
// This loop is bounded by the number of vertices.
boolean done = false;
float t2 = tMax;
int pushBackIter = 0;
for (; ; ) {
// Find the deepest point at t2. Store the witness point indices.
float s2 = fcn.findMinSeparation(indexes, t2);
// Is the final configuration separated?
if (s2 > target + tolerance) {
// Victory!
output.state = TOIOutputState.SEPARATED;
output.t = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance) {
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = fcn.evaluate(indexes[0], indexes[1], t1);
// Check for initial overlap.
// This might happen if the root finder runs out of iterations.
if (s1 < target - tolerance) {
output.state = TOIOutputState.FAILED;
output.t = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance) {
// Victory! t1 should hold the TOI (could be 0.0).
output.state = TOIOutputState.TOUCHING;
output.t = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rootIterCount = 0;
float a1 = t1, a2 = t2;
for (; ; ) {
// Use a mix of the secant rule and bisection.
float t;
if ((rootIterCount & 1) == 1) {
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
} else {
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
float s = fcn.evaluate(indexes[0], indexes[1], t);
if (FXGLMath.abs(s - target) < tolerance) {
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target) {
a1 = t;
s1 = s;
} else {
a2 = t;
s2 = s;
}
++rootIterCount;
// djm: whats with this? put in settings?
if (rootIterCount == 50) {
break;
}
}
++pushBackIter;
if (pushBackIter == JBoxSettings.maxPolygonVertices) {
break;
}
}
++iter;
if (done) {
break;
}
if (iter == MAX_ITERATIONS) {
// Root finder got stuck. Semi-victory.
output.state = TOIOutputState.FAILED;
output.t = t1;
break;
}
}
}
}