parry3d/query/ray/
ray_ball.rs

1use na::{self, ComplexField};
2
3use crate::math::{Point, Real};
4use crate::query::{Ray, RayCast, RayIntersection};
5use crate::shape::{Ball, FeatureId};
6use num::Zero;
7
8impl RayCast for Ball {
9    #[inline]
10    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
11        ray_toi_with_ball(&Point::origin(), self.radius, ray, solid)
12            .1
13            .filter(|time_of_impact| *time_of_impact <= max_time_of_impact)
14    }
15
16    #[inline]
17    fn cast_local_ray_and_get_normal(
18        &self,
19        ray: &Ray,
20        max_time_of_impact: Real,
21        solid: bool,
22    ) -> Option<RayIntersection> {
23        ray_toi_and_normal_with_ball(&Point::origin(), self.radius, ray, solid)
24            .1
25            .filter(|int| int.time_of_impact <= max_time_of_impact)
26    }
27}
28
29/// Computes the time of impact of a ray on a ball.
30///
31/// The first result element is `true` if the ray started inside of the ball.
32#[inline]
33pub fn ray_toi_with_ball(
34    center: &Point<Real>,
35    radius: Real,
36    ray: &Ray,
37    solid: bool,
38) -> (bool, Option<Real>) {
39    let dcenter = ray.origin - *center;
40
41    let a = ray.dir.norm_squared();
42    let b = dcenter.dot(&ray.dir);
43    let c = dcenter.norm_squared() - radius * radius;
44
45    // Special case for when the dir is zero.
46    if a.is_zero() {
47        if c > 0.0 {
48            return (false, None);
49        } else {
50            return (true, Some(0.0));
51        }
52    }
53
54    if c > 0.0 && b > 0.0 {
55        (false, None)
56    } else {
57        let delta = b * b - a * c;
58
59        if delta < 0.0 {
60            // no solution
61            (false, None)
62        } else {
63            let t = (-b - ComplexField::sqrt(delta)) / a;
64
65            if t <= 0.0 {
66                // origin inside of the ball
67                if solid {
68                    (true, Some(0.0))
69                } else {
70                    (true, Some((-b + delta.sqrt()) / a))
71                }
72            } else {
73                (false, Some(t))
74            }
75        }
76    }
77}
78
79/// Computes the time of impact and contact normal of a ray on a ball.
80#[inline]
81pub fn ray_toi_and_normal_with_ball(
82    center: &Point<Real>,
83    radius: Real,
84    ray: &Ray,
85    solid: bool,
86) -> (bool, Option<RayIntersection>) {
87    let (inside, inter) = ray_toi_with_ball(center, radius, ray, solid);
88
89    (
90        inside,
91        inter.map(|n| {
92            let pos = ray.origin + ray.dir * n - center;
93            let normal = pos.normalize();
94
95            RayIntersection::new(n, if inside { -normal } else { normal }, FeatureId::Face(0))
96        }),
97    )
98}
parry3d/query/ray/ray_ball.rs

parry3d/query/ray/
ray_ball.rs
