ncollide2d_updated/shape/
heightfield2.rs

1use na::{DVector, Point2, RealField};
2use std::iter;
3
4use crate::bounding_volume::AABB;
5use crate::math::Vector;
6use crate::query::{Contact, ContactKinematic, ContactPreprocessor};
7use crate::shape::Segment;
8
9#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
10#[derive(Clone, Debug)]
11/// A 2D heightfield.
12pub struct HeightField<N: RealField + Copy> {
13    heights: DVector<N>,
14    scale: Vector<N>,
15    removed: Vec<bool>,
16    aabb: AABB<N>,
17}
18
19impl<N: RealField + Copy> HeightField<N> {
20    /// Creates a new 2D heightfield with the given heights and scale factor.
21    pub fn new(heights: DVector<N>, scale: Vector<N>) -> Self {
22        assert!(
23            heights.len() > 1,
24            "A heightfield heights must have at least 2 elements."
25        );
26
27        let max = heights.max();
28        let min = heights.min();
29        let hscale = scale * na::convert::<_, N>(0.5);
30        let aabb = AABB::new(
31            Point2::new(-hscale.x, min * scale.y),
32            Point2::new(hscale.x, max * scale.y),
33        );
34
35        HeightField {
36            heights,
37            scale,
38            aabb,
39            removed: Vec::new(),
40        }
41    }
42
43    /// The number of cells of this heightfield.
44    pub fn num_cells(&self) -> usize {
45        self.heights.len() - 1
46    }
47
48    /// The height at each cell endpoint.
49    pub fn heights(&self) -> &DVector<N> {
50        &self.heights
51    }
52
53    /// The scale factor applied to this heightfield.
54    pub fn scale(&self) -> &Vector<N> {
55        &self.scale
56    }
57
58    /// The AABB of this heightfield.
59    pub fn aabb(&self) -> &AABB<N> {
60        &self.aabb
61    }
62
63    /// The width of a single cell of this heightfield.
64    pub fn cell_width(&self) -> N {
65        self.unit_cell_width() * self.scale.x
66    }
67
68    /// The width of a single cell of this heightfield, without taking the scale factor into account.
69    pub fn unit_cell_width(&self) -> N {
70        N::one() / na::convert(self.heights.len() as f64 - 1.0)
71    }
72
73    /// The left-most x-coordinate of this heightfield.
74    pub fn start_x(&self) -> N {
75        self.scale.x * na::convert(-0.5)
76    }
77
78    fn quantize_floor(&self, val: N, seg_length: N) -> usize {
79        let _0_5: N = na::convert(0.5);
80        let i = na::clamp(
81            ((val + _0_5) / seg_length).floor(),
82            N::zero(),
83            na::convert((self.num_cells() - 1) as f64),
84        );
85        na::convert_unchecked::<N, f64>(i) as usize
86    }
87
88    fn quantize_ceil(&self, val: N, seg_length: N) -> usize {
89        let _0_5: N = na::convert(0.5);
90        let i = na::clamp(
91            ((val + _0_5) / seg_length).ceil(),
92            N::zero(),
93            na::convert(self.num_cells() as f64),
94        );
95        na::convert_unchecked::<N, f64>(i) as usize
96    }
97
98    /// Index of the cell a point is on after vertical projection.
99    pub fn cell_at_point(&self, pt: &Point2<N>) -> Option<usize> {
100        let _0_5: N = na::convert(0.5);
101        let scaled_pt = pt.coords.component_div(&self.scale);
102        let seg_length = self.unit_cell_width();
103
104        if scaled_pt.x < -_0_5 || scaled_pt.x > _0_5 {
105            // Outside of the heightfield bounds.
106            None
107        } else {
108            Some(self.quantize_floor(scaled_pt.x, seg_length))
109        }
110    }
111
112    /// Iterator through all the segments of this heightfield.
113    pub fn segments<'a>(&'a self) -> impl Iterator<Item = Segment<N>> + 'a {
114        // FIXME: this is not very efficient since this wil
115        // recompute shared points twice.
116        (0..self.num_cells()).filter_map(move |i| self.segment_at(i))
117    }
118
119    /// The i-th segment of the heightfield if it has not been removed.
120    pub fn segment_at(&self, i: usize) -> Option<Segment<N>> {
121        if i >= self.num_cells() || self.is_segment_removed(i) {
122            return None;
123        }
124
125        let _0_5: N = na::convert(0.5);
126        let seg_length = N::one() / na::convert(self.heights.len() as f64 - 1.0);
127
128        let x0 = -_0_5 + seg_length * na::convert(i as f64);
129        let x1 = x0 + seg_length;
130
131        let y0 = self.heights[i + 0];
132        let y1 = self.heights[i + 1];
133
134        let mut p0 = Point2::new(x0, y0);
135        let mut p1 = Point2::new(x1, y1);
136
137        // Apply scales:
138        p0.coords.component_mul_assign(&self.scale);
139        p1.coords.component_mul_assign(&self.scale);
140
141        Some(Segment::new(p0, p1))
142    }
143
144    /// Mark the i-th segment of this heightfield as removed or not.
145    pub fn set_segment_removed(&mut self, i: usize, removed: bool) {
146        if self.removed.len() == 0 {
147            self.removed = iter::repeat(false).take(self.num_cells()).collect()
148        }
149
150        self.removed[i] = removed
151    }
152
153    /// Checks if the i-th segment has been removed.
154    pub fn is_segment_removed(&self, i: usize) -> bool {
155        self.removed.len() != 0 && self.removed[i]
156    }
157
158    /// Applies `f` to each segment of this heightfield that intersects the given `aabb`.
159    pub fn map_elements_in_local_aabb(
160        &self,
161        aabb: &AABB<N>,
162        f: &mut impl FnMut(usize, &Segment<N>, &dyn ContactPreprocessor<N>),
163    ) {
164        let _0_5: N = na::convert(0.5);
165        let ref_mins = aabb.mins.coords.component_div(&self.scale);
166        let ref_maxs = aabb.maxs.coords.component_div(&self.scale);
167        let seg_length = N::one() / na::convert(self.heights.len() as f64 - 1.0);
168
169        if ref_maxs.x < -_0_5 || ref_mins.x > _0_5 {
170            // Outside of the heightfield bounds.
171            return;
172        }
173
174        let min_x = self.quantize_floor(ref_mins.x, seg_length);
175        let max_x = self.quantize_ceil(ref_maxs.x, seg_length);
176
177        // FIXME: find a way to avoid recomputing the same vertices
178        // multiple times.
179        for i in min_x..max_x {
180            if self.is_segment_removed(i) {
181                continue;
182            }
183
184            let x0 = -_0_5 + seg_length * na::convert(i as f64);
185            let x1 = x0 + seg_length;
186
187            let y0 = self.heights[i + 0];
188            let y1 = self.heights[i + 1];
189
190            if (y0 > ref_maxs.y && y1 > ref_maxs.y) || (y0 < ref_mins.y && y1 < ref_mins.y) {
191                continue;
192            }
193
194            let mut p0 = Point2::new(x0, y0);
195            let mut p1 = Point2::new(x1, y1);
196
197            // Apply scales:
198            p0.coords.component_mul_assign(&self.scale);
199            p1.coords.component_mul_assign(&self.scale);
200
201            // Build the segment.
202            let seg = Segment::new(p0, p1);
203
204            // Build the contact preprocessor.
205            let seg_id = i;
206            let proc = HeightFieldTriangleContactPreprocessor::new(self, seg_id);
207
208            // Call the callback.
209            f(seg_id, &seg, &proc);
210        }
211    }
212}
213
214#[allow(dead_code)]
215/// The contact preprocessor dedicated to 2D heightfields.
216pub struct HeightFieldTriangleContactPreprocessor<'a, N: RealField + Copy> {
217    heightfield: &'a HeightField<N>,
218    triangle: usize,
219}
220
221impl<'a, N: RealField + Copy> HeightFieldTriangleContactPreprocessor<'a, N> {
222    /// Initialize a contact preprocessor for the given triangle of the given heightfield.
223    pub fn new(heightfield: &'a HeightField<N>, triangle: usize) -> Self {
224        HeightFieldTriangleContactPreprocessor {
225            heightfield,
226            triangle,
227        }
228    }
229}
230
231impl<'a, N: RealField + Copy> ContactPreprocessor<N>
232    for HeightFieldTriangleContactPreprocessor<'a, N>
233{
234    fn process_contact(
235        &self,
236        _c: &mut Contact<N>,
237        _kinematic: &mut ContactKinematic<N>,
238        _is_first: bool,
239    ) -> bool {
240        /*
241        // Fix the feature ID.
242        let feature = if is_first {
243            kinematic.feature1()
244        } else {
245            kinematic.feature2()
246        };
247
248        let face = &self.mesh.faces()[self.face_id];
249        let actual_feature = match feature {
250            FeatureId::Vertex(i) => FeatureId::Vertex(face.indices[i]),
251            FeatureId::Edge(i) => FeatureId::Edge(face.edges[i]),
252            FeatureId::Face(i) => {
253                if i == 0 {
254                    FeatureId::Face(self.face_id)
255                } else {
256                    FeatureId::Face(self.face_id + self.mesh.faces().len())
257                }
258            }
259            FeatureId::Unknown => FeatureId::Unknown,
260        };
261
262        if is_first {
263            kinematic.set_feature1(actual_feature);
264        } else {
265            kinematic.set_feature2(actual_feature);
266        }
267
268        // Test the validity of the LMD.
269        if c.depth > N::zero() {
270            true
271        } else {
272            // FIXME
273        }
274        */
275
276        true
277    }
278}
ncollide2d_updated/shape/heightfield2.rs

ncollide2d_updated/shape/
heightfield2.rs
