phys-raycast 2.0.0

// Copyright (C) 2020-2025 phys-raycast authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use phys_geom::math::{Real, *};
use phys_geom::shape::Capsule;

use crate::{Raycast, RaycastHitResult};

impl Raycast for Capsule {
    fn raycast(
        &self,
        local_ray: phys_geom::Ray,
        max_distance: Real,
        discard_inside_hit: bool,
    ) -> Option<RaycastHitResult> {
        let radius = self.radius();
        let half_height = self.half_height();

        // Move ray origin closer to the capsule, to avoid numerical problems.
        // Here we estimate an offset, rather than accurately calculate the distance between the
        // origin and the capsule.
        let offset = (-local_ray
            .origin
            .coords
            .dot(&local_ray.direction.into_inner())
            - (half_height + radius))
            .max(0.0);
        let translated_origin = local_ray.origin + local_ray.direction.into_inner() * offset;

        // Test ray with infinite cylinder.
        let origin_xz = Vec3::new(translated_origin.x, 0.0, translated_origin.z);
        let direction_xz = Vec3::new(local_ray.direction.x, 0.0, local_ray.direction.z);
        let discr_half_b = origin_xz.dot(&direction_xz);
        let discr_c = origin_xz.norm_squared() - radius * radius;

        // Ray is outside and pointing away
        if discr_half_b > 0.0 && discr_c > 0.0 {
            return None;
        }

        let sphere_center_y: Real;

        // Determine whether it is possible to intersect with the cylinder. If it is not possible,
        // perform an intersection test between the ray and the upper and lower spheres.
        let discr_a = direction_xz.norm_squared();
        if discr_a > 1e-8 {
            let discr = discr_half_b * discr_half_b - discr_a * discr_c;
            if discr < 0.0 {
                return None;
            }
            let mut distance = (-discr_half_b - discr.sqrt()) / discr_a;

            // If the origin is inside the infinite cylinder (circle).
            let inside = if distance < -offset {
                distance = -offset;
                true
            } else {
                false
            };

            let hit_position = translated_origin + local_ray.direction.into_inner() * distance;
            if hit_position.y > half_height {
                sphere_center_y = half_height;
            } else if hit_position.y < -half_height {
                sphere_center_y = -half_height;
            } else {
                if inside {
                    if discard_inside_hit {
                        return None;
                    }
                    return Some(RaycastHitResult {
                        distance: 0.0,
                        normal: -local_ray.direction,
                    });
                }
                distance += offset;
                if distance <= max_distance {
                    return Some(RaycastHitResult {
                        distance,
                        normal: UnitVec3::new_normalize(Vec3::new(
                            hit_position.x,
                            0.0,
                            hit_position.z,
                        )),
                    });
                }
                return None;
            }
        } else {
            // If the origin is inside the sphere.
            if discr_c < 0.0
                && translated_origin.y < half_height
                && translated_origin.y > -half_height
                && half_height > 0.0
            {
                if discard_inside_hit {
                    return None;
                }
                return Some(RaycastHitResult {
                    distance: 0.0,
                    normal: -local_ray.direction,
                });
            }

            // Determine which sphere the ray may intersect with.
            sphere_center_y = if translated_origin.y > 0.0 {
                half_height
            } else {
                -half_height
            };
        }

        // Test ray with sphere.
        let origin_relative_to_sphere = translated_origin - Point3::new(0.0, sphere_center_y, 0.0);
        let discr_half_b = origin_relative_to_sphere.dot(&local_ray.direction.into_inner());
        let discr_c = origin_relative_to_sphere.norm_squared() - radius * radius;

        // Ray is outside and pointing away
        if discr_half_b > 0.0 && discr_c > 0.0 {
            return None;
        }

        let discr = discr_half_b * discr_half_b - discr_c;
        if discr < 0.0 {
            return None;
        }

        let mut distance = -discr_half_b - discr.sqrt();

        // If the origin is inside the sphere.
        if distance < -offset {
            if discard_inside_hit {
                return None;
            }
            return Some(RaycastHitResult {
                distance: 0.0,
                normal: -local_ray.direction,
            });
        }

        let hit_position_relative_to_sphere =
            origin_relative_to_sphere + local_ray.direction.into_inner() * distance;

        distance += offset;
        if distance <= max_distance {
            Some(RaycastHitResult {
                distance,
                normal: UnitVec3::new_normalize(hit_position_relative_to_sphere),
            })
        } else {
            None
        }
    }
}

#[cfg(test)]
mod tests {
    use approx::assert_relative_eq;
    use geom::Ray;

