ncollide3d/query/point/
point_segment.rs

Help
1use crate::math::{Isometry, Point};
2use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
3use crate::shape::{FeatureId, Segment, SegmentPointLocation};
4use na::{self, RealField};
5
6impl<N: RealField + Copy> PointQuery<N> for Segment<N> {
7    #[inline]
8    fn project_point(&self, m: &Isometry<N>, pt: &Point<N>, solid: bool) -> PointProjection<N> {
9        let (projection, _) = self.project_point_with_location(m, pt, solid);
10        projection
11    }
12
13    #[inline]
14    fn project_point_with_feature(
15        &self,
16        m: &Isometry<N>,
17        pt: &Point<N>,
18    ) -> (PointProjection<N>, FeatureId) {
19        let (proj, loc) = self.project_point_with_location(m, pt, false);
20        let feature = match loc {
21            SegmentPointLocation::OnVertex(i) => FeatureId::Vertex(i),
22            SegmentPointLocation::OnEdge(..) => {
23                #[cfg(feature = "dim2")]
24                {
25                    let dir = self.scaled_direction();
26                    let dpt = *pt - proj.point;
27                    if dpt.perp(&dir) >= na::zero() {
28                        FeatureId::Face(0)
29                    } else {
30                        FeatureId::Face(1)
31                    }
32                }
33
34                #[cfg(feature = "dim3")]
35                {
36                    FeatureId::Edge(0)
37                }
38            }
39        };
40
41        (proj, feature)
42    }
43
44    // NOTE: the default implementation of `.distance_to_point(...)` will return the error that was
45    // eaten by the `::approx_eq(...)` on `project_point(...)`.
46}
47
48impl<N: RealField + Copy> PointQueryWithLocation<N> for Segment<N> {
49    type Location = SegmentPointLocation<N>;
50
51    #[inline]
52    fn project_point_with_location(
53        &self,
54        m: &Isometry<N>,
55        pt: &Point<N>,
56        _: bool,
57    ) -> (PointProjection<N>, Self::Location) {
58        let ls_pt = m.inverse_transform_point(pt);
59        let ab = self.b - self.a;
60        let ap = ls_pt - self.a;
61        let ab_ap = ab.dot(&ap);
62        let sqnab = ab.norm_squared();
63        let _1 = na::one::<N>();
64
65        let mut proj;
66        let location;
67
68        if ab_ap <= na::zero() {
69            // Voronoï region of vertex 'a'.
70            location = SegmentPointLocation::OnVertex(0);
71            proj = m * self.a;
72        } else if ab_ap >= sqnab {
73            // Voronoï region of vertex 'b'.
74            location = SegmentPointLocation::OnVertex(1);
75            proj = m * self.b;
76        } else {
77            assert!(sqnab != na::zero());
78
79            // Voronoï region of the segment interior.
80            let u = ab_ap / sqnab;
81            let bcoords = [_1 - u, u];
82            location = SegmentPointLocation::OnEdge(bcoords);
83            proj = self.a + ab * u;
84            proj = m * proj;
85        }
86
87        // FIXME: is this acceptable?
88        let inside = relative_eq!(proj, *pt);
89
90        (PointProjection::new(inside, proj), location)
91    }
92}
ncollide3d/query/point/point_segment.rs

ncollide3d/query/point/
point_segment.rs
