ncollide3d/query/ray/
ray_polyline.rs

Help
1use crate::bounding_volume::AABB;
2use crate::math::Isometry;
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor, BVH};
4use crate::query::{Ray, RayCast, RayIntersection};
5use crate::shape::{FeatureId, Polyline};
6use na::RealField;
7
8impl<N: RealField + Copy> RayCast<N> for Polyline<N> {
9    #[inline]
10    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, _: bool) -> Option<N> {
11        let ls_ray = ray.inverse_transform_by(m);
12
13        let mut visitor = PolylineRayToiVisitor {
14            polyline: self,
15            ray: &ls_ray,
16            max_toi,
17        };
18
19        self.bvt().best_first_search(&mut visitor).map(|res| res.1)
20    }
21
22    #[inline]
23    fn toi_and_normal_with_ray(
24        &self,
25        m: &Isometry<N>,
26        ray: &Ray<N>,
27        max_toi: N,
28        _: bool,
29    ) -> Option<RayIntersection<N>> {
30        let ls_ray = ray.inverse_transform_by(m);
31
32        let mut visitor = PolylineRayToiAndNormalVisitor {
33            polyline: self,
34            ray: &ls_ray,
35            max_toi,
36        };
37
38        self.bvt()
39            .best_first_search(&mut visitor)
40            .map(|(_, (best, mut res))| {
41                if let FeatureId::Face(1) = res.feature {
42                    res.feature = FeatureId::Face(best + self.edges().len());
43                } else {
44                    res.feature = FeatureId::Face(best);
45                }
46
47                res.normal = m * res.normal;
48                res
49            })
50    }
51}
52
53/*
54 * Costs functions.
55 */
56struct PolylineRayToiVisitor<'a, N: 'a + RealField + Copy> {
57    polyline: &'a Polyline<N>,
58    ray: &'a Ray<N>,
59    max_toi: N,
60}
61
62impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>> for PolylineRayToiVisitor<'a, N> {
63    type Result = N;
64
65    #[inline]
66    fn visit(
67        &mut self,
68        best: N,
69        aabb: &AABB<N>,
70        data: Option<&usize>,
71    ) -> BestFirstVisitStatus<N, Self::Result> {
72        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
73            let mut res = BestFirstVisitStatus::Continue {
74                cost: toi,
75                result: None,
76            };
77
78            if let Some(b) = data {
79                if toi < best {
80                    // FIXME: optimize this by not using Isometry identity.
81                    let segment = self.polyline.segment_at(*b);
82                    if let Some(toi) =
83                        segment.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true)
84                    {
85                        res = BestFirstVisitStatus::Continue {
86                            cost: toi,
87                            result: Some(toi),
88                        }
89                    }
90                }
91            }
92
93            res
94        } else {
95            BestFirstVisitStatus::Stop
96        }
97    }
98}
99
100struct PolylineRayToiAndNormalVisitor<'a, N: 'a + RealField + Copy> {
101    polyline: &'a Polyline<N>,
102    ray: &'a Ray<N>,
103    max_toi: N,
104}
105
106impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>>
107    for PolylineRayToiAndNormalVisitor<'a, N>
108{
109    type Result = (usize, RayIntersection<N>);
110
111    #[inline]
112    fn visit(
113        &mut self,
114        best: N,
115        aabb: &AABB<N>,
116        data: Option<&usize>,
117    ) -> BestFirstVisitStatus<N, Self::Result> {
118        if let Some(toi) = aabb.toi_with_ray(&Isometry::identity(), self.ray, self.max_toi, true) {
119            let mut res = BestFirstVisitStatus::Continue {
120                cost: toi,
121                result: None,
122            };
123
124            if let Some(b) = data {
125                if toi < best {
126                    // FIXME: optimize this by not using Isometry identity.
127                    let segment = self.polyline.segment_at(*b);
128                    if let Some(toi) = segment.toi_and_normal_with_ray(
129                        &Isometry::identity(),
130                        self.ray,
131                        self.max_toi,
132                        true,
133                    ) {
134                        res = BestFirstVisitStatus::Continue {
135                            cost: toi.toi,
136                            result: Some((*b, toi)),
137                        };
138                    }
139                }
140            }
141
142            res
143        } else {
144            BestFirstVisitStatus::Stop
145        }
146    }
147}
ncollide3d/query/ray/ray_polyline.rs

ncollide3d/query/ray/
ray_polyline.rs
