parry3d/query/ray/
ray_support_map.rs

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

parry3d/query/ray/
ray_support_map.rs
