ncollide2d/pipeline/narrow_phase/contact_generator/
composite_shape_composite_shape_manifold_generator.rs

1use crate::math::Isometry;
2use crate::pipeline::narrow_phase::{
3    ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator,
4};
5use crate::query::{
6    visitors::AABBSetsInterferencesCollector, ContactManifold, ContactPrediction,
7    ContactPreprocessor,
8};
9use crate::shape::{CompositeShape, Shape};
10use crate::utils::DeterministicState;
11use na::RealField;
12use std::collections::{hash_map::Entry, HashMap};
13
14/// Collision detector between a concave shape and another shape.
15pub struct CompositeShapeCompositeShapeManifoldGenerator<N> {
16    sub_detectors: HashMap<(usize, usize), (ContactAlgorithm<N>, usize), DeterministicState>,
17    interferences: Vec<(usize, usize)>,
18    timestamp: usize,
19}
20
21impl<N> CompositeShapeCompositeShapeManifoldGenerator<N> {
22    /// Creates a new collision detector between a concave shape and another shape.
23    pub fn new() -> CompositeShapeCompositeShapeManifoldGenerator<N> {
24        CompositeShapeCompositeShapeManifoldGenerator {
25            sub_detectors: HashMap::with_hasher(DeterministicState),
26            interferences: Vec::new(),
27            timestamp: 0,
28        }
29    }
30}
31
32impl<N: RealField + Copy> CompositeShapeCompositeShapeManifoldGenerator<N> {
33    fn do_update(
34        &mut self,
35        dispatcher: &dyn ContactDispatcher<N>,
36        m1: &Isometry<N>,
37        g1: &dyn CompositeShape<N>,
38        proc1: Option<&dyn ContactPreprocessor<N>>,
39        m2: &Isometry<N>,
40        g2: &dyn CompositeShape<N>,
41        proc2: Option<&dyn ContactPreprocessor<N>>,
42        prediction: &ContactPrediction<N>,
43        manifold: &mut ContactManifold<N>,
44    ) {
45        self.timestamp += 1;
46
47        // Find new collisions
48        let ls_m2 = m1.inverse() * m2;
49        // For transforming AABBs from g2 in the local space of g1.
50        let ls_m2_abs_rot = ls_m2.rotation.to_rotation_matrix().matrix().abs();
51
52        {
53            let mut visitor = AABBSetsInterferencesCollector::new(
54                prediction.linear(),
55                &ls_m2,
56                &ls_m2_abs_rot,
57                &mut self.interferences,
58            );
59            g1.bvh().visit_bvtt(g2.bvh(), &mut visitor);
60        }
61
62        for id in self.interferences.drain(..) {
63            match self.sub_detectors.entry(id) {
64                Entry::Occupied(mut entry) => {
65                    entry.get_mut().1 = self.timestamp;
66                }
67                Entry::Vacant(entry) => {
68                    let mut new_detector = None;
69
70                    g1.map_part_at(id.0, &Isometry::identity(), &mut |_, g1| {
71                        g2.map_part_at(id.1, &Isometry::identity(), &mut |_, g2| {
72                            new_detector = dispatcher.get_contact_algorithm(g1, g2)
73                        });
74                    });
75
76                    if let Some(new_detector) = new_detector {
77                        let _ = entry.insert((new_detector, self.timestamp));
78                    }
79                }
80            }
81        }
82
83        // Update all collisions
84        let timestamp = self.timestamp;
85        self.sub_detectors.retain(|key, detector| {
86            if detector.1 != timestamp {
87                false
88            } else {
89                let mut keep = false;
90                g1.map_part_and_preprocessor_at(
91                    key.0,
92                    m1,
93                    prediction,
94                    &mut |m1, g1, part_proc1| {
95                        g2.map_part_and_preprocessor_at(
96                            key.1,
97                            m2,
98                            prediction,
99                            &mut |m2, g2, part_proc2| {
100                                // FIXME: change the update functions.
101                                keep = detector.0.generate_contacts(
102                                    dispatcher,
103                                    m1,
104                                    g1,
105                                    Some(&(proc1, part_proc1)),
106                                    m2,
107                                    g2,
108                                    Some(&(proc2, part_proc2)),
109                                    prediction,
110                                    manifold,
111                                );
112                            },
113                        );
114                    },
115                );
116
117                keep
118            }
119        });
120    }
121}
122
123impl<N: RealField + Copy> ContactManifoldGenerator<N>
124    for CompositeShapeCompositeShapeManifoldGenerator<N>
125{
126    fn generate_contacts(
127        &mut self,
128        d: &dyn ContactDispatcher<N>,
129        ma: &Isometry<N>,
130        a: &dyn Shape<N>,
131        proc1: Option<&dyn ContactPreprocessor<N>>,
132        mb: &Isometry<N>,
133        b: &dyn Shape<N>,
134        proc2: Option<&dyn ContactPreprocessor<N>>,
135        prediction: &ContactPrediction<N>,
136        manifold: &mut ContactManifold<N>,
137    ) -> bool {
138        if let (Some(csa), Some(csb)) = (a.as_composite_shape(), b.as_composite_shape()) {
139            self.do_update(d, ma, csa, proc1, mb, csb, proc2, prediction, manifold);
140            true
141        } else {
142            false
143        }
144    }
145}
ncollide2d/pipeline/narrow_phase/contact_generator/composite_shape_composite_shape_manifold_generator.rs

ncollide2d/pipeline/narrow_phase/contact_generator/
composite_shape_composite_shape_manifold_generator.rs
