Skip to content

Commit 6329dce

Browse files
zferguszfergus
authored
Refactor Nonlinear CCD Implementation into a Class (#218)
- Introduced a new NonlinearCCD class to encapsulate nonlinear continuous collision detection (CCD) methods. - Updated point-point, point-edge, edge-edge, and point-triangle CCD methods to use the new class structure. - Modified the test cases to utilize the new NonlinearCCD class, ensuring consistency in the testing approach. - Enhanced readability by applying consistent formatting and style changes across the CCD methods. - Updated function signatures to accept Eigen::ConstRef types for better performance and clarity. - Adjusted the conservative rescaling parameter handling in the CCD methods for improved accuracy. - Removed deprecated function signatures and ensured all tests reflect the new method signatures.
1 parent 5c5494c commit 6329dce

9 files changed

Lines changed: 398 additions & 398 deletions

File tree

‎docs/source/tutorials/nonlinear_ccd.rst‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -199,13 +199,13 @@ The following code snippet shows an example of how to use interval arithmetic to
199199

200200
.. code-block:: cpp
201201
202-
#include <ipc/utils/interval.hpp>
202+
#include <ipc/math/interval.hpp>
203203
204204
using namespace ipc;
205205
206206
Vector2I position(
207-
const VectorMax3d& center,
208-
const VectorMax3d& point,
207+
Eigen::ConstRef<VectorMax3d> center,
208+
Eigen::ConstRef<VectorMax3d> point,
209209
const double omega,
210210
const Interval& t)
211211
{

‎python/src/ccd/nonlinear_ccd.cpp‎

Lines changed: 151 additions & 174 deletions
Large diffs are not rendered by default.

‎python/tests/test_ccd.py‎

Lines changed: 55 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,6 @@
11
import numpy as np
2-
32
from find_ipctk import ipctk
43
from ipctk.filib import Interval
5-
64
from utils import load_mesh
75

86

@@ -28,16 +26,44 @@ class DumbNarrowPhaseCCD(ipctk.NarrowPhaseCCD):
2826
def __init__(self):
2927
ipctk.NarrowPhaseCCD.__init__(self)
3028

31-
def point_point_ccd(self, p0_t0, p1_t0, p0_t1, p1_t1, min_distance=0.0, tmax=1.0):
29+
def point_point_ccd(
30+
self, p0_t0, p1_t0, p0_t1, p1_t1, min_distance=0.0, tmax=1.0
31+
):
3232
return True, 0.0
3333

34-
def point_edge_ccd(self, p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, min_distance=0.0, tmax=1.0):
34+
def point_edge_ccd(
35+
self, p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, min_distance=0.0, tmax=1.0
36+
):
3537
return True, 0.0
3638

37-
def point_triangle_ccd(self, p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, min_distance=0.0, tmax=1.0):
39+
def point_triangle_ccd(
40+
self,
41+
p_t0,
42+
t0_t0,
43+
t1_t0,
44+
t2_t0,
45+
p_t1,
46+
t0_t1,
47+
t1_t1,
48+
t2_t1,
49+
min_distance=0.0,
50+
tmax=1.0,
51+
):
3852
return True, 0.0
3953

40-
def edge_edge_ccd(self, ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, min_distance=0.0, tmax=1.0):
54+
def edge_edge_ccd(
55+
self,
56+
ea0_t0,
57+
ea1_t0,
58+
eb0_t0,
59+
eb1_t0,
60+
ea0_t1,
61+
ea1_t1,
62+
eb0_t1,
63+
eb1_t1,
64+
min_distance=0.0,
65+
tmax=1.0,
66+
):
4167
return True, 0.0
4268

4369
V0, E, F = load_mesh("two-cubes-close.ply")
@@ -48,10 +74,12 @@ def edge_edge_ccd(self, ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1,
4874
ipctk.set_num_threads(1)
4975

5076
assert not ipctk.is_step_collision_free(
51-
mesh, V0, V1, narrow_phase_ccd=DumbNarrowPhaseCCD())
77+
mesh, V0, V1, narrow_phase_ccd=DumbNarrowPhaseCCD()
78+
)
5279

5380
toi = ipctk.compute_collision_free_stepsize(
54-
mesh, V0, V1, narrow_phase_ccd=DumbNarrowPhaseCCD())
81+
mesh, V0, V1, narrow_phase_ccd=DumbNarrowPhaseCCD()
82+
)
5583
assert 0 <= toi <= 1
5684

5785

@@ -90,8 +118,7 @@ def detect_face_face_candidates(self):
90118

91119
assert ipctk.is_step_collision_free(mesh, V0, V1, broad_phase=broad_phase)
92120

93-
toi = ipctk.compute_collision_free_stepsize(
94-
mesh, V0, V1, broad_phase=broad_phase)
121+
toi = ipctk.compute_collision_free_stepsize(mesh, V0, V1, broad_phase=broad_phase)
95122
assert toi == 1
96123

97124
assert not ipctk.has_intersections(mesh, V0, broad_phase=broad_phase)
@@ -124,7 +151,9 @@ def max_distance_from_linear(self, t0: float, t1: float):
124151
assert p0.max_distance_from_linear(0, 1) == 0
125152
assert p1.max_distance_from_linear(0, 1) == 0
126153

127-
is_colliding, toi = ipctk.point_point_nonlinear_ccd(p0, p1)
154+
ccd = ipctk.NonlinearCCD()
155+
156+
is_colliding, toi = ccd.point_point_ccd(p0, p1)
128157

129158
assert is_colliding
130159
assert 0 < toi < 1
@@ -145,15 +174,17 @@ def max_distance_from_linear(self, t0: float, t1: float):
145174
# p0 = IntervalLinearTrajectory(np.array([-1, 0, 0]), np.array([1, 0, 0]))
146175
# p1 = IntervalLinearTrajectory(np.array([0, -1, 0]), np.array([0, 1, 0]))
147176

148-
# is_colliding, toi = ipctk.point_point_nonlinear_ccd(p0, p1)
177+
# is_colliding, toi = ccd.point_point_ccd(p0, p1)
149178

150179
# assert is_colliding
151180
# assert 0 < toi < 1
152181
# assert np.abs(toi - 0.5) < 1e-2
153182

154183
# BEGIN_RIGID_2D_TRAJECTORY
155184
class Rigid2DTrajectory(ipctk.NonlinearTrajectory):
156-
def __init__(self, position, translation, delta_translation, rotation, delta_rotation):
185+
def __init__(
186+
self, position, translation, delta_translation, rotation, delta_rotation
187+
):
157188
ipctk.NonlinearTrajectory.__init__(self)
158189
self.position = position
159190
self.translation = translation
@@ -164,9 +195,11 @@ def __init__(self, position, translation, delta_translation, rotation, delta_rot
164195
# BEGIN_RIGID_2D_CALL
165196
def __call__(self, t):
166197
theta = self.rotation + t * self.delta_rotation
167-
R = np.array([[np.cos(theta), -np.sin(theta)],
168-
[np.sin(theta), np.cos(theta)]])
198+
R = np.array(
199+
[[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]
200+
)
169201
return R @ self.position + self.translation + t * self.delta_translation
202+
170203
# END_RIGID_2D_CALL
171204

172205
# BEGIN_RIGID_2D_MAX_DISTANCE_FROM_LINEAR
@@ -177,20 +210,19 @@ def max_distance_from_linear(self, t0, t1):
177210
p_t0 = self(t0)
178211
p_t1 = self(t1)
179212
return np.linalg.norm(self((t0 + t1) / 2) - ((p_t1 - p_t0) * 0.5 + p_t0))
213+
180214
# END_RIGID_2D_MAX_DISTANCE_FROM_LINEAR
215+
181216
# END_RIGID_2D_TRAJECTORY
182217

183218
# BEGIN_TEST_RIGID_2D_TRAJECTORY
184-
p = Rigid2DTrajectory(
185-
np.array([0, 0.5]), np.zeros(2), np.zeros(2), 0, 0)
186-
e0 = Rigid2DTrajectory(
187-
np.array([-1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)
188-
e1 = Rigid2DTrajectory(
189-
np.array([1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)
219+
p = Rigid2DTrajectory(np.array([0, 0.5]), np.zeros(2), np.zeros(2), 0, 0)
220+
e0 = Rigid2DTrajectory(np.array([-1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)
221+
e1 = Rigid2DTrajectory(np.array([1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)
190222

191223
# increase the conservative_rescaling from 0.8 to 0.9 to get a more accurate estimate
192-
collision, toi = ipctk.point_edge_nonlinear_ccd(
193-
p, e0, e1, conservative_rescaling=0.9)
224+
ccd.conservative_rescaling = 0.9
225+
collision, toi = ccd.point_edge_ccd(p, e0, e1)
194226

195227
assert collision
196228
assert 0.49 <= toi <= 0.5 # conservative estimate

‎src/ipc/ccd/nonlinear_ccd.cpp‎

Lines changed: 34 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,17 @@ double IntervalNonlinearTrajectory::max_distance_from_linear(
5151

5252
// ============================================================================
5353

54-
bool conservative_piecewise_linear_ccd(
54+
NonlinearCCD::NonlinearCCD(
55+
const double _tolerance,
56+
const long _max_iterations,
57+
const double _conservative_rescaling)
58+
: tolerance(_tolerance)
59+
, max_iterations(_max_iterations)
60+
, conservative_rescaling(_conservative_rescaling)
61+
{
62+
}
63+
64+
bool NonlinearCCD::conservative_piecewise_linear_ccd(
5565
const std::function<double(const double)>& distance,
5666
const std::function<double(const double, const double)>&
5767
max_distance_from_linear,
@@ -62,15 +72,15 @@ bool conservative_piecewise_linear_ccd(
6272
const bool /*no_zero_toi*/,
6373
double& /*toi*/)>& linear_ccd,
6474
double& toi,
75+
const double min_distance,
6576
const double tmax,
66-
const double min_sep_distance,
6777
const double conservative_rescaling)
6878
{
6979
const double distance_t0 = distance(0);
70-
if (check_initial_distance(distance_t0, min_sep_distance, toi)) {
80+
if (check_initial_distance(distance_t0, min_distance, toi)) {
7181
return true;
7282
}
73-
assert(distance_t0 > min_sep_distance);
83+
assert(distance_t0 > min_distance);
7484

7585
double ti0 = 0;
7686
std::stack<double> ts;
@@ -102,38 +112,38 @@ bool conservative_piecewise_linear_ccd(
102112
return true;
103113
}
104114

105-
double min_distance = max_distance_from_linear(ti0, ti1);
115+
double min_distance_linear = max_distance_from_linear(ti0, ti1);
106116

107117
#ifndef USE_FIXED_PIECES
108118
// Check if the minimum distance is too large and we need to subdivide
109119
// (Large distances cause the slow CCD)
110-
if ((min_distance
120+
if ((min_distance_linear
111121
>= std::min((1 - conservative_rescaling) * distance_ti0, 0.01))
112122
&& (num_subdivisions < MAX_NUM_SUBDIVISIONS || ti0 == 0)) {
113123
logger().trace(
114124
"Subdividing at ti=[{:g}, {:g}] min_distance={:g} distance_ti0={:g}",
115-
ti0, ti1, min_distance, distance_ti0);
125+
ti0, ti1, min_distance_linear, distance_ti0);
116126
ts.push((ti1 + ti0) / 2);
117127
num_subdivisions++;
118128
continue;
119129
}
120130
#endif
121131

122-
min_distance += min_sep_distance;
132+
min_distance_linear += min_distance;
123133

124-
const bool is_impacting =
125-
linear_ccd(ti0, ti1, min_distance, /*no_zero_toi=*/ti0 == 0, toi);
134+
const bool is_impacting = linear_ccd(
135+
ti0, ti1, min_distance_linear, /*no_zero_toi=*/ti0 == 0, toi);
126136

127137
logger().trace(
128138
"Evaluated at ti=[{:g}, {:g}] min_distance={:g} distance_ti0={:g}; result={}{}",
129-
ti0, ti1, min_distance, distance_ti0, is_impacting,
139+
ti0, ti1, min_distance_linear, distance_ti0, is_impacting,
130140
is_impacting ? fmt::format(" toi={:g}", (ti1 - ti0) * toi + ti0)
131141
: "");
132142

133143
if (is_impacting) {
134144
toi = (ti1 - ti0) * toi + ti0;
135145
if (toi == 0) {
136-
// This is impossible because distance_t0 > min_sep_distance
146+
// This is impossible because distance_t0 > min_distance
137147
ts.push((ti1 + ti0) / 2);
138148
num_subdivisions++;
139149
continue;
@@ -150,15 +160,12 @@ bool conservative_piecewise_linear_ccd(
150160

151161
// ============================================================================
152162

153-
bool point_point_nonlinear_ccd(
163+
bool NonlinearCCD::point_point_ccd(
154164
const NonlinearTrajectory& p0,
155165
const NonlinearTrajectory& p1,
156166
double& toi,
157-
const double tmax,
158167
const double min_distance,
159-
const double tolerance,
160-
const long max_iterations,
161-
const double conservative_rescaling)
168+
const double tmax) const
162169
{
163170
return conservative_piecewise_linear_ccd(
164171
[&](const double t) {
@@ -184,19 +191,16 @@ bool point_point_nonlinear_ccd(
184191
output_tolerance, // delta_actual
185192
no_zero_toi); // no zero toi
186193
},
187-
toi, tmax, min_distance, conservative_rescaling);
194+
toi, min_distance, tmax, conservative_rescaling);
188195
}
189196

190-
bool point_edge_nonlinear_ccd(
197+
bool NonlinearCCD::point_edge_ccd(
191198
const NonlinearTrajectory& p,
192199
const NonlinearTrajectory& e0,
193200
const NonlinearTrajectory& e1,
194201
double& toi,
195-
const double tmax,
196202
const double min_distance,
197-
const double tolerance,
198-
const long max_iterations,
199-
const double conservative_rescaling)
203+
const double tmax) const
200204
{
201205
return conservative_piecewise_linear_ccd(
202206
[&](const double t) {
@@ -223,20 +227,17 @@ bool point_edge_nonlinear_ccd(
223227
output_tolerance, // delta_actual
224228
no_zero_toi); // no zero toi
225229
},
226-
toi, tmax, min_distance, conservative_rescaling);
230+
toi, min_distance, tmax, conservative_rescaling);
227231
}
228232

229-
bool edge_edge_nonlinear_ccd(
233+
bool NonlinearCCD::edge_edge_ccd(
230234
const NonlinearTrajectory& ea0,
231235
const NonlinearTrajectory& ea1,
232236
const NonlinearTrajectory& eb0,
233237
const NonlinearTrajectory& eb1,
234238
double& toi,
235-
const double tmax,
236239
const double min_distance,
237-
const double tolerance,
238-
const long max_iterations,
239-
const double conservative_rescaling)
240+
const double tmax) const
240241
{
241242
return conservative_piecewise_linear_ccd(
242243
[&](const double t) {
@@ -265,20 +266,17 @@ bool edge_edge_nonlinear_ccd(
265266
output_tolerance, // delta_actual
266267
no_zero_toi); // no zero toi
267268
},
268-
toi, tmax, min_distance, conservative_rescaling);
269+
toi, min_distance, tmax, conservative_rescaling);
269270
}
270271

271-
bool point_triangle_nonlinear_ccd(
272+
bool NonlinearCCD::point_triangle_ccd(
272273
const NonlinearTrajectory& p,
273274
const NonlinearTrajectory& t0,
274275
const NonlinearTrajectory& t1,
275276
const NonlinearTrajectory& t2,
276277
double& toi,
277-
const double tmax,
278278
const double min_distance,
279-
const double tolerance,
280-
const long max_iterations,
281-
const double conservative_rescaling)
279+
const double tmax) const
282280
{
283281
return conservative_piecewise_linear_ccd(
284282
[&](const double t) {
@@ -306,7 +304,7 @@ bool point_triangle_nonlinear_ccd(
306304
output_tolerance, // delta_actual
307305
no_zero_toi); // no zero toi
308306
},
309-
toi, tmax, min_distance, conservative_rescaling);
307+
toi, min_distance, tmax, conservative_rescaling);
310308
}
311309

312310
} // namespace ipc

0 commit comments

Comments
 (0)