ncollide3d/query/ray/
ray_compound.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
143
144
use crate::bounding_volume::AABB;
use crate::math::Isometry;
use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor, BVH};
use crate::query::{Ray, RayCast, RayIntersection};
use crate::shape::Compound;
use na::RealField;

// XXX: if solid == false, this might return internal intersection.
impl<N: RealField + Copy> RayCast<N> for Compound<N> {
    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, solid: bool) -> Option<N> {
        let ls_ray = ray.inverse_transform_by(m);

        let mut visitor = CompoundRayToiVisitor {
            compound: self,
            ray: &ls_ray,
            max_toi,
            solid: solid,
        };

        self.bvt().best_first_search(&mut visitor).map(|res| res.1)
    }

    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        max_toi: N,
        solid: bool,
    ) -> Option<RayIntersection<N>> {
        let ls_ray = ray.inverse_transform_by(m);

        let mut visitor = CompoundRayToiAndNormalVisitor {
            compound: self,
            ray: &ls_ray,
            max_toi,
            solid,
        };

        self.bvt()
            .best_first_search(&mut visitor)
            .map(|(_, mut res)| {
                res.normal = m * res.normal;
                res
            })
    }

    // XXX: We have to implement toi_and_normal_and_uv_with_ray! Otherwise, no uv will be computed
    // for any of the sub-shapes.
}

/*
 * Costs functions.
 */
struct CompoundRayToiVisitor<'a, N: 'a + RealField + Copy> {
    compound: &'a Compound<N>,
    ray: &'a Ray<N>,
    max_toi: N,
    solid: bool,
}

impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>> for CompoundRayToiVisitor<'a, N> {
    type Result = N;

    #[inline]
    fn visit(
        &mut self,
        best: N,
        aabb: &AABB<N>,
        data: Option<&usize>,
    ) -> BestFirstVisitStatus<N, Self::Result> {
        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
            let mut res = BestFirstVisitStatus::Continue {
                cost: toi,
                result: None,
            };

            if let Some(b) = data {
                if toi < best {
                    let elt = &self.compound.shapes()[*b];
                    if let Some(toi) =
                        elt.1
                            .toi_with_ray(&elt.0, self.ray, self.max_toi, self.solid)
                    {
                        res = BestFirstVisitStatus::Continue {
                            cost: toi,
                            result: Some(toi),
                        };
                    }
                }
            }

            res
        } else {
            BestFirstVisitStatus::Stop
        }
    }
}

struct CompoundRayToiAndNormalVisitor<'a, N: 'a + RealField + Copy> {
    compound: &'a Compound<N>,
    ray: &'a Ray<N>,
    max_toi: N,
    solid: bool,
}

impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>>
    for CompoundRayToiAndNormalVisitor<'a, N>
{
    type Result = RayIntersection<N>;

    #[inline]
    fn visit(
        &mut self,
        best: N,
        aabb: &AABB<N>,
        data: Option<&usize>,
    ) -> BestFirstVisitStatus<N, Self::Result> {
        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
            let mut res = BestFirstVisitStatus::Continue {
                cost: toi,
                result: None,
            };

            if let Some(b) = data {
                if toi < best {
                    let elt = &self.compound.shapes()[*b];
                    if let Some(toi) =
                        elt.1
                            .toi_and_normal_with_ray(&elt.0, self.ray, self.max_toi, self.solid)
                    {
                        res = BestFirstVisitStatus::Continue {
                            cost: toi.toi,
                            result: Some(toi),
                        }
                    }
                }
            }

            res
        } else {
            BestFirstVisitStatus::Stop
        }
    }
}