1use na::{RealField, Unit};
4
5use crate::bounding_volume::{BoundingVolume, AABB};
6use crate::math::{Isometry, Point, Rotation, Translation, Vector};
7use crate::pipeline::broad_phase::{BroadPhase, BroadPhasePairFilter, DBVTBroadPhase};
8use crate::pipeline::glue::{
9 self, FirstInterferenceWithRay, InterferencesWithAABB, InterferencesWithPoint,
10 InterferencesWithRay,
11};
12use crate::pipeline::narrow_phase::{
13 ContactAlgorithm, ContactEvents, DefaultContactDispatcher, DefaultProximityDispatcher,
14 Interaction, InteractionGraph, NarrowPhase, ProximityDetector, ProximityEvents,
15 TemporaryInteractionIndex,
16};
17use crate::pipeline::object::{
18 CollisionGroups, CollisionObject, CollisionObjectSet, CollisionObjectSlab,
19 CollisionObjectSlabHandle, CollisionObjects, GeometricQueryType,
20};
21use crate::query::{ContactManifold, DefaultTOIDispatcher, Proximity, Ray, TOIDispatcher, TOI};
22use crate::shape::{Shape, ShapeHandle};
23
24pub type BroadPhaseObject<N> = Box<dyn BroadPhase<N, AABB<N>, CollisionObjectSlabHandle>>;
26
27pub struct CollisionWorld<N: RealField + Copy, T> {
29 pub objects: CollisionObjectSlab<N, T>,
31 pub broad_phase: BroadPhaseObject<N>,
33 pub narrow_phase: NarrowPhase<N, CollisionObjectSlabHandle>,
35 pub toi_dispatcher: Box<dyn TOIDispatcher<N>>,
37 pub interactions: InteractionGraph<N, CollisionObjectSlabHandle>,
39 pub pair_filters: Option<Box<dyn BroadPhasePairFilter<N, CollisionObjectSlab<N, T>>>>,
41}
42
43impl<N: RealField + Copy, T> CollisionWorld<N, T> {
44 pub fn new(margin: N) -> CollisionWorld<N, T> {
47 let objects = CollisionObjectSlab::new();
48 let coll_dispatcher = Box::new(DefaultContactDispatcher::new());
49 let prox_dispatcher = Box::new(DefaultProximityDispatcher::new());
50 let toi_dispatcher = Box::new(DefaultTOIDispatcher);
51 let broad_phase =
52 Box::new(DBVTBroadPhase::<N, AABB<N>, CollisionObjectSlabHandle>::new(margin));
53 let narrow_phase = NarrowPhase::new(coll_dispatcher, prox_dispatcher);
54
55 CollisionWorld {
56 interactions: InteractionGraph::new(),
57 objects,
58 broad_phase,
59 narrow_phase,
60 toi_dispatcher,
61 pair_filters: None,
62 }
63 }
64
65 pub fn add(
67 &mut self,
68 position: Isometry<N>,
69 shape: ShapeHandle<N>,
70 collision_groups: CollisionGroups,
71 query_type: GeometricQueryType<N>,
72 data: T,
73 ) -> (CollisionObjectSlabHandle, &mut CollisionObject<N, T>) {
74 let entry = self.objects.objects.vacant_entry();
75 let handle = CollisionObjectSlabHandle(entry.key());
76 let (proxy_handle, graph_index) = glue::create_proxies(
77 handle,
78 &mut *self.broad_phase,
79 &mut self.interactions,
80 &position,
81 shape.as_ref(),
82 query_type,
83 );
84
85 let co = CollisionObject::new(
86 Some(proxy_handle),
87 Some(graph_index),
88 position,
89 shape,
90 collision_groups,
91 query_type,
92 data,
93 );
94
95 (handle, entry.insert(co))
96 }
97
98 pub fn update(&mut self) {
105 self.narrow_phase.clear_events();
106
107 glue::perform_all_pipeline(
108 &self.objects,
109 &mut *self.broad_phase,
110 &mut self.narrow_phase,
111 &mut self.interactions,
112 self.pair_filters.as_ref().map(|f| &**f),
113 );
114
115 for (_, co) in self.objects.iter_mut() {
117 co.clear_update_flags();
118 }
119 }
120
121 pub fn clear_events(&mut self) {
123 self.narrow_phase.clear_events();
124 }
125
126 pub fn remove(&mut self, handles: &[CollisionObjectSlabHandle]) {
130 for handle in handles {
131 let co = self.objects.remove(*handle);
132 let graph_index = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
133 let proxy_handle = co.proxy_handle().expect(crate::NOT_REGISTERED_ERROR);
134
135 if let Some((new_handle, new_index)) = glue::remove_proxies(
136 &mut *self.broad_phase,
137 &mut self.interactions,
138 proxy_handle,
139 graph_index,
140 ) {
141 self.objects[new_handle].set_graph_index(Some(new_index))
142 }
143 }
144 }
145
146 #[deprecated = "Call directly the method `.set_position` on the collision object."]
148 pub fn set_position(&mut self, handle: CollisionObjectSlabHandle, pos: Isometry<N>) {
149 if let Some(co) = self.objects.get_mut(handle) {
150 co.set_position(pos);
151 }
152 }
153
154 #[inline]
176 #[deprecated = "Call directly the method `.set_query_type` on the collision object."]
177 pub fn set_query_type(
178 &mut self,
179 handle: CollisionObjectSlabHandle,
180 query_type: GeometricQueryType<N>,
181 ) {
182 if let Some(co) = self.objects.get_mut(handle) {
183 co.set_query_type(query_type);
184 }
185 }
186
187 #[inline]
189 #[deprecated = "Call directly the method `.set_shape` on the collision object."]
190 pub fn set_shape(&mut self, handle: CollisionObjectSlabHandle, shape: ShapeHandle<N>) {
191 if let Some(co) = self.objects.get_mut(handle) {
192 co.set_shape(shape);
193 }
194 }
195
196 #[deprecated = "Call directly the method `.set_deformations` on the collision object."]
198 pub fn set_deformations(&mut self, handle: CollisionObjectSlabHandle, coords: &[N]) {
199 if let Some(co) = self.objects.get_mut(handle) {
200 co.set_deformations(coords);
201 }
202 }
203
204 pub fn set_broad_phase_pair_filter<F>(&mut self, filter: Option<F>)
211 where
212 F: BroadPhasePairFilter<N, CollisionObjectSlab<N, T>> + 'static,
213 {
214 self.pair_filters = filter
215 .map(|f| Box::new(f) as Box<dyn BroadPhasePairFilter<N, CollisionObjectSlab<N, T>>>);
216 self.broad_phase.deferred_recompute_all_proximities();
217 }
218
219 pub fn perform_broad_phase(&mut self) {
221 glue::perform_broad_phase(
222 &self.objects,
223 &mut *self.broad_phase,
224 &mut self.narrow_phase,
225 &mut self.interactions,
226 self.pair_filters.as_ref().map(|f| &**f),
227 )
228 }
229
230 pub fn perform_narrow_phase(&mut self) {
232 glue::perform_narrow_phase(
233 &self.objects,
234 &mut self.narrow_phase,
235 &mut self.interactions,
236 )
237 }
238
239 pub fn broad_phase_aabb(&self, handle: CollisionObjectSlabHandle) -> Option<&AABB<N>> {
241 let co = self.objects.collision_object(handle)?;
242 let proxy_handle = co.proxy_handle().expect(crate::NOT_REGISTERED_ERROR);
243 self.broad_phase.proxy(proxy_handle).map(|p| p.0)
244 }
245
246 #[inline]
248 pub fn collision_objects(&self) -> CollisionObjects<N, T> {
249 self.objects.iter()
250 }
251
252 #[inline]
254 pub fn collision_object(
255 &self,
256 handle: CollisionObjectSlabHandle,
257 ) -> Option<&CollisionObject<N, T>> {
258 self.objects.collision_object(handle)
259 }
260
261 #[inline]
263 pub fn get_mut(
264 &mut self,
265 handle: CollisionObjectSlabHandle,
266 ) -> Option<&mut CollisionObject<N, T>> {
267 self.objects.get_mut(handle)
268 }
269
270 #[inline]
274 pub fn collision_object_pair_mut(
275 &mut self,
276 handle1: CollisionObjectSlabHandle,
277 handle2: CollisionObjectSlabHandle,
278 ) -> (
279 Option<&mut CollisionObject<N, T>>,
280 Option<&mut CollisionObject<N, T>>,
281 ) {
282 self.objects.get_pair_mut(handle1, handle2)
283 }
284
285 #[inline]
287 #[deprecated = "Call directly the method `.set_collision_groups` on the collision object."]
288 pub fn set_collision_groups(
289 &mut self,
290 handle: CollisionObjectSlabHandle,
291 groups: CollisionGroups,
292 ) {
293 if let Some(co) = self.objects.get_mut(handle) {
294 co.set_collision_groups(groups);
295 }
296 }
297
298 #[inline]
303 pub fn sweep_test<'a>(
304 &'a self,
305 shape: &'a dyn Shape<N>,
306 isometry: &'a Isometry<N>,
307 direction: &'a Unit<Vector<N>>,
308 maximum_distance: N,
309 groups: &'a CollisionGroups,
310 ) -> impl Iterator<Item = (CollisionObjectSlabHandle, TOI<N>)> + 'a {
311 let a = shape.aabb(&isometry);
312 let b = shape.aabb(&Isometry::from_parts(
313 Translation::from(isometry.translation.vector + direction.as_ref() * maximum_distance),
314 Rotation::identity(),
315 ));
316 let aabb = a.merged(&b);
317
318 let interferences: Vec<_> = self.interferences_with_aabb(&aabb, groups).collect();
320
321 let dispatcher = &*self.toi_dispatcher;
322 interferences.into_iter().filter_map(move |(handle, x)| {
323 dispatcher
324 .time_of_impact(
325 dispatcher,
326 &isometry,
327 &direction,
328 shape,
329 x.position(),
330 &Vector::zeros(),
331 x.shape().as_ref(),
332 N::max_value().unwrap(),
333 N::zero(),
334 )
335 .unwrap_or(None)
336 .map(|toi| (handle, toi))
337 })
338 }
339
340 #[inline]
342 pub fn interferences_with_ray<'a, 'b>(
343 &'a self,
344 ray: &'b Ray<N>,
345 max_toi: N,
346 groups: &'b CollisionGroups,
347 ) -> InterferencesWithRay<'a, 'b, N, CollisionObjectSlab<N, T>> {
348 glue::interferences_with_ray(&self.objects, &*self.broad_phase, ray, max_toi, groups)
349 }
350
351 #[inline]
353 pub fn first_interference_with_ray<'a, 'b>(
354 &'a self,
355 ray: &'b Ray<N>,
356 max_toi: N,
357 groups: &'b CollisionGroups,
358 ) -> Option<FirstInterferenceWithRay<'a, N, CollisionObjectSlab<N, T>>> {
359 glue::first_interference_with_ray(&self.objects, &*self.broad_phase, ray, max_toi, groups)
360 }
361
362 #[inline]
364 pub fn interferences_with_point<'a, 'b>(
365 &'a self,
366 point: &'b Point<N>,
367 groups: &'b CollisionGroups,
368 ) -> InterferencesWithPoint<'a, 'b, N, CollisionObjectSlab<N, T>> {
369 glue::interferences_with_point(&self.objects, &*self.broad_phase, point, groups)
370 }
371
372 #[inline]
374 pub fn interferences_with_aabb<'a, 'b>(
375 &'a self,
376 aabb: &'b AABB<N>,
377 groups: &'b CollisionGroups,
378 ) -> InterferencesWithAABB<'a, 'b, N, CollisionObjectSlab<N, T>> {
379 glue::interferences_with_aabb(&self.objects, &*self.broad_phase, aabb, groups)
380 }
381
382 pub fn set_narrow_phase(&mut self, narrow_phase: NarrowPhase<N, CollisionObjectSlabHandle>) {
384 self.narrow_phase = narrow_phase;
385 self.broad_phase.deferred_recompute_all_proximities();
386 }
387
388 pub fn interaction_pairs(
399 &self,
400 effective_only: bool,
401 ) -> impl Iterator<
402 Item = (
403 CollisionObjectSlabHandle,
404 CollisionObjectSlabHandle,
405 &Interaction<N>,
406 ),
407 > {
408 self.interactions.interaction_pairs(effective_only)
409 }
410
411 pub fn contact_pairs(
416 &self,
417 effective_only: bool,
418 ) -> impl Iterator<
419 Item = (
420 CollisionObjectSlabHandle,
421 CollisionObjectSlabHandle,
422 &ContactAlgorithm<N>,
423 &ContactManifold<N>,
424 ),
425 > {
426 self.interactions.contact_pairs(effective_only)
427 }
428
429 pub fn proximity_pairs(
434 &self,
435 effective_only: bool,
436 ) -> impl Iterator<
437 Item = (
438 CollisionObjectSlabHandle,
439 CollisionObjectSlabHandle,
440 &dyn ProximityDetector<N>,
441 Proximity,
442 ),
443 > {
444 self.interactions.proximity_pairs(effective_only)
445 }
446
447 pub fn interaction_pair(
452 &self,
453 handle1: CollisionObjectSlabHandle,
454 handle2: CollisionObjectSlabHandle,
455 effective_only: bool,
456 ) -> Option<(
457 CollisionObjectSlabHandle,
458 CollisionObjectSlabHandle,
459 &Interaction<N>,
460 )> {
461 let co1 = self.objects.collision_object(handle1)?;
462 let co2 = self.objects.collision_object(handle2)?;
463 let id1 = co1.graph_index().expect(crate::NOT_REGISTERED_ERROR);
464 let id2 = co2.graph_index().expect(crate::NOT_REGISTERED_ERROR);
465 self.interactions.interaction_pair(id1, id2, effective_only)
466 }
467
468 pub fn contact_pair(
473 &self,
474 handle1: CollisionObjectSlabHandle,
475 handle2: CollisionObjectSlabHandle,
476 effective_only: bool,
477 ) -> Option<(
478 CollisionObjectSlabHandle,
479 CollisionObjectSlabHandle,
480 &ContactAlgorithm<N>,
481 &ContactManifold<N>,
482 )> {
483 let co1 = self.objects.collision_object(handle1)?;
484 let co2 = self.objects.collision_object(handle2)?;
485 let id1 = co1.graph_index().expect(crate::NOT_REGISTERED_ERROR);
486 let id2 = co2.graph_index().expect(crate::NOT_REGISTERED_ERROR);
487 self.interactions.contact_pair(id1, id2, effective_only)
488 }
489
490 pub fn proximity_pair(
495 &self,
496 handle1: CollisionObjectSlabHandle,
497 handle2: CollisionObjectSlabHandle,
498 effective_only: bool,
499 ) -> Option<(
500 CollisionObjectSlabHandle,
501 CollisionObjectSlabHandle,
502 &dyn ProximityDetector<N>,
503 Proximity,
504 )> {
505 let co1 = self.objects.collision_object(handle1)?;
506 let co2 = self.objects.collision_object(handle2)?;
507 let id1 = co1.graph_index().expect(crate::NOT_REGISTERED_ERROR);
508 let id2 = co2.graph_index().expect(crate::NOT_REGISTERED_ERROR);
509 self.interactions.proximity_pair(id1, id2, effective_only)
510 }
511
512 pub fn interactions_with(
517 &self,
518 handle: CollisionObjectSlabHandle,
519 effective_only: bool,
520 ) -> Option<
521 impl Iterator<
522 Item = (
523 CollisionObjectSlabHandle,
524 CollisionObjectSlabHandle,
525 &Interaction<N>,
526 ),
527 >,
528 > {
529 let co = self.objects.collision_object(handle)?;
530 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
531 Some(self.interactions.interactions_with(id, effective_only))
532 }
533
534 pub fn interactions_with_mut(
539 &mut self,
540 handle: CollisionObjectSlabHandle,
541 ) -> Option<(
542 &mut NarrowPhase<N, CollisionObjectSlabHandle>,
543 impl Iterator<
544 Item = (
545 CollisionObjectSlabHandle,
546 CollisionObjectSlabHandle,
547 TemporaryInteractionIndex,
548 &mut Interaction<N>,
549 ),
550 >,
551 )> {
552 let co = self.objects.collision_object(handle)?;
553 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
554 Some((
555 &mut self.narrow_phase,
556 self.interactions.interactions_with_mut(id),
557 ))
558 }
559
560 pub fn proximities_with(
565 &self,
566 handle: CollisionObjectSlabHandle,
567 effective_only: bool,
568 ) -> Option<
569 impl Iterator<
570 Item = (
571 CollisionObjectSlabHandle,
572 CollisionObjectSlabHandle,
573 &dyn ProximityDetector<N>,
574 Proximity,
575 ),
576 >,
577 > {
578 let co = self.objects.collision_object(handle)?;
579 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
580 Some(self.interactions.proximities_with(id, effective_only))
581 }
582
583 pub fn contacts_with(
588 &self,
589 handle: CollisionObjectSlabHandle,
590 effective_only: bool,
591 ) -> Option<
592 impl Iterator<
593 Item = (
594 CollisionObjectSlabHandle,
595 CollisionObjectSlabHandle,
596 &ContactAlgorithm<N>,
597 &ContactManifold<N>,
598 ),
599 >,
600 > {
601 let co = self.objects.collision_object(handle)?;
602 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
603 Some(self.interactions.contacts_with(id, effective_only))
604 }
605
606 pub fn collision_objects_interacting_with<'a>(
611 &'a self,
612 handle: CollisionObjectSlabHandle,
613 ) -> Option<impl Iterator<Item = CollisionObjectSlabHandle> + 'a> {
614 let co = self.objects.collision_object(handle)?;
615 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
616 Some(self.interactions.collision_objects_interacting_with(id))
617 }
618
619 pub fn collision_objects_in_contact_with<'a>(
625 &'a self,
626 handle: CollisionObjectSlabHandle,
627 ) -> Option<impl Iterator<Item = CollisionObjectSlabHandle> + 'a> {
628 let co = self.objects.collision_object(handle)?;
629 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
630 Some(self.interactions.collision_objects_in_contact_with(id))
631 }
632
633 pub fn collision_objects_in_proximity_of<'a>(
639 &'a self,
640 handle: CollisionObjectSlabHandle,
641 ) -> Option<impl Iterator<Item = CollisionObjectSlabHandle> + 'a> {
642 let co = self.objects.collision_object(handle)?;
643 let id = co.graph_index().expect(crate::NOT_REGISTERED_ERROR);
644 Some(self.interactions.collision_objects_in_proximity_of(id))
645 }
646
647 pub fn contact_events(&self) -> &ContactEvents<CollisionObjectSlabHandle> {
654 self.narrow_phase.contact_events()
655 }
656
657 pub fn proximity_events(&self) -> &ProximityEvents<CollisionObjectSlabHandle> {
659 self.narrow_phase.proximity_events()
660 }
661}