parry3d/query/point/
point_segment.rs

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

parry3d/query/point/
point_segment.rs
