parry2d/query/ray/
ray_ball.rs

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

parry2d/query/ray/
ray_ball.rs
