ncollide3d/pipeline/narrow_phase/contact_generator/
heightfield_shape_manifold_generator.rs

1use crate::bounding_volume::{self, BoundingVolume};
2use crate::math::Isometry;
3use crate::pipeline::{ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator};
4use crate::query::{ContactManifold, ContactPrediction, ContactPreprocessor};
5use crate::shape::{HeightField, Shape};
6use crate::utils::DeterministicState;
7use na::{self, RealField};
8use std::collections::{hash_map::Entry, HashMap};
9
10/// Collision detector between an heightfield and another shape.
11pub struct HeightFieldShapeManifoldGenerator<N: RealField + Copy> {
12    sub_detectors: HashMap<usize, (ContactAlgorithm<N>, usize), DeterministicState>,
13    flip: bool,
14    timestamp: usize,
15}
16
17impl<N: RealField + Copy> HeightFieldShapeManifoldGenerator<N> {
18    /// Creates a new collision detector between an heightfield and another shape.
19    pub fn new(flip: bool) -> HeightFieldShapeManifoldGenerator<N> {
20        HeightFieldShapeManifoldGenerator {
21            sub_detectors: HashMap::with_hasher(DeterministicState),
22            flip,
23            timestamp: 0,
24        }
25    }
26
27    fn do_update(
28        &mut self,
29        dispatcher: &dyn ContactDispatcher<N>,
30        m1: &Isometry<N>,
31        g1: &HeightField<N>,
32        proc1: Option<&dyn ContactPreprocessor<N>>,
33        m2: &Isometry<N>,
34        g2: &dyn Shape<N>,
35        proc2: Option<&dyn ContactPreprocessor<N>>,
36        prediction: &ContactPrediction<N>,
37        manifold: &mut ContactManifold<N>,
38        flip: bool,
39    ) {
40        self.timestamp += 1;
41
42        // Find new collisions
43        let ls_m2 = m1.inverse() * m2.clone();
44        let ls_aabb2 = bounding_volume::aabb(g2, &ls_m2).loosened(prediction.linear());
45
46        g1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, elt1, part_proc1| match self
47            .sub_detectors
48            .entry(i)
49        {
50            Entry::Occupied(mut entry) => {
51                let ok = if flip {
52                    entry.get_mut().0.generate_contacts(
53                        dispatcher,
54                        m2,
55                        g2,
56                        proc2,
57                        m1,
58                        elt1,
59                        Some(&(proc1, part_proc1)),
60                        prediction,
61                        manifold,
62                    )
63                } else {
64                    entry.get_mut().0.generate_contacts(
65                        dispatcher,
66                        m1,
67                        elt1,
68                        Some(&(proc1, part_proc1)),
69                        m2,
70                        g2,
71                        proc2,
72                        prediction,
73                        manifold,
74                    )
75                };
76
77                if ok {
78                    entry.get_mut().1 = self.timestamp;
79                }
80            }
81            Entry::Vacant(entry) => {
82                let new_detector = if flip {
83                    dispatcher.get_contact_algorithm(g2, elt1)
84                } else {
85                    dispatcher.get_contact_algorithm(elt1, g2)
86                };
87
88                if let Some(mut new_detector) = new_detector {
89                    if flip {
90                        let _ = new_detector.generate_contacts(
91                            dispatcher,
92                            m2,
93                            g2,
94                            proc2,
95                            m1,
96                            elt1,
97                            Some(&(proc1, part_proc1)),
98                            prediction,
99                            manifold,
100                        );
101                    } else {
102                        let _ = new_detector.generate_contacts(
103                            dispatcher,
104                            m1,
105                            elt1,
106                            Some(&(proc1, part_proc1)),
107                            m2,
108                            g2,
109                            proc2,
110                            prediction,
111                            manifold,
112                        );
113                    }
114                    let _ = entry.insert((new_detector, self.timestamp));
115                }
116            }
117        });
118
119        // Remove outdated entries.
120        let timestamp = self.timestamp;
121        self.sub_detectors
122            .retain(|_, detector| detector.1 == timestamp);
123    }
124}
125
126impl<N: RealField + Copy> ContactManifoldGenerator<N> for HeightFieldShapeManifoldGenerator<N> {
127    fn generate_contacts(
128        &mut self,
129        d: &dyn ContactDispatcher<N>,
130        ma: &Isometry<N>,
131        a: &dyn Shape<N>,
132        proc1: Option<&dyn ContactPreprocessor<N>>,
133        mb: &Isometry<N>,
134        b: &dyn Shape<N>,
135        proc2: Option<&dyn ContactPreprocessor<N>>,
136        prediction: &ContactPrediction<N>,
137        manifold: &mut ContactManifold<N>,
138    ) -> bool {
139        if !self.flip {
140            if let Some(hf) = a.as_shape::<HeightField<N>>() {
141                self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold, false);
142                return true;
143            }
144        } else {
145            if let Some(hf) = b.as_shape::<HeightField<N>>() {
146                self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold, true);
147                return true;
148            }
149        }
150
151        return false;
152    }
153
154    //    fn init_manifold(&self) -> ContactManifold<N> {
155    //        let mut res = ContactManifold::new();
156    //        res.set_tracking_mode(ContactTrackingMode::FeatureBased);
157    //        res
158    //    }
159}
ncollide3d/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs

ncollide3d/pipeline/narrow_phase/contact_generator/
heightfield_shape_manifold_generator.rs
