Skip to content

Commit f01104d

Browse files
PixelDust22PixelDust22
committed
Composable QueryDispatcher
1 parent 9e723ee commit f01104d

2 files changed

Lines changed: 338 additions & 37 deletions

File tree

‎src/query/default_query_dispatcher.rs‎

Lines changed: 104 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,9 @@ use crate::query::{
77
#[cfg(feature = "std")]
88
use crate::query::{
99
contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints},
10-
query_dispatcher::PersistentQueryDispatcher,
10+
query_dispatcher::{
11+
PersistentQueryDispatcher, PersistentQueryDispatcherComposite, QueryDispatcherComposite,
12+
},
1113
ContactManifold,
1214
};
1315
use crate::shape::{HalfSpace, Segment, Shape, ShapeType};
@@ -16,9 +18,10 @@ use crate::shape::{HalfSpace, Segment, Shape, ShapeType};
1618
#[derive(Debug, Clone)]
1719
pub struct DefaultQueryDispatcher;
1820

19-
impl QueryDispatcher for DefaultQueryDispatcher {
21+
impl QueryDispatcherComposite for DefaultQueryDispatcher {
2022
fn intersection_test(
2123
&self,
24+
root_dispatcher: &dyn QueryDispatcher,
2225
pos12: &Isometry<Real>,
2326
shape1: &dyn Shape,
2427
shape2: &dyn Shape,
@@ -66,11 +69,17 @@ impl QueryDispatcher for DefaultQueryDispatcher {
6669
#[cfg(feature = "std")]
6770
if let Some(c1) = shape1.as_composite_shape() {
6871
return Ok(query::details::intersection_test_composite_shape_shape(
69-
self, pos12, c1, shape2,
72+
root_dispatcher,
73+
pos12,
74+
c1,
75+
shape2,
7076
));
7177
} else if let Some(c2) = shape2.as_composite_shape() {
7278
return Ok(query::details::intersection_test_shape_composite_shape(
73-
self, pos12, shape1, c2,
79+
root_dispatcher,
80+
pos12,
81+
shape1,
82+
c2,
7483
));
7584
}
7685

@@ -83,6 +92,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
8392
/// Returns `0.0` if the objects are touching or penetrating.
8493
fn distance(
8594
&self,
95+
root_dispatcher: &dyn QueryDispatcher,
8696
pos12: &Isometry<Real>,
8797
shape1: &dyn Shape,
8898
shape2: &dyn Shape,
@@ -125,11 +135,17 @@ impl QueryDispatcher for DefaultQueryDispatcher {
125135
#[cfg(feature = "std")]
126136
if let Some(c1) = shape1.as_composite_shape() {
127137
return Ok(query::details::distance_composite_shape_shape(
128-
self, pos12, c1, shape2,
138+
root_dispatcher,
139+
pos12,
140+
c1,
141+
shape2,
129142
));
130143
} else if let Some(c2) = shape2.as_composite_shape() {
131144
return Ok(query::details::distance_shape_composite_shape(
132-
self, pos12, shape1, c2,
145+
root_dispatcher,
146+
pos12,
147+
shape1,
148+
c2,
133149
));
134150
}
135151

@@ -139,6 +155,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
139155

140156
fn contact(
141157
&self,
158+
root_dispatcher: &dyn QueryDispatcher,
142159
pos12: &Isometry<Real>,
143160
shape1: &dyn Shape,
144161
shape2: &dyn Shape,
@@ -181,11 +198,19 @@ impl QueryDispatcher for DefaultQueryDispatcher {
181198
));
182199
} else if let Some(c1) = shape1.as_composite_shape() {
183200
return Ok(query::details::contact_composite_shape_shape(
184-
self, pos12, c1, shape2, prediction,
201+
root_dispatcher,
202+
pos12,
203+
c1,
204+
shape2,
205+
prediction,
185206
));
186207
} else if let Some(c2) = shape2.as_composite_shape() {
187208
return Ok(query::details::contact_shape_composite_shape(
188-
self, pos12, shape1, c2, prediction,
209+
root_dispatcher,
210+
pos12,
211+
shape1,
212+
c2,
213+
prediction,
189214
));
190215
}
191216

@@ -195,6 +220,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
195220

196221
fn closest_points(
197222
&self,
223+
root_dispatcher: &dyn QueryDispatcher,
198224
pos12: &Isometry<Real>,
199225
shape1: &dyn Shape,
200226
shape2: &dyn Shape,
@@ -257,11 +283,19 @@ impl QueryDispatcher for DefaultQueryDispatcher {
257283
#[cfg(feature = "std")]
258284
if let Some(c1) = shape1.as_composite_shape() {
259285
return Ok(query::details::closest_points_composite_shape_shape(
260-
self, pos12, c1, shape2, max_dist,
286+
root_dispatcher,
287+
pos12,
288+
c1,
289+
shape2,
290+
max_dist,
261291
));
262292
} else if let Some(c2) = shape2.as_composite_shape() {
263293
return Ok(query::details::closest_points_shape_composite_shape(
264-
self, pos12, shape1, c2, max_dist,
294+
root_dispatcher,
295+
pos12,
296+
shape1,
297+
c2,
298+
max_dist,
265299
));
266300
}
267301

@@ -271,6 +305,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
271305

272306
fn cast_shapes(
273307
&self,
308+
root_dispatcher: &dyn QueryDispatcher,
274309
pos12: &Isometry<Real>,
275310
local_vel12: &Vector<Real>,
276311
shape1: &dyn Shape,
@@ -309,7 +344,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
309344
#[cfg(feature = "std")]
310345
if let Some(heightfield1) = shape1.as_heightfield() {
311346
return query::details::cast_shapes_heightfield_shape(
312-
self,
347+
root_dispatcher,
313348
pos12,
314349
local_vel12,
315350
heightfield1,
@@ -318,7 +353,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
318353
);
319354
} else if let Some(heightfield2) = shape1.as_heightfield() {
320355
return query::details::cast_shapes_shape_heightfield(
321-
self,
356+
root_dispatcher,
322357
pos12,
323358
local_vel12,
324359
shape1,
@@ -336,7 +371,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
336371
));
337372
} else if let Some(c1) = shape1.as_composite_shape() {
338373
return Ok(query::details::cast_shapes_composite_shape_shape(
339-
self,
374+
root_dispatcher,
340375
pos12,
341376
local_vel12,
342377
c1,
@@ -345,7 +380,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
345380
));
346381
} else if let Some(c2) = shape2.as_composite_shape() {
347382
return Ok(query::details::cast_shapes_shape_composite_shape(
348-
self,
383+
root_dispatcher,
349384
pos12,
350385
local_vel12,
351386
shape1,
@@ -360,6 +395,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
360395

361396
fn cast_shapes_nonlinear(
362397
&self,
398+
root_dispatcher: &dyn QueryDispatcher,
363399
motion1: &NonlinearRigidMotion,
364400
shape1: &dyn Shape,
365401
motion2: &NonlinearRigidMotion,
@@ -377,14 +413,23 @@ impl QueryDispatcher for DefaultQueryDispatcher {
377413

378414
Ok(
379415
query::details::cast_shapes_nonlinear_support_map_support_map(
380-
self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode,
416+
root_dispatcher,
417+
motion1,
418+
sm1,
419+
shape1,
420+
motion2,
421+
sm2,
422+
shape2,
423+
start_time,
424+
end_time,
425+
mode,
381426
),
382427
)
383428
} else {
384429
#[cfg(feature = "std")]
385430
if let Some(c1) = shape1.as_composite_shape() {
386431
return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape(
387-
self,
432+
root_dispatcher,
388433
motion1,
389434
c1,
390435
motion2,
@@ -395,7 +440,7 @@ impl QueryDispatcher for DefaultQueryDispatcher {
395440
));
396441
} else if let Some(c2) = shape2.as_composite_shape() {
397442
return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape(
398-
self,
443+
root_dispatcher,
399444
motion1,
400445
shape1,
401446
motion2,
@@ -418,14 +463,15 @@ impl QueryDispatcher for DefaultQueryDispatcher {
418463
}
419464

420465
#[cfg(feature = "std")]
421-
impl<ManifoldData, ContactData> PersistentQueryDispatcher<ManifoldData, ContactData>
466+
impl<ManifoldData, ContactData> PersistentQueryDispatcherComposite<ManifoldData, ContactData>
422467
for DefaultQueryDispatcher
423468
where
424469
ManifoldData: Default + Clone,
425470
ContactData: Default + Copy,
426471
{
427472
fn contact_manifolds(
428473
&self,
474+
root_dispatcher: &dyn PersistentQueryDispatcher<ManifoldData, ContactData>,
429475
pos12: &Isometry<Real>,
430476
shape1: &dyn Shape,
431477
shape2: &dyn Shape,
@@ -440,7 +486,13 @@ where
440486

441487
if let (Some(composite1), Some(composite2)) = (composite1, composite2) {
442488
contact_manifolds_composite_shape_composite_shape(
443-
self, pos12, composite1, composite2, prediction, manifolds, workspace,
489+
root_dispatcher,
490+
pos12,
491+
composite1,
492+
composite2,
493+
prediction,
494+
manifolds,
495+
workspace,
444496
);
445497

446498
return Ok(());
@@ -449,13 +501,19 @@ where
449501
match (shape1.shape_type(), shape2.shape_type()) {
450502
(ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => {
451503
contact_manifolds_trimesh_shape_shapes(
452-
self, pos12, shape1, shape2, prediction, manifolds, workspace,
504+
root_dispatcher,
505+
pos12,
506+
shape1,
507+
shape2,
508+
prediction,
509+
manifolds,
510+
workspace,
453511
);
454512
}
455513
(ShapeType::HeightField, _) => {
456514
if let Some(composite2) = composite2 {
457515
contact_manifolds_heightfield_composite_shape(
458-
self,
516+
root_dispatcher,
459517
pos12,
460518
&pos12.inverse(),
461519
shape1.as_heightfield().unwrap(),
@@ -467,14 +525,20 @@ where
467525
)
468526
} else {
469527
contact_manifolds_heightfield_shape_shapes(
470-
self, pos12, shape1, shape2, prediction, manifolds, workspace,
528+
root_dispatcher,
529+
pos12,
530+
shape1,
531+
shape2,
532+
prediction,
533+
manifolds,
534+
workspace,
471535
);
472536
}
473537
}
474538
(_, ShapeType::HeightField) => {
475539
if let Some(composite1) = composite1 {
476540
contact_manifolds_heightfield_composite_shape(
477-
self,
541+
root_dispatcher,
478542
&pos12.inverse(),
479543
pos12,
480544
shape2.as_heightfield().unwrap(),
@@ -486,18 +550,31 @@ where
486550
)
487551
} else {
488552
contact_manifolds_heightfield_shape_shapes(
489-
self, pos12, shape1, shape2, prediction, manifolds, workspace,
553+
root_dispatcher,
554+
pos12,
555+
shape1,
556+
shape2,
557+
prediction,
558+
manifolds,
559+
workspace,
490560
);
491561
}
492562
}
493563
_ => {
494564
if let Some(composite1) = composite1 {
495565
contact_manifolds_composite_shape_shape(
496-
self, pos12, composite1, shape2, prediction, manifolds, workspace, false,
566+
root_dispatcher,
567+
pos12,
568+
composite1,
569+
shape2,
570+
prediction,
571+
manifolds,
572+
workspace,
573+
false,
497574
);
498575
} else if let Some(composite2) = composite2 {
499576
contact_manifolds_composite_shape_shape(
500-
self,
577+
root_dispatcher,
501578
&pos12.inverse(),
502579
composite2,
503580
shape1,
@@ -511,7 +588,7 @@ where
511588
manifolds.push(ContactManifold::new());
512589
}
513590

514-
return self.contact_manifold_convex_convex(
591+
return root_dispatcher.contact_manifold_convex_convex(
515592
pos12,
516593
shape1,
517594
shape2,

0 commit comments

Comments
 (0)