ncollide3d/query/ray/
ray_heightfield.rs

Help
1use crate::math::Isometry;
2#[cfg(feature = "dim2")]
3use crate::query;
4use crate::query::{Ray, RayCast, RayIntersection};
5#[cfg(feature = "dim2")]
6use crate::shape::FeatureId;
7use crate::shape::HeightField;
8use na::RealField;
9
10#[cfg(feature = "dim2")]
11impl<N: RealField + Copy> RayCast<N> for HeightField<N> {
12    #[inline]
13    fn toi_and_normal_with_ray(
14        &self,
15        m: &Isometry<N>,
16        ray: &Ray<N>,
17        max_toi: N,
18        _: bool,
19    ) -> Option<RayIntersection<N>> {
20        let aabb = self.aabb();
21        let ls_ray = ray.inverse_transform_by(m);
22        let (min_t, mut max_t) = aabb.clip_ray_parameters(&ls_ray)?;
23
24        if min_t > max_toi {
25            return None;
26        }
27
28        max_t = max_t.min(max_toi);
29
30        let clip_ray_a = ls_ray.point_at(min_t);
31
32        // None may happen due to slight numerical errors.
33        let mut curr = self.cell_at_point(&clip_ray_a).unwrap_or_else(|| {
34            if ls_ray.origin.x > N::zero() {
35                self.num_cells() - 1
36            } else {
37                0_usize
38            }
39        });
40
41        /*
42         * Test the segment under the ray.
43         */
44        if let Some(seg) = self.segment_at(curr) {
45            let (s, t) = query::closest_points_line_line_parameters(
46                &ray.origin,
47                &ray.dir,
48                &seg.a,
49                &seg.scaled_direction(),
50            );
51            if s >= N::zero() && t >= N::zero() && t <= N::one() {
52                // Cast succeeded on the first element!
53                let n = seg.normal().unwrap().into_inner();
54                let fid = if n.dot(&ls_ray.dir) > N::zero() {
55                    // The ray hit the back face.
56                    curr + self.num_cells()
57                } else {
58                    // The ray hit the front face.
59                    curr
60                };
61
62                return Some(RayIntersection::new(s, m * n, FeatureId::Face(fid)));
63            }
64        }
65
66        /*
67         * Test other segments in the path of the ray.
68         */
69        if ls_ray.dir.x == N::zero() {
70            return None;
71        }
72
73        let right = ls_ray.dir.x > N::zero();
74        let cell_width = self.cell_width();
75        let start_x = self.start_x();
76
77        while (right && curr < self.num_cells()) || (!right && curr > 0) {
78            let curr_param;
79
80            if right {
81                curr += 1;
82                curr_param = (cell_width * na::convert(curr as f64) + start_x - ls_ray.origin.x)
83                    / ls_ray.dir.x;
84            } else {
85                curr_param = (ls_ray.origin.x - cell_width * na::convert(curr as f64) - start_x)
86                    / ls_ray.dir.x;
87                curr -= 1;
88            }
89
90            if curr_param >= max_t {
91                // The part of the ray after max_t is outside of the heightfield AABB.
92                return None;
93            }
94
95            if let Some(seg) = self.segment_at(curr) {
96                // TODO: test the y-coordinates (equivalent to an AABB test) before actually computing the intersection.
97                let (s, t) = query::closest_points_line_line_parameters(
98                    &ray.origin,
99                    &ray.dir,
100                    &seg.a,
101                    &seg.scaled_direction(),
102                );
103
104                if t >= N::zero() && t <= N::one() && s <= max_toi {
105                    let n = seg.normal().unwrap().into_inner();
106                    let fid = if n.dot(&ls_ray.dir) > N::zero() {
107                        // The ray hit the back face.
108                        curr + self.num_cells()
109                    } else {
110                        // The ray hit the front face.
111                        curr
112                    };
113                    return Some(RayIntersection::new(s, m * n, FeatureId::Face(fid)));
114                }
115            }
116        }
117
118        None
119    }
120}
121
122#[cfg(feature = "dim3")]
123impl<N: RealField + Copy> RayCast<N> for HeightField<N> {
124    #[inline]
125    fn toi_and_normal_with_ray(
126        &self,
127        m: &Isometry<N>,
128        ray: &Ray<N>,
129        max_toi: N,
130        solid: bool,
131    ) -> Option<RayIntersection<N>> {
132        let aabb = self.aabb();
133        let ls_ray = ray.inverse_transform_by(m);
134        let (min_t, mut max_t) = aabb.clip_ray_parameters(&ls_ray)?;
135        max_t = max_t.min(max_toi);
136        let clip_ray_a = ls_ray.point_at(min_t);
137        let mut cell = match self.cell_at_point(&clip_ray_a) {
138            Some(cell) => cell,
139            // None may happen due to slight numerical errors.
140            None => {
141                let i = if ls_ray.origin.z > N::zero() {
142                    self.nrows() - 1
143                } else {
144                    0
145                };
146
147                let j = if ls_ray.origin.x > N::zero() {
148                    self.ncols() - 1
149                } else {
150                    0
151                };
152
153                (i, j)
154            }
155        };
156
157        loop {
158            let tris = self.triangles_at(cell.0, cell.1);
159            let inter1 = tris
160                .0
161                .and_then(|tri| tri.toi_and_normal_with_ray(m, ray, max_toi, solid));
162            let inter2 = tris
163                .1
164                .and_then(|tri| tri.toi_and_normal_with_ray(m, ray, max_toi, solid));
165
166            match (inter1, inter2) {
167                (Some(mut inter1), Some(mut inter2)) => {
168                    if inter1.toi < inter2.toi {
169                        inter1.feature =
170                            self.convert_triangle_feature_id(cell.0, cell.1, true, inter1.feature);
171                        return Some(inter1);
172                    } else {
173                        inter2.feature =
174                            self.convert_triangle_feature_id(cell.0, cell.1, false, inter2.feature);
175                        return Some(inter2);
176                    }
177                }
178                (Some(mut inter), None) => {
179                    inter.feature =
180                        self.convert_triangle_feature_id(cell.0, cell.1, true, inter.feature);
181                    return Some(inter);
182                }
183                (None, Some(mut inter)) => {
184                    inter.feature =
185                        self.convert_triangle_feature_id(cell.0, cell.1, false, inter.feature);
186                    return Some(inter);
187                }
188                (None, None) => {}
189            }
190
191            /*
192             * Find the next cell to cast the ray on.
193             */
194            let (toi_x, right) = if ls_ray.dir.x > N::zero() {
195                let x = self.x_at(cell.1 + 1);
196                ((x - ls_ray.origin.x) / ls_ray.dir.x, true)
197            } else if ls_ray.dir.x < N::zero() {
198                let x = self.x_at(cell.1 + 0);
199                ((x - ls_ray.origin.x) / ls_ray.dir.x, false)
200            } else {
201                (N::max_value().unwrap(), false)
202            };
203
204            let (toi_z, down) = if ls_ray.dir.z > N::zero() {
205                let z = self.z_at(cell.0 + 1);
206                ((z - ls_ray.origin.z) / ls_ray.dir.z, true)
207            } else if ls_ray.dir.z < N::zero() {
208                let z = self.z_at(cell.0 + 0);
209                ((z - ls_ray.origin.z) / ls_ray.dir.z, false)
210            } else {
211                (N::max_value().unwrap(), false)
212            };
213
214            if toi_x > max_t && toi_z > max_t {
215                break;
216            }
217
218            if toi_x >= N::zero() && toi_x < toi_z {
219                if right {
220                    cell.1 += 1
221                } else if cell.1 > 0 {
222                    cell.1 -= 1
223                } else {
224                    break;
225                }
226            } else if toi_z >= N::zero() {
227                if down {
228                    cell.0 += 1
229                } else if cell.0 > 0 {
230                    cell.0 -= 1
231                } else {
232                    break;
233                }
234            } else {
235                break;
236            }
237
238            if cell.0 >= self.nrows() || cell.1 >= self.ncols() {
239                break;
240            }
241        }
242
243        None
244    }
245}
ncollide3d/query/ray/ray_heightfield.rs

ncollide3d/query/ray/
ray_heightfield.rs
