ncollide3d/pipeline/narrow_phase/
interaction_graph.rs

Help
1use na::RealField;
2use petgraph::graph::{NodeIndex, UnGraph};
3use petgraph::visit::EdgeRef;
4
5use crate::pipeline::narrow_phase::{ContactAlgorithm, ProximityAlgorithm, ProximityDetector};
6use crate::pipeline::object::CollisionObjectHandle;
7use crate::query::{ContactManifold, Proximity};
8use petgraph::prelude::EdgeIndex;
9use petgraph::Direction;
10
11/// Index of a node of the interaction graph.
12pub type CollisionObjectGraphIndex = NodeIndex<usize>;
13/// Temporary index to and edge of the interaction graph.
14pub type TemporaryInteractionIndex = EdgeIndex<usize>;
15
16/// An interaction between two collision objects.
17pub enum Interaction<N: RealField + Copy> {
18    /// A potential contact between two collision objects.
19    ///
20    /// Generated only for pairs of collision objects both configured
21    /// with a `GeometricQueryType::Contact(..)`.
22    Contact(ContactAlgorithm<N>, ContactManifold<N>),
23    /// A proximity between two collision objects.
24    ///
25    /// Generated only for pairs of collision objects with at least one configured
26    /// with a `GeometricQueryType::Contact(..)`.
27    Proximity(ProximityAlgorithm<N>, Proximity),
28}
29
30impl<N: RealField + Copy> Interaction<N> {
31    /// Checks if this interaction is a potential contact interaction.
32    pub fn is_contact(&self) -> bool {
33        match self {
34            Interaction::Contact(..) => true,
35            _ => false,
36        }
37    }
38
39    /// Checks if this interaction is a potential proximity interaction.
40    pub fn is_proximity(&self) -> bool {
41        match self {
42            Interaction::Proximity(..) => true,
43            _ => false,
44        }
45    }
46}
47
48/// A graph where nodes are collision objects and edges are contact or proximity algorithms.
49pub struct InteractionGraph<N: RealField + Copy, Handle: CollisionObjectHandle>(
50    pub(crate) UnGraph<Handle, Interaction<N>, usize>,
51);
52
53impl<N: RealField + Copy, Handle: CollisionObjectHandle> InteractionGraph<N, Handle> {
54    /// Creates a new empty collection of collision objects.
55    pub fn new() -> Self {
56        InteractionGraph(UnGraph::with_capacity(10, 10))
57    }
58
59    /// Adds a handle to this graph.
60    pub fn add_node(&mut self, handle: Handle) -> CollisionObjectGraphIndex {
61        self.0.add_node(handle)
62    }
63
64    /// Removes a handle from this graph and returns a handle that must have its graph index changed to `id`.
65    ///
66    /// When a node is removed, another node of the graph takes it place. This means that the `CollisionObjectGraphIndex`
67    /// of the collision object returned by this method will be equal to `id`. Thus if you maintain
68    /// a map between `CollisionObjectSlabHandle` and `CollisionObjectGraphIndex`, then you should update this
69    /// map to associate `id` to the handle returned by this method. For example:
70    ///
71    /// ```.ignore
72    /// // Let `id` be the graph index of the collision object we want to remove.
73    /// if let Some(other_handle) = graph.remove_node(id) {
74    ///    // The graph index of `other_handle` changed to `id` due to the removal.
75    ///    map.insert(other_handle, id) ;
76    /// }
77    /// ```
78    #[must_use = "The graph index of the collision object returned by this method has been changed to `id`."]
79    pub fn remove_node(&mut self, id: CollisionObjectGraphIndex) -> Option<Handle> {
80        let _ = self.0.remove_node(id);
81        self.0.node_weight(id).cloned()
82    }
83
84    /// All the interactions pairs on this graph.
85    ///
86    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
87    /// for details.
88    pub fn interaction_pairs(
89        &self,
90        effective_only: bool,
91    ) -> impl Iterator<Item = (Handle, Handle, &Interaction<N>)> {
92        self.0.edge_references().filter_map(move |e| {
93            let interaction = e.weight();
94
95            if !effective_only || Self::is_interaction_effective(interaction) {
96                Some((self.0[e.source()], self.0[e.target()], e.weight()))
97            } else {
98                None
99            }
100        })
101    }
102
103    /// All the contact pairs on this graph.
104    ///
105    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
106    /// for details.
107    pub fn contact_pairs(
108        &self,
109        effective_only: bool,
110    ) -> impl Iterator<Item = (Handle, Handle, &ContactAlgorithm<N>, &ContactManifold<N>)> {
111        self.interaction_pairs(effective_only)
112            .filter_map(|(h1, h2, inter)| match inter {
113                Interaction::Contact(algo, manifold) => Some((h1, h2, algo, manifold)),
114                _ => None,
115            })
116    }
117
118    /// All the proximity pairs on this graph.
119    ///
120    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
121    /// for details.
122    pub fn proximity_pairs(
123        &self,
124        effective_only: bool,
125    ) -> impl Iterator<Item = (Handle, Handle, &dyn ProximityDetector<N>, Proximity)> {
126        self.interaction_pairs(effective_only)
127            .filter_map(|(h1, h2, inter)| match inter {
128                Interaction::Proximity(algo, prox) => Some((h1, h2, &**algo, *prox)),
129                _ => None,
130            })
131    }
132
133    /// The interaction between the two collision objects identified by their graph index.
134    ///
135    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
136    /// for details.
137    pub fn interaction_pair(
138        &self,
139        id1: CollisionObjectGraphIndex,
140        id2: CollisionObjectGraphIndex,
141        effective_only: bool,
142    ) -> Option<(Handle, Handle, &Interaction<N>)> {
143        let inter = self.0.find_edge(id1, id2).and_then(|edge| {
144            let endpoints = self.0.edge_endpoints(edge)?;
145            let h1 = self.0.node_weight(endpoints.0)?;
146            let h2 = self.0.node_weight(endpoints.1)?;
147            Some((*h1, *h2, self.0.edge_weight(edge)?))
148        });
149
150        if effective_only {
151            inter.filter(|inter| Self::is_interaction_effective(inter.2))
152        } else {
153            inter
154        }
155    }
156
157    /// The interaction between the two collision objects identified by their graph index.
158    ///
159    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
160    /// for details.
161    pub fn interaction_pair_mut(
162        &mut self,
163        id1: CollisionObjectGraphIndex,
164        id2: CollisionObjectGraphIndex,
165    ) -> Option<(Handle, Handle, &mut Interaction<N>)> {
166        let edge = self.0.find_edge(id1, id2)?;
167        let endpoints = self.0.edge_endpoints(edge)?;
168        let h1 = self.0.node_weight(endpoints.0)?;
169        let h2 = self.0.node_weight(endpoints.1)?;
170        Some((*h1, *h2, self.0.edge_weight_mut(edge)?))
171    }
172
173    /// The contact pair between the two collision objects identified by their graph index.
174    ///
175    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
176    /// for details.
177    pub fn contact_pair(
178        &self,
179        id1: CollisionObjectGraphIndex,
180        id2: CollisionObjectGraphIndex,
181        effective_only: bool,
182    ) -> Option<(Handle, Handle, &ContactAlgorithm<N>, &ContactManifold<N>)> {
183        self.interaction_pair(id1, id2, effective_only)
184            .and_then(|inter| match inter.2 {
185                Interaction::Contact(algo, manifold) => Some((inter.0, inter.1, algo, manifold)),
186                _ => None,
187            })
188    }
189
190    /// The proximity pair between the two collision objects identified by their graph index.
191    ///
192    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
193    /// for details.
194    pub fn proximity_pair(
195        &self,
196        id1: CollisionObjectGraphIndex,
197        id2: CollisionObjectGraphIndex,
198        effective_only: bool,
199    ) -> Option<(Handle, Handle, &dyn ProximityDetector<N>, Proximity)> {
200        self.interaction_pair(id1, id2, effective_only)
201            .and_then(|inter| match inter.2 {
202                Interaction::Proximity(algo, prox) => Some((inter.0, inter.1, &**algo, *prox)),
203                _ => None,
204            })
205    }
206
207    /// The proximity pair between the two collision objects identified by their graph index.
208    ///
209    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
210    /// for details.
211    pub fn proximity_pair_mut(
212        &mut self,
213        id1: CollisionObjectGraphIndex,
214        id2: CollisionObjectGraphIndex,
215    ) -> Option<(
216        Handle,
217        Handle,
218        &mut dyn ProximityDetector<N>,
219        &mut Proximity,
220    )> {
221        let inter = self.interaction_pair_mut(id1, id2)?;
222        match inter.2 {
223            Interaction::Proximity(algo, prox) => Some((inter.0, inter.1, &mut **algo, prox)),
224            _ => None,
225        }
226    }
227
228    /// All the interaction involving the collision object with graph index `id`.
229    ///
230    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
231    /// for details.
232    pub fn interactions_with(
233        &self,
234        id: CollisionObjectGraphIndex,
235        effective_only: bool,
236    ) -> impl Iterator<Item = (Handle, Handle, &Interaction<N>)> {
237        self.0.edges(id).filter_map(move |e| {
238            let inter = e.weight();
239
240            if !effective_only || Self::is_interaction_effective(inter) {
241                let endpoints = self.0.edge_endpoints(e.id()).unwrap();
242                Some((self.0[endpoints.0], self.0[endpoints.1], e.weight()))
243            } else {
244                None
245            }
246        })
247    }
248
249    /// Gets the interaction with the given index.
250    pub fn index_interaction(
251        &self,
252        id: TemporaryInteractionIndex,
253    ) -> Option<(Handle, Handle, &Interaction<N>)> {
254        if let (Some(e), Some(endpoints)) = (self.0.edge_weight(id), self.0.edge_endpoints(id)) {
255            Some((self.0[endpoints.0], self.0[endpoints.1], e))
256        } else {
257            None
258        }
259    }
260
261    /// All the mutable references to interactions involving the collision object with graph index `id`.
262    ///
263    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
264    /// for details.
265    pub fn interactions_with_mut(
266        &mut self,
267        id: CollisionObjectGraphIndex,
268    ) -> impl Iterator<
269        Item = (
270            Handle,
271            Handle,
272            TemporaryInteractionIndex,
273            &mut Interaction<N>,
274        ),
275    > {
276        let incoming_edge = self.0.first_edge(id, Direction::Incoming);
277        let outgoing_edge = self.0.first_edge(id, Direction::Outgoing);
278
279        InteractionsWithMut {
280            graph: self,
281            incoming_edge,
282            outgoing_edge,
283        }
284    }
285
286    /// All the proximity pairs involving the collision object with graph index `id`.
287    ///
288    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
289    /// for details.
290    pub fn proximities_with(
291        &self,
292        handle: CollisionObjectGraphIndex,
293        effective_only: bool,
294    ) -> impl Iterator<Item = (Handle, Handle, &dyn ProximityDetector<N>, Proximity)> {
295        self.interactions_with(handle, effective_only)
296            .filter_map(|(h1, h2, inter)| match inter {
297                Interaction::Proximity(algo, prox) => Some((h1, h2, &**algo, *prox)),
298                _ => None,
299            })
300    }
301
302    /// All the contact pairs involving the collision object with graph index `id`.
303    ///
304    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
305    /// for details.
306    pub fn contacts_with(
307        &self,
308        handle: CollisionObjectGraphIndex,
309        effective_only: bool,
310    ) -> impl Iterator<Item = (Handle, Handle, &ContactAlgorithm<N>, &ContactManifold<N>)> {
311        self.interactions_with(handle, effective_only)
312            .filter_map(|(h1, h2, inter)| match inter {
313                Interaction::Contact(algo, manifold) => Some((h1, h2, algo, manifold)),
314                _ => None,
315            })
316    }
317
318    /// All the collision object handles of collision objects interacting with the collision object with graph index `id`.
319    ///
320    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
321    /// for details.
322    pub fn collision_objects_interacting_with<'a>(
323        &'a self,
324        id: CollisionObjectGraphIndex,
325    ) -> impl Iterator<Item = Handle> + 'a {
326        self.0.edges(id).filter_map(move |e| {
327            let inter = e.weight();
328
329            if Self::is_interaction_effective(inter) {
330                if e.source() == id {
331                    Some(self.0[e.target()])
332                } else {
333                    Some(self.0[e.source()])
334                }
335            } else {
336                None
337            }
338        })
339    }
340
341    /// All the collision object handles of collision objects in contact with the collision object with graph index `id`.
342    ///
343    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
344    /// for details.
345    pub fn collision_objects_in_contact_with<'a>(
346        &'a self,
347        id: CollisionObjectGraphIndex,
348    ) -> impl Iterator<Item = Handle> + 'a {
349        self.0.edges(id).filter_map(move |e| {
350            let inter = e.weight();
351
352            if inter.is_contact() && Self::is_interaction_effective(inter) {
353                if e.source() == id {
354                    Some(self.0[e.target()])
355                } else {
356                    Some(self.0[e.source()])
357                }
358            } else {
359                None
360            }
361        })
362    }
363
364    /// All the collision object handles of collision objects in proximity of with the collision object with graph index `id`.
365    ///
366    /// Refer to the official [user guide](https://ncollide.org/interaction_handling_and_sensors/#interaction-iterators)
367    /// for details.
368    pub fn collision_objects_in_proximity_of<'a>(
369        &'a self,
370        id: CollisionObjectGraphIndex,
371    ) -> impl Iterator<Item = Handle> + 'a {
372        self.0.edges(id).filter_map(move |e| {
373            if let Interaction::Proximity(_, prox) = e.weight() {
374                if *prox == Proximity::Intersecting {
375                    if e.source() == id {
376                        return Some(self.0[e.target()]);
377                    } else {
378                        return Some(self.0[e.source()]);
379                    }
380                }
381            }
382
383            None
384        })
385    }
386
387    // NOTE: we don't make this method public because different
388    // applications will have a different interpretation of when a
389    // contact is considered effective (for example in nphysics).
390    fn is_interaction_effective(interaction: &Interaction<N>) -> bool {
391        match interaction {
392            Interaction::Contact(_, manifold) => {
393                if let Some(ctct) = manifold.deepest_contact() {
394                    ctct.contact.depth >= N::zero()
395                } else {
396                    false
397                }
398            }
399            Interaction::Proximity(_, prox) => *prox == Proximity::Intersecting,
400        }
401    }
402}
403
404pub struct InteractionsWithMut<'a, N: RealField + Copy, Handle: CollisionObjectHandle> {
405    graph: &'a mut InteractionGraph<N, Handle>,
406    incoming_edge: Option<EdgeIndex<usize>>,
407    outgoing_edge: Option<EdgeIndex<usize>>,
408}
409
410impl<'a, N: RealField + Copy, Handle: CollisionObjectHandle> Iterator
411    for InteractionsWithMut<'a, N, Handle>
412{
413    type Item = (
414        Handle,
415        Handle,
416        TemporaryInteractionIndex,
417        &'a mut Interaction<N>,
418    );
419
420    #[inline]
421    fn next(
422        &mut self,
423    ) -> Option<(
424        Handle,
425        Handle,
426        TemporaryInteractionIndex,
427        &'a mut Interaction<N>,
428    )> {
429        if let Some(edge) = self.incoming_edge {
430            self.incoming_edge = self.graph.0.next_edge(edge, Direction::Incoming);
431            let endpoints = self.graph.0.edge_endpoints(edge).unwrap();
432            let (co1, co2) = (self.graph.0[endpoints.0], self.graph.0[endpoints.1]);
433            let interaction = self.graph.0.edge_weight_mut(edge)?;
434            return Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) }));
435        }
436
437        let edge = self.outgoing_edge?;
438        self.outgoing_edge = self.graph.0.next_edge(edge, Direction::Outgoing);
439        let endpoints = self.graph.0.edge_endpoints(edge).unwrap();
440        let (co1, co2) = (self.graph.0[endpoints.0], self.graph.0[endpoints.1]);
441        let interaction = self.graph.0.edge_weight_mut(edge)?;
442        Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) }))
443    }
444}
ncollide3d/pipeline/narrow_phase/interaction_graph.rs

ncollide3d/pipeline/narrow_phase/
interaction_graph.rs
