ncollide3d/query/ray/
ray_ball.rs

1use na;
2#[cfg(feature = "dim3")]
3use na::Point2;
4use simba::scalar::RealField;
5
6#[cfg(feature = "dim3")]
7use crate::math::Vector;
8use crate::math::{Isometry, Point};
9use crate::query::{Ray, RayCast, RayIntersection};
10use crate::shape::{Ball, FeatureId};
11
12#[cfg(feature = "dim3")]
13#[inline]
14fn ball_uv<N: RealField + Copy>(normal: &Vector<N>) -> Point2<N> {
15    let two_pi: N = RealField::two_pi();
16    let pi: N = RealField::pi();
17    let _0_5: N = na::convert(0.5f64);
18    let uvx = _0_5 + normal[2].atan2(normal[0]) / two_pi;
19    let uvy = _0_5 - normal[1].asin() / pi;
20
21    Point2::new(uvx, uvy)
22}
23
24impl<N: RealField + Copy> RayCast<N> for Ball<N> {
25    #[inline]
26    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, solid: bool) -> Option<N> {
27        ray_toi_with_ball(&Point::from(m.translation.vector), self.radius, ray, solid)
28            .1
29            .filter(|toi| *toi <= max_toi)
30    }
31
32    #[inline]
33    fn toi_and_normal_with_ray(
34        &self,
35        m: &Isometry<N>,
36        ray: &Ray<N>,
37        max_toi: N,
38        solid: bool,
39    ) -> Option<RayIntersection<N>> {
40        ray_toi_and_normal_with_ball(&Point::from(m.translation.vector), self.radius, ray, solid)
41            .1
42            .filter(|int| int.toi <= max_toi)
43    }
44
45    #[cfg(feature = "dim3")]
46    #[inline]
47    fn toi_and_normal_and_uv_with_ray(
48        &self,
49        m: &Isometry<N>,
50        ray: &Ray<N>,
51        max_toi: N,
52        solid: bool,
53    ) -> Option<RayIntersection<N>> {
54        let center = Point::from(m.translation.vector);
55        let (inside, mut inter) = ray_toi_with_ball(&center, self.radius, ray, solid);
56        inter = inter.filter(|toi| *toi <= max_toi);
57
58        inter.map(|n| {
59            let pos = ray.origin + ray.dir * n - center;
60            let normal = pos.normalize();
61            let uv = ball_uv(&normal);
62
63            RayIntersection::new_with_uvs(
64                n,
65                if inside { -normal } else { normal },
66                FeatureId::Face(0),
67                Some(uv),
68            )
69        })
70    }
71}
72
73/// Computes the time of impact of a ray on a ball.
74///
75/// The first result element is `true` if the ray started inside of the ball.
76#[inline]
77pub fn ray_toi_with_ball<N: RealField + Copy>(
78    center: &Point<N>,
79    radius: N,
80    ray: &Ray<N>,
81    solid: bool,
82) -> (bool, Option<N>) {
83    let dcenter = ray.origin - *center;
84
85    let a = ray.dir.norm_squared();
86    let b = dcenter.dot(&ray.dir);
87    let c = dcenter.norm_squared() - radius * radius;
88
89    // Special case for when the dir is zero.
90    if a.is_zero() {
91        if c > na::zero() {
92            return (false, None);
93        } else {
94            return (true, Some(na::zero()));
95        }
96    }
97
98    if c > na::zero() && b > na::zero() {
99        (false, None)
100    } else {
101        let delta = b * b - a * c;
102
103        if delta < na::zero() {
104            // no solution
105            (false, None)
106        } else {
107            let t = (-b - delta.sqrt()) / a;
108
109            if t <= na::zero() {
110                // origin inside of the ball
111                if solid {
112                    (true, Some(na::zero()))
113                } else {
114                    (true, Some((-b + delta.sqrt()) / a))
115                }
116            } else {
117                (false, Some(t))
118            }
119        }
120    }
121}
122
123/// Computes the time of impact and contact normal of a ray on a ball.
124#[inline]
125pub fn ray_toi_and_normal_with_ball<N: RealField + Copy>(
126    center: &Point<N>,
127    radius: N,
128    ray: &Ray<N>,
129    solid: bool,
130) -> (bool, Option<RayIntersection<N>>) {
131    let (inside, inter) = ray_toi_with_ball(&center, radius, ray, solid);
132
133    (
134        inside,
135        inter.map(|n| {
136            let pos = ray.origin + ray.dir * n - center;
137            let normal = pos.normalize();
138
139            RayIntersection::new(n, if inside { -normal } else { normal }, FeatureId::Face(0))
140        }),
141    )
142}