parry3d/query/ray/
ray_heightfield.rs

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

parry3d/query/ray/
ray_heightfield.rs
