phys_raycast/shapes/
capsule.rs

1// Copyright (C) 2020-2025 phys-raycast authors. All Rights Reserved.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15use phys_geom::math::{Real, *};
16use phys_geom::shape::Capsule;
17
18use crate::{Raycast, RaycastHitResult};
19
20impl Raycast for Capsule {
21    fn raycast(
22        &self,
23        local_ray: phys_geom::Ray,
24        max_distance: Real,
25        discard_inside_hit: bool,
26    ) -> Option<RaycastHitResult> {
27        let radius = self.radius();
28        let half_height = self.half_height();
29
30        // Move ray origin closer to the capsule, to avoid numerical problems.
31        // Here we estimate an offset, rather than accurately calculate the distance between the
32        // origin and the capsule.
33        let offset = (-local_ray
34            .origin
35            .coords
36            .dot(&local_ray.direction.into_inner())
37            - (half_height + radius))
38            .max(0.0);
39        let translated_origin = local_ray.origin + local_ray.direction.into_inner() * offset;
40
41        // Test ray with infinite cylinder.
42        let origin_xz = Vec3::new(translated_origin.x, 0.0, translated_origin.z);
43        let direction_xz = Vec3::new(local_ray.direction.x, 0.0, local_ray.direction.z);
44        let discr_half_b = origin_xz.dot(&direction_xz);
45        let discr_c = origin_xz.norm_squared() - radius * radius;
46
47        // Ray is outside and pointing away
48        if discr_half_b > 0.0 && discr_c > 0.0 {
49            return None;
50        }
51
52        let sphere_center_y: Real;
53
54        // Determine whether it is possible to intersect with the cylinder. If it is not possible,
55        // perform an intersection test between the ray and the upper and lower spheres.
56        let discr_a = direction_xz.norm_squared();
57        if discr_a > 1e-8 {
58            let discr = discr_half_b * discr_half_b - discr_a * discr_c;
59            if discr < 0.0 {
60                return None;
61            }
62            let mut distance = (-discr_half_b - discr.sqrt()) / discr_a;
63
64            // If the origin is inside the infinite cylinder (circle).
65            let inside = if distance < -offset {
66                distance = -offset;
67                true
68            } else {
69                false
70            };
71
72            let hit_position = translated_origin + local_ray.direction.into_inner() * distance;
73            if hit_position.y > half_height {
74                sphere_center_y = half_height;
75            } else if hit_position.y < -half_height {
76                sphere_center_y = -half_height;
77            } else {
78                if inside {
79                    if discard_inside_hit {
80                        return None;
81                    }
82                    return Some(RaycastHitResult {
83                        distance: 0.0,
84                        normal: -local_ray.direction,
85                    });
86                }
87                distance += offset;
88                if distance <= max_distance {
89                    return Some(RaycastHitResult {
90                        distance,
91                        normal: UnitVec3::new_normalize(Vec3::new(
92                            hit_position.x,
93                            0.0,
94                            hit_position.z,
95                        )),
96                    });
97                }
98                return None;
99            }
100        } else {
101            // If the origin is inside the sphere.
102            if discr_c < 0.0
103                && translated_origin.y < half_height
104                && translated_origin.y > -half_height
105                && half_height > 0.0
106            {
107                if discard_inside_hit {
108                    return None;
109                }
110                return Some(RaycastHitResult {
111                    distance: 0.0,
112                    normal: -local_ray.direction,
113                });
114            }
115
116            // Determine which sphere the ray may intersect with.
117            sphere_center_y = if translated_origin.y > 0.0 {
118                half_height
119            } else {
120                -half_height
121            };
122        }
123
124        // Test ray with sphere.
125        let origin_relative_to_sphere = translated_origin - Point3::new(0.0, sphere_center_y, 0.0);
126        let discr_half_b = origin_relative_to_sphere.dot(&local_ray.direction.into_inner());
127        let discr_c = origin_relative_to_sphere.norm_squared() - radius * radius;
128
129        // Ray is outside and pointing away
130        if discr_half_b > 0.0 && discr_c > 0.0 {
131            return None;
132        }
133
134        let discr = discr_half_b * discr_half_b - discr_c;
135        if discr < 0.0 {
136            return None;
137        }
138
139        let mut distance = -discr_half_b - discr.sqrt();
140
141        // If the origin is inside the sphere.
142        if distance < -offset {
143            if discard_inside_hit {
144                return None;
145            }
146            return Some(RaycastHitResult {
147                distance: 0.0,
148                normal: -local_ray.direction,
149            });
150        }
151
152        let hit_position_relative_to_sphere =
153            origin_relative_to_sphere + local_ray.direction.into_inner() * distance;
154
155        distance += offset;
156        if distance <= max_distance {
157            Some(RaycastHitResult {
158                distance,
159                normal: UnitVec3::new_normalize(hit_position_relative_to_sphere),
160            })
161        } else {
162            None
163        }
164    }
165}
166
167#[cfg(test)]
168mod tests {
169    use approx::assert_relative_eq;
170    use geom::Ray;
171
172    use super::*;
173
174    #[test]
175    fn test_raycast_capsule_cylinder() {
176        let capsule = Capsule::new(2.0, 0.5); // half_height=2.0, radius=0.5
177
178        // Direct hit on cylinder from outside
179        let ray = phys_geom::Ray::new(
180            Point3::new(-2.0, 0.0, 0.0),
181            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
182        );
183
184        let hit = capsule.raycast(ray, 10.0, false).unwrap();
185        assert_relative_eq!(hit.distance, 1.5);
186        assert_relative_eq!(
187            hit.normal,
188            UnitVec3::new_normalize(Vec3::new(-1.0, 0.0, 0.0))
189        );
190    }
191
192    #[test]
193    fn test_raycast_capsule_top_sphere() {
194        let capsule = Capsule::new(2.0, 0.5);
195
196        // Hit on top sphere
197        let ray = phys_geom::Ray::new(
198            Point3::new(0.0, 4.0, 0.0),
199            UnitVec3::new_normalize(Vec3::new(0.0, -1.0, 0.0)),
200        );
201
202        let hit = capsule.raycast(ray, 10.0, false).unwrap();
203        assert_relative_eq!(hit.distance, 1.5);
204        assert_relative_eq!(
205            hit.normal,
206            UnitVec3::new_normalize(Vec3::new(0.0, 1.0, 0.0))
207        );
208    }
209
210    #[test]
211    fn test_raycast_capsule_inside() {
212        let capsule = Capsule::new(2.0, 0.5);
213
214        // Ray from inside
215        let ray = phys_geom::Ray::new(
216            Point3::new(0.0, 0.0, 0.0),
217            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
218        );
219
220        let hit = capsule.raycast(ray, 10.0, false).unwrap();
221        assert_relative_eq!(hit.distance, 0.0);
222        assert_relative_eq!(
223            hit.normal,
224            UnitVec3::new_normalize(Vec3::new(-1.0, 0.0, 0.0))
225        );
226    }
227
228    #[test]
229    fn test_raycast_capsule_inside_discarded() {
230        let capsule = Capsule::new(2.0, 0.5);
231
232        // Ray from inside but discard inside hits
233        let ray = phys_geom::Ray::new(
234            Point3::new(0.0, 0.0, 0.0),
235            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
236        );
237
238        assert_eq!(capsule.raycast(ray, 10.0, true), None);
239    }
240
241    #[test]
242    fn test_raycast_capsule_miss() {
243        let capsule = Capsule::new(2.0, 0.5);
244
245        // Ray that misses the capsule
246        let ray = phys_geom::Ray::new(
247            Point3::new(-2.0, 5.0, 0.0),
248            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
249        );
250
251        assert_eq!(capsule.raycast(ray, 10.0, false), None);
252    }
253
254    #[test]
255    fn test_raycast_capsule_max_distance() {
256        let capsule = Capsule::new(2.0, 0.5);
257
258        let ray = phys_geom::Ray::new(
259            Point3::new(-5.0, 0.0, 0.0),
260            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
261        );
262
263        // Max distance too short
264        assert_eq!(capsule.raycast(ray, 1.0, false), None);
265
266        // Max distance sufficient
267        let hit = capsule.raycast(ray, 5.0, false).unwrap();
268        assert_relative_eq!(hit.distance, 4.5);
269    }
270
271    /// If the height is too small, it may cause numerical problems.
272    #[test]
273    fn test_small_segment() {
274        let capsule = Capsule::new(0.0, 1.0);
275        let ray = Ray::new_with_vec3(Point3::new(0.0, 0.0, 2.0), Vec3::new(0.0, 0.0, -1.0));
276        assert_eq!(
277            capsule.raycast(ray, 5.0, false),
278            Some(RaycastHitResult {
279                distance: 1.0,
280                normal: Vec3::z_axis(),
281            })
282        );
283    }
284
285    // If the origin is inside the capsule.
286    #[test]
287    fn test_inner() {
288        let capsule = Capsule::new(1.0, 1.0);
289        let ray_in_cylinder =
290            Ray::new_with_vec3(Point3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, -1.0));
291        assert_eq!(
292            capsule.raycast(ray_in_cylinder, 5.0, false),
293            Some(RaycastHitResult {
294                distance: 0.0,
295                normal: -ray_in_cylinder.direction,
296            })
297        );
298        let ray_in_sphere =
299            Ray::new_with_vec3(Point3::new(0.0, 1.1, 0.0), Vec3::new(0.0, 0.0, -1.0));
300        assert_eq!(
301            capsule.raycast(ray_in_sphere, 5.0, false),
302            Some(RaycastHitResult {
303                distance: 0.0,
304                normal: -ray_in_sphere.direction,
305            })
306        );
307
308        assert_eq!(capsule.raycast(ray_in_cylinder, 5.0, true), None);
309        assert_eq!(capsule.raycast(ray_in_sphere, 5.0, true), None);
310
311        assert_eq!(
312            capsule.raycast(
313                Ray::new_with_vec3(Point3::new(0.0, 0.5, 0.0), Vec3::new(0.0, 1.0, 0.0)),
314                5.0,
315                false
316            ),
317            Some(RaycastHitResult {
318                distance: 0.0,
319                normal: UnitVec3::new_unchecked(Vec3::new(0.0, -1.0, 0.0)),
320            })
321        );
322
323        assert_eq!(
324            capsule.raycast(
325                Ray::new_with_vec3(Point3::new(0.0, 0.5, 0.0), Vec3::new(0.0, 1.0, 0.0)),
326                5.0,
327                true
328            ),
329            None
330        );
331    }
332
333    #[test]
334    fn test_outer() {
335        let capsule = Capsule::new(1.0, 1.0);
336        assert_eq!(
337            capsule.raycast(
338                Ray::new_with_vec3(Point3::new(1.1, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
339                5.0,
340                false
341            ),
342            None
343        );
344
345        assert_eq!(
346            capsule.raycast(
347                Ray::new_with_vec3(Point3::new(0.75, 1.75, 0.0), Vec3::new(1.0, 0.0, 0.0)),
348                5.0,
349                false
350            ),
351            None
352        );
353
354        assert_eq!(
355            capsule.raycast(
356                Ray::new_with_vec3(Point3::new(0.75, -1.75, 0.0), Vec3::new(1.0, 0.0, 0.0)),
357                5.0,
358                false
359            ),
360            None
361        );
362    }
363
364    #[test]
365    fn test_sphere() {
366        let capsule = Capsule::new(1.0, 1.0);
367        let up_ray = Ray::new_with_vec3(Point3::new(0.0, -4.0, 0.0), Vec3::new(0.0, 1.0, 0.0));
368        assert_eq!(
369            capsule.raycast(up_ray, 20.0, false),
370            Some(RaycastHitResult {
371                distance: 2.0,
372                normal: -Vec3::y_axis(),
373            })
374        );
375        assert_eq!(capsule.raycast(up_ray, 1.0, false), None);
376        let down_ray = Ray::new_with_vec3(Point3::new(0.0, 4.0, 0.0), Vec3::new(0.0, -1.0, 0.0));
377        assert_eq!(
378            capsule.raycast(down_ray, 20.0, false),
379            Some(RaycastHitResult {
380                distance: 2.0,
381                normal: Vec3::y_axis(),
382            })
383        );
384        assert_eq!(capsule.raycast(down_ray, 1.0, false), None);
385
386        assert_eq!(
387            capsule.raycast(
388                Ray::new_with_vec3(Point3::new(1.75, 1.8, 0.0), Vec3::new(-14.0, 0.0, 6.0)),
389                10.0,
390                false
391            ),
392            None
393        );
394
395        assert_eq!(
396            capsule.raycast(
397                Ray::new_with_vec3(Point3::new(1.75, -1.8, 0.0), Vec3::new(-14.0, 0.0, 6.0)),
398                10.0,
399                false
400            ),
401            None
402        );
403
404        assert!(capsule
405            .raycast(
406                Ray::new_with_vec3(Point3::new(2.0, -1.8, 0.0), Vec3::new(-1.0, 0.0, 0.0)),
407                10.0,
408                false
409            )
410            .is_some());
411    }
412
413    #[test]
414    fn test_cylinder() {
415        let capsule = Capsule::new(1.0, 1.0);
416        let ray = Ray::new_with_vec3(Point3::new(2.0, 0.0, 2.0), Vec3::new(-2.0, 0.0, -1.0));
417        if let Some(result) = capsule.raycast(ray, 20.0, false) {
418            const EPSILON: f32 = 1e-6;
419            assert!((result.distance - Real::sqrt(5.0)).abs() < EPSILON);
420            assert!((result.normal.x - 0.0).abs() < EPSILON);
421            assert!((result.normal.y - 0.0).abs() < EPSILON);
422            assert!((result.normal.z - 1.0).abs() < EPSILON);
423        }
424
425        assert_eq!(
426            capsule.raycast(
427                Ray::new_with_vec3(Point3::new(2.0, 0.0, 0.0), Vec3::new(-2.0, 0.0, 2.0)),
428                10.0,
429                false
430            ),
431            None
432        );
433
434        assert_eq!(
435            capsule.raycast(
436                Ray::new_with_vec3(Point3::new(2.0, 0.0, 0.0), Vec3::new(-1.0, 0.0, 0.0)),
437                0.5,
438                false
439            ),
440            None
441        );
442    }
443
444    #[test]
445    fn test_from_surface_and_outward() {
446        let capsule = Capsule::new(1.0, 1.0);
447
448        // cylinder
449        assert_eq!(
450            capsule.raycast(
451                Ray::new_with_vec3(Point3::new(1.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
452                10.0,
453                true
454            ),
455            None
456        );
457
458        // upper sphere
459        assert_eq!(
460            capsule.raycast(
461                Ray::new_with_vec3(Point3::new(0.0, 2.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
462                10.0,
463                true
464            ),
465            None
466        );
467
468        // down sphere
469        assert_eq!(
470            capsule.raycast(
471                Ray::new_with_vec3(Point3::new(0.0, -2.0, 0.0), Vec3::new(0.0, -1.0, 0.0)),
472                10.0,
473                true
474            ),
475            None
476        );
477
478        // cylinder
479        assert!(capsule
480            .raycast(
481                Ray::new_with_vec3(Point3::new(1.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
482                10.0,
483                false
484            )
485            .is_some());
486
487        // upper sphere
488        assert!(capsule
489            .raycast(
490                Ray::new_with_vec3(Point3::new(0.0, 2.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
491                10.0,
492                false
493            )
494            .is_some());
495
496        // down sphere
497        assert!(capsule
498            .raycast(
499                Ray::new_with_vec3(Point3::new(0.0, -2.0, 0.0), Vec3::new(0.0, -1.0, 0.0)),
500                10.0,
501                false
502            )
503            .is_some());
504    }
505}
phys_raycast/shapes/capsule.rs

phys_raycast/shapes/
capsule.rs
