parry2d/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();
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 * (curr as Real) + start_x - ray.origin.x) / ray.dir.x;
80            } else {
81                curr_param = (ray.origin.x - cell_width * (curr as Real) - start_x) / ray.dir.x;
82                curr -= 1;
83            }
84
85            if curr_param >= max_t {
86                // The part of the ray after max_t is outside of the heightfield Aabb.
87                return None;
88            }
89
90            if let Some(seg) = self.segment_at(curr) {
91                // TODO: test the y-coordinates (equivalent to an Aabb test) before actually computing the intersection.
92                let (s, t) = query::details::closest_points_line_line_parameters(
93                    ray.origin,
94                    ray.dir,
95                    seg.a,
96                    seg.scaled_direction(),
97                );
98
99                if t >= 0.0 && t <= 1.0 && s <= max_time_of_impact {
100                    let n = seg.normal().unwrap();
101                    let fid = if n.dot(ray.dir) > 0.0 {
102                        // The ray hit the back face.
103                        curr + self.num_cells()
104                    } else {
105                        // The ray hit the front face.
106                        curr
107                    };
108                    return Some(RayIntersection::new(s, n, FeatureId::Face(fid as u32)));
109                }
110            }
111        }
112
113        None
114    }
115}
116
117#[cfg(feature = "dim3")]
118impl RayCast for HeightField {
119    #[inline]
120    fn cast_local_ray_and_get_normal(
121        &self,
122        ray: &Ray,
123        max_time_of_impact: Real,
124        solid: bool,
125    ) -> Option<RayIntersection> {
126        use num_traits::Bounded;
127
128        let aabb = self.local_aabb();
129        let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?;
130        max_t = max_t.min(max_time_of_impact);
131        let clip_ray_a = ray.point_at(min_t);
132        let mut cell = match self.cell_at_point(clip_ray_a) {
133            Some(cell) => cell,
134            // None may happen due to slight numerical errors.
135            None => {
136                let i = if ray.origin.z > 0.0 {
137                    self.nrows() - 1
138                } else {
139                    0
140                };
141
142                let j = if ray.origin.x > 0.0 {
143                    self.ncols() - 1
144                } else {
145                    0
146                };
147
148                (i, j)
149            }
150        };
151
152        loop {
153            let tris = self.triangles_at(cell.0, cell.1);
154            let inter1 = tris
155                .0
156                .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid));
157            let inter2 = tris
158                .1
159                .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid));
160
161            match (inter1, inter2) {
162                (Some(mut inter1), Some(mut inter2)) => {
163                    if inter1.time_of_impact < inter2.time_of_impact {
164                        inter1.feature =
165                            self.convert_triangle_feature_id(cell.0, cell.1, true, inter1.feature);
166                        return Some(inter1);
167                    } else {
168                        inter2.feature =
169                            self.convert_triangle_feature_id(cell.0, cell.1, false, inter2.feature);
170                        return Some(inter2);
171                    }
172                }
173                (Some(mut inter), None) => {
174                    inter.feature =
175                        self.convert_triangle_feature_id(cell.0, cell.1, true, inter.feature);
176                    return Some(inter);
177                }
178                (None, Some(mut inter)) => {
179                    inter.feature =
180                        self.convert_triangle_feature_id(cell.0, cell.1, false, inter.feature);
181                    return Some(inter);
182                }
183                (None, None) => {}
184            }
185
186            /*
187             * Find the next cell to cast the ray on.
188             */
189            let (toi_x, right) = if ray.dir.x > 0.0 {
190                let x = self.x_at(cell.1 + 1);
191                ((x - ray.origin.x) / ray.dir.x, true)
192            } else if ray.dir.x < 0.0 {
193                let x = self.x_at(cell.1);
194                ((x - ray.origin.x) / ray.dir.x, false)
195            } else {
196                (Real::max_value(), false)
197            };
198
199            let (toi_z, down) = if ray.dir.z > 0.0 {
200                let z = self.z_at(cell.0 + 1);
201                ((z - ray.origin.z) / ray.dir.z, true)
202            } else if ray.dir.z < 0.0 {
203                let z = self.z_at(cell.0);
204                ((z - ray.origin.z) / ray.dir.z, false)
205            } else {
206                (Real::max_value(), false)
207            };
208
209            if toi_x > max_t && toi_z > max_t {
210                break;
211            }
212
213            if toi_x >= 0.0 && toi_x < toi_z {
214                if right {
215                    cell.1 += 1
216                } else if cell.1 > 0 {
217                    cell.1 -= 1
218                } else {
219                    break;
220                }
221            } else if toi_z >= 0.0 {
222                if down {
223                    cell.0 += 1
224                } else if cell.0 > 0 {
225                    cell.0 -= 1
226                } else {
227                    break;
228                }
229            } else {
230                break;
231            }
232
233            if cell.0 >= self.nrows() || cell.1 >= self.ncols() {
234                break;
235            }
236        }
237
238        None
239    }
240}
parry2d/query/ray/ray_heightfield.rs

parry2d/query/ray/
ray_heightfield.rs
