ncollide3d/query/ray/
ray_ball.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
use na;
#[cfg(feature = "dim3")]
use na::Point2;
use simba::scalar::RealField;

#[cfg(feature = "dim3")]
use crate::math::Vector;
use crate::math::{Isometry, Point};
use crate::query::{Ray, RayCast, RayIntersection};
use crate::shape::{Ball, FeatureId};

#[cfg(feature = "dim3")]
#[inline]
fn ball_uv<N: RealField + Copy>(normal: &Vector<N>) -> Point2<N> {
    let two_pi: N = RealField::two_pi();
    let pi: N = RealField::pi();
    let _0_5: N = na::convert(0.5f64);
    let uvx = _0_5 + normal[2].atan2(normal[0]) / two_pi;
    let uvy = _0_5 - normal[1].asin() / pi;

    Point2::new(uvx, uvy)
}

impl<N: RealField + Copy> RayCast<N> for Ball<N> {
    #[inline]
    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, solid: bool) -> Option<N> {
        ray_toi_with_ball(&Point::from(m.translation.vector), self.radius, ray, solid)
            .1
            .filter(|toi| *toi <= max_toi)
    }

    #[inline]
    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        max_toi: N,
        solid: bool,
    ) -> Option<RayIntersection<N>> {
        ray_toi_and_normal_with_ball(&Point::from(m.translation.vector), self.radius, ray, solid)
            .1
            .filter(|int| int.toi <= max_toi)
    }

    #[cfg(feature = "dim3")]
    #[inline]
    fn toi_and_normal_and_uv_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        max_toi: N,
        solid: bool,
    ) -> Option<RayIntersection<N>> {
        let center = Point::from(m.translation.vector);
        let (inside, mut inter) = ray_toi_with_ball(&center, self.radius, ray, solid);
        inter = inter.filter(|toi| *toi <= max_toi);

        inter.map(|n| {
            let pos = ray.origin + ray.dir * n - center;
            let normal = pos.normalize();
            let uv = ball_uv(&normal);

            RayIntersection::new_with_uvs(
                n,
                if inside { -normal } else { normal },
                FeatureId::Face(0),
                Some(uv),
            )
        })
    }
}

/// Computes the time of impact of a ray on a ball.
///
/// The first result element is `true` if the ray started inside of the ball.
#[inline]
pub fn ray_toi_with_ball<N: RealField + Copy>(
    center: &Point<N>,
    radius: N,
    ray: &Ray<N>,
    solid: bool,
) -> (bool, Option<N>) {
    let dcenter = ray.origin - *center;

    let a = ray.dir.norm_squared();
    let b = dcenter.dot(&ray.dir);
    let c = dcenter.norm_squared() - radius * radius;

    // Special case for when the dir is zero.
    if a.is_zero() {
        if c > na::zero() {
            return (false, None);
        } else {
            return (true, Some(na::zero()));
        }
    }

    if c > na::zero() && b > na::zero() {
        (false, None)
    } else {
        let delta = b * b - a * c;

        if delta < na::zero() {
            // no solution
            (false, None)
        } else {
            let t = (-b - delta.sqrt()) / a;

            if t <= na::zero() {
                // origin inside of the ball
                if solid {
                    (true, Some(na::zero()))
                } else {
                    (true, Some((-b + delta.sqrt()) / a))
                }
            } else {
                (false, Some(t))
            }
        }
    }
}

/// Computes the time of impact and contact normal of a ray on a ball.
#[inline]
pub fn ray_toi_and_normal_with_ball<N: RealField + Copy>(
    center: &Point<N>,
    radius: N,
    ray: &Ray<N>,
    solid: bool,
) -> (bool, Option<RayIntersection<N>>) {
    let (inside, inter) = ray_toi_with_ball(&center, radius, ray, solid);

    (
        inside,
        inter.map(|n| {
            let pos = ray.origin + ray.dir * n - center;
            let normal = pos.normalize();

            RayIntersection::new(n, if inside { -normal } else { normal }, FeatureId::Face(0))
        }),
    )
}