ncollide3d/query/ray/
ray_support_map.rs

Help
1use na::{self, RealField};
2
3use crate::math::Isometry;
4#[cfg(feature = "dim2")]
5use crate::query;
6use crate::query::algorithms::{gjk, CSOPoint, VoronoiSimplex};
7use crate::query::{Ray, RayCast, RayIntersection};
8#[cfg(feature = "dim2")]
9use crate::shape::ConvexPolygon;
10use crate::shape::{Capsule, FeatureId, Segment, SupportMap};
11#[cfg(feature = "dim3")]
12use crate::shape::{Cone, ConvexHull, Cylinder};
13
14/// Cast a ray on a shape using the GJK algorithm.
15pub fn ray_intersection_with_support_map_with_params<N, G: ?Sized>(
16    m: &Isometry<N>,
17    shape: &G,
18    simplex: &mut VoronoiSimplex<N>,
19    ray: &Ray<N>,
20    max_toi: N,
21    solid: bool,
22) -> Option<RayIntersection<N>>
23where
24    N: RealField + Copy,
25    G: SupportMap<N>,
26{
27    let supp = shape.support_point(m, &-ray.dir);
28    simplex.reset(CSOPoint::single_point(supp - ray.origin.coords));
29
30    let inter = gjk::cast_ray(m, shape, simplex, ray, max_toi);
31
32    if !solid {
33        inter.and_then(|(toi, normal)| {
34            if toi.is_zero() {
35                // the ray is inside of the shape.
36                let ndir = ray.dir.normalize();
37                let supp = shape.support_point(m, &ndir);
38                let eps = na::convert(0.001f64);
39                let shift = (supp - ray.origin).dot(&ndir) + eps;
40                let new_ray = Ray::new(ray.origin + ndir * shift, -ray.dir);
41
42                // FIXME: replace by? : simplex.translate_by(&(ray.origin - new_ray.origin));
43                simplex.reset(CSOPoint::single_point(supp - new_ray.origin.coords));
44
45                gjk::cast_ray(m, shape, simplex, &new_ray, shift + eps).and_then(|(toi, normal)| {
46                    let toi = shift - toi;
47                    if toi <= max_toi {
48                        Some(RayIntersection::new(toi, normal, FeatureId::Unknown))
49                    } else {
50                        None
51                    }
52                })
53            } else {
54                Some(RayIntersection::new(toi, normal, FeatureId::Unknown))
55            }
56        })
57    } else {
58        inter.map(|(toi, normal)| RayIntersection::new(toi, normal, FeatureId::Unknown))
59    }
60}
61
62#[cfg(feature = "dim3")]
63impl<N: RealField + Copy> RayCast<N> for Cylinder<N> {
64    fn toi_and_normal_with_ray(
65        &self,
66        m: &Isometry<N>,
67        ray: &Ray<N>,
68        max_toi: N,
69        solid: bool,
70    ) -> Option<RayIntersection<N>> {
71        let ls_ray = ray.inverse_transform_by(m);
72
73        ray_intersection_with_support_map_with_params(
74            &Isometry::identity(),
75            self,
76            &mut VoronoiSimplex::new(),
77            &ls_ray,
78            max_toi,
79            solid,
80        )
81        .map(|mut res| {
82            res.normal = m * res.normal;
83            res
84        })
85    }
86}
87
88#[cfg(feature = "dim3")]
89impl<N: RealField + Copy> RayCast<N> for Cone<N> {
90    fn toi_and_normal_with_ray(
91        &self,
92        m: &Isometry<N>,
93        ray: &Ray<N>,
94        max_toi: N,
95        solid: bool,
96    ) -> Option<RayIntersection<N>> {
97        let ls_ray = ray.inverse_transform_by(m);
98
99        ray_intersection_with_support_map_with_params(
100            &Isometry::identity(),
101            self,
102            &mut VoronoiSimplex::new(),
103            &ls_ray,
104            max_toi,
105            solid,
106        )
107        .map(|mut res| {
108            res.normal = m * res.normal;
109            res
110        })
111    }
112}
113
114impl<N: RealField + Copy> RayCast<N> for Capsule<N> {
115    fn toi_and_normal_with_ray(
116        &self,
117        m: &Isometry<N>,
118        ray: &Ray<N>,
119        max_toi: N,
120        solid: bool,
121    ) -> Option<RayIntersection<N>> {
122        let ls_ray = ray.inverse_transform_by(m);
123
124        ray_intersection_with_support_map_with_params(
125            &Isometry::identity(),
126            self,
127            &mut VoronoiSimplex::new(),
128            &ls_ray,
129            max_toi,
130            solid,
131        )
132        .map(|mut res| {
133            res.normal = m * res.normal;
134            res
135        })
136    }
137}
138
139#[cfg(feature = "dim3")]
140impl<N: RealField + Copy> RayCast<N> for ConvexHull<N> {
141    fn toi_and_normal_with_ray(
142        &self,
143        m: &Isometry<N>,
144        ray: &Ray<N>,
145        max_toi: N,
146        solid: bool,
147    ) -> Option<RayIntersection<N>> {
148        let ls_ray = ray.inverse_transform_by(m);
149
150        ray_intersection_with_support_map_with_params(
151            &Isometry::identity(),
152            self,
153            &mut VoronoiSimplex::new(),
154            &ls_ray,
155            max_toi,
156            solid,
157        )
158        .map(|mut res| {
159            res.normal = m * res.normal;
160            res
161        })
162    }
163}
164
165#[cfg(feature = "dim2")]
166impl<N: RealField + Copy> RayCast<N> for ConvexPolygon<N> {
167    fn toi_and_normal_with_ray(
168        &self,
169        m: &Isometry<N>,
170        ray: &Ray<N>,
171        max_toi: N,
172        solid: bool,
173    ) -> Option<RayIntersection<N>> {
174        let ls_ray = ray.inverse_transform_by(m);
175
176        ray_intersection_with_support_map_with_params(
177            &Isometry::identity(),
178            self,
179            &mut VoronoiSimplex::new(),
180            &ls_ray,
181            max_toi,
182            solid,
183        )
184        .map(|mut res| {
185            res.normal = m * res.normal;
186            res
187        })
188    }
189}
190
191// FIXME: optimize this, we should use the general algorithm for triangles.
192#[cfg(feature = "dim2")]
193impl<N: RealField + Copy> RayCast<N> for crate::shape::Triangle<N> {
194    fn toi_and_normal_with_ray(
195        &self,
196        m: &Isometry<N>,
197        ray: &Ray<N>,
198        max_toi: N,
199        solid: bool,
200    ) -> Option<RayIntersection<N>> {
201        let ls_ray = ray.inverse_transform_by(m);
202
203        ray_intersection_with_support_map_with_params(
204            &Isometry::identity(),
205            self,
206            &mut VoronoiSimplex::new(),
207            &ls_ray,
208            max_toi,
209            solid,
210        )
211        .map(|mut res| {
212            res.normal = m * res.normal;
213            res
214        })
215    }
216}
217
218#[allow(unused_variables)]
219impl<N: RealField + Copy> RayCast<N> for Segment<N> {
220    fn toi_and_normal_with_ray(
221        &self,
222        m: &Isometry<N>,
223        ray: &Ray<N>,
224        max_toi: N,
225        solid: bool,
226    ) -> Option<RayIntersection<N>> {
227        #[cfg(feature = "dim2")]
228        {
229            let seg = self.transformed(m);
230            let seg_dir = seg.scaled_direction();
231            let (s, t, parallel) = query::closest_points_line_line_parameters_eps(
232                &ray.origin,
233                &ray.dir,
234                &seg.a,
235                &seg_dir,
236                N::default_epsilon(),
237            );
238
239            if parallel {
240                // The lines are parallel, we have to distinguish
241                // the case where there is no intersection at all
242                // from the case where the line are collinear.
243                let dpos = seg.a - ray.origin;
244                let normal = seg.scaled_normal();
245
246                if dpos.dot(&normal).abs() < N::default_epsilon() {
247                    // The rays and the segment are collinear.
248                    let dist1 = dpos.dot(&ray.dir);
249                    let dist2 = dist1 + seg_dir.dot(&ray.dir);
250
251                    match (dist1 >= N::zero(), dist2 >= N::zero()) {
252                        (true, true) => {
253                            if dist1 <= dist2 {
254                                Some(RayIntersection::new(
255                                    dist1 / ray.dir.norm_squared(),
256                                    normal,
257                                    FeatureId::Vertex(0),
258                                ))
259                            } else {
260                                Some(RayIntersection::new(
261                                    dist2 / ray.dir.norm_squared(),
262                                    normal,
263                                    FeatureId::Vertex(1),
264                                ))
265                            }
266                        }
267                        (true, false) | (false, true) => {
268                            // The ray origin lies on the segment.
269                            Some(RayIntersection::new(N::zero(), normal, FeatureId::Face(0)))
270                        }
271                        (false, false) => {
272                            // The segment is behind the ray.
273                            None
274                        }
275                    }
276                } else {
277                    // The rays never intersect.
278                    None
279                }
280            } else if s >= N::zero() && t >= N::zero() && t <= N::one() {
281                let normal = seg.scaled_normal();
282
283                if normal.dot(&ray.dir) > N::zero() {
284                    Some(RayIntersection::new(s, -normal, FeatureId::Face(1)))
285                } else {
286                    Some(RayIntersection::new(s, normal, FeatureId::Face(0)))
287                }
288            } else {
289                // The closest points are outside of
290                // the ray or segment bounds.
291                None
292            }
293        }
294        #[cfg(feature = "dim3")]
295        {
296            let ls_ray = ray.inverse_transform_by(m);
297
298            // XXX: implement an analytic solution for 3D too.
299            ray_intersection_with_support_map_with_params(
300                &Isometry::identity(),
301                self,
302                &mut VoronoiSimplex::new(),
303                &ls_ray,
304                max_toi,
305                solid,
306            )
307            .map(|mut res| {
308                res.normal = m * res.normal;
309                res
310            })
311        }
312    }
313}
ncollide3d/query/ray/ray_support_map.rs

ncollide3d/query/ray/
ray_support_map.rs
