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::*;
use phys_geom::shape::Cylinder;

use crate::{Raycast, RaycastHitResult};

impl Raycast for Cylinder {
    #[allow(clippy::many_single_char_names)]
    fn raycast(
        &self,
        local_ray: phys_geom::Ray,
        max_distance: Real,
        discard_inside_hit: bool,
    ) -> Option<RaycastHitResult> {
        let half_height = self.half_height();
        let radius = self.radius();

        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 and infinite cylinder
        // 'origin_xz' is ray origin ('o'), 'direction_xz' is ray direction ('d'), on plane y = 0, r
        // is circle radius
        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);

        // the circle bottom at y = circle_center_y to test
        let circle_center_y: Real;

        // test ray with circle at plane y = 0
        // point on ray is (o + td)
        // solve t in equation (o + td).dot(o + td) - r * r = 0
        // which is d.dot(d)*t^2 + 2*o.dot(d)*t + o.dot(o) - r^2 = 0
        let discr_half_b = origin_xz.dot(&direction_xz);
        let discr_c = origin_xz.norm_squared() - radius * radius;

        if discr_half_b > 0.0 && discr_c > 0.0 {
            // c > 0 means ray origin is outside the circle
            // half_b > 0 means ray is pointing away from the circle
            return None;
        }

        let discr_a = direction_xz.norm_squared();
        if discr_a > 1e-8 {
            let delta = discr_half_b * discr_half_b - discr_a * discr_c;
            if delta < 0.0 {
                return None;
            }
            // use the less solution
            let mut t = (-discr_half_b - delta.sqrt()) / discr_a;
            let inside = if t < -offset {
                t = -offset;
                true
            } else {
                false
            };

            // if hit position y in [-half_height, half_height], it is a hit on side
            // otherwise need check whether hit circle bottom or top
            let hit_position = translated_origin + local_ray.direction.into_inner() * t;
            if hit_position.y > half_height {
                circle_center_y = half_height;
            } else if hit_position.y < -half_height {
                circle_center_y = -half_height;
            } else {
                if inside {
                    if discard_inside_hit {
                        return None;
                    }
                    return Some(RaycastHitResult {
                        distance: 0.0,
                        normal: -local_ray.direction,
                    });
                }
                // hit on the side of cylinder
                let normal =
                    UnitVec3::new_normalize(Vec3::new(hit_position.x, 0.0, hit_position.z));
                let distance = t + offset;
                if distance <= max_distance {
                    return Some(RaycastHitResult { distance, normal });
                }
                return None;
            }
        } else {
            // ray is parallel to the infinite cylinder
            circle_center_y = if local_ray.direction.y > 0.0 {
                -half_height
            } else {
                half_height
            }
        }

        // test ray with circle bottom at plane y = circle_center_y
        if translated_origin.y.abs() > half_height
            && translated_origin.y * local_ray.direction.y >= 0.0
        {
            // ray origin is outside and direction toward the cylinder circle
            return None;
        }

        // ray: translated_origin + t * local_ray.direction
        // hit plane at y = circle_center_y
        let t = (circle_center_y - translated_origin.y) / local_ray.direction.y;
        let hit_position = translated_origin + local_ray.direction.into_inner() * t;

        // check hit position whether in circle
        if hit_position.x * hit_position.x + hit_position.z * hit_position.z > radius * radius {
            return None;
        }

        let distance = t + offset;
        if distance <= max_distance {
            Some(RaycastHitResult {
                distance,
                normal: UnitVec3::new_normalize(Vec3::new(
                    0.0,
                    if local_ray.direction.y < 0.0 {
                        1.0
                    } else {
                        -1.0
                    },
                    0.0,
                )),
            })
        } else {
            None
        }
    }
}

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

    use super::*;

    #[test]
    fn test_raycast_cylinder_side() {
        let cylinder = Cylinder::new(2.0, 1.0); // half_height=2.0, radius=1.0

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

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

    #[test]
    fn test_raycast_cylinder_top() {
        let cylinder = Cylinder::new(2.0, 1.0);

        // Hit on top of cylinder
        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 = cylinder.raycast(ray, 10.0, false).unwrap();
        assert_relative_eq!(hit.distance, 2.0);
        assert_relative_eq!(
            hit.normal,
            UnitVec3::new_normalize(Vec3::new(0.0, 1.0, 0.0))
        );
    }

    #[test]
    fn test_raycast_cylinder_bottom() {
        let cylinder = Cylinder::new(2.0, 1.0);

        // Hit on bottom of cylinder
        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 = cylinder.raycast(ray, 10.0, false).unwrap();
        assert_relative_eq!(hit.distance, 2.0);
        assert_relative_eq!(
            hit.normal,
            UnitVec3::new_normalize(Vec3::new(0.0, -1.0, 0.0))
        );
    }

    #[test]
    fn test_raycast_cylinder_inside() {
        let cylinder = Cylinder::new(2.0, 1.0);

        // 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 = cylinder.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_cylinder_inside_discarded() {
        let cylinder = Cylinder::new(2.0, 1.0);

        // 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!(cylinder.raycast(ray, 10.0, true), None);
    }

    #[test]
    fn test_raycast_cylinder_miss() {
        let cylinder = Cylinder::new(2.0, 1.0);

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

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

    #[test]
    fn test_raycast_cylinder_parallel() {
        let cylinder = Cylinder::new(2.0, 1.0);

        // Ray parallel to cylinder axis, hitting the top
        let ray = phys_geom::Ray::new(
            Point3::new(0.5, 4.0, 0.0),
            UnitVec3::new_normalize(Vec3::new(0.0, -1.0, 0.0)),
        );

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

    #[test]
    fn test_raycast_cylinder_max_distance() {
        let cylinder = Cylinder::new(2.0, 1.0);

        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!(cylinder.raycast(ray, 1.0, false), None);

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