parry2d/query/ray/
ray_support_map.rs

1use crate::math::Real;
2#[cfg(feature = "dim2")]
3use crate::query;
4use crate::query::gjk::{self, CsoPoint, VoronoiSimplex};
5use crate::query::{Ray, RayCast, RayIntersection};
6#[cfg(all(feature = "alloc", feature = "dim2"))]
7use crate::shape::ConvexPolygon;
8#[cfg(all(feature = "alloc", feature = "dim3"))]
9use crate::shape::ConvexPolyhedron;
10#[cfg(feature = "dim3")]
11use crate::shape::{Cone, Cylinder};
12use crate::shape::{FeatureId, Segment, SupportMap};
13
14use num::Zero;
15
16/// Cast a ray on a shape using the GJK algorithm.
17pub fn local_ray_intersection_with_support_map_with_params<G: ?Sized + SupportMap>(
18    shape: &G,
19    simplex: &mut VoronoiSimplex,
20    ray: &Ray,
21    max_time_of_impact: Real,
22    solid: bool,
23) -> Option<RayIntersection> {
24    let supp = shape.local_support_point(-ray.dir);
25    simplex.reset(CsoPoint::single_point(supp - ray.origin));
26
27    let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact);
28
29    if !solid {
30        inter.and_then(|(time_of_impact, normal)| {
31            if time_of_impact.is_zero() {
32                // the ray is inside of the shape.
33                let ndir = ray.dir.normalize();
34                let supp = shape.local_support_point(ndir);
35                let eps: Real = 0.001;
36                let shift = (supp - ray.origin).dot(ndir) + eps;
37                let new_ray = Ray::new(ray.origin + ndir * shift, -ray.dir);
38
39                // TODO: replace by? : simplex.translate_by(&(ray.origin - new_ray.origin));
40                simplex.reset(CsoPoint::single_point(supp - new_ray.origin));
41
42                gjk::cast_local_ray(shape, simplex, &new_ray, shift + eps).and_then(
43                    |(time_of_impact, outward_normal)| {
44                        let time_of_impact = shift - time_of_impact;
45                        if time_of_impact <= max_time_of_impact {
46                            Some(RayIntersection::new(
47                                time_of_impact,
48                                -outward_normal,
49                                FeatureId::Unknown,
50                            ))
51                        } else {
52                            None
53                        }
54                    },
55                )
56            } else {
57                Some(RayIntersection::new(
58                    time_of_impact,
59                    normal,
60                    FeatureId::Unknown,
61                ))
62            }
63        })
64    } else {
65        inter.map(|(time_of_impact, normal)| {
66            RayIntersection::new(time_of_impact, normal, FeatureId::Unknown)
67        })
68    }
69}
70
71#[cfg(feature = "dim3")]
72impl RayCast for Cylinder {
73    fn cast_local_ray_and_get_normal(
74        &self,
75        ray: &Ray,
76        max_time_of_impact: Real,
77        solid: bool,
78    ) -> Option<RayIntersection> {
79        local_ray_intersection_with_support_map_with_params(
80            self,
81            &mut VoronoiSimplex::new(),
82            ray,
83            max_time_of_impact,
84            solid,
85        )
86    }
87}
88
89#[cfg(feature = "dim3")]
90impl RayCast for Cone {
91    fn cast_local_ray_and_get_normal(
92        &self,
93        ray: &Ray,
94        max_time_of_impact: Real,
95        solid: bool,
96    ) -> Option<RayIntersection> {
97        local_ray_intersection_with_support_map_with_params(
98            self,
99            &mut VoronoiSimplex::new(),
100            ray,
101            max_time_of_impact,
102            solid,
103        )
104    }
105}
106
107#[cfg(feature = "dim3")]
108#[cfg(feature = "alloc")]
109impl RayCast for ConvexPolyhedron {
110    fn cast_local_ray_and_get_normal(
111        &self,
112        ray: &Ray,
113        max_time_of_impact: Real,
114        solid: bool,
115    ) -> Option<RayIntersection> {
116        local_ray_intersection_with_support_map_with_params(
117            self,
118            &mut VoronoiSimplex::new(),
119            ray,
120            max_time_of_impact,
121            solid,
122        )
123    }
124}
125
126#[cfg(feature = "dim2")]
127#[cfg(feature = "alloc")]
128impl RayCast for ConvexPolygon {
129    fn cast_local_ray_and_get_normal(
130        &self,
131        ray: &Ray,
132        max_time_of_impact: Real,
133        solid: bool,
134    ) -> Option<RayIntersection> {
135        local_ray_intersection_with_support_map_with_params(
136            self,
137            &mut VoronoiSimplex::new(),
138            ray,
139            max_time_of_impact,
140            solid,
141        )
142    }
143}
144
145#[allow(unused_variables)]
146impl RayCast for Segment {
147    fn cast_local_ray_and_get_normal(
148        &self,
149        ray: &Ray,
150        max_time_of_impact: Real,
151        solid: bool,
152    ) -> Option<RayIntersection> {
153        #[cfg(feature = "dim2")]
154        {
155            use crate::math::Vector;
156
157            let seg_dir = self.scaled_direction();
158            let (s, t, parallel) = query::details::closest_points_line_line_parameters_eps(
159                ray.origin,
160                ray.dir,
161                self.a,
162                seg_dir,
163                crate::math::DEFAULT_EPSILON,
164            );
165
166            if parallel {
167                // The lines are parallel, we have to distinguish
168                // the case where there is no intersection at all
169                // from the case where the line are collinear.
170                let dpos = self.a - ray.origin;
171                let normal = self.normal().unwrap_or(Vector::ZERO);
172
173                if dpos.dot(normal).abs() < crate::math::DEFAULT_EPSILON {
174                    // The rays and the segment are collinear.
175                    let dist1 = dpos.dot(ray.dir);
176                    let dist2 = dist1 + seg_dir.dot(ray.dir);
177
178                    match (dist1 >= 0.0, dist2 >= 0.0) {
179                        (true, true) => {
180                            let time_of_impact = dist1.min(dist2) / ray.dir.length_squared();
181                            if time_of_impact > max_time_of_impact {
182                                None
183                            } else if dist1 <= dist2 {
184                                Some(RayIntersection::new(
185                                    time_of_impact,
186                                    normal,
187                                    FeatureId::Vertex(0),
188                                ))
189                            } else {
190                                Some(RayIntersection::new(
191                                    dist2 / ray.dir.length_squared(),
192                                    normal,
193                                    FeatureId::Vertex(1),
194                                ))
195                            }
196                        }
197                        (true, false) | (false, true) => {
198                            // The ray origin lies on the segment.
199                            Some(RayIntersection::new(0.0, normal, FeatureId::Face(0)))
200                        }
201                        (false, false) => {
202                            // The segment is behind the ray.
203                            None
204                        }
205                    }
206                } else {
207                    // The rays never intersect.
208                    None
209                }
210            } else if s >= 0.0 && s <= max_time_of_impact && t >= 0.0 && t <= 1.0 {
211                let normal = self.normal().unwrap_or(Vector::ZERO);
212
213                let dot = normal.dot(ray.dir);
214                if dot > 0.0 {
215                    Some(RayIntersection::new(s, -normal, FeatureId::Face(1)))
216                } else if dot < 0.0 {
217                    Some(RayIntersection::new(s, normal, FeatureId::Face(0)))
218                } else {
219                    // dot == 0 happens when lines are parallel, which is normally handled before,
220                    // but this may happen if segment is zero length, as the ray is not considered parallel.
221                    None
222                }
223            } else {
224                // The closest points are outside of
225                // the ray or segment bounds.
226                None
227            }
228        }
229        #[cfg(feature = "dim3")]
230        {
231            // XXX: implement an analytic solution for 3D too.
232            local_ray_intersection_with_support_map_with_params(
233                self,
234                &mut VoronoiSimplex::new(),
235                ray,
236                max_time_of_impact,
237                solid,
238            )
239        }
240    }
241}
parry2d/query/ray/ray_support_map.rs

parry2d/query/ray/
ray_support_map.rs
