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
11pub type CollisionObjectGraphIndex = NodeIndex<usize>;
13pub type TemporaryInteractionIndex = EdgeIndex<usize>;
15
16pub enum Interaction<N: RealField + Copy> {
18 Contact(ContactAlgorithm<N>, ContactManifold<N>),
23 Proximity(ProximityAlgorithm<N>, Proximity),
28}
29
30impl<N: RealField + Copy> Interaction<N> {
31 pub fn is_contact(&self) -> bool {
33 match self {
34 Interaction::Contact(..) => true,
35 _ => false,
36 }
37 }
38
39 pub fn is_proximity(&self) -> bool {
41 match self {
42 Interaction::Proximity(..) => true,
43 _ => false,
44 }
45 }
46}
47
48pub 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 pub fn new() -> Self {
56 InteractionGraph(UnGraph::with_capacity(10, 10))
57 }
58
59 pub fn add_node(&mut self, handle: Handle) -> CollisionObjectGraphIndex {
61 self.0.add_node(handle)
62 }
63
64 #[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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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}