ncollide3d/query/point/
point_polyline.rs

Help
1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor, BVH};
4use crate::query::{
5    visitors::CompositePointContainmentTest, PointProjection, PointQuery, PointQueryWithLocation,
6};
7use crate::shape::{FeatureId, Polyline, SegmentPointLocation};
8use na::{self, RealField};
9
10impl<N: RealField + Copy> PointQuery<N> for Polyline<N> {
11    #[inline]
12    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
13        let (projection, _) = self.project_point_with_location(m, point, solid);
14        projection
15    }
16
17    #[inline]
18    fn project_point_with_feature(
19        &self,
20        m: &Isometry<N>,
21        point: &Point<N>,
22    ) -> (PointProjection<N>, FeatureId) {
23        let ls_pt = m.inverse_transform_point(point);
24        let mut visitor = PolylinePointProjWithFeatureVisitor {
25            polyline: self,
26            point: &ls_pt,
27        };
28
29        let (mut proj, (id, feature)) = self.bvt().best_first_search(&mut visitor).unwrap().1;
30        proj.point = m * proj.point;
31
32        let polyline_feature = self.segment_feature_to_polyline_feature(id, feature);
33
34        (proj, polyline_feature)
35    }
36
37    // FIXME: implement distance_to_point too?
38
39    #[inline]
40    fn contains_point(&self, m: &Isometry<N>, point: &Point<N>) -> bool {
41        let ls_pt = m.inverse_transform_point(point);
42        let mut visitor = CompositePointContainmentTest {
43            shape: self,
44            point: &ls_pt,
45            found: false,
46        };
47
48        self.bvt().visit(&mut visitor);
49
50        visitor.found
51    }
52}
53
54impl<N: RealField + Copy> PointQueryWithLocation<N> for Polyline<N> {
55    type Location = (usize, SegmentPointLocation<N>);
56
57    #[inline]
58    fn project_point_with_location(
59        &self,
60        m: &Isometry<N>,
61        point: &Point<N>,
62        _: bool,
63    ) -> (PointProjection<N>, Self::Location) {
64        let ls_pt = m.inverse_transform_point(point);
65        let mut visitor = PolylinePointProjVisitor {
66            polyline: self,
67            point: &ls_pt,
68        };
69
70        let (mut proj, extra_info) = self.bvt().best_first_search(&mut visitor).unwrap().1;
71        proj.point = m * proj.point;
72
73        (proj, extra_info)
74    }
75}
76
77/*
78 * Visitors
79 */
80macro_rules! gen_visitor(
81    ($Visitor: ident, $Location: ty, $project: ident $(, $args: ident)*) => {
82        struct $Visitor<'a, N: 'a + RealField + Copy> {
83            polyline: &'a Polyline<N>,
84            point: &'a Point<N>,
85        }
86
87        impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>> for $Visitor<'a, N> {
88            type Result = (PointProjection<N>, (usize, $Location));
89
90            #[inline]
91            fn visit(
92                &mut self,
93                best: N,
94                aabb: &AABB<N>,
95                data: Option<&usize>,
96            ) -> BestFirstVisitStatus<N, Self::Result> {
97                let dist = aabb.distance_to_point(&Isometry::identity(), self.point, true);
98
99                let mut res = BestFirstVisitStatus::Continue {
100                    cost: dist,
101                    result: None,
102                };
103
104                if let Some(b) = data {
105                    if dist < best {
106                        let (proj, extra_info) = self.polyline.segment_at(*b).$project(
107                            &Isometry::identity(),
108                            self.point
109                            $(, $args)*
110                        );
111
112                        let extra_info = (*b, extra_info);
113                        res = BestFirstVisitStatus::Continue {
114                            cost: na::distance(self.point, &proj.point),
115                            result: Some((proj, extra_info)),
116                        };
117                    }
118                }
119
120                res
121            }
122        }
123    }
124);
125
126gen_visitor!(
127    PolylinePointProjVisitor,
128    SegmentPointLocation<N>,
129    project_point_with_location,
130    true
131);
132gen_visitor!(
133    PolylinePointProjWithFeatureVisitor,
134    FeatureId,
135    project_point_with_feature
136);
ncollide3d/query/point/point_polyline.rs

ncollide3d/query/point/
point_polyline.rs
