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
14pub 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 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 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#[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 let dpos = seg.a - ray.origin;
244 let normal = seg.scaled_normal();
245
246 if dpos.dot(&normal).abs() < N::default_epsilon() {
247 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 Some(RayIntersection::new(N::zero(), normal, FeatureId::Face(0)))
270 }
271 (false, false) => {
272 None
274 }
275 }
276 } else {
277 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 None
292 }
293 }
294 #[cfg(feature = "dim3")]
295 {
296 let ls_ray = ray.inverse_transform_by(m);
297
298 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}