nphysics2d/world/
geometrical_world.rs

1use std::collections::{hash_map, HashMap};
2use std::marker::PhantomData;
3
4use na::RealField;
5
6use ncollide::bounding_volume::AABB;
7use ncollide::pipeline::{
8    self, BroadPhase, BroadPhasePairFilter, CollisionGroups, CollisionObjectSet, ContactAlgorithm,
9    ContactEvents, DBVTBroadPhase, DefaultContactDispatcher, DefaultProximityDispatcher,
10    Interaction, InteractionGraph, NarrowPhase, ProximityDetector, ProximityEvents,
11};
12use ncollide::query::{ContactManifold, Proximity, Ray};
13
14use crate::object::{
15    BodyHandle, BodySet, Collider, ColliderAnchor, ColliderHandle, ColliderSet, DefaultBodyHandle,
16    DefaultBodySet, DefaultColliderHandle, DefaultColliderSet,
17};
18use crate::volumetric::Volumetric;
19
20use crate::math::Point;
21
22/// The default geometrical world, that can be used with a `DefaultBodyHandle` and `DefaultColliderHandle`.
23pub type DefaultGeometricalWorld<N> = GeometricalWorld<N, DefaultBodyHandle, DefaultColliderHandle>;
24
25/// The world managing all geometric queries.
26///
27/// This is a wrapper over the `CollisionWorld` structure from `ncollide` to simplify
28/// its use with the [object::Collider] structure.
29pub struct GeometricalWorld<N: RealField + Copy, Handle: BodyHandle, CollHandle: ColliderHandle> {
30    /// The broad phase used by this collision world.
31    pub(crate) broad_phase: Box<dyn BroadPhase<N, AABB<N>, CollHandle>>,
32    /// The narrow-phase used by this collision world.
33    pub(crate) narrow_phase: NarrowPhase<N, CollHandle>,
34    /// The graph of interactions detected so far.
35    pub(crate) interactions: InteractionGraph<N, CollHandle>,
36    pub(crate) body_colliders: HashMap<Handle, Vec<CollHandle>>,
37}
38
39impl<N: RealField + Copy, Handle: BodyHandle, CollHandle: ColliderHandle>
40    GeometricalWorld<N, Handle, CollHandle>
41{
42    /// Creates a geometrical world from the provided broad-phase and narrow-phase structures.
43    pub fn from_parts<BF>(broad_phase: BF, narrow_phase: NarrowPhase<N, CollHandle>) -> Self
44    where
45        BF: BroadPhase<N, AABB<N>, CollHandle>,
46    {
47        GeometricalWorld {
48            broad_phase: Box::new(broad_phase),
49            narrow_phase,
50            interactions: InteractionGraph::new(),
51            body_colliders: HashMap::new(),
52        }
53    }
54
55    /// Creates a new collision world.
56    // FIXME: use default values for `margin` and allow its modification by the user ?
57    pub fn new() -> Self {
58        let coll_dispatcher = Box::new(DefaultContactDispatcher::new());
59        let prox_dispatcher = Box::new(DefaultProximityDispatcher::new());
60        let broad_phase = DBVTBroadPhase::new(na::convert(0.01));
61        let narrow_phase = NarrowPhase::new(coll_dispatcher, prox_dispatcher);
62        Self::from_parts(broad_phase, narrow_phase)
63    }
64
65    fn register_collider(&mut self, handle: CollHandle, collider: &mut Collider<N, Handle>) {
66        assert!(
67            collider.proxy_handle().is_none(),
68            "Cannot register a collider that is already registered."
69        );
70        assert!(
71            collider.graph_index().is_none(),
72            "Cannot register a collider that is already registered."
73        );
74
75        let proxies = pipeline::create_proxies(
76            handle,
77            &mut *self.broad_phase,
78            &mut self.interactions,
79            collider.position(),
80            collider.shape(),
81            collider.query_type(),
82        );
83
84        self.body_colliders
85            .entry(collider.body())
86            .or_insert_with(Vec::new)
87            .push(handle);
88        collider.set_proxy_handle(Some(proxies.0));
89        collider.set_graph_index(Some(proxies.1));
90    }
91
92    /// Maintain the internal structures of the geometrical world by handling body removals and colliders insersion and removals.
93    pub fn maintain<Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
94        &mut self,
95        bodies: &mut dyn BodySet<N, Handle = Handle>,
96        colliders: &mut Colliders,
97    ) {
98        self.handle_removals(bodies, colliders);
99        self.handle_insertions(bodies, colliders);
100    }
101
102    fn handle_insertions<Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
103        &mut self,
104        bodies: &mut dyn BodySet<N, Handle = Handle>,
105        colliders: &mut Colliders,
106    ) {
107        while let Some(handle) = colliders.pop_insertion_event() {
108            if let Some(collider) = colliders.get_mut(handle) {
109                self.register_collider(handle, collider);
110
111                match collider.anchor() {
112                    ColliderAnchor::OnBodyPart {
113                        body_part,
114                        position_wrt_body_part,
115                    } => {
116                        let body = bodies
117                            .get_mut(body_part.0)
118                            .expect("Invalid parent body part handle.");
119
120                        // Update the parent body's inertia.
121                        if !collider.density().is_zero() {
122                            let (com, inertia) = collider.shape().transformed_mass_properties(
123                                collider.density(),
124                                position_wrt_body_part,
125                            );
126                            body.add_local_inertia_and_com(body_part.1, com, inertia);
127                        }
128
129                        // Set the position and ndofs (note this will be done in the `sync_collider` too.
130                        let ndofs = body.status_dependent_ndofs();
131                        let part = body
132                            .part(body_part.1)
133                            .expect("Invalid parent body part handle.");
134                        let pos = part.position() * position_wrt_body_part;
135
136                        collider.set_body_status_dependent_ndofs(ndofs);
137                        collider.set_position(pos);
138                    }
139                    _ => {}
140                }
141            }
142        }
143    }
144
145    fn handle_removals<Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
146        &mut self,
147        bodies: &mut dyn BodySet<N, Handle = Handle>,
148        colliders: &mut Colliders,
149    ) {
150        let mut graph_id_remapping = HashMap::new();
151
152        while let Some((removed_handle, mut removed)) = colliders.pop_removal_event() {
153            // Adjust the graph index of the removed collider if it was affected by other removals.
154            if let Some(new_id) = graph_id_remapping.get(&removed_handle) {
155                removed.graph_index = *new_id
156            }
157
158            // Activate the bodies in contact with the deleted collider.
159            for (coll1, coll2, _, _) in self.interactions.contacts_with(removed.graph_index, false)
160            {
161                if coll1 == removed_handle {
162                    if let Some(coll) = colliders.get(coll2) {
163                        if let Some(body) = bodies.get_mut(coll.body()) {
164                            body.activate()
165                        }
166                    }
167                }
168
169                if coll2 == removed_handle {
170                    if let Some(coll) = colliders.get(coll1) {
171                        if let Some(body) = bodies.get_mut(coll.body()) {
172                            body.activate()
173                        }
174                    }
175                }
176            }
177
178            // Activate the body the deleted collider was attached to.
179            if let Some(body) = bodies.get_mut(removed.anchor.body()) {
180                // Update the parent body's inertia.
181                if !removed.density.is_zero() {
182                    if let ColliderAnchor::OnBodyPart {
183                        body_part,
184                        position_wrt_body_part,
185                    } = &removed.anchor
186                    {
187                        let (com, inertia) = removed
188                            .shape
189                            .transformed_mass_properties(removed.density, position_wrt_body_part);
190                        body.add_local_inertia_and_com(body_part.1, -com, -inertia)
191                    }
192                }
193
194                body.activate()
195            }
196
197            // Remove the collider from the list of colliders for this body.
198            match self.body_colliders.entry(removed.anchor.body()) {
199                hash_map::Entry::Occupied(mut e) => {
200                    if let Some(i) = e.get().iter().position(|h| *h == removed_handle) {
201                        let _ = e.get_mut().swap_remove(i);
202                    }
203
204                    if e.get().is_empty() {
205                        let _ = e.remove_entry();
206                    }
207                }
208                hash_map::Entry::Vacant(_) => {}
209            }
210
211            // Remove proxies and handle the graph index remapping.
212            if let Some(to_change) = pipeline::remove_proxies(
213                &mut *self.broad_phase,
214                &mut self.interactions,
215                removed.proxy_handle,
216                removed.graph_index,
217            ) {
218                if let Some(collider) = colliders.get_mut(to_change.0) {
219                    // Apply the graph index remapping.
220                    collider.set_graph_index(Some(to_change.1))
221                } else {
222                    // Register the graph index remapping for other removed colliders.
223                    let _ = graph_id_remapping.insert(to_change.0, to_change.1);
224                }
225            }
226        }
227    }
228
229    /// Synchronize all colliders with their body parent and the underlying collision world.
230    pub fn sync_colliders<Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
231        &mut self,
232        bodies: &dyn BodySet<N, Handle = Handle>,
233        colliders: &mut Colliders,
234    ) {
235        colliders.foreach_mut(|_collider_id, collider| {
236            let body = try_ret!(bodies.get(collider.body()));
237
238            collider.set_body_status_dependent_ndofs(body.status_dependent_ndofs());
239
240            if !body.update_status().colliders_need_update() {
241                return;
242            }
243
244            let new_pos = match collider.anchor() {
245                ColliderAnchor::OnBodyPart {
246                    body_part,
247                    position_wrt_body_part,
248                } => {
249                    let part = try_ret!(body.part(body_part.1));
250                    let part_pos1 = part.safe_position();
251                    let part_pos2 = part.position();
252                    Some((
253                        part_pos1 * position_wrt_body_part,
254                        part_pos2 * position_wrt_body_part,
255                    ))
256                }
257                ColliderAnchor::OnDeformableBody { .. } => None,
258            };
259
260            match new_pos {
261                Some(pos) => collider.set_position_with_prediction(pos.0, pos.1),
262                None => collider.set_deformations(body.deformed_positions().unwrap().1),
263            }
264        });
265    }
266
267    /// Returns the set of colliders attached to the specified body.
268    ///
269    /// Returns `None` if the body has no collider attached to it, of if the body does not exist.
270    pub fn body_colliders(&self, body: Handle) -> Option<&[CollHandle]> {
271        self.body_colliders.get(&body).map(|c| &c[..])
272    }
273
274    /*
275        /// Customize the selection of narrow-phase collision detection algorithms
276        pub fn set_narrow_phase(&mut self, narrow_phase: NarrowPhase<N, CollHandle>) {
277            self.gworld.set_narrow_phase(narrow_phase);
278        }
279    */
280    /// Empty the contact and proximity event pools.
281    pub fn clear_events(&mut self) {
282        self.narrow_phase.clear_events()
283    }
284    /// Executes the broad phase of the collision detection pipeline.
285    pub fn perform_broad_phase<Bodies, Colliders, Filter>(
286        &mut self,
287        bodies: &Bodies,
288        colliders: &Colliders,
289        user_filter: &Filter,
290    ) where
291        Bodies: BodySet<N, Handle = Handle>,
292        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
293        Filter: for<'a> BroadPhasePairFilter<N, BroadPhasePairFilterSets<'a, N, Bodies, Colliders>>
294            + ?Sized,
295    {
296        let pair_filter = DefaultCollisionFilter {
297            user_filter,
298            _pd: PhantomData,
299        };
300
301        pipeline::perform_broad_phase(
302            &BroadPhasePairFilterSets {
303                bodies,
304                colliders,
305                _pd: PhantomData,
306            },
307            &mut *self.broad_phase,
308            &mut self.narrow_phase,
309            &mut self.interactions,
310            Some(&pair_filter),
311        )
312    }
313
314    /// Executes the narrow phase of the collision detection pipeline.
315    pub fn perform_narrow_phase<Colliders>(&mut self, colliders: &Colliders)
316    where
317        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
318    {
319        pipeline::perform_narrow_phase(colliders, &mut self.narrow_phase, &mut self.interactions)
320    }
321
322    /// The broad-phase used by this geometrical world.
323    pub fn broad_phase(&self) -> &dyn BroadPhase<N, AABB<N>, CollHandle> {
324        &*self.broad_phase
325    }
326
327    /// Computes the interferences between every rigid bodies on this world and a ray.
328    #[inline]
329    pub fn interferences_with_ray<
330        'a,
331        'b,
332        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
333    >(
334        &'a self,
335        colliders: &'a Colliders,
336        ray: &'b Ray<N>,
337        max_toi: N,
338        groups: &'b CollisionGroups,
339    ) -> pipeline::InterferencesWithRay<'a, 'b, N, Colliders> {
340        pipeline::interferences_with_ray(&colliders, &*self.broad_phase, ray, max_toi, groups)
341    }
342
343    /// Computes the interferences between every rigid bodies of a given broad phase, and a point.
344    #[inline]
345    pub fn interferences_with_point<
346        'a,
347        'b,
348        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
349    >(
350        &'a self,
351        colliders: &'a Colliders,
352        point: &'b Point<N>,
353        groups: &'b CollisionGroups,
354    ) -> pipeline::InterferencesWithPoint<'a, 'b, N, Colliders> {
355        pipeline::interferences_with_point(&colliders, &*self.broad_phase, point, groups)
356    }
357
358    /// Computes the interferences between every rigid bodies of a given broad phase, and a aabb.
359    #[inline]
360    pub fn interferences_with_aabb<
361        'a,
362        'b,
363        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
364    >(
365        &'a self,
366        colliders: &'a Colliders,
367        aabb: &'b AABB<N>,
368        groups: &'b CollisionGroups,
369    ) -> pipeline::InterferencesWithAABB<'a, 'b, N, Colliders> {
370        pipeline::interferences_with_aabb(&colliders, &*self.broad_phase, aabb, groups)
371    }
372
373    /// The contact events pool.
374    pub fn contact_events(&self) -> &ContactEvents<CollHandle> {
375        self.narrow_phase.contact_events()
376    }
377
378    /// The proximity events pool.
379    pub fn proximity_events(&self) -> &ProximityEvents<CollHandle> {
380        self.narrow_phase.proximity_events()
381    }
382
383    /*
384     *
385     * Iterators on contacts/proximity pairs.
386     *
387     */
388    fn is_interaction_effective(
389        c1: &Collider<N, Handle>,
390        c2: &Collider<N, Handle>,
391        interaction: &Interaction<N>,
392    ) -> bool {
393        match interaction {
394            Interaction::Contact(_, manifold) => Self::is_contact_effective(c1, c2, manifold),
395            Interaction::Proximity(_, prox) => *prox == Proximity::Intersecting,
396        }
397    }
398
399    fn is_contact_effective(
400        c1: &Collider<N, Handle>,
401        c2: &Collider<N, Handle>,
402        manifold: &ContactManifold<N>,
403    ) -> bool {
404        if let Some(c) = manifold.deepest_contact() {
405            c.contact.depth >= -(c1.margin() + c2.margin())
406        } else {
407            false
408        }
409    }
410
411    #[inline(always)]
412    fn filter_interactions<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
413        colliders: &'a Colliders,
414        iter: impl Iterator<Item = (CollHandle, CollHandle, &'a Interaction<N>)>,
415        effective_only: bool,
416    ) -> impl Iterator<
417        Item = (
418            CollHandle,
419            &'a Collider<N, Handle>,
420            CollHandle,
421            &'a Collider<N, Handle>,
422            &'a Interaction<N>,
423        ),
424    > {
425        iter.filter_map(move |inter| {
426            let c1 = colliders.get(inter.0)?;
427            let c2 = colliders.get(inter.1)?;
428            if !effective_only || Self::is_interaction_effective(c1, c2, inter.2) {
429                Some((inter.0, c1, inter.1, c2, inter.2))
430            } else {
431                None
432            }
433        })
434    }
435
436    #[inline(always)]
437    fn filter_contacts<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
438        colliders: &'a Colliders,
439        iter: impl Iterator<
440            Item = (
441                CollHandle,
442                CollHandle,
443                &'a ContactAlgorithm<N>,
444                &'a ContactManifold<N>,
445            ),
446        >,
447        effective_only: bool,
448    ) -> impl Iterator<
449        Item = (
450            CollHandle,
451            &'a Collider<N, Handle>,
452            CollHandle,
453            &'a Collider<N, Handle>,
454            &'a ContactAlgorithm<N>,
455            &'a ContactManifold<N>,
456        ),
457    > {
458        iter.filter_map(move |inter| {
459            let c1 = colliders.get(inter.0)?;
460            let c2 = colliders.get(inter.1)?;
461            if !effective_only || Self::is_contact_effective(c1, c2, inter.3) {
462                Some((inter.0, c1, inter.1, c2, inter.2, inter.3))
463            } else {
464                None
465            }
466        })
467    }
468
469    #[inline(always)]
470    fn filter_proximities<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
471        colliders: &'a Colliders,
472        iter: impl Iterator<
473            Item = (
474                CollHandle,
475                CollHandle,
476                &'a dyn ProximityDetector<N>,
477                Proximity,
478            ),
479        >,
480    ) -> impl Iterator<
481        Item = (
482            CollHandle,
483            &'a Collider<N, Handle>,
484            CollHandle,
485            &'a Collider<N, Handle>,
486            &'a dyn ProximityDetector<N>,
487            Proximity,
488        ),
489    > {
490        iter.filter_map(move |prox| {
491            Some((
492                prox.0,
493                colliders.get(prox.0)?,
494                prox.1,
495                colliders.get(prox.1)?,
496                prox.2,
497                prox.3,
498            ))
499        })
500    }
501
502    // FIXME: we need to be careful with the notion of "effective_only" when dealing with
503    // contacts. Perhaps we should filter out contacts with depths that are not considered
504    // as actual contacts by the solver?
505
506    /// All the potential interactions pairs.
507    ///
508    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
509    /// for details.
510    pub fn interaction_pairs<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
511        &'a self,
512        colliders: &'a Colliders,
513        effective_only: bool,
514    ) -> impl Iterator<
515        Item = (
516            CollHandle,
517            &'a Collider<N, Handle>,
518            CollHandle,
519            &'a Collider<N, Handle>,
520            &'a Interaction<N>,
521        ),
522    > {
523        Self::filter_interactions(
524            colliders,
525            self.interactions.interaction_pairs(false),
526            effective_only,
527        )
528    }
529
530    /// All the potential contact pairs.
531    ///
532    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
533    /// for details.
534    pub fn contact_pairs<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
535        &'a self,
536        colliders: &'a Colliders,
537        effective_only: bool,
538    ) -> impl Iterator<
539        Item = (
540            CollHandle,
541            &'a Collider<N, Handle>,
542            CollHandle,
543            &'a Collider<N, Handle>,
544            &'a ContactAlgorithm<N>,
545            &'a ContactManifold<N>,
546        ),
547    > {
548        Self::filter_contacts(
549            colliders,
550            self.interactions.contact_pairs(false),
551            effective_only,
552        )
553    }
554
555    /// All the potential proximity pairs.
556    ///
557    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
558    /// for details.
559    pub fn proximity_pairs<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
560        &'a self,
561        colliders: &'a Colliders,
562        effective_only: bool,
563    ) -> impl Iterator<
564        Item = (
565            CollHandle,
566            &'a Collider<N, Handle>,
567            CollHandle,
568            &'a Collider<N, Handle>,
569            &'a dyn ProximityDetector<N>,
570            Proximity,
571        ),
572    > {
573        Self::filter_proximities(colliders, self.interactions.proximity_pairs(effective_only))
574    }
575
576    /// The potential interaction pair between the two specified colliders.
577    ///
578    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
579    /// for details.
580    pub fn interaction_pair<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
581        &'a self,
582        colliders: &'a Colliders,
583        handle1: CollHandle,
584        handle2: CollHandle,
585        effective_only: bool,
586    ) -> Option<(
587        CollHandle,
588        &'a Collider<N, Handle>,
589        CollHandle,
590        &'a Collider<N, Handle>,
591        &'a Interaction<N>,
592    )> {
593        let id1 = colliders
594            .get(handle1)?
595            .graph_index()
596            .expect(crate::NOT_REGISTERED_ERROR);
597        let id2 = colliders
598            .get(handle2)?
599            .graph_index()
600            .expect(crate::NOT_REGISTERED_ERROR);
601
602        self.interactions
603            .interaction_pair(id1, id2, false)
604            .and_then(move |inter| {
605                let c1 = colliders.get(inter.0)?;
606                let c2 = colliders.get(inter.1)?;
607
608                if !effective_only || Self::is_interaction_effective(c1, c2, inter.2) {
609                    Some((inter.0, c1, inter.1, c2, inter.2))
610                } else {
611                    None
612                }
613            })
614    }
615
616    /// The potential contact pair between the two specified colliders.
617    ///
618    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
619    /// for details.
620    pub fn contact_pair<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
621        &'a self,
622        colliders: &'a Colliders,
623        handle1: CollHandle,
624        handle2: CollHandle,
625        effective_only: bool,
626    ) -> Option<(
627        CollHandle,
628        &'a Collider<N, Handle>,
629        CollHandle,
630        &'a Collider<N, Handle>,
631        &'a ContactAlgorithm<N>,
632        &'a ContactManifold<N>,
633    )> {
634        let id1 = colliders
635            .get(handle1)?
636            .graph_index()
637            .expect(crate::NOT_REGISTERED_ERROR);
638        let id2 = colliders
639            .get(handle2)?
640            .graph_index()
641            .expect(crate::NOT_REGISTERED_ERROR);
642
643        self.interactions
644            .contact_pair(id1, id2, false)
645            .and_then(move |inter| {
646                let c1 = colliders.get(inter.0)?;
647                let c2 = colliders.get(inter.1)?;
648                if !effective_only || Self::is_contact_effective(c1, c2, inter.3) {
649                    Some((inter.0, c1, inter.1, c2, inter.2, inter.3))
650                } else {
651                    None
652                }
653            })
654    }
655
656    /// The potential proximity pair between the two specified colliders.
657    ///
658    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
659    /// for details.
660    pub fn proximity_pair<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
661        &'a self,
662        colliders: &'a Colliders,
663        handle1: CollHandle,
664        handle2: CollHandle,
665        effective_only: bool,
666    ) -> Option<(
667        CollHandle,
668        &'a Collider<N, Handle>,
669        CollHandle,
670        &'a Collider<N, Handle>,
671        &'a dyn ProximityDetector<N>,
672        Proximity,
673    )> {
674        let id1 = colliders
675            .get(handle1)?
676            .graph_index()
677            .expect(crate::NOT_REGISTERED_ERROR);
678        let id2 = colliders
679            .get(handle2)?
680            .graph_index()
681            .expect(crate::NOT_REGISTERED_ERROR);
682
683        self.interactions
684            .proximity_pair(id1, id2, effective_only)
685            .and_then(move |prox| {
686                Some((
687                    prox.0,
688                    colliders.get(prox.0)?,
689                    prox.1,
690                    colliders.get(prox.1)?,
691                    prox.2,
692                    prox.3,
693                ))
694            })
695    }
696
697    /// All the interaction pairs involving the specified collider.
698    ///
699    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
700    /// for details.
701    pub fn interactions_with<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
702        &'a self,
703        colliders: &'a Colliders,
704        handle: CollHandle,
705        effective_only: bool,
706    ) -> Option<
707        impl Iterator<
708            Item = (
709                CollHandle,
710                &'a Collider<N, Handle>,
711                CollHandle,
712                &'a Collider<N, Handle>,
713                &'a Interaction<N>,
714            ),
715        >,
716    > {
717        let idx = colliders
718            .get(handle)?
719            .graph_index()
720            .expect(crate::NOT_REGISTERED_ERROR);
721        Some(Self::filter_interactions(
722            colliders,
723            self.interactions.interactions_with(idx, false),
724            effective_only,
725        ))
726    }
727
728    /// All the contact pairs involving the specified collider.
729    ///
730    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
731    /// for details.
732    pub fn contacts_with<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
733        &'a self,
734        colliders: &'a Colliders,
735        handle: CollHandle,
736        effective_only: bool,
737    ) -> Option<
738        impl Iterator<
739            Item = (
740                CollHandle,
741                &'a Collider<N, Handle>,
742                CollHandle,
743                &'a Collider<N, Handle>,
744                &'a ContactAlgorithm<N>,
745                &'a ContactManifold<N>,
746            ),
747        >,
748    > {
749        let idx = colliders
750            .get(handle)?
751            .graph_index()
752            .expect(crate::NOT_REGISTERED_ERROR);
753        Some(Self::filter_contacts(
754            colliders,
755            self.interactions.contacts_with(idx, false),
756            effective_only,
757        ))
758    }
759
760    /// All the proximity pairs involving the specified collider.
761    ///
762    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
763    /// for details.
764    pub fn proximities_with<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
765        &'a self,
766        colliders: &'a Colliders,
767        handle: CollHandle,
768        effective_only: bool,
769    ) -> Option<
770        impl Iterator<
771            Item = (
772                CollHandle,
773                &'a Collider<N, Handle>,
774                CollHandle,
775                &'a Collider<N, Handle>,
776                &'a dyn ProximityDetector<N>,
777                Proximity,
778            ),
779        >,
780    > {
781        let idx = colliders
782            .get(handle)?
783            .graph_index()
784            .expect(crate::NOT_REGISTERED_ERROR);
785        Some(Self::filter_proximities(
786            colliders,
787            self.interactions.proximities_with(idx, effective_only),
788        ))
789    }
790
791    /// All the collider handles of colliders interacting with the specified collider.
792    ///
793    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
794    /// for details.
795    pub fn colliders_interacting_with<
796        'a,
797        Colliders: ColliderSet<N, Handle, Handle = CollHandle>,
798    >(
799        &'a self,
800        colliders: &'a Colliders,
801        handle: CollHandle,
802    ) -> Option<impl Iterator<Item = (CollHandle, &'a Collider<N, Handle>)>> {
803        Some(
804            self.interactions_with(colliders, handle, true)?
805                .map(
806                    move |(h1, c1, h2, c2, _)| {
807                        if h1 == handle {
808                            (h2, c2)
809                        } else {
810                            (h1, c1)
811                        }
812                    },
813                ),
814        )
815    }
816
817    /// All the collider handles of colliders in potential contact with the specified collision
818    /// object.
819    ///
820    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
821    /// for details.
822    pub fn colliders_in_contact_with<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
823        &'a self,
824        colliders: &'a Colliders,
825        handle: CollHandle,
826    ) -> Option<impl Iterator<Item = (CollHandle, &'a Collider<N, Handle>)>> {
827        Some(
828            self.contacts_with(colliders, handle, true)?
829                .map(
830                    move |(h1, c1, h2, c2, _, _)| {
831                        if h1 == handle {
832                            (h2, c2)
833                        } else {
834                            (h1, c1)
835                        }
836                    },
837                ),
838        )
839    }
840
841    /// All the collider handles of colliders in potential proximity of with the specified
842    /// collider.
843    ///
844    /// Refer to the official [user guide](https://nphysics.org/interaction_handling_and_sensors/#interaction-iterators)
845    /// for details.
846    pub fn colliders_in_proximity_of<'a, Colliders: ColliderSet<N, Handle, Handle = CollHandle>>(
847        &'a self,
848        colliders: &'a Colliders,
849        handle: CollHandle,
850    ) -> Option<impl Iterator<Item = (CollHandle, &'a Collider<N, Handle>)>> {
851        Some(
852            self.proximities_with(colliders, handle, true)?
853                .map(
854                    move |(h1, c1, h2, c2, _, _)| {
855                        if h1 == handle {
856                            (h2, c2)
857                        } else {
858                            (h1, c1)
859                        }
860                    },
861                ),
862        )
863    }
864}
865
866/// The default set of bodies and colliders used within broad phase collision filtering.
867pub type DefaultBroadPhasePairFilterSets<'a, N> =
868    BroadPhasePairFilterSets<'a, N, DefaultBodySet<N>, DefaultColliderSet<N>>;
869
870/// The set of bodies and colliders used within broad phase collision filtering.
871pub struct BroadPhasePairFilterSets<'a, N, Bodies, Colliders>
872where
873    N: RealField + Copy,
874    Bodies: BodySet<N>,
875    Colliders: ColliderSet<N, Bodies::Handle>,
876{
877    /// The set of colliders within the simulation.
878    colliders: &'a Colliders,
879
880    /// The set of bodies within the simulation.
881    bodies: &'a Bodies,
882
883    /// Makes the compiler happy.
884    _pd: PhantomData<N>,
885}
886
887impl<'a, N, Bodies, Colliders> BroadPhasePairFilterSets<'a, N, Bodies, Colliders>
888where
889    N: RealField + Copy,
890    Bodies: BodySet<N>,
891    Colliders: ColliderSet<N, Bodies::Handle>,
892{
893    /// Returns the body set used in the physics step.
894    pub fn bodies(&self) -> &Bodies {
895        self.bodies
896    }
897
898    /// Returns the collider set used in the physics step.
899    pub fn colliders(&self) -> &Colliders {
900        self.colliders
901    }
902}
903
904impl<'a, N, Bodies, Colliders> CollisionObjectSet<N>
905    for BroadPhasePairFilterSets<'a, N, Bodies, Colliders>
906where
907    N: RealField + Copy,
908    Bodies: BodySet<N>,
909    Colliders: ColliderSet<N, Bodies::Handle>,
910{
911    type CollisionObject = Collider<N, Bodies::Handle>;
912    type CollisionObjectHandle = Colliders::Handle;
913
914    fn collision_object(
915        &self,
916        handle: Self::CollisionObjectHandle,
917    ) -> Option<&Self::CollisionObject> {
918        self.colliders.get(handle)
919    }
920
921    fn foreach(&self, f: impl FnMut(Self::CollisionObjectHandle, &Self::CollisionObject)) {
922        CollisionObjectSet::foreach(self.colliders, f)
923    }
924}
925
926struct DefaultCollisionFilter<'a, Filter: ?Sized, Handle> {
927    user_filter: &'a Filter,
928
929    /// Makes the compiler happy.
930    _pd: PhantomData<Handle>,
931}
932
933impl<'a, N, Handle, Set, Filter> BroadPhasePairFilter<N, Set>
934    for DefaultCollisionFilter<'a, Filter, Handle>
935where
936    N: RealField + Copy,
937    Handle: BodyHandle,
938    Set: CollisionObjectSet<N, CollisionObject = Collider<N, Handle>>,
939    Filter: BroadPhasePairFilter<N, Set> + ?Sized,
940{
941    fn is_pair_valid(
942        &self,
943        h1: Set::CollisionObjectHandle,
944        h2: Set::CollisionObjectHandle,
945        set: &Set,
946    ) -> bool {
947        let (c1, c2) = match (set.collision_object(h1), set.collision_object(h2)) {
948            (Some(c1), Some(c2)) => (c1, c2),
949            _ => return false,
950        };
951
952        match (c1.anchor(), c2.anchor()) {
953            (
954                ColliderAnchor::OnBodyPart {
955                    body_part: part1, ..
956                },
957                ColliderAnchor::OnBodyPart {
958                    body_part: part2, ..
959                },
960            ) => {
961                if part1 == part2 {
962                    return false;
963                }
964            }
965            _ => {}
966        }
967
968        (c1.body_status_dependent_ndofs() != 0 || c2.body_status_dependent_ndofs() != 0)
969            && self.user_filter.is_pair_valid(h1, h2, set)
970    }
971}
nphysics2d/world/geometrical_world.rs

nphysics2d/world/
geometrical_world.rs