    use super::*;

    #[test]
    fn test_raycast_capsule_cylinder() {
        let capsule = Capsule::new(2.0, 0.5); // half_height=2.0, radius=0.5

        // Direct hit on cylinder from outside
        let ray = phys_geom::Ray::new(
            Point3::new(-2.0, 0.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
        );

        let hit = capsule.raycast(ray, 10.0, false).unwrap();
        assert_relative_eq!(hit.distance, 1.5);
        assert_relative_eq!(
            hit.normal,
            UnitVec3::new_normalize(Vec3::new(-1.0, 0.0, 0.0))
        );
    }

    #[test]
    fn test_raycast_capsule_top_sphere() {
        let capsule = Capsule::new(2.0, 0.5);

        // Hit on top sphere
        let ray = phys_geom::Ray::new(
            Point3::new(0.0, 4.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(0.0, -1.0, 0.0)),
        );

        let hit = capsule.raycast(ray, 10.0, false).unwrap();
        assert_relative_eq!(hit.distance, 1.5);
        assert_relative_eq!(
            hit.normal,
            UnitVec3::new_normalize(Vec3::new(0.0, 1.0, 0.0))
        );
    }

    #[test]
    fn test_raycast_capsule_inside() {
        let capsule = Capsule::new(2.0, 0.5);

        // Ray from inside
        let ray = phys_geom::Ray::new(
            Point3::new(0.0, 0.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
        );

        let hit = capsule.raycast(ray, 10.0, false).unwrap();
        assert_relative_eq!(hit.distance, 0.0);
        assert_relative_eq!(
            hit.normal,
            UnitVec3::new_normalize(Vec3::new(-1.0, 0.0, 0.0))
        );
    }

    #[test]
    fn test_raycast_capsule_inside_discarded() {
        let capsule = Capsule::new(2.0, 0.5);

        // Ray from inside but discard inside hits
        let ray = phys_geom::Ray::new(
            Point3::new(0.0, 0.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
        );

        assert_eq!(capsule.raycast(ray, 10.0, true), None);
    }

    #[test]
    fn test_raycast_capsule_miss() {
        let capsule = Capsule::new(2.0, 0.5);

        // Ray that misses the capsule
        let ray = phys_geom::Ray::new(
            Point3::new(-2.0, 5.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
        );

        assert_eq!(capsule.raycast(ray, 10.0, false), None);
    }

    #[test]
    fn test_raycast_capsule_max_distance() {
        let capsule = Capsule::new(2.0, 0.5);

        let ray = phys_geom::Ray::new(
            Point3::new(-5.0, 0.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(1.0, 0.0, 0.0)),
        );

        // Max distance too short
        assert_eq!(capsule.raycast(ray, 1.0, false), None);

        // Max distance sufficient
        let hit = capsule.raycast(ray, 5.0, false).unwrap();
        assert_relative_eq!(hit.distance, 4.5);
    }

    /// If the height is too small, it may cause numerical problems.
    #[test]
    fn test_small_segment() {
        let capsule = Capsule::new(0.0, 1.0);
        let ray = Ray::new_with_vec3(Point3::new(0.0, 0.0, 2.0), Vec3::new(0.0, 0.0, -1.0));
        assert_eq!(
            capsule.raycast(ray, 5.0, false),
            Some(RaycastHitResult {
                distance: 1.0,
                normal: Vec3::z_axis(),
            })
        );
    }

    // If the origin is inside the capsule.
    #[test]
    fn test_inner() {
        let capsule = Capsule::new(1.0, 1.0);
        let ray_in_cylinder =
            Ray::new_with_vec3(Point3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, -1.0));
        assert_eq!(
            capsule.raycast(ray_in_cylinder, 5.0, false),
            Some(RaycastHitResult {
                distance: 0.0,
                normal: -ray_in_cylinder.direction,
            })
        );
        let ray_in_sphere =
            Ray::new_with_vec3(Point3::new(0.0, 1.1, 0.0), Vec3::new(0.0, 0.0, -1.0));
        assert_eq!(
            capsule.raycast(ray_in_sphere, 5.0, false),
            Some(RaycastHitResult {
                distance: 0.0,
                normal: -ray_in_sphere.direction,
            })
        );

        assert_eq!(capsule.raycast(ray_in_cylinder, 5.0, true), None);
        assert_eq!(capsule.raycast(ray_in_sphere, 5.0, true), None);

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.0, 0.5, 0.0), Vec3::new(0.0, 1.0, 0.0)),
                5.0,
                false
            ),
            Some(RaycastHitResult {
                distance: 0.0,
                normal: UnitVec3::new_unchecked(Vec3::new(0.0, -1.0, 0.0)),
            })
        );

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.0, 0.5, 0.0), Vec3::new(0.0, 1.0, 0.0)),
                5.0,
                true
            ),
            None
        );
    }

    #[test]
    fn test_outer() {
        let capsule = Capsule::new(1.0, 1.0);
        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(1.1, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
                5.0,
                false
            ),
            None
        );

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.75, 1.75, 0.0), Vec3::new(1.0, 0.0, 0.0)),
                5.0,
                false
            ),
            None
        );

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.75, -1.75, 0.0), Vec3::new(1.0, 0.0, 0.0)),
                5.0,
                false
            ),
            None
        );
    }

    #[test]
    fn test_sphere() {
        let capsule = Capsule::new(1.0, 1.0);
        let up_ray = Ray::new_with_vec3(Point3::new(0.0, -4.0, 0.0), Vec3::new(0.0, 1.0, 0.0));
        assert_eq!(
            capsule.raycast(up_ray, 20.0, false),
            Some(RaycastHitResult {
                distance: 2.0,
                normal: -Vec3::y_axis(),
            })
        );
        assert_eq!(capsule.raycast(up_ray, 1.0, false), None);
        let down_ray = Ray::new_with_vec3(Point3::new(0.0, 4.0, 0.0), Vec3::new(0.0, -1.0, 0.0));
        assert_eq!(
            capsule.raycast(down_ray, 20.0, false),
            Some(RaycastHitResult {
                distance: 2.0,
                normal: Vec3::y_axis(),
            })
        );
        assert_eq!(capsule.raycast(down_ray, 1.0, false), None);

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(1.75, 1.8, 0.0), Vec3::new(-14.0, 0.0, 6.0)),
                10.0,
                false
            ),
            None
        );

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(1.75, -1.8, 0.0), Vec3::new(-14.0, 0.0, 6.0)),
                10.0,
                false
            ),
            None
        );

        assert!(capsule
            .raycast(
                Ray::new_with_vec3(Point3::new(2.0, -1.8, 0.0), Vec3::new(-1.0, 0.0, 0.0)),
                10.0,
                false
            )
            .is_some());
    }

    #[test]
    fn test_cylinder() {
        let capsule = Capsule::new(1.0, 1.0);
        let ray = Ray::new_with_vec3(Point3::new(2.0, 0.0, 2.0), Vec3::new(-2.0, 0.0, -1.0));
        if let Some(result) = capsule.raycast(ray, 20.0, false) {
            const EPSILON: f32 = 1e-6;
            assert!((result.distance - Real::sqrt(5.0)).abs() < EPSILON);
            assert!((result.normal.x - 0.0).abs() < EPSILON);
            assert!((result.normal.y - 0.0).abs() < EPSILON);
            assert!((result.normal.z - 1.0).abs() < EPSILON);
        }

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(2.0, 0.0, 0.0), Vec3::new(-2.0, 0.0, 2.0)),
                10.0,
                false
            ),
            None
        );

        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(2.0, 0.0, 0.0), Vec3::new(-1.0, 0.0, 0.0)),
                0.5,
                false
            ),
            None
        );
    }

    #[test]
    fn test_from_surface_and_outward() {
        let capsule = Capsule::new(1.0, 1.0);

        // cylinder
        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(1.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
                10.0,
                true
            ),
            None
        );

        // upper sphere
        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.0, 2.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
                10.0,
                true
            ),
            None
        );

        // down sphere
        assert_eq!(
            capsule.raycast(
                Ray::new_with_vec3(Point3::new(0.0, -2.0, 0.0), Vec3::new(0.0, -1.0, 0.0)),
                10.0,
                true
            ),
            None
        );

        // cylinder
        assert!(capsule
            .raycast(
                Ray::new_with_vec3(Point3::new(1.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)),
                10.0,
                false
            )
            .is_some());

        // upper sphere
        assert!(capsule
            .raycast(
                Ray::new_with_vec3(Point3::new(0.0, 2.0, 0.0), Vec3::new(0.0, 1.0, 0.0)),
                10.0,
                false
            )
            .is_some());

        // down sphere
        assert!(capsule
            .raycast(
                Ray::new_with_vec3(Point3::new(0.0, -2.0, 0.0), Vec3::new(0.0, -1.0, 0.0)),
                10.0,
                false
            )
            .is_some());
    }
}