parry3d/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;
10use crate::shape::{Capsule, FeatureId, Segment, SupportMap};
11#[cfg(feature = "dim3")]
12use crate::shape::{Cone, Cylinder};
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
107impl RayCast for Capsule {
108    fn cast_local_ray_and_get_normal(
109        &self,
110        ray: &Ray,
111        max_time_of_impact: Real,
112        solid: bool,
113    ) -> Option<RayIntersection> {
114        local_ray_intersection_with_support_map_with_params(
115            self,
116            &mut VoronoiSimplex::new(),
117            ray,
118            max_time_of_impact,
119            solid,
120        )
121    }
122}
123
124#[cfg(feature = "dim3")]
125#[cfg(feature = "alloc")]
126impl RayCast for ConvexPolyhedron {
127    fn cast_local_ray_and_get_normal(
128        &self,
129        ray: &Ray,
130        max_time_of_impact: Real,
131        solid: bool,
132    ) -> Option<RayIntersection> {
133        local_ray_intersection_with_support_map_with_params(
134            self,
135            &mut VoronoiSimplex::new(),
136            ray,
137            max_time_of_impact,
138            solid,
139        )
140    }
141}
142
143#[cfg(feature = "dim2")]
144#[cfg(feature = "alloc")]
145impl RayCast for ConvexPolygon {
146    fn cast_local_ray_and_get_normal(
147        &self,
148        ray: &Ray,
149        max_time_of_impact: Real,
150        solid: bool,
151    ) -> Option<RayIntersection> {
152        local_ray_intersection_with_support_map_with_params(
153            self,
154            &mut VoronoiSimplex::new(),
155            ray,
156            max_time_of_impact,
157            solid,
158        )
159    }
160}
161
162#[allow(unused_variables)]
163impl RayCast for Segment {
164    fn cast_local_ray_and_get_normal(
165        &self,
166        ray: &Ray,
167        max_time_of_impact: Real,
168        solid: bool,
169    ) -> Option<RayIntersection> {
170        #[cfg(feature = "dim2")]
171        {
172            use crate::math::Vector;
173
174            let seg_dir = self.scaled_direction();
175            let (s, t, parallel) = query::details::closest_points_line_line_parameters_eps(
176                ray.origin,
177                ray.dir,
178                self.a,
179                seg_dir,
180                crate::math::DEFAULT_EPSILON,
181            );
182
183            if parallel {
184                // The lines are parallel, we have to distinguish
185                // the case where there is no intersection at all
186                // from the case where the line are collinear.
187                let dpos = self.a - ray.origin;
188                let normal = self.normal().unwrap_or(Vector::ZERO);
189
190                if dpos.dot(normal).abs() < crate::math::DEFAULT_EPSILON {
191                    // The rays and the segment are collinear.
192                    let dist1 = dpos.dot(ray.dir);
193                    let dist2 = dist1 + seg_dir.dot(ray.dir);
194
195                    match (dist1 >= 0.0, dist2 >= 0.0) {
196                        (true, true) => {
197                            let time_of_impact = dist1.min(dist2) / ray.dir.length_squared();
198                            if time_of_impact > max_time_of_impact {
199                                None
200                            } else if dist1 <= dist2 {
201                                Some(RayIntersection::new(
202                                    time_of_impact,
203                                    normal,
204                                    FeatureId::Vertex(0),
205                                ))
206                            } else {
207                                Some(RayIntersection::new(
208                                    dist2 / ray.dir.length_squared(),
209                                    normal,
210                                    FeatureId::Vertex(1),
211                                ))
212                            }
213                        }
214                        (true, false) | (false, true) => {
215                            // The ray origin lies on the segment.
216                            Some(RayIntersection::new(0.0, normal, FeatureId::Face(0)))
217                        }
218                        (false, false) => {
219                            // The segment is behind the ray.
220                            None
221                        }
222                    }
223                } else {
224                    // The rays never intersect.
225                    None
226                }
227            } else if s >= 0.0 && s <= max_time_of_impact && t >= 0.0 && t <= 1.0 {
228                let normal = self.normal().unwrap_or(Vector::ZERO);
229
230                let dot = normal.dot(ray.dir);
231                if dot > 0.0 {
232                    Some(RayIntersection::new(s, -normal, FeatureId::Face(1)))
233                } else if dot < 0.0 {
234                    Some(RayIntersection::new(s, normal, FeatureId::Face(0)))
235                } else {
236                    // dot == 0 happens when lines are parallel, which is normally handled before,
237                    // but this may happen if segment is zero length, as the ray is not considered parallel.
238                    None
239                }
240            } else {
241                // The closest points are outside of
242                // the ray or segment bounds.
243                None
244            }
245        }
246        #[cfg(feature = "dim3")]
247        {
248            // XXX: implement an analytic solution for 3D too.
249            local_ray_intersection_with_support_map_with_params(
250                self,
251                &mut VoronoiSimplex::new(),
252                ray,
253                max_time_of_impact,
254                solid,
255            )
256        }
257    }
258}
parry3d/query/ray/ray_support_map.rs

parry3d/query/ray/
ray_support_map.rs
